#ifndef __ANALYZE_COMMON_H__
#define __ANALYZE_COMMON_H__

#include "util/util.h"
#include "param/vector3.h"
#include "INS_GPS.h"

#define SCALE_1G 9.82195408822149 // lat = 35.66892242 ���z��`

#define ACC1_MEAN 7701097
#define ACC2_MEAN 8969922
#define ACC3_MEAN 7900336
#define ACC1_1G 3464550
#define ACC2_1G 3503620
#define ACC3_1G 3444280
#define ACC_STD 10000

#define GYRO1_MEAN 7563316
#define GYRO2_MEAN 8235709
#define GYRO3_MEAN 8142101
#define GYRO_1DEG 41943.04
#define GYRO_STD 25000

#define INTERVAL_UNIT 0.001 //[sec]

class VoidPacketException : public exception{
  
};

struct Packet{
  int itow;
  
  Packet(istream &in){in >> itow;}
  
  int interval(const Packet &next) const {
    return next.itow - itow;
  }
};

struct A_Packet : Packet {
  int ch[8];
  
  A_Packet(istream &in) : Packet(in){
    for(int i = 0; i < 8; i++){
      in >> ch[i];
    }
  }
  
  accuracy acc_1() const{return (accuracy)(ch[0] - ACC1_MEAN) / ACC1_1G * SCALE_1G;}
  accuracy acc_2() const{return (accuracy)(ch[1] - ACC2_MEAN) / ACC2_1G * SCALE_1G;}
  accuracy acc_3() const{return (accuracy)(ch[2] - ACC3_MEAN) / ACC3_1G * SCALE_1G;}
  
  accuracy gyro_1() const{return deg2rad((accuracy)(ch[3] - GYRO1_MEAN) / GYRO_1DEG);}
  accuracy gyro_2() const{return deg2rad((accuracy)(ch[4] - GYRO2_MEAN) / GYRO_1DEG);}
  accuracy gyro_3() const{return deg2rad((accuracy)(ch[5] - GYRO3_MEAN) / GYRO_1DEG);}
  
  Vector3<accuracy> accel() const{
    Vector3<accuracy> a(-acc_3(), -acc_2(), acc_1());   //brute32(+++++-)
    //Vector3<accuracy> a(-acc_3(), -acc_2(), acc_1()); //brute
    return a;
  }
  
  Vector3<accuracy> gyro() const{
    Vector3<accuracy> omega(-gyro_2(), gyro_3(), gyro_1());     //brute32(+++++-)
    //Vector3<accuracy> omega(-gyro_2(), gyro_3(), -gyro_1());  //brute
    return omega;
  }
};

struct G_Packet : Packet {
  accuracy llh[3];
  accuracy acc_2d, acc_v;
  accuracy vel_ned[3];
  accuracy acc_vel;
  
  G_Packet(istream &in) throw(exception) : Packet(in) {
    for(int i = 0; i < 3; i++){
      in >> llh[i];
    }
    in >> acc_2d >> acc_v;
    
    for(int i = 0; i < 3; i++){
      in >> vel_ned[i];
      if(in.fail()){
        throw VoidPacketException();
      }
    }
    in >> acc_vel;
  }
  
  GPS_UBLOX_3D<accuracy> convert() const{
    GPS_UBLOX_3D<accuracy> packet;
    {
      packet.v_n = vel_ned[0];
      packet.v_e = vel_ned[1];
      packet.v_d = vel_ned[2];
      packet.sigma_vel = acc_vel;
      packet.latitude = deg2rad(llh[1]);
      packet.longitude = deg2rad(llh[0]);
      packet.height = llh[2];
      packet.sigma_2d = acc_2d;
      packet.sigma_height = acc_v;
    }
    return packet;
  }
  
  GPS_NMEA_SIM_3D_with_Vd<accuracy> convert2() const{
    GPS_NMEA_SIM_3D_with_Vd<accuracy> packet;
    {
#define pow2(x) ((x)*(x))
      packet.v = sqrt(pow2(vel_ned[0]) + pow2(vel_ned[1]));
      packet.v_heading = (packet.v == 0 ? 0 : acos(vel_ned[0] / packet.v));
      if(vel_ned[1] < 0){packet.v_heading *= -1;}
      packet.v_down = vel_ned[2];
      packet.v_norm_sigma = acc_vel;
      packet.v_direction_sigma = deg2rad(1);
      packet.latitude = deg2rad(llh[1]);
      packet.longitude = deg2rad(llh[0]);
      packet.height = llh[2];
      packet.long2D_sigma = acc_2d;
      packet.short2D_sigma = acc_2d;
      packet.height_sigma = acc_v;
#undef pow2
    }
    return packet;
  }
};


string pretty_deg2(double degree){
  char buffer[128];
  (degree -= (int)degree) *= (60 * 60);
  sprintf(buffer, "%f", degree);
  return string(buffer);
}

#endif /* __ANALYZE_COMMON_H__ */