#include <iostream>

#ifndef PI
  #define PI 3.14159265358973
#endif

#define DEBUG 0
#define TEST 0

#define DELTA 0.01
#define OBSERVE 1.0
#define LIMIT 30.0

#define I_xx 1.9
#define I_yy 1.6
#define I_zz 2.0

#define TEST_THRESHOLD 0.01

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

typedef double accuracy;

using namespace std;
  
void calc(const Matrix<accuracy> &gain){

  Vector3<accuracy> omega(0.1, 17.0 * (PI * 2 / 60) + 1.0, 0.0);
  Quarternion<accuracy> q(1.0, 0.0, 0.0, 0.0);
  
  //–Ú•Wó‘Ô
  const Vector3<accuracy> omega_achieve(0.0, 17.0 * (PI * 2 / 60), 0.0);
  const Vector3<accuracy> dcm_achieve(0.0, 1.0, 0.0);
  
  Vector3<accuracy> torque;
  Vector3<accuracy> productI(I_xx, I_yy, I_zz);
 
  EulerEoM<accuracy> euler;
  QuarternionEoM<accuracy> qEoM;

#if !TEST & !DEBUG  
  cout << "TIME(s)" << ","
       << "ƒĒƒÖ_x(rad/s)" << ","
       << "ƒĒƒÖ_y(rad/s)" << ","
       << "ƒĒƒÖ_z(rad/s)" << ","
       << "ƒĒdcm_12" << ","
       << "ƒĒdcm_22" << ","
       << "ƒĒdcm_32" << endl;
#endif


  Matrix<accuracy> omega_p(3, 1);
  Matrix<accuracy> dcm_p(3, 1);

#if TEST
  //·‚Šč‡’lˆČ‰š‚É‚Č‚Á‚―Žž‚ð‹L˜^‚·‚é
  Matrix<accuracy> satisfy_time(6, 1);
#endif

  for(accuracy time = DELTA; time <= LIMIT; time += DELTA){

    //§Œäƒgƒ‹ƒN‚ĖÝ’č
    Vector3<accuracy> contorol;
    {
      Matrix<accuracy> omega_p_now = (omega - omega_achieve).toMatrix();
      Matrix<accuracy> dcm_p_now = q.getDCM().columnVector(2) - dcm_achieve.toMatrix();
      Matrix<accuracy> omega_d = (omega_p_now - omega_p) / DELTA;
      Matrix<accuracy> dcm_d = (dcm_p_now - dcm_p) / DELTA;
      omega_p = omega_p_now;
      dcm_p = dcm_p_now;
      
      Matrix<accuracy> state(12, 1);
      for(int i = 0; i < 3; i++){state(i, 0) = omega_p(i, 0);}
      for(int i = 0; i < 3; i++){state(i + 3, 0) = dcm_p(i, 0);}
      for(int i = 0; i < 3; i++){state(i + 6, 0) = omega_d(i, 0);}
      for(int i = 0; i < 3; i++){state(i + 9, 0) = dcm_d(i, 0);}

#if DEBUG
  cout << state << endl;
#endif
      
      contorol = Vector3<accuracy>(gain * state);
    }
      
    qEoM.setOmega(omega);
    q = nextByRK4(qEoM, time, q, DELTA).regularize();  
      
    euler.set(torque - contorol, productI);
    omega = nextByRK4(euler, time, omega, DELTA);
    
    Vector3<accuracy> delta_omega((omega - omega_achieve).toMatrix());
    Vector3<accuracy> delta_dcm(q.getDCM().columnVector(2) - dcm_achieve.toMatrix());
#if TEST
    for(int i = 0; i < 3; i++){
      if(abs(delta_omega[i]) < TEST_THRESHOLD){
        if(satisfy_time(i, 0) == 0){satisfy_time(i, 0) = time;}
      }else if(satisfy_time(i, 0) != 0){satisfy_time(i, 0) = 0;}
    }
    for(int i = 0; i < 3; i++){
      if(abs(delta_dcm[i]) < TEST_THRESHOLD){
        if(satisfy_time(i + 3, 0) == 0){satisfy_time(i + 3, 0) = time;}
      }else if(satisfy_time(i + 3, 0) != 0){satisfy_time(i + 3, 0) = 0;}
    }
#elif !DEBUG
    //o—Í
    cout << time << ","
         << delta_omega[0]<< ","
         << delta_omega[1] << ","
         << delta_omega[2] << ","
         << delta_dcm[0] << ","
         << delta_dcm[1] << ","
         << delta_dcm[2] << endl;
#endif
  }

#if TEST
  for(int i = 0; i < satisfy_time.rows()*satisfy_time.columns(); i++){
    cout << "," << satisfy_time((int)(i / satisfy_time.columns()), i % satisfy_time.columns());
  }
  cout << endl;
#endif
}

int main(int argc, char **argv){
  
  //ƒQƒCƒ“Ý’č
  Matrix<accuracy> gain(3, 12);
  {
#if TEST
    if(argc > 1){
      for(int i = 0; i < min(gain.rows()*gain.columns(), argc-1); i++){
        gain((int)(i / gain.columns()), i % gain.columns()) = atof(*(argv + i + 1));
      }
    }
    for(int i = 0; i < gain.rows()*gain.columns(); i++){
      if(i > 0){cout << ",";}
      cout << gain((int)(i / gain.columns()), i % gain.columns());
    }
#else
    gain(0, 0) = 0.5; 
    gain(0, 2) = 0.05;
    gain(0, 8) = 0.1;
    gain(1, 1) = 1.0;
    gain(2, 2) = 0.4;
    gain(2, 9) = 0.06;
    gain(0, 11) = -0.8;
    gain(2, 0) = -0.06;
    gain(2, 6) = -0.2;
#endif
  }

  calc(gain);
}