/*
 * 
 */
#include <iostream>
#include <iomanip>
#include <strstream>
#include <exception>
#include <math.h>
#include <time.h>

#if _MSC_VER >= 1400
#define sprintf sprintf_s
#endif

using namespace std;

typedef double accuracy;

#include "common.h"

#include "INS_GPS2.h"
#include "INS_GPS_BE.h"
#include "sensor/MEMS.h"

typedef MEMS_Accelerometer mems_a;
typedef MEMS_Gyro mems_g;

//#define DUMP_UPDATE
#define DUMP_CORRECT

//#define BIAS_ON

#define DUMP_TARGET 9
const char *label[DUMP_TARGET] = {"東経[秒]", "北緯[秒]", "高度[m]", "北方向速度[m/s]", "東方向速度[m/s]", "重力方向速度[m/s]", "Ψ[deg]", "Θ[deg]", "Φ[deg]"};

class Status{
  private:
    bool bool_init;
    
#ifdef BIAS_ON
    typedef INS_GPS2_BiasEstimated<accuracy> NAV;
#else
    typedef INS_GPS2<accuracy> NAV;
#endif
    
    NAV nav;
    
  public:
    Status() : bool_init(false), nav(){
      Matrix<accuracy> 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+0;
        P(7, 7) = P(8, 8) = P(9, 9) = 1E-4;
#ifdef BIAS_ON
        P(10, 10) = P(11, 11) = P(12, 12) = 1E-4;
        P(13, 13) = P(14, 14) = P(15, 15) = 1E-6;
#endif
      }
      Matrix<accuracy> Q(nav.getFilter().getQ());    
      {
        Q(0, 0) = pow(pow(0.01, mems_a.WN_SLOPE) * mems_a.WN_INTERCEPT / mems_a.SF, 2);
        Q(1, 1) = pow(pow(0.01, mems_a.WN_SLOPE) * mems_a.WN_INTERCEPT / mems_a.SF, 2);
        Q(2, 2) = pow(pow(0.01, mems_a.WN_SLOPE) * mems_a.WN_INTERCEPT / mems_a.SF, 2);
        Q(3, 3) = pow(pow(0.01, mems_g.WN_SLOPE) * mems_g.WN_INTERCEPT / mems_g.SF, 2);
        Q(4, 4) = pow(pow(0.01, mems_g.WN_SLOPE) * mems_g.WN_INTERCEPT / mems_g.SF, 2);
        Q(5, 5) = pow(pow(0.01, mems_g.WN_SLOPE) * mems_g.WN_INTERCEPT / mems_g.SF, 2);
        Q(6, 6) = 1E-14;
#ifdef BIAS_ON
        Q(7, 7) = 1E-10;
        Q(8, 8) = 1E-10;
        Q(9, 9) = 1E-10;
        Q(10, 10) = pow(mems_g.ROOT_N, 2) * 1. / 100;
        Q(11, 11) = pow(mems_g.ROOT_N, 2) * 1. / 100;
        Q(12, 12) = pow(mems_g.ROOT_N, 2) * 1. / 100;
#endif
      }
      //Q *= 2;
      nav.getFilter().init();
    }
    
    NAV &get_nav() {return nav;}
    
    static void dump_label(){
      cout << "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)
    }
    
    void dump(){
      cout << setprecision(10) 
           << 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)
           
    }
    
    void update(const A_Packet &current, const A_Packet &next){
      
#define pow2(x) ((x) * (x))
      //cout << current.acc_x() << ' ' << current.acc_y() << ' ' << current.acc_z() << endl;
      //cout << current.gyro_x() << ' ' << current.gyro_y() << ' ' << current.gyro_z() << endl;
      //cout << sqrt(pow2(current.acc_x()) + pow2(current.acc_y()) + pow2(current.acc_z())) << endl;
       
      if(!bool_init){return;}
      
      //cout << "update:  " << current.itow << ' ' << current.interval(next) << endl;
      nav.update(current.accel(), current.gyro(), INTERVAL_UNIT * current.interval(next));

#ifdef DUMP_UPDATE
      cout << "U\t" << current.itow << "\t";
      dump();
      cout << endl;
#endif
    }
    
    void correct(const G_Packet &g_packet){
      //cout << g_packet.acc_2d << "," << g_packet.acc_v << endl;
      
      if(bool_init){
        //cout << "correct: " << g_packet.itow << endl;
        
        nav.correct(g_packet.convert());
        //nav.correct(g_packet.convert2());

#ifdef DUMP_CORRECT 
        cout << "C\t" << g_packet.itow << "\t";
        dump();
        cout << endl;
#endif
      }else if(!bool_init && g_packet.acc_2d < 100.){
        bool_init = true;
        /*cout << g_packet.llh[0] << ','
             << g_packet.llh[1] << ','
             << g_packet.llh[2] << endl;*/
        nav.initPosition(deg2rad(g_packet.llh[1]), deg2rad(g_packet.llh[0]), g_packet.llh[2]);
        nav.initVelocity(0., 0., 0.);
        nav.initAttitude(deg2rad(-15), deg2rad(1.5), 0.);
      }
    }
};



int main(){
  
  Status status;
  
  cout << "Type\t" << "ITOW(ms)\t";
  Status::dump_label();
  cout << endl;
  
  char buf[1024];
  strstream line(buf, sizeof(buf), strstream::in);
  
  A_Packet *a_packet_current(NULL), *a_packet_next(NULL);
  G_Packet *g_packet(NULL);
  
  while(!cin.eof()){
    cin.getline(buf, sizeof(buf));
    buf[cin.gcount()] = '\0';
    //cout << buf << endl;
    
    char header;
    line.seekg(0);
    
    line >> header;
    if('A' == header){
      delete(a_packet_current);
      a_packet_current = a_packet_next;
      a_packet_next = new A_Packet(line);
      if(a_packet_current){
        status.update(*a_packet_current, *a_packet_next); 
      }
    }else if('G' == header){
      try{
        g_packet = new G_Packet(line);
        if(a_packet_next->interval(*g_packet) > 5){continue;}
      }catch(exception &e){
        line.clear();
      }
    }
    
    if(g_packet){
      status.correct(*g_packet);
      delete(g_packet);
      g_packet = NULL;
    }
  }
  
  cerr << "P:" << status.get_nav().getFilter().getP() << endl;
  delete(a_packet_next);
}