# Transformation ITRF2020 --> ETRF89/DREF91 Realisierung 2025

import sympy as sp
from Berechnungen import Einheitenumrechnung

# Helmetert Paramteter zur Referenzepoche t0
t0 = 2015.0
T1 = Einheitenumrechnung.mm_to_m(41.1393)
T2 = Einheitenumrechnung.mm_to_m(51.9830)
T3 = Einheitenumrechnung.mm_to_m(-101.1455)
D = Einheitenumrechnung.ppb(7.8918)
R1 = Einheitenumrechnung.mas_to_rad(0.8878)
R2 = Einheitenumrechnung.mas_to_rad(12.7748)
R3 = Einheitenumrechnung.mas_to_rad(-22.2616)
dotT1 = Einheitenumrechnung.mm_to_m(0)
dotT2 = Einheitenumrechnung.mm_to_m(0)
dotT3 = Einheitenumrechnung.mm_to_m(0)
dotD = 0
dotR1 = Einheitenumrechnung.mas_to_rad(0.086)
dotR2 = Einheitenumrechnung.mas_to_rad(0.519)
dotR3 = Einheitenumrechnung.mas_to_rad(-0.753)

# Testdatensatz der AdV
tc = 2021.48

BRMG = sp.Matrix([4245557.0412, 568958.1394, 4710200.0645])
RANT = sp.Matrix([3645376.2264, 531202.1700, 5189297.0638])
TIT2 = sp.Matrix([3993787.0533, 450204.1794, 4936131.8526])
LDB2 = sp.Matrix([3798344.6978, 955553.3244, 5017221.8937])
FFMJ = sp.Matrix([4053455.6399, 617729.9375, 4869395.8850])

# 1) Epochendifferenz
dt = tc - t0

# 2) Parameter von Epoche t0 auf tc umrechnen
T1_c = T1 + dotT1 * dt
T2_c = T2 + dotT2 * dt
T3_c = T3 + dotT3 * dt
R1_c = R1 + dotR1 * dt
R2_c = R2 + dotR2 * dt
R3_c = R3 + dotR3 * dt
D_c = D + dotD * dt

# 3) Matrizen aufstellen
R_Matrix = sp.Matrix([[D_c, -R3_c, R2_c], [R3_c, D_c, -R1_c], [-R2_c, R1_c, D_c]])
T_Vektor = sp.Matrix([T1_c, T2_c, T3_c])

# 4) Helmerttransformation
print(f"BRMG = {(BRMG + T_Vektor + R_Matrix * BRMG).evalf()}")
print(f"RANT = {(RANT + T_Vektor + R_Matrix * RANT).evalf()}")
print(f"TIT2 = {(TIT2 + T_Vektor + R_Matrix * TIT2).evalf()}")
print(f"LDB2 = {(LDB2 + T_Vektor + R_Matrix * LDB2).evalf()}")
print(f"FFMJ = {(FFMJ + T_Vektor + R_Matrix * FFMJ).evalf()}")