/**
 * @file Sylphide ログ 航法リアルタイム処理用ヘッダ
 * 
 */

#ifndef __SYLPH_NAV_H__
#define __SYLPH_NAV_H__

#include <std.h>

#include <cmath>

#include "sylph_state.h"

#include "matrix.h"
#include "vector3.h"
#include "complex.h"

#include "navigation/INS_GPS2.h"
#define BIAS_EST_MODE 2 // 初期ゼロ点推定含む
#define ZERO_CONST_ALPHA 0 //(FloatT(1) / 1E4) // 0
#include "navigation/INS_GPS_BE.h"

#include "note/070423/calibration.h"
#include "note/070423/packet.h"

struct NAVOptions {
  bool dump_update;             ///< 時間更新(Time Update)の際に現在の値を表示するか
  bool dump_correct;            ///< 観測更新(Measurement Update)の際に現在の値を表示するか
  static const bool use_udkf; ///< KFにUDKFを使うか(重い)
  static const bool bias_on;  ///< バイアス推定を行うか
  NAVOptions()
      : dump_update(true), dump_correct(false) {}
  ~NAVOptions(){}
} nav_options;
const bool NAVOptions::use_udkf = false;
const bool NAVOptions::bias_on = true;

class NAV : public NAVData {
  public:
    virtual void init(
        const float_sylph_t &latitude, 
        const float_sylph_t &longitude, 
        const float_sylph_t &height,
        const float_sylph_t &v_north, 
        const float_sylph_t &v_east, 
        const float_sylph_t &v_down,
        const float_sylph_t &yaw, 
        const float_sylph_t &pitch, 
        const float_sylph_t &roll) = 0;
    virtual void overwrite_yaw(
        const float_sylph_t &yaw){}
    virtual float_sylph_t &operator[](unsigned index) = 0;
    virtual NAV &update(
        const Vector3<float_sylph_t> &accel, 
        const Vector3<float_sylph_t> &gyro, 
        const float_sylph_t &deltaT) = 0;
    
    virtual NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps,
        const float_sylph_t &deltaT) = 0;
};

template <class INS_GPS>
class INS_GPS_NAV : public NAV {
  protected:
    class INS_GPS_RT;
    struct snapshot_content_t {
      INS_GPS_RT *ins_gps;
      Matrix<float_sylph_t> A;
      Matrix<float_sylph_t> GQGt;
      float_sylph_t deltaT_update;
      snapshot_content_t(){}
      snapshot_content_t(
          INS_GPS_RT *_ins_gps,
          const Matrix<float_sylph_t> &_A,
          const Matrix<float_sylph_t> &_GQGt, 
          const float_sylph_t &_deltaT)
          : ins_gps(_ins_gps), A(_A), GQGt(_GQGt), 
          deltaT_update(_deltaT){
      }
      snapshot_content_t &operator=(const snapshot_content_t &another){
        ins_gps = another.ins_gps;
        A = another.A;
        GQGt = another.GQGt;
        deltaT_update = another.deltaT_update;
        return *this;
      }
    };
    typedef FIFO<snapshot_content_t, operator_eq_t> snapshots_t;
    snapshots_t snapshots;
    class INS_GPS_RT : public INS_GPS, public NAVData {
      protected:
        snapshots_t &snapshots;
      public:
        INS_GPS_RT(snapshots_t &_snapshots) 
            : INS_GPS(), snapshots(_snapshots) {}
        INS_GPS_RT(
            const INS_GPS_RT &orig, 
            const bool deepcopy = false)
            : INS_GPS(orig, deepcopy), snapshots(orig.snapshots){}
        INS_GPS_RT &operator=(const INS_GPS_RT &another){
          snapshots = another.snapshots;
          INS_GPS::operator=(another);
          return *this;
        }
#define MAKE_COMMIT_FUNC(fname) \
float_sylph_t fname() const {return INS_GPS::fname();}
        MAKE_COMMIT_FUNC(longitude);
        MAKE_COMMIT_FUNC(latitude);
        MAKE_COMMIT_FUNC(height);
        MAKE_COMMIT_FUNC(v_north);
        MAKE_COMMIT_FUNC(v_east);
        MAKE_COMMIT_FUNC(v_down);
        MAKE_COMMIT_FUNC(heading);
        MAKE_COMMIT_FUNC(euler_phi);
        MAKE_COMMIT_FUNC(euler_theta);
        MAKE_COMMIT_FUNC(euler_psi);
        MAKE_COMMIT_FUNC(azimuth);
#undef MAKE_COMMIT_FUNC
        
      protected:
        /**
         * 時間更新におけるコールバック関数。
         * 
         * @param A A行列
         * @param B B行列
         * @patam deltaT 時間間隔
         */
        void before_update_INS(
            const Matrix<float_sylph_t> &A, const Matrix<float_sylph_t> &B, 
            const float_sylph_t &deltaT){
          
          // 空きがなかったら古いものを捨てる
          if(!snapshots.has_margin()){
            delete snapshots[0].ins_gps;
            snapshots.skip(1);
          }
          
          Matrix<float_sylph_t> Gamma(B * deltaT);
          
          snapshot_content_t snapshot(new INS_GPS_RT(*this, false), 
              A, Gamma * INS_GPS::getFilter().getQ() * Gamma.transpose(),
              deltaT);
          
          snapshots.push(&snapshot);
        }
    };
    INS_GPS_RT nav;
  public:
    INS_GPS_NAV() 
        : NAV(), snapshots(64), 
        nav(snapshots) {}
    ~INS_GPS_NAV() {}
    
  public:
    void init(
        const float_sylph_t &latitude, 
        const float_sylph_t &longitude, 
        const float_sylph_t &height,
        const float_sylph_t &v_north, 
        const float_sylph_t &v_east, 
        const float_sylph_t &v_down,
        const float_sylph_t &yaw, 
        const float_sylph_t &pitch, 
        const float_sylph_t &roll){
      
      nav.initPosition(latitude, longitude, height);
      nav.initVelocity(v_north, v_east, v_down);
      nav.initAttitude(yaw, pitch, roll);

      /**
       * Kalman Filterの誤差共分散行列(P)の初期値
       * 対角成分にある程度大きな値に設定のこと
       * それぞれの要素が持つ意味は
       *  0〜2 速度の分散(単位はm/s)
       *  3〜5 緯度、経度、アジマスをあわせてQuaternion表現した分散(1E-8程度で十分大きい)
       *  6 高度の分散(単位はm)
       *  7〜9 姿勢をQuaternion表現した分散(1E-4程度で十分大きい)
       */
      {
        Matrix<float_sylph_t> P(nav.getFilter().getP());
        P(0, 0) = P(1, 1) = P(2, 2) = 1E+1;
        P(3, 3) = P(4, 4) = P(5, 5) = 1E-8;
        P(6, 6) = 1E+2;
        P(7, 7) = P(8, 8) = P(9, 9) = 1E-4; //CRS03 1E-1;   //-4
        nav.getFilter().setP(P);
      }
      
      /**
       * Kalman Filterの誤差共分散行列(Q)の炎�値
       * 対角成分にそれぞれ
       *  0〜2 : 加速度計X,Y,Zの分散(単位はm/s^2)
       *  3〜5 : ジャイロX,Y,Zの分散(単位はrad/s)
       *  6 : 重力加速度の分散(単位はm/s^2)、通常小さな値(1E-6)でよい
       * を設定のこと
       */
      {
        Matrix<float_sylph_t> Q(nav.getFilter().getQ());
        Q(0, 0) = pow(A_Packet::calibrator->sigma_accel()[0], 2);
        Q(1, 1) = pow(A_Packet::calibrator->sigma_accel()[1], 2);
        Q(2, 2) = pow(A_Packet::calibrator->sigma_accel()[2], 2);
        Q(3, 3) = pow(A_Packet::calibrator->sigma_gyro()[0], 2);
        Q(4, 4) = pow(A_Packet::calibrator->sigma_gyro()[1], 2);
        Q(5, 5) = pow(A_Packet::calibrator->sigma_gyro()[2], 2);
        Q(6, 6) = 1E-6; //1E-14
        nav.getFilter().setQ(Q);
      }
    }
    
    void overwrite_yaw(
        const float_sylph_t &yaw){
      nav.initAttitude(yaw, nav.euler_theta(), nav.euler_phi());
    }

#define MAKE_COMMIT_FUNC(fname, rtype, attr) \
rtype fname() attr {return nav.fname();}
    MAKE_COMMIT_FUNC(longitude, float_sylph_t, const);
    MAKE_COMMIT_FUNC(latitude, float_sylph_t, const);
    MAKE_COMMIT_FUNC(height, float_sylph_t, const);
    MAKE_COMMIT_FUNC(v_north, float_sylph_t, const);
    MAKE_COMMIT_FUNC(v_east, float_sylph_t, const);
    MAKE_COMMIT_FUNC(v_down, float_sylph_t, const);
    MAKE_COMMIT_FUNC(heading, float_sylph_t, const);
    MAKE_COMMIT_FUNC(euler_phi, float_sylph_t, const);
    MAKE_COMMIT_FUNC(euler_theta, float_sylph_t, const);
    MAKE_COMMIT_FUNC(euler_psi, float_sylph_t, const);
    MAKE_COMMIT_FUNC(azimuth, float_sylph_t, const);
#undef MAKE_COMMIT_FUNC
    
    float_sylph_t &operator[](unsigned index){return nav[index];}
    
    NAV &update(
        const Vector3<float_sylph_t> &accel, 
        const Vector3<float_sylph_t> &gyro, 
        const float_sylph_t &deltaT){
      nav.update(accel, gyro, deltaT);
      return *this;
    }
  protected:
    /**
     * snapshotsの調整
     * GPS情報と同期するタイ~ングの情報がsnapshotsの先頭になるようにする
     * 
     * @param deltaT GPSのタイミング(負の値が前の時刻)
     * @return bool 調整がうまくいった場合true
     */
    bool pre_correct_info(float_sylph_t deltaT){
      // 未来の時間を返すのはおかしい 
      if(deltaT > 0){return false;}
      
      for(int i(-1); i >= -snapshots.stored(); i--){
        deltaT += snapshots[i].deltaT_update;
        if(deltaT > float_sylph_t(-0.005)){ // 一番近い時刻を探してくる
          int bounds(snapshots.stored() + i);
          for(int j(0); j < bounds; j++){
            delete snapshots[j].ins_gps;
          }
          snapshots.skip(snapshots.stored() + i);
          return true;
        }
      }
      
      // 予定したものよりも古い時刻のGPSデータなので使えない
      return false;
    }
    
    /**
     * 得られた修正情報を加工し、実際に修正を加えるまでを行う
     * 
     * @param info 修正情報
     */
    void post_correct_info(CorrectInfo<float_sylph_t> &info){
      Matrix<float_sylph_t> sum_A(info.H.columns(), info.H.columns());
      Matrix<float_sylph_t> sum_GQGt(sum_A.rows(), sum_A.rows());
      float_sylph_t bar_delteT(0);
      int i(0);
      for(; i < snapshots.stored(); i++){
        sum_A += snapshots[i].A;
        sum_GQGt += snapshots[i].GQGt;
        bar_delteT += snapshots[i].deltaT_update;
      }
      if(i > 0){
        bar_delteT /= i;
        Matrix<float_sylph_t> sum_A_GQGt(sum_A * sum_GQGt);
        info.R += info.H
            * (sum_GQGt -= ((sum_A_GQGt + sum_A_GQGt.transpose()) *= (bar_delteT * (i + 1) / (2 * i))))
            * info.H.transpose();
        info.H *= (Matrix<float_sylph_t>::getI(sum_A.rows()) - sum_A * bar_delteT);
      }
      nav.correct(info);
    }
  
  public:
    /**
     * アーム長を使わないモード
     * 
     * @param gps GPSの情報
     * @param deltaT いつのタイミングの情報か(負の値が前の時刻)
     */
    NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps,
        const float_sylph_t &deltaT){
      if(pre_correct_info(deltaT)){
        CorrectInfo<float_sylph_t> info(
            snapshots[0].ins_gps->correct_info(gps));
        post_correct_info(info);
      }
      return *this;
    }
};

template <class INS_GPS_BE>
class INS_GPS_BE_NAV : public INS_GPS_NAV<INS_GPS_BE> {
  public:
    typedef INS_GPS_NAV<INS_GPS_BE> super_t;
    INS_GPS_BE_NAV() : super_t() {
      /**
       * バイアス項に関する設定を行う
       */
      
      super_t::nav.beta_accel() *= 0.1;
      super_t::nav.beta_gyro() *= 0.1;//mems_g.BETA;
      
      {
        Matrix<float_sylph_t> P(super_t::nav.getFilter().getP());
        P(10, 10) = P(11, 11) = P(12, 12) = 1E-2;
        P(13, 13) = P(14, 14) = P(15, 15) = 1E-6;
        super_t::nav.getFilter().setP(P);
      }
      {
        Matrix<float_sylph_t> Q(super_t::nav.getFilter().getQ());
        Q(7, 7) = 1E-4; //1E-10;
        Q(8, 8) = 1E-4; //1E-10;
        Q(9, 9) = 1E-4; //1E-10;
        Q(10, 10) = 1E-6; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16; //sim6と同じ設定
        Q(11, 11) = 1E-6; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
        Q(12, 12) = 1E-6; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
        super_t::nav.getFilter().setQ(Q);
      }
    }
    ~INS_GPS_BE_NAV(){}
};

// TIのコンパイラが賢くないので、特殊化で回避(定数を定数として認識しない!!)
template <>
const unsigned Filtered_INS_BiasEstimated<
    float_sylph_t, 
    KalmanFilter<float_sylph_t>, 
    Filtered_INS2<float_sylph_t, KalmanFilter<float_sylph_t> > >::P_SIZE
        = 16;
template <>
const unsigned Filtered_INS_BiasEstimated<
    float_sylph_t, 
    KalmanFilterUD<float_sylph_t>, 
    Filtered_INS2<float_sylph_t, KalmanFilterUD<float_sylph_t> > >::P_SIZE
        = 16;

// どのタイプのINS/GPSを使うか
template <bool bias_on, bool use_udkf>
struct NAV_instance_t { 
  typedef INS_GPS_NAV<
                INS_GPS2<
                    float_sylph_t,
                    KalmanFilter<float_sylph_t> > > type_name;
};

template <>
struct NAV_instance_t<true, true> { 
  typedef INS_GPS_BE_NAV<
              INS_GPS2_BiasEstimated<float_sylph_t> > type_name;
};

template <>
struct NAV_instance_t<true, false> { 
  typedef INS_GPS_BE_NAV<
              INS_GPS2_BiasEstimated<
                  float_sylph_t,
                  KalmanFilter<float_sylph_t> > > type_name;
};

template <>
struct NAV_instance_t<false, true> { 
  typedef INS_GPS_NAV<
              INS_GPS2<float_sylph_t> > type_name;
};


/**
 * 現在の状態を表現するクラス
 * 
 */
class NAVStatus {
  public:
    static const float_sylph_t INTERVAL_THRESHOLD;
    static const float_sylph_t INTERVAL_FORCE_VALUE;
  protected:    
    bool initialized;
    typedef typename NAV_instance_t<
        NAVOptions::bias_on, NAVOptions::use_udkf>::type_name nav_t;
    nav_t nav;
    Vector3<float_sylph_t> on_bias_accel, on_bias_gyro;
    int before_init_counter;
    int heading_correction_counter;
    A_Packet before_init_a_packet_mean;
    A_Packet previous_a_packet;
    bool valid_previous_a_packet;
    unsigned char seq_num;
    int telemetry_dump_count;

  public:
    NAVStatus() 
        : initialized(false), 
        nav(), 
        on_bias_accel(), on_bias_gyro(), 
        before_init_counter(0), before_init_a_packet_mean(),
        heading_correction_counter(0),
        previous_a_packet(), valid_previous_a_packet(false),
        seq_num(0), telemetry_dump_count(0) {
      
      for(int i(0); 
      		i < sizeof(before_init_a_packet_mean.ch) / sizeof(before_init_a_packet_mean.ch[0]); 
      		i++){
        before_init_a_packet_mean.ch[i] = 0;
      }
    }
    ~NAVStatus(){}
  
  public:
    NAV &navigation(){return nav;}
    
    enum dump_mode_t {
      DUMP_UPDATE, DUMP_CORRECT
    };
    
    /**
     * 現在の状態を記録する関数
     * 
     * @param label 動作モード
     * @param itow 現在時刻
     */
    void dump(const dump_mode_t mode, const float_sylph_t &itow){
      
      float_sylph_t _itow(itow);
      
      switch(mode){
        case DUMP_UPDATE:
          if(!nav_options.dump_update){return;}
          break;
        case DUMP_CORRECT:
          if(!nav_options.dump_correct){return;}
          // MUをしたときはTUの最新の状態まで更新される
          _itow = previous_a_packet.itow;
          break;
        default:
          return;
      }
      
      current.update_nav(_itow, nav, initialized);
      
      char buf[PAGE_SIZE];
      nav.encode_N0(_itow, buf, seq_num++, initialized);
      external_write(buf, sizeof(buf));
      
      // 5回に1回、テレメトリとして送る
      if(++telemetry_dump_count < 5){return;}
      telemetry_dump_count = 0;
      telemetry_write(buf, sizeof(buf));
    }
    
    /**
     * 時間更新を行う関数
     * 
     * 一定時間間隔で行われるAD変換結果より得られる
     * 加速度(加速度計に依存)、角速度(ジャイロに依存)を利用する。
     * 
     * @param a_packet AD変換結果
     */
    void time_update(const A_Packet &a_packet){
      Vector3<float_sylph_t> 
        accel(a_packet.accel() - on_bias_accel),
        omega(a_packet.gyro() - on_bias_gyro);
      
      current.update_accel_omega(a_packet.itow, accel, omega);
      
#define pow2(x) ((x) * (x))
      if(!initialized){
        for(int i(0); 
            i < sizeof(a_packet.ch) / sizeof(a_packet.ch[0]); 
            i++){
          before_init_a_packet_mean.ch[i] *= 63; // 整数なのでスケーリングしておく
          if(i == 8){
            // 温度にはゲタを履かせておく
            before_init_a_packet_mean.ch[i] += (a_packet.ch[i] << 8);
          }else{
            before_init_a_packet_mean.ch[i] += a_packet.ch[i];
          }
          before_init_a_packet_mean.ch[i] >>= 6; // (/= 64)
        }
        before_init_counter++;
        
        /*if(before_init_counter == 0x400){
          // 室内で強制的にイニシャライズするためのコード
          G_Packet g_packet;
          measurement_update(g_packet);
        }*/
        
        return;
      }
      
      if(valid_previous_a_packet){
#if DEBUG
        LOG_printf(&trace, "TU : %f", a_packet.itow);
#endif

        //前回時刻よりの差分を計算
        float_sylph_t interval(previous_a_packet.interval(a_packet));
        if((interval < 0) || (interval >= INTERVAL_THRESHOLD)){
          // 時刻の不連続点を観測したら、強制的に書き換える
          interval = INTERVAL_FORCE_VALUE;
        }
        nav.update(accel, omega, interval);
      }
      
      previous_a_packet = a_packet;
      valid_previous_a_packet = true;
    }
    
    /**
     * 観測更新を行う関数
     * 
     * GPS受信機より観測量(位置、速度)が得られたときに行う
     * 
     * @param g_packet GPS受信機より得られる観測量
     */
    void measurement_update(const G_Packet &g_packet){
      
      if(initialized){
        LOG_printf(&trace, "MU : %f", g_packet.itow);
        
        nav.correct(
            g_packet.convert(),
            g_packet.itow - previous_a_packet.itow);
        
        // ヘディングの推定精度を改善するために、
        // 動き始めについては、速度方向からヘディングの方向を求める
        if((heading_correction_counter < 0x20) && (g_packet.acc_v < 2.0)){
          float_sylph_t vel_2d(
              pow2(g_packet.vel_ned[0]) + pow2(g_packet.vel_ned[1]));
          if(vel_2d > 2.0){ // 2m/s以上
            heading_correction_counter++;
            nav.overwrite_yaw(
                atan2(g_packet.vel_ned[1], g_packet.vel_ned[0])); // Y, Xの順
          }
        }
      }else if((before_init_counter >= 0x200) // ON時バイアス推定のため、ある程度待つ
          && (g_packet.acc_2d <= 20.) && (g_packet.acc_v <= 10.)){
        /*
         * GPS受信機の出力する推定2D位置誤差が20m以内、
         * 高度誤差が10m以内になったときをフィルター開始時点とする
         * 以後、Time UpdateとMeasurement Updateが有効になる
         */
        LOG_printf(&trace, "Init : %f", g_packet.itow);
        nav.init(
            deg2rad(g_packet.llh[0]), deg2rad(g_packet.llh[1]), g_packet.llh[2],
            g_packet.vel_ned[0], g_packet.vel_ned[1], g_packet.vel_ned[2],
            deg2rad(0), deg2rad(0), 0.);
        initialized = true;
        
        // スケーリングしてある部分をもとに戻す
        before_init_a_packet_mean.ch[8] >>= 8;
        
        // ON時バイアスの推定
        if(before_init_counter){
          on_bias_accel = before_init_a_packet_mean.accel();  
#if 0
          // 3 k^2 - 2 * (x + y + z) k + x^2 + y^2 + z^2 = g^2
          // 3 k^2 - 2 * (x + y + z) k + (x^2 + y^2 + z^2 - g^2) = 0
          float_sylph_t a = 3;
          float_sylph_t b = - 2 * (on_bias_accel[0] + on_bias_accel[1] + on_bias_accel[2]);
          float_sylph_t c = (on_bias_accel.abs2() - pow(WGS84::gravity(deg2rad(g_packet.llh[0])), 2));
          float_sylph_t k_p = (-b + sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
          float_sylph_t k_n = (-b - sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
          //cerr << k_p << "," << k_n << endl;
          float_sylph_t k = (abs(k_p) < abs(k_n)) ? k_p : k_n;
          
          on_bias_accel[0] = k;
          on_bias_accel[1] = k;
          on_bias_accel[2] = k;
#elif 0
          // (x(1-k))^2 + (y(1-k))^2 + (z(1-k))^2 = g^2
          // (x^2 + y^2 + z^2) k^2 - 2(x^2 + y^2 + z^2) k + (x^2 + y^2 + z^2 - g^2) = 0
          //cerr << on_bias_accel << endl;
          float_sylph_t a = on_bias_accel.abs2();
          float_sylph_t b = - on_bias_accel.abs2() * 2;
          float_sylph_t c = (on_bias_accel.abs2() - pow(WGS84::gravity(deg2rad(g_packet.llh[0])), 2));
          //cerr << a << "," << b << "," << c << endl;
          float_sylph_t k_p = (-b + sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
          float_sylph_t k_n = (-b - sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
          //cerr << k_p << "," << k_n << endl;
          float_sylph_t k = (abs(k_p) < abs(k_n)) ? k_p : k_n;
          
          on_bias_accel *= k;
#else
          // 何もしない
          on_bias_accel[0] = 0;
          on_bias_accel[1] = 0;
          on_bias_accel[2] = 0;
#endif
          on_bias_gyro = before_init_a_packet_mean.gyro();
        }
      }
      
      LOG_printf(&trace, "MU_TU done");
    }
    
    bool is_initialized(){return initialized;}
} nav_status;

const float_sylph_t NAVStatus::INTERVAL_THRESHOLD = 10;
const float_sylph_t NAVStatus::INTERVAL_FORCE_VALUE = 1E-3 * NAV_UPDATE_RATIO_ms;

#endif /* __SYLPH_NAV_H__ */