# -*- coding: utf-8 -*-
# Méthodes d'estimation – SIE 3e semestre, GC 5e semestre
# ESO-TOPO / 24.10.23

from covmat2cormat import covmat2cormat
import numpy as np
np.set_printoptions(precision=4)

print('-------------------------------------------------------------------')
print('compensation conditionnelle pour un gaz parfait mesuré dans 2 états')
print('-------------------------------------------------------------------')

# Les données comprennent 5 états et peuvent contenir des fautes.

# Le code est conçu pour 2 états et doit être généralisé pour m états.
# En clair, il faut mécaniser la construction de w, de B et de Kll.

# Lorsque votre code tourne, la seconde moitié du travail peut commencer:
# INTERPRETER LES RESULTATS.

# Selon l'alerte émise lors de l'exercice précédent (voir le listing
# commenté), les données corrigées sont disponibles sous forme de
# commentaires.

# 1. Mesures

#  [mb]      [ml]        [K]

P1 = 1493.0 ; V1 = 1502.0 ; T1 = 675.0
P2 =  752.0 ; V2 = 2668.0 ; T2 = 699.0
P3 = 2008.0 ; V3 = 1497.0 ; T3 = 902.0
P4 = 2001.0 ; V4 =  720.0 ; T4 = 419.0
P5 =  996.0 ; V5 =  993.0 ; T5 = 303.0

#T2 = 599.0  # T2 corrigé
#V4 = 702.0; # V4 corrigé

# 2. Ecarts-types (a priori)

sigmaP = 5.0   # [mb]
sigmaV = 3.0   # [ml]
sigmaT = 2.0   # [K]

# ============================================================================

# Formation du vecteur des observations

obs = np.array([P1, V1, T1, P2, V2, T2, P3, V3, T3, P4, V4, T4, P5, V5, T5])

obs = obs.reshape((-1, 1))               # vecteur-colonne

# nombre d'états mesurés

n = len(obs)
m = int(n/3)

# SURDETERMINATION
# nombre de conditions (au moins partiellement) indépendantes

r = int(m-1)
print('\n', 'r = ', r)

# MODELE FONCTIONNEL
# conditions et écarts de fermeture w
# construction de la matrice des dérivées partielles B

# réservation d'espaces contigus en mémoire
w = np.zeros((r, 1))
B = np.zeros((r, m*3))

#w[0] = obs[0,0]*obs[1,0]*obs[5,0]-obs[3,0]*obs[4,0]*obs[2,0]
print('\n obs = ', obs)

for i in range(0,r):

    ja = 3*i
    jb = 3*i + 3

    w[i,0] = obs[ja,0]*obs[ja+1,0]*obs[jb+2,0] - obs[jb,0]*obs[jb+1,0]*obs[ja+2,0]
    
    print('\n', 'w [%1d]= %f' % (i,w[i,0]) )

# linéarisation des conditions en fonction de Pa, Va, Ta, Pb, Vb, Tb

    B[i,ja:ja+3] = [ obs[ja+1,0]*obs[jb+2,0], obs[ja,0]*obs[jb+2,0], -obs[jb,0]*obs[jb+1,0] ]
    B[i,jb:jb+3] = [-obs[jb+1,0]*obs[ja+2,0], -obs[jb,0]*obs[ja+2,0], obs[ja,0]*obs[ja+1,0] ]
    #print('\n', 'B[%1d] = %.1f' % (i,B[i,ja:jb+3]) ) 


print('\n', 'w = ', w)
print('\n', 'B = ', B)


# MODELE STOCHASTIQUE
# matrice de covariance pour un état

sigma = np.array([sigmaP, sigmaV, sigmaT])
K33 = np.diag(sigma*sigma)


# pour plusieurs états non corrélés

Kll = np.zeros((m*3, m*3))

for i in range(0, m):
    j = 3*i
    Kll[j:j+3, j:j+3] = K33

print('\n', 'Kll =', '\n', Kll)

# COMPENSATION

# Des matrices auxiliaires sont introduites pour simplifier des calculs
# ultérieurs.

Aux1 = Kll @ np.transpose(B)
Aux2 = Aux1 @ np.linalg.inv(B @ Aux1)

# 1. résidus compensés (v_chapeau)

#vcomp = Aux2*w
print('size Aux2 = ', np.size(Aux2))
print('size w = ', np.size(w))

vcomp = Aux2 @ w

print('\n', 'vcomp =', '\n', vcomp)

# 2. observations compensées (l_chapeau)

np.set_printoptions(precision=2)
lcomp = np.zeros((n, 1))
for i in range(n):
    #lcomp = obs-vcomp
    lcomp[i,0] = obs[i,0]-vcomp[i,0]

print('\n', 'lcomp =', '\n', lcomp)


# Contrôle
# Les écarts de fermeture calculés avec les valeurs compensées devraient
# être nuls. Généralement, ils ne le sont pas pour des raisons numériques,
# mais leur valeur doit être négligeable compte tenu de la nature du
# problème (et la précision des observations).
    
wcomp = np.zeros((r, 1))

print('\nControl des écartes de fermeture des valeurs compensés:')
for i in range(0,r):
    ja = 3*i
    jb = 3*i + 3

    wcomp[i,0] = lcomp[ja,0]*lcomp[ja+1,0]*lcomp[jb+2,0] - lcomp[jb,0]*lcomp[jb+1,0]*lcomp[ja+2,0]

print('\n', 'wcomp = ', wcomp)

# -------------------------------------------------------------------------
# Description et interprétation des résultats
# La théorie sous-jacente sera abordée prochainement.
# -------------------------------------------------------------------------

# matrices de covariance des résidus et des observations compensés
# Les matrices auxiliaires d'avant sont bien pratiques!

Kvcomp = Aux2 @ np.transpose(Aux1)
Klcomp = Kll-Kvcomp

# Telles quelles, l'interprétation de ces matrices est difficile,
# mais comme toujours pour des matrices de covariance, on peut calculer
# les écarts-types des valeurs compensées, ainsi que leurs corrélations.


[sigmavcomp, Rvcomp] = covmat2cormat(Kvcomp)
print('\n', 'sigmavcomp =', '\n', sigmavcomp)
print('\n', 'Rvcomp =', '\n', Rvcomp)

# Notamment, on remarque que les résidus compensés sont entièrement
# corrélées au sein d'un même état. Par exemple pour le 2e état:
# sous-matrice Rvcomp[3:6,3:6] = [1 1 -1; 1 1 -1; -1 -1 1]
# Si P2*V2/T2 doit augmenter pour s'adapter aux conditions, le changement
# se répartit: P2 et V2 augmentent et T2 diminue dans la même proportion.

[sigmalcomp, Rlcomp] = covmat2cormat(Klcomp)
print('\n', 'sigmalcomp =', '\n', sigmalcomp)

# Les écarts-types des observations compensées sont plus petits que les
# valeurs a priori attribuées aux observations.

print('\n', 'Rlcomp =', '\n', Rlcomp)

# Les corrélations ne sont pas triviales. Tous les états sont liés par les
# conditions et toutes les observations compensées sont corrélées.

# Estimation de la précision
# Vu que les observations sont de nature différente, on a renoncé à
# introduire des cofacteurs.
# Implicitement, on a posé sigma0=1 [sans dimension], donc Kll=Qll et
# P=np.linalg.inv(Kll). Sous cette forme, le calcul de l'écart-type a posteriori
# donne un nombre [sans dimension] qui exprime le rapport global des
# écarts-types a posteriori / a priori pour tous les types de mesures.

P = np.linalg.inv(Kll)

QuotientGlobal = np.sqrt((np.transpose(vcomp) @ P @ vcomp)/r)
np.set_printoptions(precision=4)
print('\n', 'QuotientGlobal = ', QuotientGlobal)