#ifndef __FILTERED_INS_EULER2_H__
#define __FILTERED_INS_EULER2_H__

#include "INS_Euler.h"
#include "param/matrix.h"
#include "algorithm/kalman.h"

template <class FloatT, typename Filter = KalmanFilterUD<FloatT> >
class Filtered_INS_Euler2 : public INS_Euler<FloatT>{
   
  protected:
    Filter m_filter;
  
    /**
     * dx/dt = A x + B w
     * における Aを返します
     * 
     * @return A行列
     */
#define R_STRICT
    virtual Matrix<FloatT> getA(
        const Vector3<FloatT> &accel, 
        const Vector3<FloatT> &gyro,
        const Matrix<FloatT> &dcm_e2n,
        const Matrix<FloatT> &dcm_n2b){

#define dcm_e2n(r, c) (const_cast<Matrix<FloatT> *>(&dcm_e2n)->operator()(r, c))
#define dcm_n2b(r, c) (const_cast<Matrix<FloatT> *>(&dcm_n2b)->operator()(r, c))

#ifdef pow2
#define POW2_ALREADY_DEFINED
#else
#define pow2(x) ((x) * (x))
#endif

      FloatT R(Earth::R_e + (*this)[7]);
      FloatT R2(pow2(R));
#define Rn_1 R
#define Rm_1 R
#define Rn_2 R2
#define Rm_2 R2

      //行列Aの計算
      Matrix<FloatT> A(10, 10);
      {
       
        Vector3<FloatT> omega_1(this->omega_e2i_4n * 2 + this->omega_n2e_4n);

        FloatT R(Earth::R_e + INS_Euler<FloatT>::h);
        FloatT R2(pow2(R));
    
        FloatT sp(sin(this->phi)), cp(cos(this->phi)), tp(tan(this->phi)),
          sl(sin(this->lambda)), cl(cos(this->lambda)), tl(tan(this->lambda)),
          sa(sin(this->alpha)), ca(cos(this->alpha)), ta(tan(this->alpha));
        
        // -(2 omega_{e/i}^{n} + omega_{n/e}^{n}) × Δ \dot{r}_{e}^{n}
        {
          A(0, 1) =  omega_1[2];
          A(0, 2) = -omega_1[1];
          
          A(1, 0) = -omega_1[2];
          A(1, 2) =  omega_1[0];
          
          A(2, 0) =  omega_1[1];
          A(2, 1) = -omega_1[0];
        }
    
        // - 2 * DCM[q*_{n}^{b}] a × Δ u_{n}^{b}
        {
          A(0, 8) = (dcm_n2b(0, 2) * accel.get(0) + dcm_n2b(1, 2) * accel.get(1) + dcm_n2b(2, 2) * accel.get(2)) *  2;
          A(0, 9) = (dcm_n2b(0, 1) * accel.get(0) + dcm_n2b(1, 1) * accel.get(1) + dcm_n2b(2, 1) * accel.get(2)) * -2;
    
          A(1, 7) = -A(0, 8);
          A(1, 9) = (dcm_n2b(0, 0) * accel.get(0) + dcm_n2b(1, 0) * accel.get(1) + dcm_n2b(2, 0) * accel.get(2)) *  2;
          
          A(2, 7) = -A(0, 9);
          A(2, 8) = -A(1, 9);
        }
        
        // \dot{r}_{e}^{n} × ( 2 Δomega_{e/i}^{n} + Δomega_{n/e}^{n} )
        {
          // 2 Δomega_{e/i}^{n}
          A(0, 3) += Earth::Omega_Earth * 2 * (sa * sp * (- (this->v_2e_4n[2])) - cp * (this->v_2e_4n[1]));
          A(1, 3) += Earth::Omega_Earth * 2 * (-ca * sp * (this->v_2e_4n[2]) + cp * (this->v_2e_4n[0]));
          A(2, 3) += Earth::Omega_Earth * 2 * (ca * sp * (this->v_2e_4n[1]) + sa * sp * (this->v_2e_4n[0]));
          
          A(0, 5) += Earth::Omega_Earth * 2 * (ca * cp * (this->v_2e_4n[2]));
          A(1, 5) += Earth::Omega_Earth * 2 * (-sa * cp * (this->v_2e_4n[2]));
          A(2, 5) += Earth::Omega_Earth * 2 * (sa * cp * (this->v_2e_4n[1]) - ca * cp * (this->v_2e_4n[0]));
          
          // Δomega_{n/e}^{n}
          A(0, 0) += (this->v_2e_4n[2]) / R;
          A(0, 6) += -(this->v_2e_4n[2]) * (this->v_2e_4n[0]) / R2;
          A(1, 1) += (this->v_2e_4n[2]) / R;
          A(1, 6) += -(this->v_2e_4n[2]) * (this->v_2e_4n[1]) / R2;
          A(2, 0) += -(this->v_2e_4n[0]) / R;
          A(2, 1) += -(this->v_2e_4n[1]) / R;
          A(2, 6) += (pow2(this->v_2e_4n[0]) + pow2(this->v_2e_4n[1])) / R2;
        }
        
        // ΔDCM (omega_{e/i}^{e} × (omega_{e/i}^{e} × r_{e}))
        {
          Matrix<FloatT> centripetal_f(3, 1);
          {
            centripetal_f(0, 0) = -cl;
            centripetal_f(1, 0) = -sl;
            //centripetal_f(2, 0) = 0;
          }
          centripetal_f *= pow2(Earth::Omega_Earth) * beta();
          
          {
            Matrix<FloatT> delta_DCM_phi(3, 3);
            {
              delta_DCM_phi(0, 0) = -ca * cp * cl;
              delta_DCM_phi(0, 1) = -ca * sp * sl;
              delta_DCM_phi(0, 2) = -ca * sp;
              delta_DCM_phi(1, 0) = sa * cp * cl;
              delta_DCM_phi(1, 1) = sa * cp * sl;
              delta_DCM_phi(1, 2) = sa * sp;
              delta_DCM_phi(2, 0) = sp * cl;
              delta_DCM_phi(2, 1) = sp * sl;
              delta_DCM_phi(2, 2) = -cp;
            }
            delta_DCM_phi *= centripetal_f;
            A.pivotMerge(0, 3, delta_DCM_phi);
          }
          
          {
            Matrix<FloatT> delta_DCM_lambda(3, 3);
            {
              delta_DCM_lambda(0, 0) = (ca * sp * sl - sa * cl);
              delta_DCM_lambda(0, 1) = (ca * sp * cl + sa * sl);
              //delta_DCM_lambda(0, 2) = 0;
              delta_DCM_lambda(1, 0) = -(sa * sp * sl + ca * cl);
              delta_DCM_lambda(1, 1) = (sa * sp * cl - ca * sl);
              //delta_DCM_lambda(1, 2) = 0;
              delta_DCM_lambda(2, 0) = cp * sl;
              delta_DCM_lambda(2, 1) = -cp * cl;
              //delta_DCM_lambda(2, 2) = 0;
            }
            delta_DCM_lambda *= centripetal_f;
            A.pivotMerge(0, 4, delta_DCM_lambda);
          }
          
          {
            Matrix<FloatT> delta_DCM_alpha(3, 3);
            {
              delta_DCM_alpha(0, 0) = (sa * sp * cl - ca * sl);
              delta_DCM_alpha(0, 1) = (sa * sp * sl + ca * cl);
              delta_DCM_alpha(0, 2) = -sa * cp;
              delta_DCM_alpha(1, 0) = (ca * sp * cl + sa * sl);
              delta_DCM_alpha(1, 1) = (ca * sp * sl - sa * cl);
              delta_DCM_alpha(1, 2) = sa * sp;
              //delta_DCM_alpha(2, 0) = 0;
              //delta_DCM_alpha(2, 1) = 0;
              //delta_DCM_alpha(2, 2) = 0;
            }
            delta_DCM_alpha *= centripetal_f;
            A.pivotMerge(0, 5, delta_DCM_alpha);
          }
        }
        
        // DCM Δ(omega_{e/i}^{e} × (omega_{e/i}^{e} × r_{e}))
        {
          Matrix<FloatT> coef_h_phi_lambda(3, 3);
          {
            coef_h_phi_lambda(0, 0) = cp * -cl;
            coef_h_phi_lambda(1, 0) = cp * sl;
            //coef_h_phi_lambda(2, 0) = 0;
            coef_h_phi_lambda(0, 1) = sp * cl * R;
            coef_h_phi_lambda(1, 1) = sp * sl * R;
            //coef_h_phi_lambda(2, 1) = 0;
            coef_h_phi_lambda(0, 2) = cp * sl * R;
            coef_h_phi_lambda(1, 2) = cp * -cl * R;
            //coef_h_phi_lambda(2, 2) = 0;
          }
          coef_h_phi_lambda *= pow2(Earth::Omega_Earth);
          Matrix<FloatT> result_h_phi_lambda(dcm_e2n * coef_h_phi_lambda);
          A.pivotMerge(0, 6, result_h_phi_lambda.columnVector(0));
          A.pivotMerge(0, 3, result_h_phi_lambda.columnVector(1));
          A.pivotMerge(0, 4, result_h_phi_lambda.columnVector(2));
        }
    
        {
          A(3, 0) = ca / R;
          A(3, 1) = -sa / R;
          A(3, 5) = sa * (this->omega_e2i_4n[1]) - ca * (this->omega_e2i_4n[0]);
          A(3, 6) = (-ca * this->v_2e_4n[0] + sa * this->v_2e_4n[1]) / R2;
        }
        
        FloatT lambda_dot((-sa * this->omega_n2e_4n[1] + ca * this->omega_n2e_4n[0]) / cp);
    
        {
          A(4, 0) = sa / R / cp;
          A(4, 1) = ca / R / cp;
          A(4, 3) = lambda_dot * tp;
          A(4, 5) = (-ca * this->omega_n2e_4n[1] - sa * this->omega_n2e_4n[0]) / cp;
          A(4, 6) = -(sa * this->v_2e_4n[0] + ca * this->v_2e_4n[1]) / R2 / cp;
        }
        
        {
          A(5, 0) = A(4, 0) * sp;
          A(5, 1) = A(4, 1) * sp;
          A(5, 3) = A(4, 3) * sp + lambda_dot * cp;
          A(5, 5) = A(4, 5) * sp;
          A(5, 6) = A(4, 6) * sp;
        }
    
        {
          A(6, 2) = -1;
        }
    
        // (omega_{e/i}^{n} + omega_{n/e}^{n}) × Δ u_{n}^{b}
        Vector3<FloatT> omega_2(this->omega_e2i_4n + this->omega_n2e_4n);
        {
          A(7, 8) =  omega_2[2];
          A(7, 9) = -omega_2[1];
        
          A(8, 7) = -omega_2[2];
          A(8, 9) =  omega_2[0];
        
          A(9, 7) =  omega_2[1];
          A(9, 8) = -omega_2[0];
        }
        
        // -Δ omega_{e/i}^{n} / 2
        {
          A(7, 3) -= -ca * sp;
          A(8, 3) -= sa * sp;
          A(9, 3) -= -cp;
          
          A(7, 5) -= -sa * cp;
          A(8, 5) -= -ca * cp;
        }
        
        // -Δ omega_{n/e}^{n} / 2
        {
          A(7, 1) -= 1. / R / 2;
          A(7, 6) -= -(this->v_2e_4n[1]) / R2 / 2;
          A(8, 0) -= -1. / R / 2;
          A(8, 6) -= (this->v_2e_4n[0]) / R2 / 2;
        }
      }
#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif

#undef dcm_e2n
#undef dcm_n2b
      
      return A;      
    }
    
    /**
     * dx/dt = A x + B w
     * における Aを返します
     * 
     * @return A行列
     */
    Matrix<FloatT> getA(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro){
      return getA(accel, gyro, this->dcm_e2n(), this->q_n2b.getDCM());  
    }
    
    /**
     * dx/dt = A x + B w
     * における Bを返します
     * 
     * @return B行列
     */
    virtual Matrix<FloatT> getB(
        const Vector3<FloatT> &accel, 
        const Vector3<FloatT> &gyro, 
        const Matrix<FloatT> &dcm_n2b){

#define dcm_n2b(r, c) (const_cast<Matrix<FloatT> *>(&dcm_n2b))->operator()(r, c)

      //行列Bの計算
      Matrix<FloatT> B(10, 7);
      {
        B(0, 0) = dcm_n2b(0, 0);
        B(0, 1) = dcm_n2b(1, 0);
        B(0, 2) = dcm_n2b(2, 0);
    
        B(1, 0) = dcm_n2b(0, 1);
        B(1, 1) = dcm_n2b(1, 1);
        B(1, 2) = dcm_n2b(2, 1);
    
        B(2, 0) = dcm_n2b(0, 2);
        B(2, 1) = dcm_n2b(1, 2);
        B(2, 2) = dcm_n2b(2, 2);
        
        B(2, 6) = 1; // 重力誤差Zのみ
    
        B(7, 3) = dcm_n2b(0, 0) / 2;
        B(7, 4) = dcm_n2b(1, 0) / 2;
        B(7, 5) = dcm_n2b(2, 0) / 2;
    
        B(8, 3) = dcm_n2b(0, 1) / 2;
        B(8, 4) = dcm_n2b(1, 1) / 2;
        B(8, 5) = dcm_n2b(2, 1) / 2;
    
        B(9, 3) = dcm_n2b(0, 2) / 2;
        B(9, 4) = dcm_n2b(1, 2) / 2;
        B(9, 5) = dcm_n2b(2, 2) / 2;
      }
#undef dcm_n2b
      
      return B;      
    }
    
    /**
     * dx/dt = A x + B w
     * における Bを返します
     * 
     * @return B行列
     */
    Matrix<FloatT> getB(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro){
      return getB(accel, gyro, (this->q_n2b).getDCM());
    }
  
  public:
    static const unsigned P_SIZE = 10;
    static const unsigned Q_SIZE = 7;
    Filtered_INS_Euler2() 
        : INS_Euler<FloatT>(),
          m_filter(Matrix<FloatT>::getI(P_SIZE), Matrix<FloatT>::getI(Q_SIZE)){
      
      // 共分散行列の初期化
    }
    
    Filtered_INS_Euler2(const Matrix<FloatT> &P, const Matrix<FloatT> &Q) 
        : INS<FloatT>(), m_filter(P, Q){}
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    Filtered_INS_Euler2(const Filtered_INS_Euler2 &orig, const bool deepcopy = false)
      : INS_Euler<FloatT>(orig, deepcopy),
        m_filter(orig.m_filter, deepcopy){}
    
    virtual ~Filtered_INS_Euler2(){}

  protected:
    /**
     * 時間更新において後処理をするためのコールバック関数。
     * デフォルトでは何もしません。
     * 
     * @param A A行列
     * @param B B行列
     * @patam deltaT 時間間隔
     */
    virtual inline void before_update_INS(
        const Matrix<FloatT> &A, const Matrix<FloatT> &B, 
        const FloatT &deltaT
      ){}
      
  public:    
    /**
     * 時間更新
     * 
     * @param accel 加速度計の値
     * @param gyro ジャイロの値
     * @param deltaT 時間間隔
     */
    void update(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro, const FloatT &deltaT){
      
      // 回転行列の計算
      Matrix<FloatT> dcm_e2n(this->dcm_e2n());
      Matrix<FloatT> dcm_n2b((this->q_n2b).getDCM());
    
      Matrix<FloatT> A(getA(accel, gyro, dcm_e2n, dcm_n2b));
      Matrix<FloatT> B(getB(accel, gyro, dcm_n2b));
      m_filter.predict(A, B, deltaT);
      before_update_INS(A, B, deltaT);
      INS_Euler<FloatT>::update(accel, gyro, deltaT);
    }
  
  protected:
    /**
     * 修正において後処理をするためのコールバック関数。
     * デフォルトでは何もしません。
     * 
     * @param H H行列
     * @param R R行列
     * @patam K K行列、カルマンゲイン
     * @param v (z - H x)ここではEKFでxがゼロであるためzに等しい
     * @param x_hat 修正量
     */
    virtual inline void before_correct_INS(
        const Matrix<FloatT> &H,
        const Matrix<FloatT> &R,
        const Matrix<FloatT> &K,
        const Matrix<FloatT> &v,
        Matrix<FloatT> &x_hat
      ){}
    
    /**
     * Kalman Filterによって得られた@f$ \Hat{x} @f$を利用して、INSを修正します。
     * 
     * @param x_hat Kalman Filterによって得られたx_hat
     */
    inline void correct_INS(Matrix<FloatT> &x_hat){
      // 速度
      for(unsigned i = 0; i < 3; i++){(*this)[i] -= x_hat(i, 0);}
      
      // 2D位置修正
#ifdef pow2
#define POW2_ALREADY_DEFINED
#else
#define pow2(x) ((x) * (x))
#endif
      for(unsigned i = 3; i < 6; i++){(*this)[i] -= x_hat(i, 0);}
      
      // z位置
      (*this)[6] -= x_hat(6, 0);
      
      // 姿勢修正
      /*Quarternion<FloatT> delta_q_n2b(
                                  sqrt(1. - (pow2(x_hat(7, 0)) + pow2(x_hat(8, 0)) + pow2(x_hat(9, 0)))),
                                  -x_hat(7, 0), 
                                  -x_hat(8, 0), 
                                  -x_hat(9, 0)
                                );*/
      Quarternion<FloatT> delta_q_n2b(1, -x_hat(7, 0), -x_hat(8, 0), -x_hat(9, 0));
      (this->q_n2b) = delta_q_n2b * (this->q_n2b);
#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif

      for(unsigned i = 10; i < x_hat.rows(); i++){(*this)[i + 2] -= x_hat(i, 0);}

      INS_Euler<FloatT>::recalc();
    }

  public:
//#define DEBUG_DUMP_CORRECTION
//#define DEBUG_DUMP_Q_N2B
    /**
     * 修正します。
     * 
     * @param H 観測行列
     * @param z 観測量
     * @param R 誤差共分散行列
     */
    void correct(const Matrix<FloatT> &H, const Matrix<FloatT> &z, const Matrix<FloatT> &R){
      
      // 修正量の計算
      Matrix<FloatT> K(m_filter.correct(H, R)); //カルマンゲイン
      Matrix<FloatT> x_hat(K * z);
      before_correct_INS(H, R, K, z, x_hat);

#ifdef DEBUG_DUMP_CORRECTION
      static bool is_first(true);
      
      if(is_first){
        for(int i = 0; i < z.rows(); i++) cerr << "z" << i << '\t';
        for(int i = 0; i < 10; i++) cerr << "x_hat" << i << '\t';
        cerr << endl;
        is_first = false;
      }
     
      for(int i = 0; i < z.rows(); i++) cerr << const_cast<Matrix<FloatT> *>(&z)->operator()(i, 0) << '\t';
      for(int i = 0; i < x_hat.rows(); i++) cerr << x_hat(i, 0) << '\t';
      cerr << endl;
#endif
      
      correct_INS(x_hat);
      
#ifdef DEBUG_DUMP_Q_N2B
      for(int i = 0; i < 4; i++) cerr << this->q_n2b[i] << ",";
      for(int i = 0; i < 3; i++) cerr << (m_filter.getP())(i + 7, i + 7) << ",";
      cerr << endl;
#endif
    }
    
    Filter &getFilter(){return m_filter;}
};

#endif /* __FILTERED_INS_EULER2_H__ */