#!/usr/bin/env python
# coding: utf-8

# ## Frames
# 
# - **WGS-84 [lat,lon,alt]:** World Geodetic System 1984 standard for maps. This standard defines the oblate spheroid typically used to model Earth’s shape
#     - Semi-major: a = 6,378,137 m
#     - Semi-minor: b = 6,356,752.3142 m
#     - This is the frame GPS reports position in
# - **The Earth-Centred, Earth-Fixed Frame (ECEF)[x,y,z]:** a global, attached to Earth itself, and always uses cartesian coordinates
# - **Local Geographic Frame (LGF)[x,y,z]:**  At any point on Earth’s surface, the local geographic frame is defined by the almost flat ground and the vertical direction, and is relevant because it is the basic reference the aircraft flies against, defining straight and level flight and of course the direction of down. This is an intuitive frame for any scenario whose total extent is no more than some tens of kilometres. The conversion is $p_{NED} = R(p_{ECEF} - p_{ref})$, where $R$ is the rotation from ECEF to either NED or ENU.
#     - NED: North,  East, Down
#     - ENU: East, North, Up
# - **Body Frame [x,y,z]:** A frame attached at the center of mass of the vehicle with the *x-axis* pointing out the front, *y-axis* out the right side, and the *z-axis* pointing down *typically*. However, various authors/engineers use different orientations.
# 
# ## Convention
# 
# Moving from referene frame body ($\textbf{F}^b$) to the navigation frame ($\textbf{F}^n$) is done via a rotation matrix. All rotation matrices are special orthogonal groups in three dimensions or SO(3). Thus they have the following properties:
# 
# $$
# R^{-1}(\theta) = R^T(\theta) \\
# det(R(\theta)) = 1
# $$
# 
# All rotations can be broken up into their corresponding Euler angles. By convention, rotating vectors in the body frame to the navigation frame is done by:
# 
# $$
# R_b^n = R_Z(\psi) R_y(\theta) R_x(\phi) = R_{123}(\phi, \theta,\psi) 
# $$
# 
# Now we can use this to rotate vectors between reference frames:
# 
# $$
# p^n = R_b^n p^b = R_{123}(\phi, \theta,\psi) p^b \\
# v^n = R_{123}(\phi, \theta,\psi) v^b \\
# \omega^n = R_{123}(\phi, \theta,\psi) \omega^b \\
# F^n = R_{123}(\phi, \theta,\psi) F^b
# $$
# 
# where $n$ refers to a navigation frame and $b$ refers to a body frame.
# 
# ## References
# 
# - Wikipedia: [Euler angles](https://en.wikipedia.org/wiki/Euler_angles)
# - Wikipedia: [Local tangental coordinate frames](https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates)
# - MIT Open Courseware: [Designing Electromechanical Robotic Systems](https://ocw.mit.edu/courses/mechanical-engineering/2-017j-design-of-electromechanical-robotic-systems-fall-2009/course-text/)
# - StackExchange: [Rotational Kinematics and Angular Velocity Vector Transformation
# ](https://physics.stackexchange.com/a/429094)

# In[1]:


get_ipython().run_line_magic('load_ext', 'autoreload')
get_ipython().run_line_magic('autoreload', '2')


# In[36]:


import numpy as np
from numpy import sin, cos, pi, sqrt
from numpy.testing import assert_allclose
deg2rad = pi/180
rad2deg = 180/pi


# In[4]:


from rotations import frames
from rotations import rotations


# In[41]:


class Nav:
    def __init__(self, orig,):
        self.r = self.rot_ecef2ned(*orig[:2])
        self.o = self.wgs2ecef(*orig)
    
    def ecef2ned(self, pt):
        """ECEF(x,y,z) to NED(x,y,z)"""
        #to_nav()
        return self.r.dot(pt-self.o)
    
    def ned2ecef(self, pt):
        """ECEF(x,y,z) to NED(x,y,z)"""
        #to_ecef
        return self.r.T.dot(pt)+self.o
    
    def wgs2ned(self, lat, lon, alt):
        x = self.wgs2ecef(lat,lon,alt)
        return self.ecef2ned(x)
    
    def rot_ecef2ned(self, lat, lon, degrees=True):
        """ECEF to NED rotation matrix"""
        if degrees:
            lat *= deg2rad
            lon *= deg2rad
        return np.array([
            [-sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)],
            [-sin(lon), cos(lon), 0],
            [-cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)]
        ])
    
    def wgs2ecef(self, lat,lon, alt, degrees=True):
        """wgs84(lat,lon,alt) to ECEF(x,y,z)"""
        if degrees:
            lat *= deg2rad
            lon *= deg2rad

        a = 6378137.0 # WGS semi-major axis
        b = 6356752.314245 # WGS semi-minor axis
        e = sqrt(1-(b/a)**2)
        d = sqrt(1-e**2*sin(lat)**2)
        f = (a/d+alt)*cos(lat)

        x = f*cos(lon)
        y = f*sin(lon)
        z = (a*(1-e*e)/d+alt)*sin(lat)

        return np.array([x,y,z])


# In[42]:


ned = Nav((45,45,0))
print(ned.r)
print(ned.o)
print(np.linalg.norm(ned.o))


# In[43]:


p = ned.wgs2ecef(45,45,0)
print(p)
m=ned.ecef2ned(p)
print(m)


# In[44]:


pp = ned.ned2ecef(m)
print(pp)
assert_allclose(pp,p)


# In[47]:


ned.wgs2ned(45.1,45,0)


# In[ ]: