#ifndef __INS_GPS_EULER2_H__
#define __INS_GPS_EULER2_H__

#include "Filtered_INS_Euler2.h"
#include "INS_GPS.h"
#include "algorithm/kalman.h"

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

template <
  class FloatT, 
  typename Filter = KalmanFilterUD<FloatT>,
  typename FINS = Filtered_INS_Euler2<FloatT, Filter> 
>
class INS_GPS_Euler2 : public FINS{
  public:
    INS_GPS_Euler2() : FINS(){}
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    INS_GPS_Euler2(const INS_GPS_Euler2 &orig, const bool deepcopy = false) :
      FINS(orig, deepcopy){
      
    }
    
    ~INS_GPS_Euler2(){}
    
    using FINS::correct;
    
    /**
     * 2D測位時
     * 
     */
    void correct(const GPS_NMEA_2D<FloatT> &gps){
      
      FloatT azimuth(this->azimuth());
      
      //行列Hの作成
      Matrix<FloatT> H(4, FINS::P_SIZE);
      {
        // 速度(2D)
        H(0, 0) = 1;
        H(1, 1) = 1;
        
        // 位置(緯度・経度)
        H(2, 3) = 1;
        H(3, 4) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(4, 1);
      {
        z(0, 0) = (*this)[0] - gps.v * cos(gps.v_heading - azimuth);
        z(1, 0) = (*this)[1] - gps.v * sin(gps.v_heading - azimuth);
        z(2, 0) = (*this)[3] - gps.latitude;
        z(3, 0) = (*this)[4] - gps.longitude;
      }
      
      //観測値誤差行列R
      Matrix<FloatT> R(4, 4);
      {
        R(0, 0) = 1;
        R(1, 1) = 1;
        R(2, 2) = pow2(meter2lat(gps.direction2D_sigma));
        R(3, 3) = pow2(meter2longt(gps.direction2D_sigma));
      }

      //修正
      FINS::correct(H, z, R);
    }
    
    /**
     * 3D測位時
     * 
     */
    void correct(const GPS_NMEA_3D<FloatT> &gps){
      
      FloatT azimuth(this->azimuth());
      
      //行列Hの作成
      Matrix<FloatT> H(5, FINS::P_SIZE);
      {
        // 速度(2D)
        H(0, 0) = 1;
        H(1, 1) = 1;
        // 位置(3D)
        H(2, 3) = 1;
        H(3, 4) = 1;
        H(4, 6) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(5, 1);
      {
        z(0, 0) = (*this)[0] - gps.v * cos(gps.v_heading - azimuth);
        z(1, 0) = (*this)[1] - gps.v * sin(gps.v_heading - azimuth);
        z(2, 0) = (*this)[3] - gps.latitude;
        z(3, 0) = (*this)[4] - gps.longitude;
        z(4, 0) = (*this)[6] - gps.height;
      }
      
      //観測値誤差行列R
      Matrix<FloatT> R(5, 5);
      {
        R(0, 0) = 1;
        R(1, 1) = 1;
        
        R(2, 2) = pow2(meter2lat(gps.long2D_sigma));
        R(3, 3) = pow2(meter2long(gps.long2D_sigma));
        R(4, 4) = pow2(20);
      }
      
      //修正
      FINS::correct(H, z, R);
    }
    
    /**
     * シミュレータ用
     * 
     */
    void correct(const GPS_NMEA_SIM_3D<FloatT> &gps){
                   
      FloatT azimuth(this->azimuth());
      
      //cout << "heading:" << v_heading / 3.1415926535897932384626433832795 * 180 << endl;
      //cout << "azimuth:" << azimuth   / 3.1415926535897932384626433832795 * 180 << endl;
      
      //行列Hの作成
      Matrix<FloatT> H(5, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 3) = 1;
        H(3, 4) = 1;
        H(4, 6) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(5, 1);
      {
        z(0, 0) = (*this)[0] - gps.v * cos(gps.v_heading - azimuth);
        z(1, 0) = (*this)[1] - gps.v * sin(gps.v_heading - azimuth);
        z(2, 0) = (*this)[3] - gps.latitude;
        z(3, 0) = (*this)[4] - gps.longitude;
        z(4, 0) = (*this)[6] - gps.height;
      }

      
      //cout << z << endl;
            
      //観測値誤差行列R
#define _abs(x) (((x) < 0) ? -(x) : (x))
      Matrix<FloatT> R(5, 5);
      {
        R(0, 0) = pow2(gps.v_norm_sigma * cos(gps.v_heading - azimuth))
                    + pow2(gps.v * sin(gps.v_heading - azimuth) * deg2rad(gps.v_direction_sigma));
        R(1, 1) = pow2(gps.v_norm_sigma * sin(gps.v_heading - azimuth))
                    + pow2(gps.v * cos(gps.v_heading - azimuth) * deg2rad(gps.v_direction_sigma));

        R(2, 2) = pow2(meter2lat(gps.long2D_sigma));
        R(3, 3) = pow2(meter2long(gps.long2D_sigma));
        
        R(4, 4) = pow2(gps.height_sigma);
      }
#undef _abs

      //for(int i = 0; i < R.rows(); i++){R(i, i) *= 2;}
      
      //cout << R << endl;
      
      //修正
      FINS::correct(H, z, R);
    }

    void correct(const GPS_NMEA_SIM_3D_with_Vd<FloatT> &gps){
                   
      FloatT azimuth(this->azimuth());
      
      //cout << "heading:" << v_heading / 3.1415926535897932384626433832795 * 180 << endl;
      //cout << "azimuth:" << azimuth   / 3.1415926535897932384626433832795 * 180 << endl;
      
      //行列Hの作成
      Matrix<FloatT> H(6, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 2) = 1;
        
        H(3, 3) = 1;
        H(4, 4) = 1;
        H(5, 6) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(6, 1);
      {
        z(0, 0) = (*this)[0] - gps.v * cos(gps.v_heading - azimuth);
        z(1, 0) = (*this)[1] - gps.v * sin(gps.v_heading - azimuth);
        z(2, 0) = (*this)[2] - gps.v_down;
        z(3, 0) = (*this)[3] - gps.latitude;
        z(4, 0) = (*this)[4] - gps.longitude;
        z(5, 0) = (*this)[6] - gps.height;
      }
      
      //cout << z << endl;
      
      //観測値誤差行列R
#define _abs(x) (((x) < 0) ? -(x) : (x))
      Matrix<FloatT> R(6, 6);
      {
        /*R(0, 0) = pow2(gps.v_norm_sigma * cos(gps.v_heading - azimuth))
                    + pow2(gps.v * sin(gps.v_heading - azimuth) * deg2rad(gps.v_direction_sigma));
        R(1, 1) = pow2(gps.v_norm_sigma * sin(gps.v_heading - azimuth))
                    + pow2(gps.v * cos(gps.v_heading - azimuth) * deg2rad(gps.v_direction_sigma));
        R(2, 2) = pow2(gps.v_norm_sigma);*/
        
        R(0, 0) = R(1, 1) = R(2, 2) = 1E0;
        
        R(3, 3) = pow2(meter2lat(gps.long2D_sigma));
        R(4, 4) = pow2(meter2long(gps.long2D_sigma))
        R(5, 5) = pow2(gps.height_sigma);
      }
#undef _abs

      //for(int i = 0; i < R.rows(); i++){R(i, i) *= 2;}
      
      //cout << R << endl;
      
      //修正
      FINS::correct(H, z, R);
    }
    
    /**
     * U-blox用
     * 
     */
    void correct(const GPS_UBLOX_3D<FloatT> &gps){
                   
      FloatT azimuth(this->azimuth());
      
      //行列Hの作成
      Matrix<FloatT> H(6, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 2) = 1;
        
        H(3, 3) = 1;
        H(4, 4) = 1;
        H(5, 6) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(6, 1);
      {
        z(0, 0) = (*this)[0] - (gps.v_n * cos(azimuth) + gps.v_e * sin(azimuth));
        z(1, 0) = (*this)[1] - (gps.v_n * -sin(azimuth) + gps.v_e * cos(azimuth));
        z(2, 0) = (*this)[2] - gps.v_d;
        z(3, 0) = (*this)[3] - gps.latitude;
        z(4, 0) = (*this)[4] - gps.longitude;
        z(5, 0) = (*this)[6] - gps.height;
      }
      
      //観測値誤差行列R
      Matrix<FloatT> R(6, 6);
      {
        R(0, 0) = pow2(gps.sigma_vel);
        R(1, 1) = pow2(gps.sigma_vel);
        R(2, 2) = pow2(gps.sigma_vel);
        
        R(3, 3) = pow2(meter2lat(gps.sigma_2d));
        R(4, 4) = pow2(meter2long(gps.sigma_2d));
        
        R(5, 5) = pow2(gps.sigma_height);
      }

      //for(int i = 0; i < z.rows(); i++) cout << z(i, 0) << '\t';
      //for(int i = 0; i < R.rows(); i++) cout << R(i, i) << '\t';
      //cout << endl;
      
      /*{
        R(0, 0) = R(1, 1) = 1.;
        R(2, 2) = R(3, 3) = R(4, 4) = R(5, 5) = 1E-12;
        R(6, 6) = pow2(); 
      }*/
      //for(int i = 0; i < R.rows(); i++){R(i, i) *= 2;}
      
      //修正
      FINS::correct(H, z, R);
    }
};

#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif

#endif /* __INS_GPS_EULER2_H__ */