"""
Méthodes d'estimation
SIE 3e semestre, GC 5e semestre

Compensation conditionnelle

ESO/TOPO 05.11.23
"""

import numpy as np
np.set_printoptions(precision=3, floatmode='fixed')

print(' ')
print('--------------------------------------------------------')
print("Compensation conditionnelle des angles d'un quadrilatère")
print('--------------------------------------------------------')

# angles alpha et beta mesurés 2 fois

# *** Si d'autres données ont été utilisées pendant le cours, veuillez
# remplacer celles qui figurent ci-dessous. ***

# alpha1, alpha2, beta1, beta2, gamma et delta
ang = np.array([[111.928, 111.911, 107.313, 107.315, 95.463, 85.292]]).T
print('\n','ang =\n',ang)

n=len(ang);

print('\n','a) Anticiper la compensation.\n',
'   INTUITION >>> Faire la moyenne des mesures doubles et diviser\n',
'   leur variance par 2, ce qui revient à doubler leur poids.')

alphamoy = np.mean(ang[0:2])
betamoy  = np.mean(ang[2:4])

print('\n',"alphamoy = %.3f" % alphamoy)
print('\n',"betamoy = %.3f" % betamoy)

# surdétermination

r = 1
print('\n','r = ',r)

# écarts de fermeture

w = np.array([alphamoy + betamoy + ang[4] + ang[5] - 400])
print('\n','w = ',w)

# matrice des coefficients des conditions
# cas linéaire!

B = np.array([[1, 1, 1, 1]])
print('\n','B = ',B)

# modèle stochastique: cofacteurs

Q = np.diag([0.5, 0.5, 1, 1])
print('\n','Q =\n',Q)

# résidus compensés

Aux1 = Q @ (B).T
Aux2 = Aux1 @ np.linalg.inv(B @ Aux1)
vcomp = Aux2 @ w
print('\n','vcomp =\n',vcomp)

# observations compensées

angcomp = np.array([[alphamoy, betamoy, ang[4,0], ang[5,0]]]).T - vcomp
print('\n','angcomp =\n',angcomp)

# vérification des écarts de fermeture compensés
# Ils doivent être exactement nuls puisque le modèle fonctionnel est
# linéaire!
 
wcomp = B @ angcomp - 400
print('\n','wcomp = ',wcomp)

# écart-type a posteriori d'une observation de poids unitaire

sigmapos = np.sqrt(vcomp.T @ np.linalg.inv(Q) @ vcomp/r)
print('\n','sigmapos = ',sigmapos)

print('\n','Juste, mais:\n',
"- on n'obtient pas les résidus des mesures originales de alpha et de beta;\n",
"- les répétitions ne contribuent pas à estimer la précision, voir c) et/ou d).")

print('\n','-------------------------------------------')

print('\n',"b) Mélanger les modèles fonctionnel et stochastique.\n",
"   INTUITION (mauvaise) >>> Puisque chaque mesure brute a le même poids,\n",
"   on impose: alpha1/2+alpha2/2+beta1/2+beta2/2+gamma+theta-400 = 0.")

r = 1
print('\n','r = ',r)

B = np.array([[0.5, 0.5, 0.5, 0.5, 1, 1]])
print('\n','B = ',B)

w = B @ ang - 400
print('\n','w = ',w)

Q = np.eye(6)
print('\n','Q =\n',Q)

Aux1 = Q @ (B).T
Aux2 = Aux1 @ np.linalg.inv(B @ Aux1)
vcomp = Aux2 @ w
print('\n','vcomp =\n',vcomp)

angcomp = ang - vcomp
print('\n','angcomp =\n',angcomp)
 
wcomp = B @ angcomp - 400
print('\n','wcomp = ',wcomp)

sigmapos = np.sqrt(vcomp.T @ np.linalg.inv(Q) @ vcomp/r)
print('\n','sigmapos = ',sigmapos)


print('\n',"Cela paraît simple, mais c'est faux! Les 2 mesures de alpha obtiennent\n",
'forcément le même résidu. Puisque les deux valeurs mesurées du même angle\n',
'sont différentes (cas général), leurs valeurs compensées le sont aussi.\n',
"C'est illogique puisque'il s'agit du même angle!")

print('\n','-------------------------------------------')
print('\n',"c) Corriger l'approche b) en imposant les répétitions.")

r = 3
print('\n','r = ',r)

B = np.zeros((r,n))
B[0,0:2]=[1, -1]                    # répétition de alpha
B[1,2:4]=[1, -1]                    # répétition de beta
B[2,0:6]=[0.5, 0.5, 0.5, 0.5, 1, 1] # condition reprise de b)
print('\n','B = ',B)

# terme constant des conditions

b0 = np.array([[0, 0, 400]]).T
print('\n','b0 =\n',b0)

w = B @ ang - b0
print('\n','w =\n',w)

Q = np.eye(6)
print('\n','Q =\n',Q)

Aux1 = Q @ (B).T
Aux2 = Aux1 @ np.linalg.inv(B @ Aux1)
vcomp = Aux2 @ w
print('\n','vcomp =\n',vcomp)

angcomp = ang - vcomp
print('\n','angcomp =\n',angcomp)
 
wcomp = B @ angcomp - b0
print('\n','wcomp =\n',wcomp)

sigmapos = np.sqrt(vcomp.T @ np.linalg.inv(Q) @ vcomp/r)
print('\n','sigmapos = ',sigmapos)


print('\n',"En imposant l'égalité des deux mesures compensées de alpha et beta\n",
"l'illogisme de b) est réparé.\n",
"remarque: En formant une combinaison linéaire des conditions: 1e * 1/2 + 2e * 1/2 + 3e\n",
"on obtient la 3e condition de l'approche d).")

print('\n','-------------------------------------------')
print('\n','d) Laisser la compensation faire son travail.\n',
"   INTUITION >>> Exprimer tout ce qu'on sait de façon aussi directe que possible,\n",
'   bien séparer le modèle fonctionnel (= les conditions que les mesures doivent\n',
'   respecter) et le modèle stochastique (= leurs propriétés statistiques).')

r = 3
print('\n','r = ',r)

B = np.zeros((r,n))
B[0,0:2]=[1, -1]            # répétition de alpha
B[1,2:4]=[1, -1]            # répétition de beta
B[2,0:6]=[1, 0, 1, 0, 1, 1] # somme des angles du polygone
print('\n','B =\n',B)

# terme constant des conditions

b0 = np.array([[0, 0, 400]]).T
print('\n','b0 =\n',b0)

w = B @ ang - b0
print('\n','w =\n',w)

Q = np.eye(6)
print('\n','Q =\n',Q)

Aux1 = Q @ (B).T
Aux2 = Aux1 @ np.linalg.inv(B @ Aux1)
vcomp = Aux2 @ w
print('\n','vcomp =\n',vcomp)

angcomp = ang - vcomp
print('\n','angcomp =\n',angcomp)

wcomp = B @ angcomp - b0
print('\n','wcomp =\n',wcomp)

sigmapos = np.sqrt(vcomp.T @ np.linalg.inv(Q) @ vcomp/r)
print('\n','sigmapos = ',sigmapos)

print('\n','Avantages: - la pondération des mesures doubles est automatique;\n',
'           - on obtient les résidus des mesures originales;\n',
'           - les valeurs compensées des mesures répétées sont égales;\n',
'           - les répétitions contribuent à estimer la précision.')

print('\n','-------------------------------------------')
print('\n',"e) Succomber à la tentation d'écrire des conditions linéairement dépendantes\n",
      "Ecrire toutes les combinaisons des mesures répétées dans le quadrilatère.")



r = 4   # redundant (à commenter avec #) 
#r = 3   # correct (à de-commenter) 

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

B = np.zeros((r,n))
B[0,0:6]=[1, 0, 1, 0, 1, 1]
B[1,0:6]=[0, 1, 0, 1, 1, 1]
B[2,0:6]=[1, 0, 0, 1, 1, 1]
if r > 3:
  B[3,0:6]=[0, 1, 1, 0, 1, 1]
print('\n','B =\n',B)

# terme constant des conditions

b0 = np.array([[400, 400, 400]]).T
if r > 3:
  b0 = np.array([[400, 400, 400, 400]]).T
print('\n','b0 =\n',b0)

w = B @ ang - b0
print('\n','w =\n',w)

Q = np.eye(6)
print('\n','Q =\n',Q)

Aux1 = Q @ (B).T

print('\n',"Quand r>3 l'opération suivante va faire surgir un message d'erreur,\n",
"car la matrice à inverser B*Q*BT est singulière.\n",
"Les 4 conditions sont dépendantes: il faut enlever l'une d'entre elles.\n",
"Concrètement, il faut enlever une ligne de B et l'élément correspondant de w.\n")

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

# Et voilà le travail !!!

vcomp2 = Aux2 @ w
print('\n','vcomp =\n',vcomp2)

angcomp2 = ang - vcomp2
print('\n','angcomp =\n',angcomp2)

wcomp2 = B @ angcomp - b0
print('\n','wcomp =\n',wcomp2)

print('\n',"Et voilà les résidus compensés et les angles compensés ont les valeurs,\n", 
      "comme dans le cas précédent avec le B différent.\n")