/**
 * @file Sylphide ログ 航法後処理用プログラム
 * 
 * Sylphideのログからセンサデータを取り出し、後処理をかけるプログラムです。
 * 複数アンテナにも対応しています。
 */

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

#include <cstdio>
#include <cmath>
#include <cstring>

#include <vector>
#include <utility>
#include <deque>

//#define DEBUG 3

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

typedef double float_sylph_t;
typedef SylphideProcessor<float_sylph_t> Processor_t;
typedef Processor_t::A_Observer_t A_Observer_t;
typedef Processor_t::G_Observer_t G_Observer_t;
typedef Processor_t::M_Observer_t M_Observer_t;

#include "param/matrix.h"
#include "param/vector3.h"
#include "param/quarternion.h"
#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 "MagneticField.h"

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

#include "analyze_common.h"

struct Options : public GlobalOptions<float_sylph_t> {
  bool back_propagate;  //< 逆側への情報伝播を行う場合
  
  typedef GlobalOptions<float_sylph_t> super_t;

  /**
   * どの程度の深さまで逆側の修正を行うか正負の値で指定する
   * ゼロだと前回correctがあったところまで、負だとそれよりも深く
   */
  float_sylph_t back_propagate_depth;
  
  bool rotated; ///< 回転しているかどうか
  bool gps_fake_lock; ///< 疑似GPS受信モード
  
  Options()
      : super_t(),
      back_propagate(false), 
      back_propagate_depth(0),
      rotated(false),
      gps_fake_lock(false) {};
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(const char *spec){
    using std::cerr;
    using std::endl;
    
#define CHECK_OPTION(name, operation, disp) { \
  const char *value(get_value(spec, #name)); \
  if(value){ \
    {operation;} \
    std::cerr << #name << ": " << disp << std::endl; \
    return true; \
  } \
}
    CHECK_OPTION(back_propagate,
        back_propagate = is_true(value),
        (back_propagate ? "on" : "off"));
    CHECK_OPTION(bp_depth,
        back_propagate_depth = std::atof(value),
        back_propagate_depth);
    CHECK_OPTION(fake_lock,
        gps_fake_lock = is_true(value),
        (gps_fake_lock ? "on" : "off"));
#undef CHECK_OPTION

    return super_t::check_spec(spec);
  }
} options;

class NAV : public NAVData, public Loggable {
  public:
    struct back_propagated_item_t {
      float_sylph_t deltaT_from_last_correct;
      NAVData &nav;
      back_propagated_item_t(
          const float_sylph_t &_deltaT,
          NAVData &_nav) : deltaT_from_last_correct(_deltaT), nav(_nav) {}
      ~back_propagated_item_t(){}
      back_propagated_item_t &operator=(const back_propagated_item_t &another){
        deltaT_from_last_correct = another.deltaT_from_last_correct;
        nav = another.nav;
        return *this;
      }
    };
    typedef std::vector<
        back_propagated_item_t> back_propagated_list_t;
    virtual ~NAV(){}
  public:
    virtual back_propagated_list_t back_propagated() = 0;
    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) = 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){
      return correct(gps);
    }
    virtual NAV &correct_yaw(const float_sylph_t &delta_yaw){
      return *this;
    }

    /**
     * 地磁気からヨー角補正量を求める
     *
     * @param attitude 姿勢
     * @return ヨー角補正量[rad]
     */
    static float_sylph_t get_mag_delta_yaw(
        const Vector3<float_sylph_t> &mag,
        const Quarternion<float_sylph_t> &attitude,
        const float_sylph_t &latitude, const float_sylph_t &longitude, const float_sylph_t &altitude){

      typedef Vector3<float_sylph_t> vec_t;
      typedef Quarternion<float_sylph_t> quat_t;

      // 姿勢の影響をキャンセルする(逆回転)
      vec_t mag_horizontal((attitude * quat_t(0, mag) * attitude.conj()).vector());

      // 磁気モデルを呼び出す
      MagneticField::filed_components_res_t mag_model(
          MagneticField::filed_components(IGRF11::IGRF2010,
              latitude, longitude, altitude));
      vec_t mag_filed(mag_model.north, mag_model.east, mag_model.down);

      // 地磁気モデルを考慮してヨー角補正量を求める
      return std::atan2(mag_filed[1], mag_filed[0])
          - std::atan2(mag_horizontal[1], mag_horizontal[0]);
    }

    float_sylph_t get_mag_delta_yaw(
        const Vector3<float_sylph_t> &mag,
        const Quarternion<float_sylph_t> &attitude){

      return get_mag_delta_yaw(mag, attitude, latitude(), longitude(), height());
    }

    float_sylph_t get_mag_delta_yaw(
        const Vector3<float_sylph_t> &mag){

      return get_mag_delta_yaw(mag,
          INS<float_sylph_t>::euler2q(euler_psi(), euler_theta(), euler_phi()),
          latitude(), longitude(), height());
    }

    /**
     * 地磁気からヨー角を得る
     *
     */
    static float_sylph_t get_mag_yaw(
        const Vector3<float_sylph_t> &mag,
        const float_sylph_t &pitch, const float_sylph_t &roll,
        const float_sylph_t &latitude, const float_sylph_t &longitude, const float_sylph_t &altitude){

      return get_mag_delta_yaw(
          mag,
          INS<float_sylph_t>::euler2q(0, pitch, roll),
          latitude, longitude, altitude);
    }

    float_sylph_t get_mag_yaw(const Vector3<float_sylph_t> &mag){

      return get_mag_yaw(
          mag,
          euler_theta(), euler_phi(),
          latitude(), longitude(), height());
    }
};

template <class INS_GPS>
class INS_GPS_NAV : public NAV {
  protected:
    class INS_GPS_back_propagate;
    struct snapshot_content_t {
      INS_GPS_back_propagate *ins_gps;
      Matrix<float_sylph_t> Phi;
      Matrix<float_sylph_t> GQGt;
      float_sylph_t deltaT_from_last_correct;
      snapshot_content_t(
          INS_GPS_back_propagate *_ins_gps,
          const Matrix<float_sylph_t> &_Phi,
          const Matrix<float_sylph_t> &_GQGt, 
          const float_sylph_t &_deltaT)
          : ins_gps(_ins_gps), Phi(_Phi), GQGt(_GQGt), 
          deltaT_from_last_correct(_deltaT){
      }
      snapshot_content_t &operator=(const snapshot_content_t &another){
        ins_gps = another.ins_gps;
        Phi = another.Phi;
        GQGt = another.GQGt;
        deltaT_from_last_correct = another.deltaT_from_last_correct;
        return *this;
      }
    };
    typedef std::vector<snapshot_content_t> snapshots_t;
    snapshots_t snapshots;
    class INS_GPS_back_propagate : public INS_GPS, public NAVData {
      protected:
        snapshots_t &snapshots;
      public:
        INS_GPS_back_propagate(snapshots_t &_snapshots) 
            : INS_GPS(), snapshots(_snapshots) {}
        INS_GPS_back_propagate(
            const INS_GPS_back_propagate &orig, 
            const bool deepcopy = false)
            : INS_GPS(orig, deepcopy), snapshots(orig.snapshots){}
        INS_GPS_back_propagate &operator=(const INS_GPS_back_propagate &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);
          
          float_sylph_t deltaT_from_last_correct(deltaT);
          if(!snapshots.empty()){
            deltaT_from_last_correct += snapshots.back().deltaT_from_last_correct;
          }
          
          snapshots.push_back(
              snapshot_content_t(new INS_GPS_back_propagate(*this, true), 
                  Phi, Gamma * INS_GPS::getFilter().getQ() * Gamma.transpose(),
                  deltaT_from_last_correct));
        }
    
        /**
         * 修正におけるコールバック関数。
         * 
         * @param H H行列
         * @param R R行列
         * @patam K K行列、カルマンゲイン
         * @param v =(z - H x)
         * @param x_hat 修正量
         */
        void before_correct_INS(
            const Matrix<float_sylph_t> &H,
            const Matrix<float_sylph_t> &R,
            const Matrix<float_sylph_t> &K,
            const Matrix<float_sylph_t> &v,
            Matrix<float_sylph_t> &x_hat){
          if(!snapshots.empty()){
            
            // 時刻の修正と、どこまで後修正を行うかは
            // correct中この関数が初めて呼び出されたときに行う
            // そして、以下の部分がcorrect中一回しか実行されないようにするためのif文
            float_sylph_t mod_deltaT(snapshots.back().deltaT_from_last_correct);
            if(mod_deltaT > 0){
              
              // 最新側から見ていって、ある程度以上の時間が離れたcorrectが見つかった場合、
              // それよりも昔のものは削除して後から修正がかからないようにする
              for(typename snapshots_t::reverse_iterator it(snapshots.rbegin());
                  it != snapshots.rend();
                  ++it){
                //cerr << it->deltaT_from_last_correct << " => ";
                // ここでどこまで修正するかコントロールしている
                if(it->deltaT_from_last_correct < options.back_propagate_depth){
                  if(mod_deltaT > 0.1){ // スナップショットが溜まっていない場合は削除しない
                    for(typename snapshots_t::reverse_iterator it2(it);
                        it2 != snapshots.rend();
                        ++it2){
                      delete it2->ins_gps;
                    }
                    snapshots.erase(snapshots.begin(), it.base());
                    //cerr << "[erase]" << endl;
                    if(snapshots.empty()){return;}
                  }
                  break;
                }
                // 正の値のものはまだ一回も修正を受けたことが無いもの
                // 一方、負の値のものは一度修正を受けたもの
                it->deltaT_from_last_correct -= mod_deltaT;
                //cerr << it->deltaT_from_last_correct << "," << mod_deltaT << endl;
              }
            }
            
            snapshot_content_t previous(snapshots.back());
            snapshots.pop_back();
            
            // あとから得られた情報を使って以前の内容を修正
            Matrix<float_sylph_t> H_dash(H * previous.Phi);
            Matrix<float_sylph_t> R_dash(R + H * previous.GQGt * H.transpose());
            //cerr << R_dash.trace() << endl;
            //double before(previous.ins_gps->getFilter().getP().trace());
            previous.ins_gps->correct(H_dash, v, R_dash);
            //cerr << previous.deltaT_from_last_correct << " : "
            //    << before  << " => " << previous.ins_gps.getFilter().getP().trace() << endl;
            
            snapshots.push_back(previous);
          }
        }
    }; 
    INS_GPS *_nav, &nav;
  public:
    INS_GPS_NAV() 
        : NAV(), snapshots(), 
        _nav(options.back_propagate ? new INS_GPS_back_propagate(snapshots) : new INS_GPS()),
        nav(*_nav) {}
    ~INS_GPS_NAV() {
      delete _nav;
    }
    back_propagated_list_t back_propagated() {
      back_propagated_list_t res;
      if(options.back_propagate){
        for(typename snapshots_t::iterator it(snapshots.begin());
            it != snapshots.end();
            ++it){
          res.push_back(
              NAV::back_propagated_item_t(it->deltaT_from_last_correct, *(it->ins_gps)));
        }
        //cerr << "snapshots.size() : " << snapshots.size() << endl;
      }
      return res;
    }
    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){
      nav.update(accel, gyro, deltaT);
      return *this;
    }
  
  public:
    NAV &correct(const GPS_UBLOX_3D<float_sylph_t> &gps){
      nav.correct(gps);
      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){
      nav.correct(gps, lever_arm_b, omega_b2i_4b);
      return *this;
    }
    NAV &correct_yaw(const float_sylph_t &delta_yaw){
      nav.correct_yaw(delta_yaw, pow(deg2rad(options.mag_heading_accuracy_deg), 2));
      return *this;
    }
    
    /**
     * 状態に対応するラベルを出力する
     * 
     */
    void label(std::ostream &out = std::cout) const {
      NAV::label(out);
      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);
      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-6; //1E-10;
        Q(8, 8) = 1E-6; //1E-10;
        Q(9, 9) = 1E-6; //1E-10;
        Q(10, 10) = 1E-8; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16; //sim6と同じ設定
        Q(11, 11) = 1E-8; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
        Q(12, 12) = 1E-8; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
        super_t::nav.getFilter().setQ(Q);
      }
    }
    ~INS_GPS_BE_NAV(){}
    
    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){
      return super_t::correct(
          gps, lever_arm_b, omega_b2i_4b - super_t::nav.bias_gyro());
    }
    
    /**
     * 状態を出力する関数のためのラベル
     * 
     */
    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;

void a_packet_handler(const A_Observer_t &);
void g_packet_handler(const G_Observer_t &);
void m_packet_handler(const M_Observer_t &);

bool require_switch_processor(false);

class StreamProcessor{
  protected:
    Processor_t processor;       // ストリーム処理機
    int invoked;
    istream *in_renewed;
    istream &_in;
    
  public:
    LeverArm *lever_arm;
    Calibrator *calibrator;
    deque<A_Packet> a_packet_deque;
    G_Packet g_packet;
    bool g_packet_updated;
    int g_packet_wn;
    struct M_Packet {
      float_sylph_t itow;
      Vector3<float_sylph_t> mag;
    };
    deque<M_Packet> m_packet_deque;
    StreamProcessor(istream &in, bool require_all_handler)
        : processor(),
        in_renewed(options.in_sylphide ? new SylphideIStream(in, PAGE_SIZE) : NULL),
        _in(in_renewed ? (*in_renewed) : in), invoked(0),
        lever_arm(NULL), calibrator(NULL),
        a_packet_deque(),
        g_packet(), g_packet_updated(false), g_packet_wn(0),
        m_packet_deque() {
      if(require_all_handler){
        processor.set_a_handler(a_packet_handler);  // Aページの際の処理を登録
        if(options.use_magnet){
          processor.set_m_handler(m_packet_handler);  // Mページの際の処理を登録
        }
      }
      processor.set_g_handler(g_packet_handler);  // Gページの際の処理を登録
    }
    ~StreamProcessor(){
      delete in_renewed;
    }
    
    /**
     * ファイル等のストリームから1ページ単位で処理を行う関数
     * 
     * @param in ストリーム
     * @return (bool) 処理が成功したかどうか
     */
    bool process_1page(){
      char buffer[PAGE_SIZE];
      
      int read_count;
      _in.read(buffer, PAGE_SIZE);
      read_count = _in.gcount();
      if(_in.fail() || (read_count == 0)){return false;}
      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
      }
      
      processor.process(buffer, read_count);
      return true;
    }

    Vector3<float_sylph_t> get_mag() {
      return m_packet_deque.empty()
          ? Vector3<float_sylph_t>(1, 0, 0) // heading is north
          : m_packet_deque.back().mag;
    }

    Vector3<float_sylph_t> get_mag(const float_sylph_t &itow){
      if(m_packet_deque.size() < 2){
        return Vector3<float_sylph_t>(1, 0, 0); // heading is north
      }
      deque<M_Packet>::iterator
          previous_it(m_packet_deque.begin()),
          next_it(m_packet_deque.begin() + 1);
      for(int i(distance(previous_it, m_packet_deque.end()));
          i > 2;
          i--, previous_it++, next_it++){
        if(next_it->itow >= itow){break;}
      }
      float_sylph_t
          weight_previous((next_it->itow - itow) / (next_it->itow - previous_it->itow)),
          weight_next(1. - weight_previous);
      // 多大な外挿の禁止
      // Mページは複数タイミングを集約したものであるため、リアルタイムに比べて遅れることも。
      // その場合、外挿が必要となる。
      if(weight_previous > 3){ // 2ステップより前に外挿禁止
        weight_previous = 1;
        weight_next = 0;
      }else if(weight_next > 3){ // 2ステップより後に外挿禁止
        weight_next = 1;
        weight_previous = 0;
      }
      return (previous_it->mag * weight_previous) + (next_it->mag * weight_next);
    }
} *current_processor;

typedef vector<StreamProcessor *> processor_storage_t;
processor_storage_t processor_storage;

/**
 * 現在の状態を表現するクラス
 * 
 */
class Status{
  private:
    bool initalized;
    NAV &nav;
    Vector3<float_sylph_t> on_bias_accel, on_bias_gyro;
    int min_a_packets_for_init; // 1以上であること
    deque<A_Packet> recent_a_packets;
    unsigned int max_recent_a_packets;
    
    // 調査用パーツ
    Logger debugger;

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

  public:
    Status(NAV &_nav) : initalized(false), nav(_nav), gyro_index(0), gyro_init(false),
        on_bias_accel(0, 0, 0), on_bias_gyro(0, 0, 0),
        min_a_packets_for_init(options.has_initial_attitude ? 1 : 0x10),
        recent_a_packets(), max_recent_a_packets(max(min_a_packets_for_init, 0x100)),
        debugger(options.spec2ostream(options.debug_fname)) {
    
      debugger.out() << setprecision(16);
      debugger.add(nav);
    }
  
  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, 
    };
  
  protected:
    /**
     * 現在の状態を記録する関数
     * 
     * @param label 動作モード
     * @param itow 現在時刻
     * @param target 出力するNAV
     */
    static void dump(const char *label, const float_sylph_t &itow, const NAVData &target){
      
      if(!options.out_is_N_packet){
        options.out() << label << "\t"
            << itow << "\t"
            << target << endl;
      }else{
        char buf[PAGE_SIZE];
        target.encode_N0(itow, buf);
        options.out().write(buf, sizeof(buf));
      }
    }

  public:
    /**
     * 現在の状態を記録する関数
     *
     * @param label 動作モード
     * @param itow 現在時刻
     * @param target 出力するNAV
     */
    void dump(const dump_mode_t mode, const float_sylph_t &itow){

      if(!initalized){return;}
      
      switch(mode){
        case DUMP_UPDATE:
          if(!options.dump_update){return;}
          if(options.back_propagate){return;} // 後から修正が加わる場合、表示しない
          dump("TU", itow, nav);
          break;
        case DUMP_CORRECT:
          // 後から修正が加わった部分を処理
          if(options.back_propagate){
            NAV::back_propagated_list_t list(nav.back_propagated());
            int index(0);
            for(NAV::back_propagated_list_t::iterator it(list.begin());
                it != list.end();
                ++it, index++){
              if(it->deltaT_from_last_correct >= options.back_propagate_depth){
                break;
              }
              const char *label;
              if(index == 0){
                if(!options.dump_correct){continue;}
                label = "BP_MU";
              }else{
                if(!options.dump_update){continue;}
                label = "BP_TU";
              }
              dump(label, itow + it->deltaT_from_last_correct, it->nav);
            }
            break;
          }
          
          if(!options.dump_correct){return;}
          dump("MU", itow, nav);
          break;
        default:
          return;
      }
      
      debugger.out() << itow;
      debugger.flush();
      debugger.out() << endl;
    }
    
    /**
     * 時間更新を行う関数
     * 
     * 一定時間間隔で行われるAD変換結果より得られる
     * 加速度(加速度計に依存)、角速度(ジャイロに依存)を利用する。
     * 
     * @param a_packet AD変換結果
     */
    void time_update(const A_Packet &a_packet){
      Vector3<float_sylph_t> accel(a_packet.accel()), gyro(a_packet.gyro());
      
      if(initalized){
        const A_Packet &previous(recent_a_packets.back());

        // for LAE
        {
          gyro_storage[gyro_index++] = gyro - on_bias_gyro;
          if(gyro_index == (sizeof(gyro_storage) / sizeof(gyro_storage[0]))){
            gyro_index = 0;
            if(!gyro_init) gyro_init = true;
          }
        }

#if DEBUG
        cerr << "TU : " << a_packet.itow << endl;
#endif

        //前回時刻よりの差分を計算
        float_sylph_t interval(previous.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(
            accel - on_bias_accel,
            gyro - on_bias_gyro,
            interval);
      }

      recent_a_packets.push_back(a_packet);
      if(recent_a_packets.size() > max_recent_a_packets){
        recent_a_packets.pop_front();
      }
    }

    /**
     * 観測更新を行う関数
     * 
     * GPS受信機より観測量(位置、速度)が得られたときに行う
     * 
     * @param g_packet GPS受信機より得られる観測量
     */
    void measurement_update(const G_Packet &g_packet){
      
      if(g_packet.acc_2d >= 100.){return;} // 誤差が大きい場合は捨てる
      if(initalized){
        cerr << "MU : " << setprecision(10) << g_packet.itow << endl;
        
        if(gyro_init && (current_processor->lever_arm)){
          // LAE on
          Vector3<float_sylph_t> omega_b2i_4n;
          for(int i(0); i < (sizeof(gyro_storage) / sizeof(gyro_storage[0])); i++){
            omega_b2i_4n += gyro_storage[i];
          }
          omega_b2i_4n /= (sizeof(gyro_storage) / sizeof(gyro_storage[0]));
          nav.correct(
              g_packet.convert(), 
              current_processor->lever_arm->lever_arm(),
              omega_b2i_4n);
        }else{
          // LEA off
          nav.correct(g_packet.convert());
        }
        if(!current_processor->m_packet_deque.empty()){ // 地磁気が有効の場合(先頭のログに限る)、ヨー補正をする
          if((options.yaw_correct_with_mag_when_speed_less_than_ms > 0)
              && (pow(g_packet.vel_ned[0], 2) + pow(g_packet.vel_ned[1], 2)) < pow(options.yaw_correct_with_mag_when_speed_less_than_ms, 2)){
            nav.correct_yaw(nav.get_mag_delta_yaw(current_processor->get_mag(g_packet.itow)));
          }
        }
      }else if((current_processor == processor_storage.front()) // 必ず先頭のログで初期化を行う
          && (recent_a_packets.size() >= min_a_packets_for_init)
          && (std::abs(recent_a_packets.front().itow - g_packet.itow) < (0.1 * recent_a_packets.size())) // 時刻同期チェック
          && (g_packet.acc_2d <= 20.) && (g_packet.acc_v <= 10.)){
        /*
         * GPS受信機の出力する推定位置誤差が20m(2D), 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]);
        float_sylph_t latitude(deg2rad(g_packet.llh[0])), longitude(deg2rad(g_packet.llh[1]));
        
        while(!options.has_initial_attitude){
          // 加速度計と地磁気を使って初期姿勢を推定する(静止状態を仮定)
          
          typedef Vector3<float_sylph_t> vec_t;
          
          // まずは正規化
          vec_t acc(0, 0, 0);
          for(deque<A_Packet>::iterator it(recent_a_packets.begin());
              it != recent_a_packets.end();
              ++it){
            acc += it->accel();
          }
          acc /= recent_a_packets.size();
          vec_t acc_reg(-acc / acc.abs());
          
          // ピッチ角を求める, x方向は最後のロール回転で影響をうけないことを利用
          pitch = -asin(acc_reg[0]);
          // 続いてロール角を求める、xとz成分の比から
          roll = atan2(acc_reg[1], acc_reg[2]);
          
          // 地磁気データをインポートしてくる
          if(!current_processor->m_packet_deque.empty()){ // データの無効判定
            yaw = nav.get_mag_yaw(current_processor->get_mag(g_packet.itow), pitch, roll, latitude, longitude, g_packet.llh[2]);
          }
          
          break;
        }
        
        cerr << "Init : " << setprecision(10) << g_packet.itow << endl;
        initalized = true;
        nav.init(
            latitude, longitude, g_packet.llh[2],
            g_packet.vel_ned[0], g_packet.vel_ned[1], g_packet.vel_ned[2],
            yaw, pitch, roll);
        cerr << "Initial attitude (yaw, pitch, roll) [deg]: "
            << rad2deg(yaw) << ", "
            << rad2deg(pitch) << ", "
            << rad2deg(roll) << endl;
        
        // ON時バイアスの推定
        /*{
          on_bias_accel /= before_init_counter;
          
#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;
#endif

          // (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;
          
          on_bias_gyro /= before_init_counter;
        }*/
        
        dump_label();
      }
    }
    
    bool is_initalized(){return initalized;}
};

/**
 * Aページ(AD変換結果)の処理用関数
 * 
 * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Aページのオブザーバー
 */
void a_packet_handler(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;
  
  deque<A_Packet> &a_packet_deque(
        current_processor->a_packet_deque);

  while(a_packet_deque.size() >= 128){
    a_packet_deque.pop_front();
  }
  a_packet_deque.push_back(packet);
}

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * 
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x01, 0x02}のとき位置情報が得られる
 * {class, id} = {0x01, 0x12}のとき速度情報が得られる
 * 
 * @param obsrever Gページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(!observer.validate()){return;}
  
  G_Packet &packet(current_processor->g_packet);
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  switch(packet_type.mclass){
    case 0x01: {
      switch(packet_type.mid){
        case 0x02: { // NAV-POSLLH
          G_Observer_t::position_t 
            position(observer.fetch_position());
          G_Observer_t::position_acc_t
            position_acc(observer.fetch_position_acc());
          
          //cerr.precision(10);
          //cerr << "G_Arrive 0x02 : " << observer.fetch_ITOW() << endl;
          
          packet.itow = observer.fetch_ITOW();
          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 0x06: { // NAV-SOL
          G_Observer_t::solution_t solution(observer.fetch_solution());
          if(solution.status_flags & G_Observer_t::solution_t::WN_VALID){
            current_processor->g_packet_wn = solution.week;
          }
          break;
        }
        case 0x12: { // NAV-VELNED
          G_Observer_t::velocity_t
            velocity(observer.fetch_velocity());
          G_Observer_t::velocity_acc_t
            velocity_acc(observer.fetch_velocity_acc());
          
          //cerr.precision(10);
          //cerr << "G_Arrive 0x12 : " << observer.fetch_ITOW() << endl;
          
          if(packet.itow == observer.fetch_ITOW()){
            packet.vel_ned[0] = velocity.north;
            packet.vel_ned[1] = velocity.east;
            packet.vel_ned[2] = velocity.down;
            packet.acc_vel = velocity_acc.acc;
            
            current_processor->g_packet_updated = true;
            
            require_switch_processor = true;
          }
            
          break;
        }
      }
      break;
    }
    case 0x02: { 
      break;
    }
    default:
      break;
  }
}

void m_packet_handler(const M_Observer_t &observer){
  if(!observer.validate()){return;}
  
  M_Observer_t::values_t values(observer.fetch_values());
        
  // 値の妥当性チェック(異常値があった場合は採用しない)
  short *targets[] = {values.x, values.y, values.z};
  static const int threshold(200);
  for(int j(0); j < sizeof(targets) / sizeof(targets[0]); j++){
    for(int i(0); i < 3; i++){
      int diff_abs(abs(targets[j][i] - targets[j][3]));
      if((diff_abs > threshold) && (diff_abs < (4096 * 2 - threshold))){
        return; // データを廃棄
      }
    }
  }
  
  // 座標変換をしつつ、磁気を確定
  while(current_processor->m_packet_deque.size() > 0x40){
    current_processor->m_packet_deque.pop_front();
  }

  // 磁気センサの軸は加速度やジャイロと同じであること
  Vector3<float_sylph_t> mag(values.x[3], values.y[3], values.z[3]);
  if(options.rotated){
    mag = static_cast<RotatedCalibrator *>(A_Packet::calibrator)->rotate(mag);
  }
  StreamProcessor::M_Packet m_packet = {observer.fetch_ITOW(), mag};
  current_processor->m_packet_deque.push_back(m_packet);
}

void loop(){
  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));
  
  while(true){
    for(processor_storage_t::iterator it(processor_storage.begin());
        it != processor_storage.end();
        ++it){
      if((*it)->g_packet_updated){continue;}
      current_processor = *it;
      require_switch_processor = false;
      while(!require_switch_processor){
        if(!current_processor->process_1page()){
          if(it == processor_storage.begin()){
            return;
          }else{
            break;
          }
        }
      }
    }
    
    // 取得したg_packetの時刻チェック、及び並び替え
    typedef vector<StreamProcessor *> mu_queue_t;
    mu_queue_t mu_queue;
    
    mu_queue.push_back(*processor_storage.begin());
    bool require_search_again(false);
    for(processor_storage_t::iterator it(processor_storage.begin() + 1);
        it != processor_storage.end();
        ++it){
      if(!((*it)->g_packet_updated)){continue;}
      float_sylph_t itow((*it)->g_packet.itow);
      float_sylph_t interval(
          itow - processor_storage.front()->g_packet.itow);
      
      if(_abs(interval) <= 0.2){  // 時刻が近ければ処理対象に、時刻順に並べ替え
        mu_queue_t::iterator it2(mu_queue.begin());
        while(it2 != mu_queue.end()){
          if((*it2)->g_packet.itow > itow){break;}
          ++it2;
        }
        mu_queue.insert(it2, *it);
      }else if(interval < 0){     // 昔のデータであれば破棄して再検索
        (*it)->g_packet_updated = false;
        require_search_again = true;
        break;
      }
    }
    
    if(require_search_again){continue;}
    
    float_sylph_t latest_measurement_update_itow(0);
    int latest_measurement_update_gpswn(0);
    for(mu_queue_t::iterator it_mu(mu_queue.begin());
        it_mu != mu_queue.end();
        ++it_mu){
      
      current_processor = *it_mu;
      G_Packet g_packet(current_processor->g_packet);
      current_processor->g_packet_updated = false;

      if((options.start_gpswn > current_processor->g_packet_wn) // 週番号チェック
          || (options.start_gpstime > g_packet.itow)){ // 時刻チェック
        continue;
      }
      
      deque<A_Packet> &a_packet_deque(
          processor_storage.front()->a_packet_deque);
      deque<A_Packet>::iterator it_tu(a_packet_deque.begin()), it_tu_end(a_packet_deque.end());
      const bool a_packet_deque_has_item(it_tu != it_tu_end);

      if(options.gps_fake_lock){
        if(a_packet_deque_has_item){
          g_packet.itow = (it_tu_end - 1)->itow;
        }
        g_packet.llh[0] = g_packet.llh[1] = g_packet.llh[2] = 0;
        g_packet.acc_2d = g_packet.acc_v = 1E+1;
        g_packet.vel_ned[0] = g_packet.vel_ned[1] = g_packet.vel_ned[2] = 0;
        g_packet.acc_vel = 1;
      }
    
      // Time UpdateをGPS観測時刻前までする
      for(; 
          (it_tu != it_tu_end) && (it_tu->itow < g_packet.itow);
          ++it_tu){
        status.time_update(*it_tu);
        status.dump(Status::DUMP_UPDATE, it_tu->itow);
      }
      
      // GPS観測時刻までの補完
      if(a_packet_deque_has_item){
        A_Packet interpolation((it_tu != it_tu_end) ? *it_tu : *(it_tu - 1));
        interpolation.itow = g_packet.itow;
        status.time_update(interpolation);
      }
      
      a_packet_deque.erase(a_packet_deque.begin(), it_tu);
      
      // GPS観測量を用いてMeasurement Updateをする
      status.measurement_update(g_packet);
      latest_measurement_update_itow = g_packet.itow;
      latest_measurement_update_gpswn = current_processor->g_packet_wn;
    }
    
    status.dump(Status::DUMP_CORRECT, latest_measurement_update_itow);
    
    if((latest_measurement_update_itow >= options.end_gpstime)
        && (latest_measurement_update_gpswn >= options.end_gpswn)){
      break;
    }
  }
}

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

  cerr << "Navigation Post-processor for Sylphide log." << endl;
  cerr << "Usage: (exe) IMU_mode_for_log0 [options_for_global/log0] log0.dat "
      "[options_for_global/log1 log1.dat ...]" << endl;
  if(argc < 3){
    // はじめのlog0のみがM,A,Gを供給、残りはGのみ供給
    cerr << "Error: too few arguments; " << argc << " < min(3)" << endl;
    return -1;
  }
  
  Calibrator *calibrator(NULL);
  bool rotated(false);
  
  cerr << "IMU Mode checking..." << endl;
  for(int i(0); i < (sizeof(calibrators) / sizeof(calibrators[0])); i++){
    cerr << calibrators[i].name << " : ";
    if(!strcmp(argv[1], calibrators[i].name)){
      cerr << "Ready !!" << endl;
      calibrator = calibrators[i].calibrator;
      break;
    }else{
      cerr << "NG" << endl;
    }
    if(i == ((sizeof(calibrators) / sizeof(CalibratorItem)) - 1)){
      cerr << "Target mode not found" << endl;
      return -1;
    } 
  }
  
  // option check...
  cerr << "Option checking..." << endl;
  
  for(int arg_index(2); arg_index < argc; arg_index++){
    
    const char *value;

    // ex) --rotate=-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;
          processor_storage.back()->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;
      }
      calibrator = new RotatedCalibrator(*calibrator, decoded);
      rotated = true;
      continue;
    }
    if(value = Options::get_value(argv[arg_index], "calib_file", false)){
      fstream fin(value);
      if(fin.fail()){continue;}
      cerr << "calib_file: " << value << endl;
      if(strcmp(argv[1], CustomStandardCalibrator::id) != 0){continue;}
      CustomStandardCalibrator *target(
          static_cast<CustomStandardCalibrator *>(calibrator));
      char buf[1024];
      while(!fin.eof()){
        fin.getline(buf, sizeof(buf));
        target->custom(buf);
      }
      calibrator = target;
      continue;
    }
    if(options.check_spec(argv[arg_index])){continue;}
    
    bool is_first_processor(processor_storage.empty());
    
    // オプションでなければログファイルが指定されているはず
    cerr << "Log file(" << processor_storage.size() << ") : ";
    processor_storage.push_back(
        new StreamProcessor(options.spec2istream(argv[arg_index]), is_first_processor));
  }
  
  // TODO: calibrator/rotatedをクラス変数で管理するのはやめたい
  A_Packet::calibrator = calibrator;
  options.rotated = rotated;
  
  if(options.out_sylphide){
    options._out = new SylphideOStream(options.out(), PAGE_SIZE);
  }else{
    options.out() << setprecision(10);
  }

  loop();
  
  for(processor_storage_t::iterator it(processor_storage.begin());
      it != processor_storage.end();
      ++it){delete *it;}
  
  return 0;
}