#include "rts.h"

orc_RT_X4_Diags_T1::orc_RT_X4_Diags_T1(char *name,ProcedureRobot* prr,PhR *phr,char *cfgfile)	 :RobotTask(name,prr,phr,10000),
									       mt_X4_Diags_T1_0(this, (char *)"mt_X4_MT_0",0,0.005,CLOCKSOURCEPERIODIC,35,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_1(this, (char *)"mt_X4_MT_1",0,0.005,CLOCKSOURCEPERIODIC,25,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_2(this, (char *)"mt_X4_MT_2",0,0.005,CLOCKSOURCEPERIODIC,10,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_3(this, (char *)"mt_X4_MT_3",0,0.005,CLOCKSOURCEPERIODIC,7,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_4(this, (char *)"mt_X4_MT_4",0,0.005,CLOCKSOURCEPERIODIC,5,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_5(this, (char *)"mt_X4_MT_5",0,0.005,CLOCKSOURCEPERIODIC,4,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_6(this, (char *)"mt_X4_MT_6",0,0.005,CLOCKSOURCEPERIODIC,3,0,SKIP_CONSTRAINT),
mt_X4_Diags_T1_7(this, (char *)"mt_X4_MT_7",0,0.005,CLOCKSOURCEPERIODIC,2,0,SKIP_CONSTRAINT)
{
	 Param = new Parameters(MAX_RT_PARAM);
	 strcpy(cfgFile,cfgfile); 
	 Parametrized = NON_PARAMETRIZED; 
	 insert((ModuleTask *) &mt_X4_Diags_T1_0);
	 insert((ModuleTask *) &mt_X4_Diags_T1_1);
	 insert((ModuleTask *) &mt_X4_Diags_T1_2);
	 insert((ModuleTask *) &mt_X4_Diags_T1_3);
	 insert((ModuleTask *) &mt_X4_Diags_T1_4);
	 insert((ModuleTask *) &mt_X4_Diags_T1_5);
	 insert((ModuleTask *) &mt_X4_Diags_T1_6);
	 insert((ModuleTask *) &mt_X4_Diags_T1_7);
	 
	 // Links construction
	 sharedMemoryOnLink1 = new SingleReaderACM<double>(7);
	 sharedMemoryOnLink2 = new SingleReaderACM<double>(6);
	 sharedMemoryOnLink3 = new SingleReaderACM<double>(6);
	 sharedMemoryOnLink4 = new SingleReaderACM<double>(4);

	 printf("pRT depuis rts %p \n", this);
}

orc_RT_X4_Diags_T1::~orc_RT_X4_Diags_T1()
{ 
	 delete Param;

	 delete sharedMemoryOnLink1;
	 delete sharedMemoryOnLink2;
	 delete sharedMemoryOnLink3;
	 delete sharedMemoryOnLink4;
	 
}

int orc_RT_X4_Diags_T1::SetParam()
{
	 // Set RT Parameters by file
	 return(OK);
}

// int orc_RT_X4_Diags_T1::SetT1Exceptions()
// { 	 
//   PutSensor_FailE(aut_output_Sensor_FailE);
// 	 aut_output_Sensor_FailE = 0; 
//   PutMotor_FailE(aut_output_Motor_FailE);
// 	 aut_output_Motor_FailE = 0;
// 	 return OK;
// }

int orc_RT_X4_Diags_T1::TestTimeout()
{ 
	 return OK; 
}

int orc_RT_X4_Diags_T1::ReadLink_1(double *port) 
{
 return sharedMemoryOnLink1->read(port);
}

int orc_RT_X4_Diags_T1::ReadLink_2(double *port) 
{ 
  return sharedMemoryOnLink2->read(port); 
}


int orc_RT_X4_Diags_T1::ReadLink_3(double *port) 
{ 
  return sharedMemoryOnLink3->read(port); 
}

int orc_RT_X4_Diags_T1::ReadLink_4(double *port) 
{ 
  return sharedMemoryOnLink4->read(port); 
}


void orc_RT_X4_Diags_T1::WriteLink_1(double *port) 
{
  sharedMemoryOnLink1->write(port);
 }

void orc_RT_X4_Diags_T1::WriteLink_2(double *port) 
{
  sharedMemoryOnLink2->write(port);
}

void orc_RT_X4_Diags_T1::WriteLink_3(double *port) 
{ 
  sharedMemoryOnLink3->write(port);
}

void orc_RT_X4_Diags_T1::WriteLink_4(double *port) 
{ 
  sharedMemoryOnLink4->write(port);
}

void orc_RT_X4_Diags_T1::PutSensor_FailE(int _Sensor_FailE) { 
Sensor_FailE = _Sensor_FailE; 
} 

void orc_RT_X4_Diags_T1::PutMotor_FailE(int _Motor_FailE) { 
Motor_FailE = _Motor_FailE; 
} 

int orc_RT_X4_Diags_T1::Get_SensorState(){
  return SensorState;
}
void orc_RT_X4_Diags_T1::Put_SensorState(int T1value){
  SensorState = T1value;
  return;
}

int orc_RT_X4_Diags_T1::Get_MotorState(){
  return MotorState;
}
void orc_RT_X4_Diags_T1::Put_MotorState(int T1value){
  MotorState = T1value;
  return;
}