#ifndef __INS_EULER_H__
#define __INS_EULER_H__

#include <iostream>
#include <math.h>

#ifndef M_PI
  #define M_PI 3.1415926535897932384626433832795
#endif

#include "param/vector3.h"
#include "param/quarternion.h"
#include "param/matrix.h"
#include "WGS84.h"

typedef WGS84 Earth;

template <class FloatT>
class INS_Euler{
  protected:
    Vector3<FloatT> v_2e_4n;       //n-frameにおける速度
    FloatT v_N, v_E;               //北方向、東方向の速度
    
    FloatT phi, lambda, alpha;     //緯度、経度、Azimuth角
    FloatT h;                      //現在の高度[m]
    
    Quarternion<FloatT> q_n2b;     //n-frame -> b-frmae、すなわち現在の姿勢
    
    Vector3<FloatT> omega_e2i_4e;  //Earth Rate
  
    Vector3<FloatT> omega_e2i_4n;  //ω_{e/i}^{n}
    Vector3<FloatT> omega_n2e_4n;  //ω_{n/e}^{n}
    
  public:
    static const unsigned STATE_VALUES = 11;
    
    FloatT beta() const{return (Earth::R_normal(phi) + h) * cos(phi);}
    
    /* 現在のAzimuth角を元にq_e2nを計算する */
    Quarternion<FloatT> q_e2n(const FloatT &latitude, const FloatT &longitude) const{
      using std::cos;
      using std::sin;
      using std::sqrt;
      FloatT clat(cos(latitude / 2)), slat(sin(latitude / 2));
      FloatT sqrt2(sqrt(2.));
      Quarternion<FloatT> q(
          cos((longitude + alpha) / 2) * (clat - slat) / sqrt2,
          sin((longitude - alpha) / 2) * (clat + slat) / sqrt2,
         -cos((longitude - alpha) / 2) * (clat + slat) / sqrt2,
          sin((longitude + alpha) / 2) * (clat - slat) / sqrt2);
      return q;
    }
    
    inline Quarternion<FloatT> q_e2n() const{
      return q_e2n(phi, lambda);
    }
    
    /* 現在のAzimuth角を元にq_e2nを計算する */
    Matrix<FloatT> dcm_e2n(const FloatT &latitude, const FloatT &longitude) const{
      //return q_e2n(latitude, longitude).getDCM();
      Matrix<FloatT> dcm(3, 3);
      {
        FloatT ca(cos(alpha)),
          sa(sin(alpha)),
          cp(cos(latitude)),
          sp(sin(latitude)),
          cl(cos(longitude)),
          sl(sin(longitude));
        
        dcm(0, 0) = -ca * sp * cl - sa * sl;
        dcm(0, 1) =  sa * sp * cl - ca * sl;
        dcm(0, 2) = -cp * cl;
        
        dcm(1, 0) = -ca * sp * sl + sa * cl;
        dcm(1, 1) =  sa * sp * sl + ca * cl;
        dcm(1, 2) = -cp * sl;
        
        dcm(2, 0) =  ca * cp;
        dcm(2, 1) = -sa * cp;
        dcm(2, 2) = -sp;
      }
      return dcm.transpose();
    }
    
    inline Matrix<FloatT> dcm_e2n() const{
      return dcm_e2n(phi, lambda);
    }
  
  private:
    inline FloatT update_v_N(const FloatT &calpha, const FloatT &salpha){
      return v_N = v_2e_4n.get(0) * calpha - v_2e_4n.get(1) * salpha;
    }
    inline FloatT update_v_E(const FloatT &calpha, const FloatT &salpha){
      return v_E = v_2e_4n.get(0) * salpha + v_2e_4n.get(1) * calpha;
    }
    
    inline void update_omega_e2i_4n(){
      omega_e2i_4n = (q_e2n().conj() * omega_e2i_4e * q_e2n()).vector();
    }
    inline void update_omega_n2e_4n(const FloatT &calpha, const FloatT &salpha){
      FloatT omega_n2e_4g_0 = v_E / (Earth::R_normal(phi) + h);
      FloatT omega_n2e_4g_1 = -v_N / (Earth::R_meridian(phi) + h);
      
      omega_n2e_4n[0] =  omega_n2e_4g_0 * calpha + omega_n2e_4g_1 * salpha;
      omega_n2e_4n[1] = -omega_n2e_4g_0 * salpha + omega_n2e_4g_1 * calpha;
    }
    inline void update_omega_n2e_4n(){
      update_omega_n2e_4n(std::cos(alpha), std::sin(alpha));
    }
  protected:
    inline void recalc(const bool regularize = true){
      //正規化
      if(regularize){
        q_n2b = q_n2b.regularize();
      }

      FloatT ca(std::cos(alpha)), sa(std::sin(alpha));
            
      //v_north, v_eastの更新
      update_v_N(ca, sa);
      update_v_E(ca, sa);
      
      //ω_{e/i}^{n}
      update_omega_e2i_4n();
      
      //ω_{n/e}^{n}
      update_omega_n2e_4n(ca, sa);
    }
    
  public:
    /**
     * 位置を初期化します。
     * 
     */
    void initPosition(const FloatT &latitude, const FloatT &longitude, const FloatT &height){
      phi = latitude;
      lambda = longitude;
      alpha = 0;
      h = height;
      
      update_omega_e2i_4n();
      update_omega_n2e_4n();
    }
    
    /**
     * 速度を初期化します。
     * 
     */
    void initVelocity(const FloatT &v_north, const FloatT &v_east, const FloatT &v_down){
      v_2e_4n[0] = v_N = v_north;
      v_2e_4n[1] = v_E = v_east;
      v_2e_4n[2] = v_down;
      
      update_omega_n2e_4n();
    }
    
    /**
     * 姿勢を初期化します。
     * 
     */
    void initAttitude(const FloatT &yaw, const FloatT &pitch, const FloatT &roll){
      using std::cos;
      using std::sin;
      FloatT cy(cos(yaw / 2)), cp(cos(pitch / 2)), cr(cos(roll / 2));
      FloatT sy(sin(yaw / 2)), sp(sin(pitch / 2)), sr(sin(roll / 2));

      q_n2b[0] = cy * cp * cr + sy * sp * sr;
      q_n2b[1] = cy * cp * sr - sy * sp * cr;
      q_n2b[2] = cy * sp * cr + sy * cp * sr;
      q_n2b[3] = sy * cp * cr - cy * sp * sr;
    }
  
    INS_Euler() : omega_e2i_4e(0, 0, Earth::Omega_Earth){
      initPosition(0, 0, 0);
      initVelocity(0, 0, 0);
      initAttitude(0, 0, 0); 
    }
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    INS_Euler(const INS_Euler &orig, const bool deepcopy = false)
        : v_2e_4n(deepcopy ? orig.v_2e_4n.copy() : orig.v_2e_4n), 
        v_N(orig.v_N), v_E(orig.v_E), 
        h(orig.h), phi(orig.phi), lambda(orig.lambda), alpha(orig.alpha),
        q_n2b(deepcopy ? orig.q_n2b.copy() : orig.q_n2b), 
        omega_e2i_4e(deepcopy ? orig.omega_e2i_4e.copy() : orig.omega_e2i_4e), 
        omega_e2i_4n(deepcopy ? orig.omega_e2i_4n.copy() : orig.omega_e2i_4n), 
        omega_n2e_4n(deepcopy ? orig.omega_n2e_4n.copy() : orig.omega_n2e_4n){
      
    }
    
    virtual ~INS_Euler(){}
    
    virtual FloatT &operator[](const unsigned &index){
      switch(index){
        case 1:  return v_2e_4n[1];
        case 2:  return v_2e_4n[2];
        case 3:  return phi;
        case 4:  return lambda;
        case 5:  return alpha;
        case 6:  return h;
        case 7:  return q_n2b[0];
        case 8:  return q_n2b[1];
        case 9:  return q_n2b[2];
        case 10: return q_n2b[3];
        case 0:  default: return v_2e_4n[0]; // インデックス範囲外の場合、[0]の要素を返す
      }
    }
    
    const FloatT &get(const unsigned &index) const {
      return const_cast<INS_Euler<FloatT> *>(this)->operator[](index);
    }
    
    void set(const unsigned &index, const FloatT &v){
      (*this)[index] = v;
    }
    
    virtual void update(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro, const FloatT &deltaT){
      
      //速度の運動方程式
      Vector3<FloatT> delta_v_2e_4n((q_n2b * accel * q_n2b.conj()).vector());
      delta_v_2e_4n[2] += Earth::gravity(phi);
      delta_v_2e_4n -= (omega_e2i_4n * 2 + omega_n2e_4n) * v_2e_4n;
#ifdef pow2
#define POW2_ALREADY_DEFINED
#else
#define pow2(x) ((x)*(x))
#endif
      Vector3<FloatT> centripetal_f(-cos(lambda), -sin(lambda), 0);
      centripetal_f *= (pow2(Earth::Omega_Earth) * beta());
      //delta_v_2e_4n -= (e2n_DCM() * centripetal_f)の高速化
      Matrix<FloatT> dcm(dcm_e2n());
      for(int i = 0; i < delta_v_2e_4n.OUT_OF_INDEX; i++){
        delta_v_2e_4n[i] 
            -= (dcm(i, 0) * centripetal_f[0] + dcm(i, 1) * centripetal_f[1]);
      }
#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif
      
      //位置(緯度、経度、Azimuth角)の運動方程式
      // P.52 + p.57
      FloatT delta_phi(-cos(alpha) * omega_n2e_4n[1] - sin(alpha) * omega_n2e_4n[0]);
      // 89.5度でリミットを掛けておく
#define PHI_LIMITER 89.5
      FloatT cos_phi((fabs(phi) < (M_PI / 180 * PHI_LIMITER)) ? cos(phi) : cos(M_PI / 180 * PHI_LIMITER));
#undef PHI_LIMITER
      FloatT delta_lambda(-(sin(alpha) * omega_n2e_4n[1] - cos(alpha) * omega_n2e_4n[0]) / cos_phi);
      FloatT delta_alpha(delta_phi * sin(phi));
      FloatT delta_h(v_2e_4n[2] * -1);
      
      //姿勢の運動方程式
      Quarternion<FloatT> dot_q_n2b(0, omega_e2i_4n + omega_n2e_4n);
      dot_q_n2b *= q_n2b;
      dot_q_n2b -= q_n2b * gyro;
      dot_q_n2b /= (-2);
      
      //更新
      v_2e_4n += delta_v_2e_4n * deltaT;
      phi += delta_phi * deltaT;
      lambda += delta_lambda * deltaT;
      alpha += delta_alpha * deltaT;
      h += delta_h * deltaT;
      q_n2b += dot_q_n2b * deltaT;
      
      //付随的情報の再計算
      recalc();
    }
    
    FloatT v_north() const{return v_N;}
    FloatT v_east() const{return v_E;}
    FloatT v_down() const{return v_2e_4n.get(2);}
    
    FloatT latitude() const{return phi;}
    FloatT longitude() const{return lambda;}
    FloatT height() const{return h;}
    FloatT azimuth() const{return alpha;}
    
    Quarternion<FloatT> n2b() const{return q_n2b.copy();}
    
    /* オイラー角 */
#ifdef pow2
#define POW2_ALREADY_DEFINED
#else
#define pow2(x) ((x)*(x))
#endif
    FloatT euler_psi() const{
      return atan2((q_n2b.get(1) * q_n2b.get(2) + q_n2b.get(0) * q_n2b.get(3)) * 2,
                      (pow2(q_n2b.get(0)) + pow2(q_n2b.get(1)) - pow2(q_n2b.get(2)) - pow2(q_n2b.get(3))));
    }
    FloatT euler_theta() const{return asin((q_n2b.get(0) * q_n2b.get(2) - q_n2b.get(1) * q_n2b.get(3)) * 2);}
    FloatT euler_phi() const{
      return atan2((q_n2b.get(2) * q_n2b.get(3) + q_n2b.get(0) * q_n2b.get(1)) * 2,
                      (pow2(q_n2b.get(0)) - pow2(q_n2b.get(1)) - pow2(q_n2b.get(2)) + pow2(q_n2b.get(3))));
    }
#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif
    FloatT heading() const{
      FloatT _heading(euler_psi() + azimuth());
      return _heading > M_PI ? (_heading - (2 * M_PI)) : (_heading < -M_PI ? (_heading + (2 * M_PI)) : _heading);
    }
    
    Vector3<FloatT> omega_e2i() const{return omega_e2i_4n.copy();}
    Vector3<FloatT> omega_n2e() const{return omega_n2e_4n.copy();}
    
    /* 現在の曲率 */
    FloatT meter2lat(const FloatT &distance) const{return distance / Earth::R_meridian(phi);}
    FloatT meter2long(const FloatT &distance) const{return distance / (beta() * cos(phi));}
    
    /**
     * 現在の状態量を見やすい形で出力します。
     * 
     */
    friend ostream &operator<<(ostream &out, const INS_Euler<FloatT> &ins){
      for(unsigned i = 0; i < STATE_VALUES; i++){
        cout << (i == 0 ? "" : "\t") << (*const_cast<INS_Euler<FloatT> *>(&ins))[i];
      }
      return out;  
    }
};

#endif /* __INS_EULER_H__ */