/**
 * @file Sylphide ログ 航法リアルタイム処理用プログラム
 * 
 * Sylphideからセンサデータを取り出し、リアルタイム処理をかけるプログラムです。
 */

#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <exception>

#include <cstdio>
#include <cstdlib>
#include <io.h>
#include <fcntl.h>

#include <vector>

//#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "SylphideProcessor.h"
#include "SylphideStream.h"

typedef double float_sylph_t;

#include "param/matrix.h"
#include "param/vector3.h"
VECTOR3_NO_FLY_WEIGHT(float_sylph_t);
#include "param/quarternion.h"
QUARTERNION_NO_FLY_WEIGHT(float_sylph_t);
#include "param/complex.h"

#include "util/util.h"
#include "util/logger.h"

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

#include "calibration.h"
#include "lever_arm_effect.h"
#include "packet.h"

#include "analyze_common.h"

struct Options : public GlobalOptions<float_sylph_t> {
  
  typedef GlobalOptions<float_sylph_t> super_t;
  bool dsp_emulate;
  int rt_mode;
  int on_bias_mode;
  
  Options()
      : super_t(),
      dsp_emulate(false),
      rt_mode(0),
      on_bias_mode(0) {}
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(const char *spec){
    using std::cerr;
    using std::endl;
    
    const char *value;

    // DSPエミュレートモード
    if(value = get_value(spec, "dsp_emulate")){
      dsp_emulate = is_true(value);
      if(dsp_emulate){
        in_sylphide = true;
        out_sylphide = true;
        out_is_N_packet = true;
        rt_mode = 1;
      }
      cerr << "dsp_emulate: " << (dsp_emulate ? "on" : "off") << endl;
      return true;
    }
    if(value = get_value(spec, "rt_mode", false)){
      rt_mode = std::atoi(value);
      cerr << "rt_mode: " << rt_mode << endl;
      return true;
    }
    if(value = get_value(spec, "on_bias_mode", false)){
      on_bias_mode = std::atoi(value);
      cerr << "on_bias_mode: " << on_bias_mode << endl;
      return true;
    }
    
    return super_t::check_spec(spec);
  }
} options;

class NAV : public NAVData, public Loggable {
  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 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;
    virtual NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps, 
        const Vector3<float_sylph_t> &lever_arm_b,
        const Vector3<float_sylph_t> &omega_b2i_4b,
        const float_sylph_t &deltaT){
      return correct(gps, deltaT);
    }
};

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> Phi_inv;
      Matrix<float_sylph_t> GQGt;
      float_sylph_t deltaT_update;
      snapshot_content_t(
          INS_GPS_RT *_ins_gps,
          const Matrix<float_sylph_t> &_A,
          const Matrix<float_sylph_t> &_Phi_inv,
          const Matrix<float_sylph_t> &_GQGt, 
          const float_sylph_t &_deltaT)
          : ins_gps(_ins_gps), A(_A), Phi_inv(_Phi_inv), GQGt(_GQGt), 
          deltaT_update(_deltaT){
      }
      snapshot_content_t &operator=(const snapshot_content_t &another){
        ins_gps = another.ins_gps;
        A = another.A;
        Phi_inv = another.Phi_inv;
        GQGt = another.GQGt;
        deltaT_update = another.deltaT_update;
        return *this;
      }
    };
    typedef std::vector<snapshot_content_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){
          Matrix<float_sylph_t> Phi(A * deltaT);
          for(unsigned i(0); i < A.rows(); i++){Phi(i, i) += 1;}
          Matrix<float_sylph_t> Gamma(B * deltaT);
          
          snapshots.push_back(
              snapshot_content_t(new INS_GPS_RT(*this), 
                  A, Phi.inverse(), Gamma * INS_GPS::getFilter().getQ() * Gamma.transpose(),
                  deltaT));
        }
    };
    INS_GPS_RT _nav;
    INS_GPS &nav;

    Vector3<float_sylph_t> accel_ms2;
    Vector3<float_sylph_t> omega_rads;
  public:
    INS_GPS_NAV() 
        : NAV(), snapshots(), 
        _nav(snapshots), nav(_nav), accel_ms2(), omega_rads() {}
    ~INS_GPS_NAV() {}
    void inspect(std::ostream &out) const {
      if(options.debug_system_cov){
        Matrix<float_sylph_t> P(nav.getFilter().getP());
        for(int i(0); i < P.rows(); i++){
          for(int j(0); j < P.rows(); j++){
            out << P(i, j) << '\t';
          }
        }
      }
    }
    
  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);
      }
    }

#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){
      accel_ms2 = accel;
      omega_rads = gyro;
      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;} // 未来の時間のデータはおかしい
      
      //cerr << "Before(" << deltaT << ") : " << snapshots.size() << " => ";
      for(typename snapshots_t::reverse_iterator it(snapshots.rbegin());
          it != snapshots.rend();
          ++it){
        deltaT += it->deltaT_update;
        if(deltaT > float_sylph_t(-0.005)){ // 一番近い時刻を探してくる
          if(it == snapshots.rbegin()){
            // スナップショットが全部なくなる問題を回避する
            it++;
          }
          for(typename snapshots_t::iterator it2(snapshots.begin());
              it2 != it.base();
              ++it2){
            delete it2->ins_gps;
          }
          snapshots.erase(snapshots.begin(), it.base());
          return true;
        }
      }
      //cerr << snapshots.size() << ", " << deltaT << endl;
      
      return false; // 古すぎるデータ
    }
    
    /**
     * 得られた修正情報を加工し、実際に修正を加えるまでを行う
     * 
     * @param info 修正情報
     */
    void post_correct_info(CorrectInfo<float_sylph_t> &info){
      Matrix<float_sylph_t> &H(info.H), &R(info.R);
      //cerr << "Before(H) : " << info.H << endl;
      //cerr << "Before(R) : " << info.R << endl;
      switch(options.rt_mode){
        case 1: 
          if(!snapshots.empty()){
            Matrix<float_sylph_t> sum_A(H.columns(), H.columns());
            Matrix<float_sylph_t> sum_GQGt(sum_A.rows(), sum_A.rows());
            float_sylph_t bar_delteT(0);
            for(typename snapshots_t::iterator it(snapshots.begin());
                it != snapshots.end();
                ++it){
              sum_A += it->A;
              sum_GQGt += it->GQGt;
              bar_delteT += it->deltaT_update;
            }
            int n(snapshots.size());
            bar_delteT /= n;
            Matrix<float_sylph_t> sum_A_GQGt(sum_A * sum_GQGt);
            R += H
                * (sum_GQGt -= ((sum_A_GQGt + sum_A_GQGt.transpose()) *= (bar_delteT * (n + 1) / (2 * n))))
                * H.transpose();
            H *= (Matrix<float_sylph_t>::getI(sum_A.rows()) - sum_A * bar_delteT);
          }
          break;
        default:
          for(typename snapshots_t::iterator it(snapshots.begin());
              it != snapshots.end();
              ++it){
            //cerr << "inv:" << (it->Phi_inv) << endl;
            H *= it->Phi_inv;
            R += H * it->GQGt * H.transpose();
          }
      }
      //cerr << "After(H) : " << info.H << endl;
      //cerr << "After(R) : " << info.R << endl;
      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;
    }
    /**
     * アーム長を使うモード
     * 
     * @param gps GPSの情報
     * @param lever_arm_b アーム
     * @param omega_b2i_4b 観測角速度
     * @param deltaT いつのタイミングの情報か(負の値が前の時刻)
     */
    NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps, 
        const Vector3<float_sylph_t> &lever_arm_b,
        const Vector3<float_sylph_t> &omega_b2i_4b,
        const float_sylph_t &deltaT){
      if(pre_correct_info(deltaT)){
        CorrectInfo<float_sylph_t> info(
            snapshots[0].ins_gps->correct_info(gps, lever_arm_b, omega_b2i_4b));
        post_correct_info(info);
      }
      return *this;
    }
    
    /**
     * 状態を出力する関数のためのラベル
     * 
     */
    void label(std::ostream &out = std::cout) const {
      NAV::label(out);
      out << "accel_X" << "\t"
          << "accel_Y" << "\t"
          << "accel_Z" << "\t"
          << "omega_X" << "\t"
          << "omega_Y" << "\t"
          << "omega_Z" << "\t";
      if(options.dump_q_n2b){
        out << "q_n2b_0" << "\t"
            << "q_n2b_1" << "\t"
            << "q_n2b_2" << "\t"
            << "q_n2b_3" << "\t";
      }
      if(options.dump_stddev){
        out << "stddev(longitude)" << "\t"
            << "stddev(latitude)" << "\t"
            << "stddev(height)" << "\t"
            << "stddev(v_north)" << "\t"
            << "stddev(v_east)" << "\t"
            << "stddev(v_down)" << "\t"
            << "stddev(Yaw(Ψ))" << "\t"        //Ψ(yaw)
            << "stddev(Pitch(Θ))" << "\t"      //Θ(pitch)
            << "stddev(Roll(Φ))" << "\t";      //Φ(roll)
      }
    }
  
  protected:
    /**
     * 現在の状態を出力する関数
     * 
     * @param itow 現在時刻
     */
    void dump(std::ostream &out) const {
      NAV::dump(out);
      for(int i(0); i < accel_ms2.OUT_OF_INDEX; ++i){
        out << accel_ms2.get(i) << "\t";
      }
      for(int i(0); i < omega_rads.OUT_OF_INDEX; ++i){
        out << rad2deg(omega_rads.get(i)) << "\t";
      }
      if(options.dump_q_n2b){
        for(int i(8); i <= 11; i++){
          out << nav[i] << "\t";
        }
      }
      if(options.dump_stddev){
        typename INS_GPS::StandardDeviations sigma(nav.getSigma());
        out << rad2deg(sigma.longitude_rad) << "\t"
            << rad2deg(sigma.latitude_rad) << "\t"
            << sigma.height_m << "\t"
            << sigma.v_north_ms << "\t"
            << sigma.v_east_ms << "\t"
            << sigma.v_down_ms << "\t"
            << rad2deg(sigma.heading_rad) << "\t"
            << rad2deg(sigma.pitch_rad) << "\t"
            << rad2deg(sigma.roll_rad) << "\t";
      }
    }
};

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(){}
    
    NAV &update(
        const Vector3<float_sylph_t> &accel,
        const Vector3<float_sylph_t> &gyro,
        const float_sylph_t &deltaT){
      super_t::update(accel, gyro, deltaT);
      super_t::accel_ms2 -= super_t::nav.bias_accel();
      super_t::omega_rads -= super_t::nav.bias_gyro();
      return *this;
    }

    NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps, 
        const Vector3<float_sylph_t> &lever_arm_b,
        const Vector3<float_sylph_t> &omega_b2i_4b,
        const float_sylph_t &deltaT){
      return super_t::correct(
          gps, lever_arm_b, omega_b2i_4b - super_t::nav.bias_gyro(), deltaT);
    }
    
    /**
     * 状態を出力する関数のためのラベル
     * 
     */
    void label(std::ostream &out = std::cout) const {
      super_t::label(out);
      out << "bias_accel(X)" << "\t"  //Bias
         << "bias_accel(Y)" << "\t"
         << "bias_accel(Z)" << "\t"
         << "bias_gyro(X)" << "\t"
         << "bias_gyro(Y)" << "\t"
         << "bias_gyro(Z)" << "\t";
      if(options.dump_stddev){
        out << "stddev(bias_accel(X))" << "\t"
            << "stddev(bias_accel(Y))" << "\t"
            << "stddev(bias_accel(Z))" << "\t"
            << "stddev(bias_gyro(X))" << "\t"
            << "stddev(bias_gyro(Y))" << "\t"
            << "stddev(bias_gyro(Z))" << "\t";
      }
    }
  
  protected:
    /**
     * 現在の状態を出力する関数
     * 
     * @param itow 現在時刻
     */
    void dump(std::ostream &out) const {
      super_t::dump(out);
      Vector3<float_sylph_t> &ba(super_t::nav.bias_accel());
      Vector3<float_sylph_t> &bg(super_t::nav.bias_gyro());
      out << ba.getX() << "\t"     // Bias
           << ba.getY() << "\t"
           << ba.getZ() << "\t"
           << bg.getX() << "\t"
           << bg.getY() << "\t"
           << bg.getZ() << "\t";
      if(options.dump_stddev){
        Matrix<float_sylph_t> &P(
            const_cast<Matrix<float_sylph_t> &>(super_t::nav.getFilter().getP()));
        for(int i(0); i < 6; ++i){
          out << sqrt(P(10 + i, 10 + i)) << "\t";
        }
      }
    }
};

using namespace std;

/**
 * 現在の状態を表現するクラス
 * 
 */
class Status {
  protected:
    bool initalized;
    NAV &nav;
    vector<A_Packet> init_A_packets;
    Vector3<float_sylph_t> on_bias_accel, on_bias_gyro;
    LeverArm *lever_arm;
    A_Packet previous_a_packet;
    bool valid_previous_a_packet;
    
    // 調査用パーツ
    Logger debugger;

  // Lever Arm Effect用の値
#define GYRO_STORAGE 16
    Vector3<float_sylph_t> gyro_storage[GYRO_STORAGE];
    int gyro_index;
    bool gyro_init;

  public:
    Status(NAV &_nav, LeverArm *_lever_arm = NULL) 
        : initalized(false), nav(_nav), gyro_index(0), gyro_init(false),
        init_A_packets(),
        on_bias_accel(), on_bias_gyro(), lever_arm(_lever_arm),
        debugger(options.spec2ostream(options.debug_fname)),
        previous_a_packet(), valid_previous_a_packet(false) {
    
      debugger.out() << setprecision(16);
      debugger.add(nav);
    }
    ~Status(){}
  
  public:
    /**
     * Filter自身を取り出す関数
     * 
     */
    NAV &get_nav() {return nav;}
    
    /**
     * 状態に対応するラベルを出力する
     * 
     */
    void dump_label(){
      if(!options.out_is_N_packet){
        options.out() << "mode" << "\t"
            << "itow" << "\t";
        nav.label(options.out());
        options.out() << endl;
      }
    }
    
    enum dump_mode_t {
      DUMP_UPDATE, DUMP_CORRECT
    };
    
    /**
     * 現在の状態を記録する関数
     * 
     * @param label 動作モード
     * @param itow 現在時刻
     */
    void dump(const dump_mode_t mode, const float_sylph_t &itow){
      
      if(!initalized){return;}
      
      const char *label;
      float_sylph_t _itow(itow);
      
      switch(mode){
        case DUMP_UPDATE:
          if(!options.dump_update){return;}
          label = "TU";
          break;
        case DUMP_CORRECT:
          if(!options.dump_correct){return;}
          label = "MU";
          // MUをしたときはTUの最新の状態まで更新される
          _itow = previous_a_packet.itow;
          break;
        default:
          return;
      }
      
      if(!options.out_is_N_packet){
        options.out() << label << "\t"
            << _itow << "\t"
            << nav << endl;
      }else{
        char buf[PAGE_SIZE];
        nav.encode_N0(_itow, buf);
        options.out().write(buf, sizeof(buf));
      }
      
      debugger.out() << _itow;
      debugger.flush();
      debugger.out() << endl;
    }
    
    /**
     * 時間更新を行う関数
     * 
     * 一定時間間隔で行われるAD変換結果より得られる
     * 加速度(加速度計に依存)、角速度(ジャイロに依存)を利用する。
     * 
     * @param a_packet AD変換結果
     */
    void time_update(const A_Packet &a_packet){
      
#define pow2(x) ((x) * (x))
      if(!initalized){
        init_A_packets.push_back(a_packet);
        return;
      }
      
      // for LAE
      {
        gyro_storage[gyro_index++] = a_packet.gyro() - on_bias_gyro;
        if(gyro_index == GYRO_STORAGE){
          gyro_index = 0;
          if(!gyro_init) gyro_init = true;
        }
      }
      
      if(valid_previous_a_packet){
#if DEBUG
        cerr << "TU : " << a_packet.itow << endl;
#endif

        //前回時刻よりの差分を計算
        float_sylph_t interval(previous_a_packet.interval(a_packet));
#define INTERVAL_THRESHOLD 10
#define INTERVAL_FORCE_VALUE 0.01
        if((interval < 0) || (interval >= INTERVAL_THRESHOLD)){
          // 時刻の不連続点を観測したら、強制的に書き換える
          interval = INTERVAL_FORCE_VALUE;
        }

        nav.update(
            a_packet.accel() - on_bias_accel, 
            a_packet.gyro() - on_bias_gyro, 
            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(initalized){
        cerr << "MU : " << g_packet.itow << endl;
        
        if(gyro_init && lever_arm){
          // LAE on
          Vector3<float_sylph_t> omega_b2i_4n;
          for(int i = 0; i < GYRO_STORAGE; i++){
            omega_b2i_4n += gyro_storage[i];
          }
          omega_b2i_4n /= GYRO_STORAGE;
          nav.correct(
              g_packet.convert(), 
              lever_arm->lever_arm(),
              omega_b2i_4n, 
              g_packet.itow - previous_a_packet.itow);
        }else{
          // LEA off
          nav.correct(
              g_packet.convert(),
              g_packet.itow - previous_a_packet.itow);
        }
      }else if((g_packet.acc_2d <= 20.) && (g_packet.acc_v <= 10.)){

        /*
         * GPS受信機の出力する推定2D位置誤差が20m以内、
         * 高度誤差が10m以内になったときをフィルター開始時点とする
         * 以後、Time UpdateとMeasurement Updateが有効になる
         */

        float_sylph_t attitude[3];
        for(int i(0); i < sizeof(attitude) / sizeof(attitude[0]); ++i){
          attitude[i] = deg2rad(options.init_attitude_deg[i]);
        }
        float_sylph_t&yaw(attitude[0]), &pitch(attitude[1]), &roll(attitude[2]);

        // TODO 初期姿勢推定

        Vector3<float_sylph_t> 
            mu_accel, sigma_accel, 
            mu_gyro, sigma_gyro;
            
        // 平均値と分散の計算
        if(!init_A_packets.empty()){
          for(vector<A_Packet>::iterator it(init_A_packets.begin());
              it != init_A_packets.end();
              ++it){
            Vector3<float_sylph_t> a(it->accel()), g(it->gyro());
            Vector3<float_sylph_t> 
                a2(pow2(a[0]), pow2(a[1]), pow2(a[2])), 
                g2(pow2(g[0]), pow2(g[1]), pow2(g[2]));
            mu_accel += a;
            sigma_accel += a2;
            mu_gyro += g;
            sigma_gyro += g2;
          }
        
          mu_accel /= init_A_packets.size();
          mu_gyro /= init_A_packets.size();
        }
        
        {
          // s^2 = (sum (i - mu)^2) / n = (sum (i^2 - 2 i mu + mu^2)) / n 
          // = (sum i^2) / n - mu^2
          Vector3<float_sylph_t> 
              mu_a2(pow2(mu_accel[0]), pow2(mu_accel[1]), pow2(mu_accel[2])), 
              mu_g2(pow2(mu_gyro[0]), pow2(mu_gyro[1]), pow2(mu_gyro[2]));
          
          sigma_accel /= init_A_packets.size();
          sigma_gyro /= init_A_packets.size();
              
          sigma_accel -= mu_a2;
          sigma_gyro -= mu_g2;
          
          for(int i(0); i < Vector3<float_sylph_t>::OUT_OF_INDEX; i++){
            sigma_accel[i] = sqrt(sigma_accel[i]);
            sigma_gyro[i] = sqrt(sigma_gyro[i]);
          }
        }
        
        cerr << "Init : " << g_packet.itow << endl;
        initalized = true;
        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(options.init_attitude_deg[0]), deg2rad(0), 0.);
        cerr << "Initial attitude (yaw, pitch, roll) [deg]: "
            << rad2deg(yaw) << ", "
            << rad2deg(pitch) << ", "
            << rad2deg(roll) << endl;


        // TODO 必要? (ON時バイアスの推定)
        switch(options.on_bias_mode){
          case 0: {
            // 加速度計、バイアス推定せず
            on_bias_accel[0] = on_bias_accel[1] = on_bias_accel[2] = 0;
            
            // ジャイロはバイアスを平均値とする
            on_bias_gyro = mu_gyro;
            
            break;
          }
          case 1: {
            // 加速度計、バイアス推定あり、以下のモデルに従ってkを推定
            // 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
            on_bias_accel = mu_accel;
            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] = on_bias_accel[1] = on_bias_accel[2] = k;
            
            // ジャイロはバイアスを平均値とする
            on_bias_gyro = mu_gyro;
            
            break;
          }
          case 2: {
            // 加速度計、バイアス推定あり、以下のモデルに従ってkを推定
            // (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
            on_bias_accel = mu_accel;
            //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;
            
            // ジャイロはバイアスを平均値とする
            on_bias_gyro = mu_gyro;
            
            break;
          }
          case 3: {
            // 加速度計、バイアス推定せず
            on_bias_accel[0] = on_bias_accel[1] = on_bias_accel[2] = 0;
            
            // ジャイロもバイアス推定なし
            on_bias_accel[0] = on_bias_accel[1] = on_bias_accel[2] = 0;
            
            break;
          }
        }
        
        dump_label();
      }
    }
    
    bool is_initalized(){return initalized;}
};

class StreamProcessor 
    : public AbstractSylphideProcessor<float_sylph_t> {
  public:
    static const unsigned int buffer_size;
  protected:
    typedef AbstractSylphideProcessor<float_sylph_t> super_t;
    typedef A_Packet_Observer<float_sylph_t> A_Observer_t;
    typedef G_Packet_Observer<float_sylph_t> G_Observer_t;
    
    /**
     * Aページ(AD変換結果)の処理用関数
     * 
     * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Aページのオブザーバー
     */
    struct AHandler : public A_Observer_t {
      Status &status;
      bool previous_seek_next;
      AHandler(Status &_status) : A_Observer_t(buffer_size), status(_status) {
        previous_seek_next = A_Observer_t::ready();
      }
      ~AHandler(){}
      void operator()(const A_Observer_t &observer){
        if(!observer.validate()){return;}
      
        A_Packet packet;
        packet.itow = observer.fetch_ITOW();
        
        A_Observer_t::values_t values(observer.fetch_values());
        for(int i = 0; i < 8; i++){
          packet.ch[i] = values.values[i];
        }
        packet.ch[8] = values.temperature;
        
        status.time_update(packet);
        status.dump(Status::DUMP_UPDATE, packet.itow);
      }
    };
    
#define itow_round(t) ((float_sylph_t)((int)((t) * 100 + 0.5)) / 100)  
    
    /**
     * Gページ(u-bloxのGPS)の処理用関数
     * 
     * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * {class, id} = {0x01, 0x02}のとき位置情報が得られる
     * {class, id} = {0x01, 0x12}のとき速度情報が得られる
     * 
     * @param obsrever Aページのオブザーバー
     */
    struct GHandler : public G_Observer_t  {
      Status &status;
      unsigned int itow_ms_0x0102, itow_ms_0x0112;
      G_Packet packet;
      bool previous_seek_next;
      typedef G_Observer_t::status_t status_t;
      unsigned int gps_status;
      bool exit_flag;
      
      GHandler(Status &_status) 
          : G_Observer_t(buffer_size), status(_status),
          packet(), itow_ms_0x0102(0), itow_ms_0x0112(0),
          gps_status(status_t::NO_FIX),
          exit_flag(false) {
        previous_seek_next = G_Observer_t::ready();
      }
      ~GHandler(){}
      void operator()(const G_Observer_t &observer){
        if(!observer.validate()){return;}
        
        bool change_itow(false);
        G_Observer_t::packet_type_t
            packet_type(observer.packet_type());
        switch(packet_type.mclass){
          case 0x01: {
            switch(packet_type.mid){
              case 0x02: {
                G_Observer_t::position_t 
                  position(observer.fetch_position());
                G_Observer_t::position_acc_t
                  position_acc(observer.fetch_position_acc());
                
                //cerr << "G_Arrive 0x02 : " << observer.fetch_ITOW() << endl;
                
                itow_ms_0x0102 = observer.fetch_ITOW_ms();
                change_itow = true;
                
                packet.llh[0] = position.latitude;
                packet.llh[1] = position.longitude;
                packet.llh[2] = position.altitude;
                packet.acc_2d = position_acc.horizontal;
                packet.acc_v = position_acc.vertical;
                
                break;
              }
              case 0x03: {
                status_t status(observer.fetch_status());
                gps_status = status.fix_type;
                break;
              }
              case 0x12: {
                G_Observer_t::velocity_t
                    velocity(observer.fetch_velocity());
                G_Observer_t::velocity_acc_t
                    velocity_acc(observer.fetch_velocity_acc());
                
                //cerr << "G_Arrive 0x12 : " << current_itow << " =? " << packet.itow << endl;
                
                itow_ms_0x0112 = observer.fetch_ITOW_ms();
                change_itow = true;
                
                packet.vel_ned[0] = velocity.north;
                packet.vel_ned[1] = velocity.east;
                packet.vel_ned[2] = velocity.down;
                packet.acc_vel = velocity_acc.acc;
                
                break;
              }
            }
            break;
          }
          case 0x02: { 
            break;
          }
          default:
            break;
        }
        
        if(change_itow
            && (itow_ms_0x0102 == itow_ms_0x0112)){
          
          packet.itow = (float_sylph_t)1E-3 * itow_ms_0x0102;
          
          if(options.start_gpstime > packet.itow){return;}

          if(gps_status == status_t::FIX_3D){
            // 3D Fixをしていた場合、GPS観測量を用いてMeasurement Updateをする
            status.measurement_update(packet);
          }
          status.dump(Status::DUMP_CORRECT, packet.itow);
          
          if(packet.itow >= options.end_gpstime){
            exit_flag = true;
          }
        }
      }
    };
    
  public:
    StreamProcessor() : super_t() {
      
    }
    virtual ~StreamProcessor(){}
    
    /**
     * ファイル等のストリームから1ページ単位で処理を行う関数
     * 
     * @param in ストリーム
     * @param status 更新を通知するインスタンス
     */
    void process(istream &in, Status &status){
      char buffer[PAGE_SIZE];
      int invoked(0);
      int read_count;
      AHandler a_handler(status);
      GHandler g_handler(status);
      
      while(true){
        in.read(buffer, sizeof(buffer));
        read_count = in.gcount();
        if(in.fail() || (read_count == 0)){break;}
        invoked++;
      
#if DEBUG
        cerr << "--read-- : " << invoked << " page" << endl;
        cerr << hex;
        for(int i = 0; i < read_count; i++){
          cerr 
            << setfill('0') 
            << setw(2)
            << (unsigned int)((unsigned char)buffer[i]) << ' ';
        }
        cerr << dec;
        cerr << endl;
#endif
      
        if(read_count < PAGE_SIZE){
#if DEBUG
          cerr << "--skipped-- : " << invoked << " page ; count = " << read_count << endl;
#endif
        }
        
        if(options.dsp_emulate){
          // パススルーする
          options.out().write(buffer, sizeof(buffer));
        }
        
        switch(buffer[0]){
          case 'A':
            super_t::process_packet(
                buffer, read_count, 
                a_handler, a_handler.previous_seek_next, a_handler);
            break;
          case 'G':
            super_t::process_packet(
                buffer, read_count, 
                g_handler, g_handler.previous_seek_next, g_handler);
            break;
        }
        
        if(g_handler.exit_flag){break;}
      }
    }
};

const unsigned int StreamProcessor::buffer_size = PAGE_SIZE * 32;

int main(int argc, char *argv[]){
  
  cout << setprecision(10);
  cerr << setprecision(10);

  cerr << "Navigation Realtime-processor for Sylphide log." << endl;
  if(argc < 3){
    cerr << "Usage: (exe) IMU_mode log.dat "
        "[options ...]" << endl;
    return -1;
  }
  
  cerr << "Mode checking..." << endl;
  for(int i = 0; i < (sizeof(calibrators) / sizeof(CalibratorItem)); i++){
    cerr << calibrators[i].name << " : ";
    if(!strcmp(argv[1], calibrators[i].name)){
      cerr << "Ready !!" << endl;
      A_Packet::calibrator = calibrators[i].calibrator;
      break;
    }else{
      cerr << "NG" << endl;
    }
    if(i == ((sizeof(calibrators) / sizeof(CalibratorItem)) - 1)){
      cerr << "Target mode not found" << endl;
      return -1;
    } 
  }
  
  // ログファイルの指定
  istream &in(options.spec2istream(argv[2]));
  
  // option check...
  cerr << "Option checking..." << endl;
  
  LeverArm *lever_arm(NULL);
  for(int arg_index(3); arg_index < argc; arg_index++){
    
    const char *value;

    // ex) --ratate=-Y-Z-X
    if(value = Options::get_value(argv[arg_index], "lae", false)){
      cerr << "LAE Mode checking..." << endl;
      for(int i(0); i < (sizeof(lever_arms) / sizeof(lever_arms[0])); i++){
        cerr << lever_arms[i].name << " : ";
        if(strcmp(value, lever_arms[i].name) == 0){
          cerr << "Ready !!" << endl;
          lever_arm = lever_arms[i].lever_arm;
          break;
        }else{
          cerr << "NG" << endl;
        }
        if(i == ((sizeof(lever_arms) / sizeof(LeverArmItem)) - 1)){
          cerr << "Target mode not found" << endl;
          return -1;
        } 
      }
      continue;
    }
    if(value = Options::get_value(argv[arg_index], "rotate", false)){
      cerr << "rotate: " << value << endl;
      RotatedCalibrator::rotate_spec_t decoded;
      if(!RotatedCalibrator::decode(value, decoded)){
        cerr << "Invalid Rotation!!" << endl;
        return -1;
      }
      A_Packet::calibrator = new RotatedCalibrator(*A_Packet::calibrator, decoded);
      continue;
    }
    if(value = Options::get_value(argv[arg_index], "calib_file", false)){
      fstream fin(value);
      if(fin.fail()){
        cerr << value << " => File not found!!" << endl;
        exit(-1);
      }
      cerr << "calib_file: " << value << endl;
      if(strcmp(argv[1], CustomStandardCalibrator::id) != 0){continue;}
      CustomStandardCalibrator *target(
          static_cast<CustomStandardCalibrator *>(A_Packet::calibrator));
      char buf[1024];
      while(!fin.eof()){
        fin.getline(buf, sizeof(buf));
        target->custom(buf);
      }      
      continue;
    }
    if(options.check_spec(argv[arg_index])){continue;}
    
    cerr << "Unknown option!! : " << argv[arg_index] << endl;
    return -1;
  }
  
  INS_GPS_NAV<
      INS_GPS2<
          float_sylph_t,
          KalmanFilter<float_sylph_t> > > nav;
  INS_GPS_NAV<INS_GPS2<float_sylph_t> > nav_udkf;
  
  INS_GPS_BE_NAV<
      INS_GPS2_BiasEstimated<
          float_sylph_t,
          KalmanFilter<float_sylph_t> > > nav_bias;
  INS_GPS_BE_NAV<INS_GPS2_BiasEstimated<float_sylph_t> > nav_bias_udkf;
  
  Status status(
      (options.use_udkf
          ? (options.est_bias ? (NAV &)nav_bias_udkf : (NAV &)nav_udkf)
          : (options.est_bias ? (NAV &)nav_bias : (NAV &)nav)),
      lever_arm);
  
  StreamProcessor processor;
  
  if(options.out_sylphide){
    options._out = new SylphideOStream(options.out(), PAGE_SIZE);
  }else{
    options.out() << setprecision(10);
  }
  
  // DSPが動作しているように見せかける
  if(options.in_sylphide){
    SylphideIStream sylph_in(in, PAGE_SIZE);
    processor.process(sylph_in, status);
  }else{
    processor.process(in, status);
  }
  
  if(options.out_sylphide){
    delete options._out;
  }
  
  return 0;
}