#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <exception>
#include <map>
#include <vector>
#include <utility>

#include <cstdio>

//#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#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;

#define BUFFER_SIZE (PAGE_SIZE * 1) // 32
#define OBSERVER_SIZE (PAGE_SIZE * 32) // 1024

#include "param/matrix.h"
#include "param/vector3.h"
#include "param/quarternion.h"
#include "param/complex.h"

#include "util/util.h"
#include "util/fifo.h"

#include "INS_GPS2_Tight.h"
#include "RINEX.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 "inspector.h"
#include "analyze_common.h"

//#define BIAS_ON

#ifdef BIAS_ON
typedef INS_GPS2_BiasEstimated<float_sylph_t> NAV;
#else
typedef INS_GPS2_Tight<float_sylph_t> NAV;
#endif

using namespace std;

struct Options : public GlobalOptions<float_sylph_t> {
  typedef GlobalOptions<float_sylph_t> super_t;
  Options() : super_t() {};
} options;

/**
 * 現在の状態を表現するクラス
 * 
 */
class Status{
  private:
    bool initalized;
    NAV nav;
    Vector3<float_sylph_t> on_bias_accel, on_bias_gyro;
    int before_init_counter;
    
    // GPS関係
  public:
    typedef NAV::space_node_t space_node_t;
    typedef space_node_t::gps_time_t gps_time_t;
    typedef NAV::sat_range_t sat_range_t;
    typedef map<int, vector<space_node_t::Satellite::Ephemeris> > nav_data_t;
    
    
    // 調査用パーツ
  private:
    Logger logger;
    FilterP_Observer<NAV> p_observer;

  // Lever Arm Effect用の値
  protected:
#define GYRO_STORAGE 8
    Vector3<float_sylph_t> gyro_storage[GYRO_STORAGE];
    int gyro_index;
    bool gyro_init;
    
    bool _ephemeris_lock;
    space_node_t _space_node;
    nav_data_t nav_data;

  public:
    static LeverArm *lever_arm;
    Status() : initalized(false), nav(), gyro_index(0), gyro_init(false),
        on_bias_accel(), on_bias_gyro(),
        before_init_counter(0),
        logger("p_log.csv"), p_observer(nav) {
    
      logger.out() << setprecision(16);
      logger.add(p_observer);
    }
    
    bool lock_ephemeris(bool flag){
      return _ephemeris_lock = flag;
    }
    space_node_t &space_node(){return _space_node;}
  
  protected:  
    void filter_init() {
#ifdef BIAS_ON
      nav.beta_accel() *= 0.1;
      nav.beta_gyro() *= 0.1;//mems_g.BETA;
#endif

      /**
       * 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
#ifdef BIAS_ON
        P(10, 10) = P(11, 11) = P(12, 12) = 1E-2;
        P(13, 13) = P(14, 14) = P(15, 15) = 1E-6;
#endif
        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
#ifdef BIAS_ON
        Q(7, 7) = 1E-8; //1E-10;
        Q(8, 8) = 1E-8; //1E-10;
        Q(9, 9) = 1E-8; //1E-10;
        Q(10, 10) = 1E-10; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16; //sim6と同じ設定
        Q(11, 11) = 1E-10; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
        Q(12, 12) = 1E-10; //pow(mems_g.ROOT_N, 2) * 1. / 100 * 16;
#endif
        nav.getFilter().setQ(Q);
      }
    }
  
  public:
    /**
     * Filter自身を取り出す関数
     * 
     */
    NAV &get_nav() {return nav;}
    
    /**
     * 状態を出力する関数のためのラベル
     * 
     */
    static void dump_label(){
      cout << "itow" << "\t"
           << "longitude" << "\t"
           << "latitude" << "\t"
           << "height" << "\t" 
           << "v_north" << "\t"
           << "v_east" << "\t"
           << "v_down" << "\t"
           << "Yaw(Ψ)" << "\t"        //Ψ(yaw)
           << "Pitch(Θ)" << "\t"      //Θ(pitch)
           << "Roll(Φ)" << "\t"       //Φ(roll)
           << "Azimuth(α)" << "\t";   //α(azimuth)
      if(options.dump_q_n2b){
        cout << "q_n2b_0" << "\t"
             << "q_n2b_1" << "\t"
             << "q_n2b_2" << "\t"
             << "q_n2b_3" << "\t";
      }
#ifdef BIAS_ON
      cout << "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";
#endif
    }
    
    /**
     * 現在の状態を出力する関数
     * 
     * @param itow 現在時刻
     */
    void dump(const float_sylph_t &itow){
      cout << setprecision(10)
           << itow << "\t" 
           << rad2deg(nav.longitude()) << "\t"
           << rad2deg(nav.latitude()) << "\t"
           << nav.height() << "\t" 
           << nav.v_north() << "\t"
           << nav.v_east() << "\t"
           << nav.v_down() << "\t"
           << rad2deg(nav.heading()) << "\t"      //Ψ(yaw)   <- q_{g}^{b}
           << rad2deg(nav.euler_theta()) << "\t"  //Θ(pitch) <- q_{n}^{b}
           << rad2deg(nav.euler_phi()) << "\t"    //Φ(roll)  <- q_{n}^{b}
           << rad2deg(nav.azimuth()) << "\t";     //α(azimuth)
      if(options.dump_q_n2b){
        for(int i(8); i <= 11; i++){cout << nav[i] << "\t";}
      }
#ifdef BIAS_ON
      cout << nav.bias_accel().getX() << "\t"         // Bias
           << nav.bias_accel().getY() << "\t"
           << nav.bias_accel().getZ() << "\t"
           << nav.bias_gyro().getX() << "\t"
           << nav.bias_gyro().getY() << "\t"
           << nav.bias_gyro().getZ() << "\t";
#endif
    }
    
    /**
     * 時間更新を行う関数
     * 
     * 一定時間間隔で行われるAD変換結果より得られる
     * 加速度(加速度計に依存)、角速度(ジャイロに依存)を利用する。
     * 
     * @param a_packet AD変換結果
     */
    void time_update(const A_Packet &a_packet){
      static A_Packet previous;
      static bool has_previous(false);

#ifndef pow2      
#define pow2(x) ((x) * (x))
#endif
      if(!initalized){
        //on_bias_accel += a_packet.accel();
        //on_bias_gyro += a_packet.gyro();
        before_init_counter++;
        return;
      }
      
      // for LAE
      {
        gyro_storage[gyro_index++]
#ifdef BIAS_ON
            = a_packet.gyro() - nav.bias_gyro();
#else
            = a_packet.gyro();
#endif
        if(gyro_index == GYRO_STORAGE){
          gyro_index = 0;
          if(!gyro_init) gyro_init = true;
        }
      }
      
      if(has_previous){
#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(
            a_packet.accel() - on_bias_accel, 
            a_packet.gyro() - on_bias_gyro, 
            interval);

        if(options.dump_update){
          cout << "TU\t";
          dump(a_packet.itow);
          cout << endl;
          
          p_observer.time_stamp() = a_packet.itow;
          if(options.log_system_cov){logger.flush();}
        }
      }
      previous = a_packet;
      has_previous = true;
    }
    
    /**
     * 初期化を行う関数
     * 
     * GPS受信機より観測量(位置、速度)の精度が閾値以上になったとき初期化を完了する
     * 
     * @param g_packet GPS受信機より得られる観測量
     */
    void init(const G_Packet &g_packet){
      
      if(!initalized && (g_packet.acc_2d < 20.) && (options.start_gpstime <= g_packet.itow)){
        /*
         * GPS受信機の出力する推定位置誤差が20m以内になったときを
         * フィルター開始時点とする
         * 以後、Time UpdateとMeasurement Updateが有効になる
         */
        cerr << "Init : " << setprecision(10) << g_packet.itow << endl;
        initalized = true;
        nav.initPosition(deg2rad(g_packet.llh[0]), deg2rad(g_packet.llh[1]), g_packet.llh[2]);
        nav.initVelocity(g_packet.vel_ned[0], g_packet.vel_ned[1], g_packet.vel_ned[2]);
        nav.initAttitude(deg2rad(options.init_attitude_deg[0]), deg2rad(0), 0.);
        filter_init();
        
        cout << "mode\t";
        dump_label();
        cout << endl;
        
        cout << "Init\t";
        dump(g_packet.itow);
        cout << endl;
        
        p_observer.time_stamp() = g_packet.itow;
        logger.flush();
      }
    }
    
    /**
     * 観測更新を行う関数
     * 
     * GPS受信機より観測量(擬似距離)が得られたときに行う
     * 
     * @param g_packet GPS受信機より得られる観測量
     */
    void measurement_update(
        const sat_range_t &sat_range, 
        gps_time_t &t_epoc){
      
      if(initalized){
        cerr << "MU : " << setprecision(10) << t_epoc.seconds << endl;
        
        sat_range_t available_sat_range;
      
        for(sat_range_t::const_iterator it(sat_range.begin());
            it != sat_range.end();
            ++it){
          
          int prn(it->first);
          float_sylph_t pseudo_range(it->second);
          
          cerr << "PRN: " << prn << " => " << pseudo_range;
          
          bool available(false);
          bool search_ephemeris(true);
          if(_space_node.has_satellite(it->first)){
            available = true;
            if(!_space_node.satellite(prn).ephemeris().maybe_newer_one_avilable(t_epoc)){
              search_ephemeris = false;
            }
          }
          
          // エフェメリスの設定
          
          // 前にエフェメリスがない場合、
          // あるいは新しいものが利用可能である場合は設定する
          if(search_ephemeris){
            
            // エフェメリスがあるか
            if(nav_data.find(prn) != nav_data.end()){
            
              // エフェメリスを検索
              struct comparator {
                const gps_time_t &_t;
                comparator(const gps_time_t &t) : _t(t) {} 
                bool operator()(
                    const space_node_t::Satellite::Ephemeris &eph1,
                    const space_node_t::Satellite::Ephemeris &eph2){
                  return _abs(eph1.period_from_time_of_clock(_t)) 
                      < _abs(eph2.period_from_time_of_clock(_t));
                }
              };
              _space_node.satellite(prn).ephemeris()
                  = *min_element(nav_data[prn].begin(), nav_data[prn].end(), 
                      comparator(t_epoc));
              available = true;
            }
          }
          
          // エフェメリスがないものについては無効とする
          if(!available){
            cerr << endl;
            continue;
          }
          
          cerr << " @ Ephemeris: t_oc => "
               << _space_node.satellite(prn).ephemeris().WN << " week " 
               << _space_node.satellite(prn).ephemeris().t_oc
               << endl;
          
          // 擬似距離の再設定
          available_sat_range.push_back(sat_range_t::value_type(
              prn, pseudo_range));
        } 
        
        if(gyro_init){
          // 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(
              t_epoc, 
              _space_node,
              available_sat_range, 
              Status::lever_arm->lever_arm(),
              omega_b2i_4n
            );
        }else{
          // LEA off
          //nav.correct(g_packet.convert());
        }
      }else{return;}
      
      if(options.dump_correct){
        cout << "MU\t";
        dump(t_epoc.seconds);
        cout << endl;
        
        p_observer.time_stamp() = t_epoc.seconds;
        if(options.log_system_cov){logger.flush();}
      }
    }
    
    bool is_initalized(){return initalized;}
    
    void regist_epehemris(
        const space_node_t::Satellite::Ephemeris &eph){
              
      // 衛星順、時刻順に並べる
      if(nav_data[eph.svid].empty()){
        nav_data[eph.svid].push_back(eph);
      }else{
        for(nav_data_t::mapped_type::reverse_iterator it(nav_data[eph.svid].rbegin()); 
            it != nav_data[eph.svid].rend(); 
            ++it){
          if(gps_time_t(eph.WN, eph.t_oc) >= gps_time_t(it->WN, it->t_oc)){
            if(!_ephemeris_lock){
              if(eph.t_oc != it->t_oc){
                nav_data[eph.svid].insert(it.base(), eph);
              }else{
                (*it) = eph;
              }
            }
            break;
          }
        }
      }
      
      cerr << "NAV data : " << nav_data.size()
           << " ; " << eph.svid << " => " 
           << nav_data[eph.svid].size() << ", " << eph.WN << ", " << eph.t_oc
           << endl;
    }
    
    void regist_epehemris(
        const G_Observer_t::ephemeris_t &ephemeris,
        const int cycle_NO = 0){
      
      // Ephemeris情報の変換
      space_node_t::Satellite::Ephemeris eph;
      {
#define CONVERT(item1, item2) eph.item1 = ephemeris.item2;
    
        CONVERT (svid, sv_number);        // 衛星番号
        
        // Subframe.1
        CONVERT (WN,         wn);         // 週番号
        eph.WN += 1024 * cycle_NO;
        CONVERT (URA,        ura);        // 測距精度
        CONVERT (SV_health,  sv_health);  // 衛星健康状態
        CONVERT (iodc,       iodc);       // クロック情報番号
        CONVERT (t_GD,       t_gd);       // 郡遅延           (s)
        CONVERT (t_oc,       t_oc);       // エポック時刻
        CONVERT (a_f2,       a_f2);       // クロック補正係数 (s/s^2)
        CONVERT (a_f1,       a_f1);       // クロック補正係数 (s/s)
        CONVERT (a_f0,       a_f0);       // クロック補正係数 (s)
        
        // Subframe.2
        CONVERT (iode,       iode);       // 軌道情報番号
        CONVERT (c_rs,       c_rs);       // 軌道補正係数 (m)
        CONVERT (delta_n,    delta_n);    //              (rad/s) 
        CONVERT (m0,         m_0);        // 平均近点角   (rad)
        CONVERT (c_uc,       c_uc);       // 軌道補正係数 (rad)
        CONVERT (e,          e);          // 離心率       
        CONVERT (c_us,       c_us);       // 軌道補正係数 (rad)
        CONVERT (sqrt_A,     root_a);     // 軌道半径     (sqrt(m))
        CONVERT (t_oe,       t_oe);       // エポック時間 (s)
        
        // Subframe.3
        CONVERT (c_ic,       c_ic);       // 軌道補正係数 (rad)
        CONVERT (Omega0,     omega_0);    // 昇交点赤経   (rad)
        CONVERT (c_is,       c_is);       // 軌道補正係数 (rad)
        CONVERT (i0,         i_0);        // 軌道傾斜角   (rad)
        CONVERT (c_rc,       c_rc);       // 軌道補正係数 (m)
        CONVERT (omega,      omega);      // 近地点引数   (rad)
        CONVERT (dot_Omega0, omega_0_dot);// Omega0変化率 (rad/s)
        CONVERT (dot_i0,     i_0_dot);    // i0変化率     (rad/s)
        
        // フィット間隔 (ICD:20.3.4.4)
        if(ephemeris.fit == false){
          // normal operation
          eph.fit_interval = 4 * 60 * 60;
        }else{
          // short/long-trem extended operation
          if(eph.iodc >= 240 && eph.iodc <= 247){
            eph.fit_interval = 8 * 60 * 60;
          }else if((eph.iodc >= 248 && eph.iodc <= 255) || (eph.iodc == 496)){
            eph.fit_interval = 14 * 60 * 60;
          }else if(eph.iodc >= 497 && eph.iodc <= 503){
            eph.fit_interval = 26 * 60 * 60;
          }else if(eph.iodc >= 504 && eph.iodc <= 510){
            eph.fit_interval = 50 * 60 * 60;
          }else if((eph.iodc == 511) || (eph.iodc >= 752 && eph.iodc <= 756)){
            eph.fit_interval = 74 * 60 * 60;
          }else if(eph.iodc >= 757 && eph.iodc <= 763){
            eph.fit_interval = 98 * 60 * 60;
          }else if((eph.iodc >= 764 && eph.iodc <= 767) || (eph.iodc >= 1008 && eph.iodc <= 1010)){
            eph.fit_interval = 122 * 60 * 60;
          }else if(eph.iodc >= 1011 && eph.iodc <= 1020){
            eph.fit_interval = 146 * 60 * 60;
          }else{
            eph.fit_interval = 6 * 60 * 60;
          }
        }
#undef CONVERT
      }
      
      regist_epehemris(eph);
    }
} status;

LeverArm *(Status::lever_arm) = NULL;

FIFO<A_Packet, operator_eq_t> a_packet_fifo(128);

/**
 * 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;
  
  while(!a_packet_fifo.margin()){
    a_packet_fifo.skip(1);
  }
  a_packet_fifo.push(&packet);
}  

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * 
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x01, 0x02}のとき位置情報が得られる
 * {class, id} = {0x01, 0x12}のとき速度情報が得られる
 * 
 * @param obsrever Aページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(!observer.validate()){return;}
  
  static int cycle_NO(0);
  static float_sylph_t current_itow(0);
  static G_Packet packet;
  
  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.precision(10);
          //cerr << "G_Arrive 0x02 : " << observer.fetch_ITOW() << endl;
          
          current_itow = 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 0x12: {
          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(current_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;
            
            status.init(packet);
          }
          
          current_itow = 0;
            
          break;
        }
      }
      break;
    }
    case 0x02: { 
      switch(packet_type.mid){
        case 0x10: {
          Status::sat_range_t sat_range;
          Status::gps_time_t t_epoc(observer.fetch_WN(), observer.fetch_ITOW());
          cycle_NO = t_epoc.week / 1024;
          unsigned int num_of_sv((unsigned char)observer[6 + 6]);
          cerr << "GPS_Time : " << t_epoc << endl;
          //cerr << "Number of satellites : " << num_of_sv << endl;
          for(int i = 0; i < num_of_sv; i++){
            G_Observer_t::raw_measurement_t raw_data(observer.fetch_raw(i));
            
            // 擬似距離の設定
            sat_range.push_back(Status::sat_range_t::value_type(
                raw_data.sv_number, raw_data.pseudo_range));
          }
          
          // Time UpdateをGPS観測時刻前までする
          unsigned stored(a_packet_fifo.stored());
          int i = 0;
          for(; 
              (i < stored) && (a_packet_fifo[i].itow < t_epoc.seconds);
              i++){
            status.time_update(a_packet_fifo[i]);
          }
          
          // GPS観測時刻までの補完
          if(stored){
            A_Packet interpolation(a_packet_fifo[(stored > i) ? i : (i - 1)]);
            interpolation.itow = t_epoc.seconds;
            status.time_update(interpolation);
          }
          
          a_packet_fifo.skip(i);
          
          // GPS観測量を用いてMeasurement Updateをする
          status.measurement_update(sat_range, t_epoc);
          
          break;
        }
        case 0x31: {
          G_Observer_t::ephemeris_t ephemeris(observer.fetch_ephemeris());
          if(!ephemeris.valid){break;}
          
          status.regist_epehemris(ephemeris, cycle_NO);
        }
      }
      break;
    }
    default:
      break;
  }
}

/**
 * ファイル等のストリームからページ単位で切り出す関数
 * 
 * @param in ストリーム
 */
void stream_processor(istream &in){
  char buffer[PAGE_SIZE];
  
  Processor_t processor(OBSERVER_SIZE);       // ストリーム処理機を生成
  processor.set_a_handler(a_packet_handler);  // Aページの際の処理を登録
  processor.set_g_handler(g_packet_handler);  // Gページの際の処理を登録
  
  int count(0);

  while(!in.eof()){
    count++;
    int read_count;
    in.read(buffer, PAGE_SIZE);
    read_count = in.gcount();

#if DEBUG
    cerr << "--read-- : " << count << " 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-- : " << count << " page ; count = " << read_count << endl;
#endif
      continue;
    }

    processor.process(buffer, read_count);
  }
}

int main(int argc, char *argv[]){

  //stream_processor(cin);

  cerr << "sample log processor." << endl;
  if(argc < 5){
    cerr << "Usage: (exe) IMU_mode LAE_mode log.dat rinex.nav [options]" << endl;
    return -1;
  }
  
  cerr << "IMU 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;
    } 
  }
  
  cerr << "LAE Mode checking..." << endl;
  for(int i = 0; i < (sizeof(lever_arms) / sizeof(LeverArmItem)); i++){
    cerr << lever_arms[i].name << " : ";
    if(!strcmp(argv[2], lever_arms[i].name)){
      cerr << "Ready !!" << endl;
      Status::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;
    } 
  }
  
  fstream fin(argv[3], std::ios::in | ios::binary);    
  if(fin.fail()){
    cerr << "Log file not found : " << argv[3] << endl;
    return -1;
  }
  
  // RINEX航法ファイルの読み込み
  fstream fin_n(argv[4], ios::in | ios::binary);
  if(fin_n.fail()){
    cerr << "Not found!! : " << argv[4] << endl;
    return -1;
  }
  
  // 航法ファイルの処理
  typedef RINEX_Nav_Reader<> reader_n_t;
  reader_n_t reader_n(fin_n);
  
  // 電離層遅延係数の処理
  if(!reader_n.extract_iono_coef(status.space_node())){
    cerr << "Iono coef not found!!" << endl;
    return -1;
  }
  
  // option check...
  cerr << "Option checking..." << endl;
  for(int arg_index(5); arg_index < argc; arg_index++){
    
    // ex) --ratate=-Y-Z-X
    if(strstr(argv[arg_index], "--rotate=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--rotate="));
      cerr << "rotate: " << spec << endl;
      RotatedCalibrator::rotate_spec_t decoded;
      if(!RotatedCalibrator::decode(spec, decoded)){
        cerr << "Invalid Rotation!!" << endl;
        return -1;
      }
      A_Packet::calibrator = new RotatedCalibrator(*A_Packet::calibrator, decoded);
      continue;
    }
    if(strstr(argv[arg_index], "--calib_file=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--calib_file="));
      fstream fin(spec);
      if(fin.fail()){continue;}
      cerr << "calib_file: " << spec << endl;
      if(strcmp(argv[1], CustomStandardCalibrator::id)){continue;}
      CustomStandardCalibrator *target(
          static_cast<CustomStandardCalibrator *>(A_Packet::calibrator));
      char buf[1024];
      while(!fin.eof()){
        fin.getline(buf, sizeof(buf));
        target->custom(buf);
      }      
      continue;
    }
    // RINEX NAVのEphemeris情報を使用するかどうか
    if(strstr(argv[arg_index], "--use_nav_eph") == argv[arg_index]){
      // すべての航法情報の取得を行い、登録
      while(reader_n.has_next()){
        reader_n_t::SatelliteInfo info(reader_n.next());
        status.regist_epehemris(info.ephemeris);
      }
      status.lock_ephemeris(true);
      cerr << "use RINEX Nav Ephemeris" << endl;
      continue;
    }
    options.check_spec(argv[arg_index]);
  }
  
  stream_processor(fin);
  
  return 0;
}