//----------------------------------------------------------
// Module QuaternionT1
//----------------------------------------------------------

#include "module_QuaternionT1.h"
#include "Exec/prr.h"
#include "rts.h"

void orc_Mod_QuaternionT1::init(const double AccI[3], const double MagI[3], const double GyroI[3], const double VrelI[3], const int &Sensor_RecoverI)
{
// Orccad Version: 3.2
// Module : QuaternionT1
// Initialisation File
// 
// Module Ports :
// 	input INTEGER Sensor_RecoverI
// 	input DOUBLE VrelI[3]
// 	output DOUBLE QO[7]
// 	input DOUBLE GyroI[3]
// 	input DOUBLE MagI[3]
// 	input DOUBLE AccI[3]
// 
// Time Methods usable in module files:
// 	 GetLocalTime(): return relative period time as double value
// 	 GetCurrentTime(): return global period time as double value 
// 
// Date of creation : Thu Oct  2 09:30:18 2008
  int i;
  double xinit[NEQ], tol[2], qinit[4];
  x4readinitstate(xinit, tol);
 eul2quat(xinit,qinit);
 for(i=0;i<4;i++) {xinit[i] = qinit[i];}
 printf("initial quaternion %lf %lf %lf %lf\n", xinit[0], xinit[1], xinit[2], xinit[3]); 
DtQ = Mt_ptr->GetSampleTime();
 pRT=(orc_RT_X4_Diags_T1 *)(Mt_ptr->GetRobotTaskPtr());
    for(i=0;i<4;i++) q_hat[i] = xinit[i];
    for(i=0;i<3;i++) vel_hat[i] = xinit[i+10];
//     q_hat[0] = 0.0262;
//     q_hat[1] = -0.6802;
//     q_hat[2] = -0.0877;
//     q_hat[0] = 0.7273;
    //printf("MagI %lf , GyroI %lf , VrelI %lf , AccI %lf \n", MagI[0], GyroI[0], VrelI[0], AccI[0]);
 //printf("QO[0] %lf \n", QO[0]);
    //  observer_NL(MagI, GyroI, VrelI, AccI, q_hat, vel_hat, DtQ);

      observer(MagI, GyroI, VrelI, AccI, q_hat, vel_hat, DtQ);
 //for(i=0;i<4;i++) q_hat[i] = QO[i];
 //for(i=4;i<7;i++) vel_hat[i-4] = VrelI[i];
  for(i=0;i<4;i++) QO[i] = q_hat[i];
  for(i=4;i<7;i++) QO[i] = GyroI[i-4];
  cpt = 0;
  //printf("exec init %lld \n", MTgetExecTime(this->mt);

} 
void orc_Mod_QuaternionT1::param()
{
	 plugParam();



} 
void orc_Mod_QuaternionT1::reparam()
{

} 
void orc_Mod_QuaternionT1::compute(const double AccI[3], const double MagI[3], const double GyroI[3], const double VrelI[3], const int &Sensor_RecoverI )
{
// Orccad Version: 3.2
// Module : QuaternionT1
// Computation File 
// 
// Module Ports :
// 	input INTEGER Sensor_RecoverI
// 	input DOUBLE VrelI[3]
// 	output DOUBLE QO[7]
// 	input DOUBLE GyroI[3]
// 	input DOUBLE MagI[3]
// 	input DOUBLE AccI[3]
// 
// 
// Date of creation : Thu Oct  2 09:30:18 2008
  int i;
  long long passed;
  passed = orcGetCpuTime();
  //observer_NL(MagI, GyroI, VrelI, AccI, q_hat, vel_hat, DtQ);
  //printf("Gyro in Quat %lf %lf %lf \n", GyroI[0], GyroI[1], GyroI[2]);
   observer(MagI, GyroI, VrelI, AccI, q_hat, vel_hat, DtQ);
  for(i=0;i<4;i++) QO[i] = q_hat[i];
  for(i=4;i<7;i++) QO[i] = GyroI[i-4];
  //printf("QO %lf %lf %lf %lf \n", QO[0], QO[1], QO[2], QO[3]);
  cpt++;
  //printf("Sensor_RecoverI = %d\n", Sensor_RecoverI);
 //  if(Sensor_RecoverI == 1) printf("SensorValue %d \n",  pRT->Get_SensorState());
//   else printf("Sensor_Fail not received \n");

 //  printf("observer %lld \n", orcGetCpuTime() -passed);
//   printf("orcGetExecTime %lld \n",(orcGetExecTime(Mt_ptr->get_MtId()))/cpt);

} 
void orc_Mod_QuaternionT1::end()
{
// Orccad Version: 3.2
// Module :  QuaternionT1
// End File 
// 
// Module Ports :
// 	input INTEGER Sensor_RecoverI
// 	input DOUBLE VrelI[3]
// 	output DOUBLE QO[7]
// 	input DOUBLE GyroI[3]
// 	input DOUBLE MagI[3]
// 	input DOUBLE AccI[3]
// 
// 
// Date of creation : Thu Oct  2 09:30:18 2008
  printf("cpt %d \n", cpt);
  printf("FINAL Quaternion %lld \n",(orcGetExecTime(Mt_ptr->get_MtId())));
  printf("MOY Quaternion %lld \n",(orcGetExecTime(Mt_ptr->get_MtId()))/cpt);
  return;
} 
// End class orc_Mod_QuaternionT1