using namespace std;

#include <iostream>
#include <math.h>
#include <time.h>

typedef double accuracy;

#define FREQUENCY 25

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

#include "INS.h"
#include "INS_GPS.h"

#include "sensor/MEMS.h"

int main(){

  INS_GPS<accuracy> ins_gps;
  ins_gps.initPosition(deg2rad(35. + 42. / 60 + 50.818 / 3600),
                       deg2rad(139. + 45. / 60 + 48.670/ 3600),
                       0);
  Vector3<accuracy> accel(10, 0, Earth::gravity(ins_gps.latitude()));
  Vector3<accuracy> gyro(-Earth::Omega_Earth, 0, 0);
    
  long t = (long)time(NULL);
    
  for(int i = 0; i < FREQUENCY * 60 * 5; i++){
  
    /*
    cout << "STEP[" << i << "] "
         << "ƒÓ: " << pretty_deg(rad2deg(ins_gps.latitude())) << ", "
         << "ă: " << pretty_deg(rad2deg(ins_gps.longitude())) << ", "
         << "h: " << ins_gps.hight()
         << endl;
    */
      
    ins_gps.update(accel, gyro, 1. / FREQUENCY);
  }
  
  cout << "V_{north} : " << ins_gps.v_north() << endl;
  cout << "V_{east}  : " << ins_gps.v_east()  << endl;
  
  cout << ((long)time(NULL) - t) << "[sec]" << endl;

  /*
	Vector3<accuracy> ada(1.23, 2.45, 3.69);
	Vector3<accuracy> hoge(2.678, 3.3, 4.12);
	
	{
		Vector3<accuracy> basha = ada;
		cout << "BASHA" << basha << endl;
	}
	
	cout << "ADA" << ada << endl;
	cout << ada.innerp(hoge) << endl;
	
	Quarternion<accuracy> q1(1, 2, 3, 4);
	Quarternion<accuracy> q2(2, 3, 4, 5);
	cout << q1.regularize() << endl;
	cout << q1.getTheta() * 360 / (2 * PI) << endl;
	cout << q1.getAxis() << endl;
	cout << q1.getDCM() << endl;
	cout << q1 * q2 << endl;
  cout << q1.conj() * q1 << endl;
  cout << (q1.conj() += q1) << endl;
  
  accuracy theta = PI / 2;
  Vector3<accuracy> axis(0, 1, 0);
  cout << axis * sin(theta / 2) << endl;
  Quarternion<accuracy> q_a_b(cos(theta / 2), axis * sin(theta / 2));
  cout << q_a_b << endl;
  Vector3<accuracy> target(1, 0, 0);
  
  cout << (q_a_b.conj() * target * q_a_b).vector() << endl;
  
  cout << "WGS84 Test" << endl;
  
  double latitude = deg2rad(0);
  
  cout << Earth::R_meridian(latitude) << endl;
  cout << Earth::R_normal(latitude) << endl;
  cout << Earth::gravity(latitude) << endl;
  */
  
  return 0;
}