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

#ifndef PI
  #define PI 3.14159265358973
#endif

#define DELTA 0.01

#include "strategy.h"
#include "parameter.h"
#include "equation.h"

typedef double acuuracy;

using namespace std;

int main(){  
  Vector3<acuuracy> omega(0.1, 17 * 2 * PI / 60 + 1.0, 0.0);
  Quarternion<acuuracy> q(1.0, 0.0, 0.0, 0.0);
  
  Vector3<acuuracy> torque(0.0, 0.0, 0.0);
  Vector3<acuuracy> productI(1.9, 1.6, 2.0);
  
  EulerEoM<acuuracy> euler(torque, productI);
  QuarternionEoM<acuuracy> qEoM;
  
  cout << "TIME(s)" << ","
       << "ƒÖ_x(rad/s)" << ","
       << "ƒÖ_y(rad/s)" << ","
       << "ƒÖ_z(rad/s)" << ","
       << "q_0" << ","
       << "q_1" << ","
       << "q_2" << ","
       << "q_3" << endl;
  
  for(acuuracy time = 0; time < 100.0; time += DELTA){
   
    qEoM.setOmega(omega);
    q = nextByRK4(qEoM, time, q, DELTA).regularize();
    omega = nextByRK4(euler, time, omega, DELTA);
    
    cout << time << ","
         << omega.getX() << ","
         << omega.getY() << ","
         << omega.getZ() << ","
         << q[0] << ","
         << q[1] << ","
         << q[2] << ","
         << q[3] << endl;
  }
  return 0; 
}