//----------------------------------------------------------
// Module Diag_CapteursT1
//----------------------------------------------------------

#include "module_Diag_CapteursT1.h"
#include "Exec/prr.h"

#include "rts.h"


void orc_Mod_Diag_CapteursT1::init(const double AccI[3], const double MagI[3], const double GyroI[3], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const double PosI[6])
{
// Orccad Version: 3.2
// Module : Diag_CapteursT1
// Initialisation File
// 
// Module Ports :
// 	output EVENT Sensor_FailE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	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:27:47 2008

  int i;
  neq=NEQ;
configx4();
itol = 1;
itask = 4;
istate = 1;
iopt = 0;
liw = LIW;
lrw = LRW;
jt = 2;
//neq = NEQ;
 for(i=0;i<NEQ;i++) x_model[i] = 0.0;
 for(i=0;i<NMOT;i++) Vmot_l[i] = s_ref_diag[4] = VmotI[i];
 //for(i=0;i<NMOT;i++) Volts[i] = 0.0;
for (i=0;i<3;i++) {
   xp0[i] = 0.0;
   xp1[i] = 0.0;
   xp2[i] = 0.0;
   xp3[i] = 0.0;
 }
 //x_model is filled with initial state
  x4readinitstate(x_model, tol);
  //converts position from euler to quat
  eul2quat(x_model,qinit);
  for(i=0;i<4;i++) {x_model[i] = qinit[i];}
  //linear velocities
  for(i=0;i<3;i++) vel_gx[i] = x_model[i+3];
  // for(i=0;i<17;i++) dot_xinit[i] = 0;
  for(i=0;i<4;i++) qmodel[i]=x_model[i]; //q_hat[i] = xinit[i];
    for(i=0;i<3;i++) velmodel[i] = x_model[i+3];
    //  for(i=0;i<17;i++) dx_model[i] = 0.0;
    for(i=0;i<4;i++) q_gx[i]=x_model[i];

 printf("init Diagnostic \n");

 DtQ = Mt_ptr->GetSampleTime();
 pRT=(orc_RT_X4_Diags_T1 *)(Mt_ptr->GetRobotTaskPtr());
 abstol = tol[0]; rtol = tol[1];
TZERO = (double) orcGetCpuTime();
printf("abstol %lf rtol %lf \n", abstol, rtol);
 tint = 0.0;
 Time = 
   //Time = Mt_ptr->GetRobotTaskPtr()->GetGlobalTime();
Time = ((double) orcGetCpuTime() - TZERO)/1.e9;
 tout = Time;
 
//        Volts[0] = motor_control1( Vmot_l[0], s_ref_diag[0]); 
// 	 Volts[1] = motor_control1( Vmot_l[1], s_ref_diag[1]);
// 	 Volts[2] = motor_control1( Vmot_l[2], s_ref_diag[2]); 
// 	 Volts[3] = motor_control1( Vmot_l[3], s_ref_diag[3]); 
 // Volts[0] = motor_control(xp0, Vmot_l[0], s_ref_diag[0], DtQ);
//  Volts[1] = motor_control(xp1, Vmot_l[1], s_ref_diag[1], DtQ);
//  Volts[2] = motor_control(xp2, Vmot_l[2], s_ref_diag[2], DtQ);
//  Volts[3] = motor_control(xp3, Vmot_l[3], s_ref_diag[3], DtQ);

 modelx4_velocity(&neq, &tout, x_model, dx_model, s_ref_diag);
          for (i=0; i<17;i++) extended_state_model[i] = x_model[i];
          for (i=0; i<3;i++) extended_state_model[i+17] = dx_model[i+10];
	  output(extended_state_model, b_model, velmodel, pos_model, vit_model, acc_model, vitmot_model);

  double velm[3];
  velm[0]=velmodel[0];
  velm[1]=GyroI[1];
  velm[2]=GyroI[2];
  double q_test[3];
       observer_gx(MagI, GyroI, VrelI , AccI, q_gx, vel_gx, DtQ);
       for (i=0;i<4;i++)
       q_test[i]=q_gx[i];
 
       double q_res[4];
  double q_th[4];
  q_th[0]=q_mod[0];
  q_th[1]=-q_mod[1];
  q_th[2]=-q_mod[2];
  q_th[3]=-q_mod[3];
  
 double norm;
    norm=q_gx[0]*q_gx[0]+q_gx[1]*q_gx[1]+q_gx[2]*q_gx[2]+q_gx[3]*q_gx[3];

    q_gx[0]=q_gx[0]/norm;
    q_gx[1]=q_gx[1]/norm;
    q_gx[2]=q_gx[2]/norm;
    q_gx[3]=q_gx[3]/norm;
    double d_q[4][4];
  d_q[0][0]=q_gx[0];
  d_q[0][1]=-q_gx[1];
  d_q[0][2]=-q_gx[2];
  d_q[0][3]=-q_gx[3];
  d_q[1][0]=q_gx[1];
  d_q[1][1]=q_gx[0];
  d_q[1][2]=-q_gx[3];
  d_q[1][3]=q_gx[2];
  d_q[2][0]=q_gx[2];
  d_q[2][1]=q_gx[3];
  d_q[2][2]=q_gx[0];
  d_q[2][3]=-q_gx[1];
  d_q[3][0]=q_gx[3];
  d_q[3][1]=-q_gx[2];
  d_q[3][2]=q_gx[1];
  d_q[3][3]=q_gx[0];


 quatprod(q_gx, q_mod, q_res);
   q_res[0]=((d_q[0][0]*q_th[0])+(d_q[0][1]*q_th[1])+(d_q[0][2]*q_th[2])+(d_q[0][3]*q_th[3]));
   //  printf("q_res est %lf \n",q_res[0]);
 double d_q_gx;
  d_q_gx=(2*acos(q_res[0]))*180/3.14;
  //printf("le residu de gyro x est : %lf \n", d_q_gx);
    // for(i=0;i<4;i++) v_l[i]=VO[i];  
cpt = 0;
//printf("exec init %lld \n", MTgetExecTime(Mt_ptr);
strcpy(qf1mod , "./q1mod") ;
strcpy(qf2mod , "./q2mod") ;
strcpy(qf3mod , "./q3mod") ;
strcpy(qf4mod , "./q4mod") ;
strcpy(qf1obs , "./q1obs") ;
strcpy(qf2obs , "./q2obs") ;
strcpy(qf3obs , "./q3obs") ;
strcpy(qf4obs , "./q4obs") ;
strcpy(residu , "./res") ;

q1mod = fopen(qf1mod,"w");
q2mod = fopen(qf2mod,"w");
q3mod = fopen(qf3mod,"w");
q4mod = fopen(qf4mod,"w");
q1obs = fopen(qf1obs,"w");
q2obs = fopen(qf2obs,"w");
q3obs = fopen(qf3obs,"w");
q4obs = fopen(qf4obs,"w");
res = fopen(residu,"w");

 fprintf(q1mod,"%f %f \n",Time,x_model[0]);
 fprintf(q2mod,"%f %f \n",Time,x_model[1]);
 fprintf(q3mod,"%f %f \n",Time,x_model[2]);
 fprintf(q4mod,"%f %f \n",Time,x_model[3]);
 fprintf(q1obs,"%f %f \n",Time,q_test[0]);
 fprintf(q2obs,"%f %f \n",Time,q_test[1]);
 fprintf(q3obs,"%f %f \n",Time,q_test[2]);
 fprintf(q4obs,"%f %f \n",Time,q_test[3]);
 fprintf(res,"%f %f \n",Time,d_q_gx);


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



} 
void orc_Mod_Diag_CapteursT1::reparam()
{

} 
void orc_Mod_Diag_CapteursT1::compute(const double AccI[3], const double MagI[3], const double GyroI[3], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const double PosI[6] )
{
// Orccad Version: 3.2
// Module : Diag_CapteursT1
// Computation File 
// 
// Module Ports :
// 	output EVENT Sensor_FailE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	input DOUBLE GyroI[3]
// 	input DOUBLE MagI[3]
// 	input DOUBLE AccI[3]
// 
// 
// Date of creation : Thu Oct  2 09:27:47 2008
  long long passed;
   passed = orcGetCpuTime();
  cpt++;
  // printf("Diagnostic \n");
  // tint=0.0;
  int i;
  //printf("Compute  Diagnostic \n");
 DtQ = Mt_ptr->GetSampleTime();
Time = ((double) orcGetCpuTime() - TZERO)/1.e9;
//  Time = Mt_ptr->GetRobotTaskPtr()->GetGlobalTime();

       double tmp_q_D[3];
       double D_Pos[3];
       for(i=0;i<3;i++)D_Pos[i]=0.0;
 for(i=0;i<NMOT;i++) Vmot_l[i] = VmotI[i];
  eul2quat(D_Pos,tmp_q_D);//conversion angle en quaternion (ref)

  double quat_err[4];
  double Quat1[4];
  for(i=0;i<4;i++)
    Quat1[i]=x_model[i];
      quatprod( tmp_q_D,  Quat1, quat_err );//produit de quaternion entre la ref et lestimer
      double couple_ref_diag[3];
   loi_icra(velmodel, quat_err, couple_ref_diag);//calcul les couples desire
   //   double s_ref_diag[4];
vitesse_desire(couple_ref_diag, s_ref_diag);//calcul les 4 vitesses desirees

//      Volts[0] = motor_control1( Vmot_l[0], s_ref_diag[0]); 
//  	 Volts[1] = motor_control1( Vmot_l[1], s_ref_diag[1]); 
// 	 Volts[2] = motor_control1( Vmot_l[2], s_ref_diag[2]); 
// 	 Volts[3] = motor_control1( Vmot_l[3], s_ref_diag[3]); 
//  Volts[0] = motor_control(xp0, Vmot_l[0], s_ref_diag[0], DtQ);
//  Volts[1] = motor_control(xp1, Vmot_l[1], s_ref_diag[1], DtQ);
//  Volts[2] = motor_control(xp2, Vmot_l[2], s_ref_diag[2], DtQ);
//  Volts[3] = motor_control(xp3, Vmot_l[3], s_ref_diag[3], DtQ);


 if (Time > tint) /*integrate from tint to current Time*/
{
  tout = Time;
   rwork[0] = tout;
   istate = 1;

   lsoda((void(*)(void))modelx4_velocity, &(neq), x_model, &(tint), &(tout), &(itol), &(rtol), &(abstol), &(itask), &(istate), &(iopt), rwork, &(lrw), iwork, &(liw), (void(*)( void))jdum, &(jt), s_ref_diag);
 }
 else printf("null integration step \n");

 modelx4_velocity(&neq,&tout, x_model, dx_model, s_ref_diag);

          for (i=0; i<4;i++) q_mod[i]=x_model[i];
          for (i=0; i<17;i++) extended_state_model[i] = x_model[i];
          for (i=0; i<3;i++) extended_state_model[i+17] = dx_model[i+10];
      output(extended_state_model, b_model, velmodel, pos_model, vit_model, acc_model, vitmot_model);

  double velm[3];
  velm[0]=velmodel[0];//mesure model
  velm[1]=GyroI[1];//mesure capteur
  velm[2]=GyroI[2];//mesure capteur
 double q_test[4];
       observer_gx(MagI, GyroI, VrelI , AccI, q_gx, vel_gx, DtQ);
       for (i=0;i<4;i++)
	 q_test[i]=q_gx[i];
  double q_res[4];
  double q_th[4];
  q_th[0]=q_mod[0];
  q_th[1]=-q_mod[1];
  q_th[2]=-q_mod[2];
  q_th[3]=-q_mod[3];
  
 double norm;
    norm=q_gx[0]*q_gx[0]+q_gx[1]*q_gx[1]+q_gx[2]*q_gx[2]+q_gx[3]*q_gx[3];

    q_gx[0]=q_gx[0]/norm;
    q_gx[1]=q_gx[1]/norm;
    q_gx[2]=q_gx[2]/norm;
    q_gx[3]=q_gx[3]/norm;
    double d_q[4][4];
  d_q[0][0]=q_gx[0];
  d_q[0][1]=-q_gx[1];
  d_q[0][2]=-q_gx[2];
  d_q[0][3]=-q_gx[3];
  d_q[1][0]=q_gx[1];
  d_q[1][1]=q_gx[0];
  d_q[1][2]=-q_gx[3];
  d_q[1][3]=q_gx[2];
  d_q[2][0]=q_gx[2];
  d_q[2][1]=q_gx[3];
  d_q[2][2]=q_gx[0];
  d_q[2][3]=-q_gx[1];
  d_q[3][0]=q_gx[3];
  d_q[3][1]=-q_gx[2];
  d_q[3][2]=q_gx[1];
  d_q[3][3]=q_gx[0];


 quatprod(q_gx, q_mod, q_res);
   q_res[0]=((d_q[0][0]*q_th[0])+(d_q[0][1]*q_th[1])+(d_q[0][2]*q_th[2])+(d_q[0][3]*q_th[3]));
   //  printf("q_res est %lf \n",q_res[0]);
 double d_q_gx;
  d_q_gx=(2*acos(q_res[0]))*180/3.14;
  //printf("le residu de gyro x est : %lf \n", d_q_gx);
   //printf("fin diagnostic\n");
   //printf("fin diagnostic\n");
 fprintf(q1mod,"%f %f \n",Time,x_model[0]);
 fprintf(q2mod,"%f %f \n",Time,x_model[1]);
 fprintf(q3mod,"%f %f \n",Time,x_model[2]);
 fprintf(q4mod,"%f %f \n",Time,x_model[3]);
 fprintf(q1obs,"%f %f \n",Time,q_test[0]);
 fprintf(q2obs,"%f %f \n",Time,q_test[1]);
 fprintf(q3obs,"%f %f \n",Time,q_test[2]);
 fprintf(q4obs,"%f %f \n",Time,q_test[3]);
 fprintf(res,"%f %f \n",Time,d_q_gx);

  //printf("le quaternion du diag  est : %lf %lf %lf %lf \n", q_gx[0], q_gx[1], q_gx[2], q_gx[3]);
  
  SensorValue = cpt%4;
  //printf("SensorDiag = %d \n", SensorValue);
  if(SensorValue <= 2){
    pRT->Put_SensorState(SensorValue);
    Sensor_FailE = SET_EVENT;
  }
  //else printf("Sensor_Fail not sent \n");

} 
void orc_Mod_Diag_CapteursT1::end()
{
// Orccad Version: 3.2
// Module :  Diag_CapteursT1
// End File 
// 
// Module Ports :
// 	output EVENT Sensor_FailE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	input DOUBLE GyroI[3]
// 	input DOUBLE MagI[3]
// 	input DOUBLE AccI[3]
// 
// 
// Date of creation : Thu Oct  2 09:27:47 2008
  printf("cpt %d \n", cpt);
  printf("FINAL Diagnostic %lld \n",(orcGetExecTime(Mt_ptr->get_MtId())));
  printf("MOY Diagnostic %lld \n",(orcGetExecTime(Mt_ptr->get_MtId()))/cpt);
 fclose(q1mod);
 fclose(q2mod);
 fclose(q3mod);
 fclose(q4mod);
 fclose(q1obs);
 fclose(q2obs);
 fclose(q3obs);
 fclose(q4obs);
 fclose(res);
  return;

} 
// End class orc_Mod_Diag_CapteursT1