#ifndef __INS_GPS_H__
#define __INS_GPS_H__

/** @file
 * @brief INS/GPS(Additive)
 * 
 * 慣性航法装置(INS)とGPSの統合をLoose-couplingで記述したファイル。
 * INSの誤差計算は加算型(Addtive)を利用しています。
 * 
 * またINS/GPSの統合以外にGPSのデータ型についても定義をしています。
 * 
 * @see Filtered_INS.h
 */

#include "Filtered_INS.h"
#include "algorithm/kalman.h"

/**
 * @brief GPSのデータ型(NMEA_2D)
 * 
 * NMEAでのデータ型。2次元測位の場合。
 * 
 * @param FloatT 演算精度
 */
template <class FloatT>
struct GPS_NMEA_2D{
  FloatT v,                    ///< 2次元速度ベクトルの絶対値
         v_heading;            ///< 2次元速度ベクトル方向
  FloatT direction2D_sigma;    ///< 2次元速度ベクトル方向の推定誤差
  FloatT latitude,             ///< 緯度
         longitude;            ///< 経度
  FloatT long2D_sigma;         ///< 位置における誤差(楕円長半径)
};

/**
 * @brief GPSのデータ型(NMEA_3D)
 * 
 * NMEAでのデータ型。3次元測位の場合。
 * 
 * @param FloatT 演算精度
 * @see GPS_NMEA_2D
 */
template <class FloatT>
struct GPS_NMEA_3D : public GPS_NMEA_2D<FloatT>{
  FloatT height;               ///< 高度
  FloatT short2D_sigma;        ///< 位置における誤差(楕円短半径)
};

/**
 * @brief GPSのデータ型(NMEA_SIM_3D)
 * 
 * NMEAでのデータ型。3次元測位の場合。
 * 高度の推定誤差も求められた場合に使用します。
 * 
 * @param FloatT 演算精度
 * @see GPS_NMEA_3D
 */
template <class FloatT>
struct GPS_NMEA_SIM_3D : public GPS_NMEA_3D<FloatT>{
  FloatT v_norm_sigma,         ///< 速度ベクトルの絶対値の推定誤差 
         v_direction_sigma;    ///< 2次元速度ベクトル方向の推定誤差
  FloatT height_sigma;         ///< 高度方向の推定誤差
};

/**
 * @brief GPSのデータ型(NMEA_3D)
 * 
 * NMEAでのデータ型。3次元測位の場合。
 * 下方向の速度がさらに得られた場合に使用します。
 * 
 * @param FloatT 演算精度
 * @see GPS_NMEA_SIM_3D
 */
template <class FloatT>
struct GPS_NMEA_SIM_3D_with_Vd : public GPS_NMEA_SIM_3D<FloatT>{
  FloatT v_down;               ///< 下方向速度
};

/**
 * @brief GPSのデータ型(u-blox)
 * 
 * u-blox GPSでのデータ型。3次元測位の場合。
 * 
 * @param FloatT 演算精度
 */
template <class FloatT>
struct GPS_UBLOX_3D{
  FloatT v_n,          ///< 北方向速度
         v_e,          ///< 東方向速度
         v_d;          ///< 下方向速度
  FloatT sigma_vel;    ///< 速度ベクトルの絶対値の推定誤差
  FloatT latitude,     ///< 緯度
         longitude,    ///< 経度
         height;       ///< 高度
  FloatT sigma_2d,     ///< 水平面上の推定誤差
         sigma_height; ///< 高度方向の推定誤差
  
  struct sigma_vel_ned_t {
    FloatT n, e, d;
  };
  /**
   * TODO: 要検討
   * 
   * NED成分に分解した速度誤差を求めます。
   * 
   * @return (sigma_vel_ned_t) NED成分に分解した速度誤差
   */
  sigma_vel_ned_t sigma_vel_ned() const {
    FloatT v2((v_n * v_n) + (v_e * v_e) + (v_d * v_d));
    sigma_vel_ned_t res;
    FloatT sigma_pos(std::sqrt((sigma_2d * sigma_2d) + (sigma_height * sigma_height)));
    res.n = res.e = sigma_vel * sigma_2d / sigma_pos; 
    res.d = sigma_vel * sigma_height / sigma_pos;
    return res;
  }
};

template <class FloatT>
struct CorrectInfo {
  Matrix<FloatT> H;
  Matrix<FloatT> z;
  Matrix<FloatT> R;
  CorrectInfo(
      const Matrix<FloatT> &_H,
      const Matrix<FloatT> &_z,
      const Matrix<FloatT> &_R) : H(_H), z(_z), R(_R) {}
  ~CorrectInfo(){}
  CorrectInfo(const CorrectInfo &another)
      : H(another.H), z(another.z), R(another.R) {}
  CorrectInfo &operator=(const CorrectInfo &another){
    H = another.H;
    z = another.z;
    R = another.R;
    return *this;
  }
};

#ifndef pow2
#define pow2(x) ((x) * (x))
#else
#define ALREADY_POW2_DEFINED
#endif

/**
 * @brief INS/GPS(Additive)
 * 
 * INS/GPS Loose-couplingをカルマンフィルタで実現したクラス。
 * INSのシステム誤差方程式は加算型で表現されています。
 * 
 * @param FloatT 演算精度
 * @param Filter カルマンフィルタ
 * @param FINS 他の航法装置を統合する為のINS拡張クラス
 * @see Filtered_INS
 */
template <
  class FloatT, 
  typename Filter = KalmanFilterUD<FloatT>,
  typename FINS =  Filtered_INS<FloatT, Filter>
>
class INS_GPS : public FINS{
  public:
    INS_GPS() : FINS(){}  /**< コンストラクタ */
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    INS_GPS(const INS_GPS &orig, const bool deepcopy = false) :
      FINS(orig, deepcopy){
      
    }
    
    ~INS_GPS(){}          /**< デストラクタ */
    
    using FINS::correct;
    
    /**
     * 観測更新(Measurement Update)を行います。
     * 
     * @param info 修正情報
     */
    void correct(const CorrectInfo<FloatT> &info){
      FINS::correct(info.H, info.z, info.R);
    }
    
    /**
     * 2D測位時における観測更新(Measurement Update)用の情報を求めます。
     * 
     * @param gps GPS出力データ
     * @return (CorrectInfo) 観測更新用のデータ
     */
    CorrectInfo<FloatT> correct_info(const GPS_NMEA_2D<FloatT> &gps){
      
      FloatT azimuth = INS<FloatT>::azimuth();
      
      Quarternion<FloatT> q_e2n_gps(INS<FloatT>::e2n(gps.latitude, gps.longitude));
      
      //行列Hの作成
      Matrix<FloatT> H(6, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 3) = 1;
        H(3, 4) = 1;
        H(4, 5) = 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)[3] - q_e2n_gps[0];
        z(3, 0) = (*this)[4] - q_e2n_gps[1];
        z(4, 0) = (*this)[5] - q_e2n_gps[2];
        z(5, 0) = (*this)[6] - q_e2n_gps[3];
      }
      
      //観測値誤差行列R
      Matrix<FloatT> R(6, 6);
      {
        R(0, 0) = 1;
        R(1, 1) = 1;
        R(2, 2) = R(3, 3) = R(4, 4) = R(5, 5) = 1E-12;
      }

      return CorrectInfo<FloatT>(H, z, R);
    }
    
    /**
     * 2D測位時における観測更新(Measurement Update)を行います。
     * 
     * @param gps GPS出力データ
     */
    void correct(const GPS_NMEA_2D<FloatT> &gps){
      correct(correct_info(gps));
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)用の情報を求めます。
     * 
     * @param gps GPS出力データ
     * @return (CorrectInfo) 観測更新用のデータ
     */
    CorrectInfo<FloatT> correct_info(const GPS_NMEA_3D<FloatT> &gps){
      
      FloatT azimuth = INS<FloatT>::azimuth();
      
      Quarternion<FloatT> q_e2n_gps(INS<FloatT>::e2n(gps.latitude, gps.longitude));
      
      //行列Hの作成
      Matrix<FloatT> H(7, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 3) = 1;
        H(3, 4) = 1;
        H(4, 5) = 1;
        H(5, 6) = 1;
        H(6, 7) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(7, 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] - q_e2n_gps[0];
        z(3, 0) = (*this)[4] - q_e2n_gps[1];
        z(4, 0) = (*this)[5] - q_e2n_gps[2];
        z(5, 0) = (*this)[6] - q_e2n_gps[3];
        z(6, 0) = (*this)[7] - gps.height;
      }
      
      FloatT lat_sigma = meter2lat(gps.long2D_sigma);
      FloatT long_sigma = meter2long(gps.long2D_sigma);
      
      //観測値誤差行列R
      Matrix<FloatT> R(7, 7);
      {
        R(0, 0) = 1;
        R(1, 1) = 1;
        R(2, 2) = pow2(cos((gps.longitude + azimuth) / 2) 
                      * (-sin(-gps.latitude / 2) * (-lat_sigma / 2) + cos(-gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(-gps.latitude / 2) + sin(-gps.latitude / 2))) / 2;
        R(3, 3) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (-sin(-gps.latitude / 2) * (-lat_sigma / 2) - cos(-gps.latitude / 2) * (-lat_sigma / 2))
                    + cos((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(-gps.latitude / 2) - sin(-gps.latitude / 2))) / 2;
        R(4, 4) = pow2(-cos((gps.longitude + azimuth) / 2) 
                      * (-sin(-gps.latitude / 2) * (-lat_sigma / 2) - cos(-gps.latitude / 2) * (-lat_sigma / 2))
                    + sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(-gps.latitude / 2) - sin(-gps.latitude / 2))) / 2;
        R(5, 5) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (-sin(-gps.latitude / 2) * (-lat_sigma / 2) + cos(-gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(-gps.latitude / 2) + sin(-gps.latitude / 2))) / 2;
        //R(2, 2) = R(3, 3) = R(4, 4) = R(5, 5) = 1E-12;
        R(6, 6) = pow2(20);
      }
      
      return CorrectInfo<FloatT>(H, z, R);
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)を行います。
     * 
     * @param gps GPS出力データ
     */
    void correct(const GPS_NMEA_3D<FloatT> &gps){
      correct(correct_info(gps));
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)用の情報を求めます。
     * シミュレーション用。
     * 
     * @param gps GPS出力データ
     * @return (CorrectInfo) 観測更新用のデータ
     */
    CorrectInfo<FloatT> correct_info(const GPS_NMEA_SIM_3D<FloatT> &gps){
                   
      FloatT azimuth = INS<FloatT>::azimuth();
      
      Quarternion<FloatT> q_e2n_gps(INS<FloatT>::e2n(gps.latitude, gps.longitude));
      
      //cout << "heading:" << v_heading / 3.1415926535897932384626433832795 * 180 << endl;
      //cout << "azimuth:" << azimuth   / 3.1415926535897932384626433832795 * 180 << endl;
      
      //行列Hの作成
      Matrix<FloatT> H(7, FINS::P_SIZE);
      {
        H(0, 0) = 1;
        H(1, 1) = 1;
        H(2, 3) = 1;
        H(3, 4) = 1;
        H(4, 5) = 1;
        H(5, 6) = 1;
        H(6, 7) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(7, 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] - q_e2n_gps[0];
        z(3, 0) = (*this)[4] - q_e2n_gps[1];
        z(4, 0) = (*this)[5] - q_e2n_gps[2];
        z(5, 0) = (*this)[6] - q_e2n_gps[3];
        z(6, 0) = (*this)[7] - gps.height;
      }
      
      FloatT lat_sigma = meter2lat(gps.long2D_sigma);
      FloatT long_sigma = meter2long(gps.long2D_sigma);
      
      //観測値誤差行列R
      Matrix<FloatT> R(7, 7);
      {
#define _abs(x) (((x) < 0) ? -(x) : (x))
        R(0, 0) = pow2(gps.v_norm_sigma * _abs(cos(gps.v_heading - azimuth)) 
                    + gps.v * _abs(sin(gps.v_heading - azimuth)) * deg2rad(gps.v_direction_sigma));
        R(1, 1) = pow2(gps.v_norm_sigma * _abs(sin(gps.v_heading - azimuth))
                    + gps.v * _abs(cos(gps.v_heading - azimuth)) * deg2rad(gps.v_direction_sigma));
#undef _abs
        R(2, 2) = pow2(cos((gps.longitude + azimuth) / 2)
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) + cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) - sin(gps.latitude / 2))) / 2;
        R(3, 3) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) - cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + cos((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) + sin(gps.latitude / 2))) / 2;
        R(4, 4) = pow2(-cos((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) - cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) + sin(gps.latitude / 2))) / 2;
        R(5, 5) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) + cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) - sin(gps.latitude / 2))) / 2;
        R(6, 6) = pow2(gps.height_sigma);
      }

      //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;}
      
      return CorrectInfo<FloatT>(H, z, R);
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)を行います。
     * シミュレーション用。
     * 
     * @param gps GPS出力データ
     */
    void correct(const GPS_NMEA_SIM_3D<FloatT> &gps){
      correct(correct_info(gps));
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)用の情報を求めます。
     * u-blox用。
     * 
     * @param gps GPS出力データ
     * @return (CorrectInfo) 観測更新用のデータ
     */
    CorrectInfo<FloatT> correct_info(const GPS_UBLOX_3D<FloatT> &gps){
                   
      FloatT azimuth = INS<FloatT>::azimuth();
      
      Quarternion<FloatT> q_e2n_gps(INS<FloatT>::e2n(gps.latitude, gps.longitude));
      
      //行列Hの作成
      Matrix<FloatT> H(8, 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, 5) = 1;
        H(6, 6) = 1;
        H(7, 7) = 1;
      }
      
      //観測量z
      Matrix<FloatT> z(8, 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] - q_e2n_gps[0];
        z(4, 0) = (*this)[4] - q_e2n_gps[1];
        z(5, 0) = (*this)[5] - q_e2n_gps[2];
        z(6, 0) = (*this)[6] - q_e2n_gps[3];
        z(7, 0) = (*this)[7] - gps.height;
      }
      
      FloatT lat_sigma = meter2lat(gps.sigma_2d);
      FloatT long_sigma = meter2long(gps.sigma_2d);
      
      //観測値誤差行列R
      Matrix<FloatT> R(8, 8);
      {
        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(cos((gps.longitude + azimuth) / 2)
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) + cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) - sin(gps.latitude / 2))) / 2;
        R(4, 4) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) - cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + cos((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) + sin(gps.latitude / 2))) / 2;
        R(5, 5) = pow2(-cos((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) - cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) + sin(gps.latitude / 2))) / 2;
        R(6, 6) = pow2(sin((gps.longitude + azimuth) / 2) 
                      * (sin(gps.latitude / 2) * (-lat_sigma / 2) + cos(gps.latitude / 2) * (-lat_sigma / 2))
                    + -sin((gps.longitude + azimuth) / 2) * (long_sigma / 2) * (cos(gps.latitude / 2) - sin(gps.latitude / 2))) / 2;
        R(7, 7) = 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;}
      
      return CorrectInfo<FloatT>(H, z, R);
    }
    
    /**
     * 3D測位時における観測更新(Measurement Update)を行います。
     * u-blox用。
     * 
     * @param gps GPS出力データ
     */
    void correct(const GPS_UBLOX_3D<FloatT> &gps){
      correct(correct_info(gps));
    }
};

#ifdef ALREADY_POW2_DEFINED
#undef ALREADY_POW2_DEFINED
#else
#undef pow2
#endif

#endif /* __INS_GPS_H__ */