#ifndef __INS_GPS2_TIGHT_H__
#define __INS_GPS2_TIGHT_H__

/** @file
 * @brief INS/GPS(Multiplicative)
 * 
 * 慣性航法装置(INS)とGPSの統合をTight-couplingで記述したファイル。
 * INSの誤差計算は積算型(Multiplicative)を利用しています。
 * 
 * INS/GPSの統合に用いるGPSの各データ型についてはINS_GPS.hを参照してください。
 * 
 * @see Filtered_INS2.h
 * @see GPS.h
 */

#include <vector>
#include <utility>

#include "Filtered_INS2.h"
#include "coordinate.h"
#include "GPS.h"
#include "algorithm/kalman.h"

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

/**
 * @brief INS/GPS(Multiplicative)
 * 
 * INS/GPS Tight-couplingをカルマンフィルタで実現したクラス。
 * INSのシステム誤差方程式は積算型で表現されています。
 * 
 * @param FloatT 演算精度
 * @param Filter カルマンフィルタ
 * @param FINS 他の航法装置を統合する為のINS拡張クラス
 * @see Filtered_INS2
 */
template <
  class FloatT, 
  typename Filter = KalmanFilterUD<FloatT>,
  typename FINS = Filtered_INS2<FloatT, Filter> 
>
class INS_GPS2_Tight : public FINS{
  public:
    typedef GPS_SpaceNode<FloatT> space_node_t;
    typedef typename space_node_t::gps_time_t gps_time_t;
    
    typedef typename space_node_t::xyz_t xyz_t;
    typedef typename space_node_t::llh_t llh_t;
    typedef typename space_node_t::enu_t enu_t;
    
    typedef std::vector<std::pair<int, FloatT> > sat_range_t;
  
  protected:
    FloatT receiver_error;
  
  public:
    INS_GPS2_Tight() : FINS(), receiver_error(0) {} /**< コンストラクタ */
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    INS_GPS2_Tight(const INS_GPS2_Tight &orig, const bool deepcopy = false) :
      FINS(orig, deepcopy), receiver_error(orig.receiver_error){
      
    }
    
    ~INS_GPS2_Tight(){} /**< デストラクタ */
    
    using FINS::correct;
    
    /**
     * 3D測位時における観測更新(Measurement Update)を行います。
     * u-blox用。
     * lever arm effectを考慮。
     * 
     * @param receiver_time 受信機時刻
     * @param space_node GPS情報
     * @param sat_range 利用可能衛星番号, 擬似距離
     * @param lever_arm_b lever armの大きさ
     * @param omega_b2i_4b 観測時のジャイロの値
     */
    void correct(gps_time_t &receiver_time,
        const space_node_t &space_node,
        const sat_range_t &sat_range,
        const Vector3<FloatT> &lever_arm_b,
        const Vector3<FloatT> &omega_b2i_4b){
                   
      FloatT azimuth(INS<FloatT>::azimuth());
      FloatT c_az(cos(azimuth)), s_az(sin(azimuth));
      
      gps_time_t true_time_est(receiver_time);
      true_time_est -= (receiver_error / space_node_t::light_speed);
      
      // アーム長の処理
      Vector3<FloatT> lever_arm_n(
        ((INS<FloatT>::q_n2b) * lever_arm_b * (INS<FloatT>::q_n2b).conj()).vector());
      Vector3<FloatT> lever_arm_g(
          lever_arm_n[0] * c_az - lever_arm_n[1] * s_az,
          lever_arm_n[0] * s_az + lever_arm_n[1] * c_az,
          lever_arm_n[2]
        );
      FloatT lever_lat(INS<FloatT>::meter2lat(lever_arm_g[0]));
      FloatT lever_long(INS<FloatT>::meter2long(lever_arm_g[1]));
      
      //行列Hの作成
      Matrix<FloatT> H(sat_range.size(), FINS::P_SIZE);
      
      //観測量z
      Matrix<FloatT> z(H.rows(), 1);
      
      //観測値誤差行列R
      Matrix<FloatT> R(H.rows(), H.rows());
      
      llh_t usr_llh(FINS::latitude() + lever_lat, 
          FINS::longitude() + lever_long, 
          FINS::height() - lever_arm_g[2]);
      xyz_t usr_position(usr_llh.xyz());
      
      unsigned i(0);
      for(sat_range_t::const_iterator it(sat_range.begin());
          it != sat_range.end();
          ++it, ++i){
        
        FloatT range(it->second);
        space_node_t::Satellite &sat(
            const_cast<space_node_t *>(&space_node)->satellite(it->first));
        
        // 幾何距離(暫定値)
        range -= receiver_error;
        //cerr << "r1 => " << range << endl;
        
        // 衛星のクロック誤差を補正
        range += sat.clock_error(true_time_est, range) * space_node_t::light_speed;
        //cerr << "r2 => " << range << endl; 
        
        // 衛星の位置を計算   
        xyz_t sat_position(sat.whereis(true_time_est, range));
        //cerr << it->first << ": " << sat_position << endl;
        FloatT range_est(usr_position.dist(sat_position));
        
        // 幾何距離残差を計算
        range -= range_est;
        
        enu_t relative_pos(enu_t::relative(sat_position, usr_position));
        FloatT rel_el(relative_pos.elevation()), rel_az(relative_pos.azimuth());
        FloatT s_rel_el(sin(rel_el)), c_rel_el(cos(rel_el)),
            s_rel_az(sin(rel_az)), c_rel_az(cos(rel_az));
        
        // 精密な補正を行う
        {
          // 電離層遅延補正
          FloatT iono_delta_r(space_node.iono_correction(relative_pos, usr_llh, true_time_est));
          //cerr << "iono => " << iono_delta_r << endl;
          range += iono_delta_r;
          
          // 対流圏遅延補正
          FloatT tropo_delta_r(space_node.tropo_correction(relative_pos, usr_llh));
          //cerr << "tropo => " << tropo_delta_r << endl;
          range += tropo_delta_r;
        }
        
        // 観測量残渣
        z(i, 0) = range;
        
        // 重みの設定
        if(range > 30.0){
          // 残差が大きい衛星は重みを小さくして計算に使用されないようにする
          R(i, i) = 1E+8;
          cerr << "PRN(" << it->first << ") is neglected, delta_r = " << range << endl;
        }else{
          // GPS実用プログラミングより
          R(i, i) *= pow2(0.8/s_rel_el);
          R(i, i) = min_macro(R(i, i), 1E+3); 
        }
        
        FloatT R_m(Earth::R_meridian(usr_llh.latitude()) + h);
        FloatT R_b(Earth::R_normal(usr_llh.longitude() + h) * cos(usr_llh.latitude()));
        
        FloatT coef_1(c_rel_el * c_rel_az),
            coef_2(c_rel_el * s_rel_az),
            coef_3(s_rel_el);
        
        FloatT c_lambda(cos(longitude())), 
               s_lambda(sin(longitude())), 
               t_phi(tan(latitude()));
        
        // 観測行列
        // \Delta u_{n}^{e} {}_{1}
        H(i, 3) = -c_lambda * t_phi * 2 * R_m * coef_1
            + s_lambda * 2 * R_b * coef_2;
        // \Delta u_{n}^{e} {}_{2}
        H(i, 4) = -s_lambda * t_phi * 2 * R_m * coef_1
            - c_lambda * 2 * R_b * coef_2;
        // \Delta u_{n}^{e} {}_{3}
        H(i, 5) = 2.0 * R_m * coef_1;
        
        // \Delta h 
        H(i, 6) = coef_3;
        
        // \Delta u_{n}^{b} {}_{1}
        H(i, 7) = (s_az * lever_arm_n[2] * coef_1
            - c_az * lever_arm_n[2] * coef_2
            + lever_arm_n[1] * coef_3) * -2;
        // \Delta u_{n}^{b} {}_{2}
        H(i, 8) = (c_az * lever_arm_n[2] * coef_1
            + s_az * lever_arm_n[2] * coef_2
            - lever_arm_n[0] * coef_3) * -2;
        // \Delta u_{n}^{b} {}_{3}
        H(i, 9) = ((-c_az * lever_arm_n[1] - s_az * lever_arm_n[0]) * coef_1
            + (-s_az * lever_arm_n[1] + c_az * lever_arm_n[0]) * coef_2) * -2;
      }
      
      cout << "z:" << z << endl;
      cout << "R:" << R << endl;
      cout << "H:" << H << endl;

      //修正
      FINS::correct(H, z, R);
    }
};

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

#endif /* __INS_GPS2_TIGHT_H__ */