#!/usr/bin/python

#
#    Etant donné un mobile dont la position est donnée par (Lat, Lon, Alt), dont la route vraie est Rou,
#    la vitesse horizontale Vh, la vitesse verticale Vv, calculer sa position au bout de Tim secondes
#    Lat est un nombre relatif de -90 à 90 : positif pour les latitudes Nord, négatif pour les latitudes Sud
#    Lon est un nombre positif de 0 à 360 de longitude Ouest
#    
#    Le calcul de la position finale est basé sur une approximation consistant à assimiler à un plan
#    la portion de sphère où se déplace l'avion. C'est légitime tant que la distance parcourue par l'avion
#    reste très petite par rapport au rayon de la sphère.
#
import numpy as np

#   Les constantes que l'on peut modifier
Lat = -45  #   en degrés
Lon = 10   #   en degrés
Alt = 5000   #   en mètres
Rou = 200  #   en degrés
Vh =  80   #   en mètres par seconde (environ 300 km/h)
Vv = 0     #   en mètres par secondes
Tim = 100    #   en secondes

#   Les constantes que l'on ne peut pas modifier
Rt = 6378137  #   rayon en mètres de la sphère IAG-GRS80 (modèle de la terre)
R = Rt + Alt  #   rayon de la sphère où se trouve l'avion


#   On converti tout en radians
lat = Lat *2*np.pi/360
lon = Lon *2*np.pi/360
rou = Rou *2*np.pi/360

#   Je prends arbitrairement un axe x dans le sens des longitudes croissantes (vers l'Ouest)
#   Un axe y dans les sens des latitudes Nord croissantes
#   Je calcule les coordonnées du vecteur vitesse dans ce système d'axes

Vhx = -Vh * np.sin(rou)
Vhy = Vh * np.cos(rou)    #   car cos(x) = cos(-x)

#    distances parcourues dans le laps de temps time
x = Vhx * Tim
y = Vhy * Tim

#    Calcul des deltas en latitude et longitude
#    le dy se fait sur un grand cercle de la sphère, mais pas le dx
dy = y / R
r = np.abs(R * np.sin(lat))
dx = x / r

#    Calcul de la nouvelle position
nlon = lon + dx
nlat = lat + dy

print("Pour une route au ", Rou, " pendant ", Tim, "s ")
print("Position initiale (latitude, longitude) : ", Lat, ", ", Lon)
print("Position finale   (latitude, longitude) : ", nlat*360/(2*np.pi), ", ", nlon*360/(2*np.pi))

#
#   Calcul de l'erreur commise par les approximations
#
l = Vh * Tim    #   la vraie distance parcourue

#    Calcul de la distance entre les deux points avec la formule de la trigonométrie sphérique
a = np.arccos(np.sin(lat)*np.sin(nlat) + np.cos(lat)*np.cos(nlat)*np.cos(lon - nlon))
L = R * a    #   distance entre point initial et point final calculé

print("Pour une distance parcourue de ", l, "m, l'erreur relative sur la position finale est estimée à : ", np.abs((L-l)/l))