#ifndef __ANALYZE_COMMON_H__
#define __ANALYZE_COMMON_H__

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

#define SCALE_1G 9.79811154729054 // lat = N35.54.23 = 35.9063888888889 �������`

#define ACC1_1G 3464550
#define ACC2_1G 3503620
#define ACC3_1G 3444280
#define ACC1_MEAN (get_mean(0) + ACC1_1G) // 7701097
#define ACC2_MEAN get_mean(1) // 8969922
#define ACC3_MEAN get_mean(2) // 7900336
#define ACC_STD 10000

#define GYRO1_1DEG 42377.7
#define GYRO2_1DEG 43348.2
#define GYRO3_1DEG 42689.0
#define GYRO1_MEAN get_mean(3) // 7563316
#define GYRO2_MEAN get_mean(4) // 8235709
#define GYRO3_MEAN get_mean(5) // 8142101
#define GYRO_STD 25000

#define INTERVAL_UNIT 0.001 //[sec]


#define MEAN_A {0.075135, 0.009627, 0.277115, 0.086298, -0.135166, -0.038820}
//#define MEAN_B {3637276,  9002773,  5850356,  6839857,  9354601,   8471370}
//#define CHANNEL6_0 0

#define MEAN_B {4233509, 8902195, 8061869, 7553474, 8248284, 8142491}
#define CHANNEL6_0 8388582


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 get_mean(const int &channel) const{
    static const accuracy mean_a[] = MEAN_A;
    static const accuracy mean_b[] = MEAN_B;
    return mean_a[channel] * (ch[6] - CHANNEL6_0) + mean_b[channel];
  }
  
  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) / GYRO1_1DEG);}
  accuracy gyro_2() const{return deg2rad(((accuracy)ch[4] - GYRO2_MEAN) / GYRO2_1DEG);}
  accuracy gyro_3() const{return deg2rad(((accuracy)ch[5] - GYRO3_MEAN) / GYRO3_1DEG);}
  
  Vector3<accuracy> accel() const{
    Vector3<accuracy> a(-acc_2(), acc_3(), acc_1());   //brute32(+++++-)
    //Vector3<accuracy> a(-acc_3(), -acc_2(), acc_1()); //brute
    return a;
  }
  
  Vector3<accuracy> gyro() const{
    Vector3<accuracy> omega(gyro_3(), gyro_2(), 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__ */