//----------------------------------------------------------
// Module GPS_CtrlT1
//----------------------------------------------------------

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

#include "rts.h"

void orc_Mod_GPS_CtrlT1::init(const double QI[7], const double PosI[6], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const int &Motor_RecoverI)
{
// Orccad Version: 3.2
// Module : GPS_CtrlT1
// Initialisation File
// 
// Module Ports :
// 	input INTEGER Motor_RecoverI
// 	output EVENT PanicE
// 	output EVENT X4_ReadyE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	output DOUBLE VO[4]
// 	input DOUBLE PosI[6]
// 	input DOUBLE QI[7]
// 
// 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:28:52 2008
  int i;
  cpt = 0;
  double xinit[NEQ], tol[2], qinit[4];
  TZERO = (double) orcGetCpuTime();
Time = ((double) orcGetCpuTime() - TZERO)/1.e9;
//Time = Mt_ptr->GetRobotTaskPtr()->GetGlobalTime();

DtC = Mt_ptr->GetSampleTime();
pRT=(orc_RT_X4_Diags_T1 *)(Mt_ptr->GetRobotTaskPtr());

 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]); 
 for(i=0; i<4;i++) Quat[i] = xinit[i];
 for(i=0; i<3;i++) gyro[i] = xinit[i+4];
 for(i=0; i<4;i++) VO[i] = UO[i] = 0.0;
 for (i=0;i<4;i++) {
   VO[i] = UO[i] = xinit[i+13];
 }
 for(i=0;i<3;i++) {
   Pos[i] = PosI[i];
   D_Pos[i] = D_PosI[i];
 }
 for (i=0;i<3;i++) {
   xp0[i] = 0.0;
   xp1[i] = 0.0;
   xp2[i] = 0.0;
   xp3[i] = 0.0;
 }
 quat2eul(Quat,rtl);


X4_ReadyE = SET_EVENT;

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

} 
void orc_Mod_GPS_CtrlT1::reparam()
{

} 
void orc_Mod_GPS_CtrlT1::compute(const double QI[7], const double PosI[6], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const int &Motor_RecoverI )
{
// Orccad Version: 3.2
// Module : GPS_CtrlT1
// Computation File 
// 
// Module Ports :
// 	input INTEGER Motor_RecoverI
// 	output EVENT PanicE
// 	output EVENT X4_ReadyE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	output DOUBLE VO[4]
// 	input DOUBLE PosI[6]
// 	input DOUBLE QI[7]
// 
// 
// Date of creation : Thu Oct  2 09:28:52 2008
int i;
 double tmp;
 double tmp_q_D[4];
 double tmp_couple[3];
 double tmp_couple1[3];
  // Time = Mt_ptr->GetRobotTaskPtr()->GetGlobalTime();
Time = ((double) orcGetCpuTime() - TZERO)/1.e9;

 for(i=0;i<3;i++) {
   Pos[i] = PosI[i];
   D_Pos[i] = D_PosI[i];
   tmp_couple[i]=0;
 }
  for(i=0;i<4;i++) {
    Quat[i] = QI[i];
    tmp_q_D[i]=0;
      }
  for(i=0;i<3;i++) {
    gyro[i] = QI[i+4];
      }
  tmp = sqrt(SQR(Quat[0])+SQR(Quat[1])+SQR(Quat[2])+SQR(Quat[3]));
 
  if(tmp < 0.1){
    printf("Bad Quarternion norm %lf \n", tmp);}
  else{   
    //-------------------fonctions avant--------------------------------------------------------------
//     position_control(Pos, D_Pos, Quat, VrelI, D_attitude); 
//     //printf("pos est %lf %lf %lf\n",D_Pos[0],D_Pos[1],D_Pos[2]);
//     eul_d[0] = D_attitude[0];
//     eul_d[1] = D_attitude[1];
//     eul_d[2] = 0.0;

//     error_computation(eul_d, Quat, gyro, quat_err, omega_err);

//      attitude_control(quat_err, omega_err, D_attitude[2], s_ref );

//      //DtC = 0.005;
// //     VO[0] = motor_control(xp0, VmotI[0], s_ref[0], DtC);
// //     VO[1] = motor_control(xp1, VmotI[1], s_ref[1], DtC);
// //     VO[2] = motor_control(xp2, VmotI[2], s_ref[2], DtC);
// //     VO[3] = motor_control(xp3, VmotI[3], s_ref[3], DtC);

// //    VO[0] = motor_control1( VmotI[0], s_ref[0]);
// //    VO[1] = motor_control1( VmotI[1], s_ref[1]);
// //    VO[2] = motor_control1( VmotI[2], s_ref[2]);
// //    VO[3] = motor_control1( VmotI[3], s_ref[3]);

//      for(i=0; i<4;i++) { // speed control in now in modelx4
//        VO[i] = UO[i] = s_ref[i];
//      }
//      //for(i=0;i<4;i++) VO[i] = 0.0;
//     //printf("DtC %lf \n", DtC);
//      //printf("VO %lf \n",VO[0]); 
//     quat2eul(Quat,rtl);
 //----------------------fonctions C.Berbra----------------------------------------------------------
      eul2quat(D_Pos,tmp_q_D);//conversion angle en quaternion (ref)
//   printf("la position desire en angle Euler est: %lf %lf %lf \n",D_Pos[0], D_Pos[1], D_Pos[2]);
//  printf("la position desire en quaternion  est: %lf %lf %lf %lf \n",tmp_q_D[0],tmp_q_D[1],tmp_q_D[2],tmp_q_D[3] );
//      // printf("la position estime en quaternion  est: %lf %lf %lf %lf \n",Quat[0],Quat[1],Quat[2],Quat[3] );
     
 //quat2eul(Quat, PosI);

      quatprod( tmp_q_D,  Quat, quat_err );//produit de quaternion entre la ref et lestimer
//      //  printf("l erreur de  position  en quaternion  est: %lf %lf %lf %lf \n",quat_err[0],quat_err[1],quat_err[2],quat_err[3] );
//    //  printf("la vitesse estime est: %lf %lf %lf \n",gyro[0],gyro[1],gyro[2]);
 
   loi_icra(gyro, quat_err, couple_ref);//calcul les couples desire
   
//  printf("les couples sont : %lf %lf %lf \n",couple_ref[0], couple_ref[1],couple_ref[2]);
   vitesse_desire(couple_ref, s_ref);//calcul les 4 vitesses desirees

//   // printf("les quatres vitesses desirees sont: %lf %lf %lf %lf \n",s_ref[0],s_ref[1],s_ref[2],s_ref[3] );

//    VO[0] = motor_control1( VmotI[0], s_ref[0]);
//    VO[1] = motor_control1( VmotI[1], s_ref[1]);
//    VO[2] = motor_control1( VmotI[2], s_ref[2]);
//    VO[3] = motor_control1( VmotI[3], s_ref[3]);


     for(i=0; i<4;i++) { // speed control in now in modelx4
       VO[i] = UO[i] = s_ref[i];
     }

 quat2eul(Quat,rtl);
// printf("PosI %lf %lf %lf \n", PosI[0],PosI[1],PosI[2]);
 //printf("VO %lf %lf %lf %lf \n", VO[0], VO[1], VO[2], VO[3]);
    //-----------------------------------------------------------------------------------------------
 cpt++;
 //printf("CTRL  %lld \n",(orcGetExecTime(Mt_ptr->get_MtId()))/cpt);
  }
  //printf("Motor_RecoverI = %d\n", Motor_RecoverI);

  // if(Motor_RecoverI == 1) printf("MotorValue %d \n",  pRT->Get_MotorState());
//   else printf("Motor_Fail not received \n");

  if (Time >= 10.0){ 
    printf("QNORM %lf \n", tmp);
    printf("la position estime en quaternion  est: %lf %lf %lf %lf \n",Quat[0],Quat[1],Quat[2],Quat[3] );
  PanicE = SET_EVENT;
 
  }

} 
void orc_Mod_GPS_CtrlT1::end()
{
// Orccad Version: 3.2
// Module :  GPS_CtrlT1
// End File 
// 
// Module Ports :
// 	input INTEGER Motor_RecoverI
// 	output EVENT PanicE
// 	output EVENT X4_ReadyE
// 	input DOUBLE D_PosI[6]
// 	input DOUBLE VrelI[3]
// 	input DOUBLE VmotI[4]
// 	output DOUBLE VO[4]
// 	input DOUBLE PosI[6]
// 	input DOUBLE QI[7]
// 
// 
// Date of creation : Thu Oct  2 09:28:52 2008

  return;
} 
// End class orc_Mod_GPS_CtrlT1