/*
 * INS‚Μ‚έ
 */
#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.h"

//#define DUMP_UPDATE
#define DUMP_CORRECT

#define DUMP_TARGET 9
const char *label[DUMP_TARGET] = {"“ŒŒo[•b]", "–kˆά[•b]", "‚“x[m]", "–k•ϋŒό‘¬“x[m/s]", "“Œ•ϋŒό‘¬“x[m/s]", "d—Ν•ϋŒό‘¬“x[m/s]", "ƒ΅[deg]", "ƒ¦[deg]", "ƒ³[deg]"};

class Status{
  private:
    bool bool_init;
    
    typedef INS<accuracy> NAV;
    NAV nav;
    
  public:
    Status() : bool_init(false), nav(){}
    
    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());

#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;
    }
  }
  
  delete(a_packet_next);
}