#include "Exec/mt.h"
#include "Exec/rt.h"
#include "Exec/clockgen.h"
#include "Exec/datadbg.h"

#include "rts.h"
#include "interface.h"
#include "mt_X4_Diags_T1_2.h"

void orc_Mt_X4_Diags_T1_2::InitTrace()
{

}

void orc_Mt_X4_Diags_T1_2::Param()
{

} 
void orc_Mt_X4_Diags_T1_2::ReParam()
{

}
int orc_Mt_X4_Diags_T1_2::SetT1Exceptions()
{
  //pRT->PutSensor_FailE(pRT->aut_output_Sensor_FailE);
  pRT->/*aut_output_*/Sensor_FailE = 0;
	 return OK;
};
// int orc_Mt_X4_Diags_T1_2::ResetT1Exceptions()
// {
//   //pRT->PutSensor_FailE(pRT->aut_output_Sensor_FailE);
//        pRT->aut_output_Sensor_FailE = 0;
// 	 return OK;
// };
void fct_X4_Diags_T1_2(orc_Mt_X4_Diags_T1_2* mt)
{
  orc_RT_X4_Diags_T1 *pRT=(orc_RT_X4_Diags_T1 *) (mt->RT);
	 int Current_Iteration; 
	 int status_rt, access_drv, do_reinit = FALSE;
	 //init all index 
	 int _mi_QuaternionT1_28 = mt->QuaternionT1_28.getIndexClk();
	 while (!mt->CondKill) {
	       if (mt->SemaphoreGive(Create_ok)==ERROR) return;
	       if (mt->SemaphoreTake(Init,WAIT_FOREVER)==ERROR) return;
	       access_drv = (mt->RT->GetPhR())->GetState();
	       // Begin Param
	       mt->Param();

	       mt->EndReparam();
	       // End Param
	       // Begin Init
	               if (access_drv == PHR_ACCESS) {
	                   mt->X4_GPS_PhR->GET_AccO();
	                   mt->X4_GPS_PhR->GET_MagO();
	                   mt->X4_GPS_PhR->GET_GyroO();
	                   mt->X4_GPS_PhR->GET_VrelO();
	              }
		       mt->QuaternionT1_28.init(mt->X4_GPS_PhR->AccO, mt->X4_GPS_PhR->MagO, mt->X4_GPS_PhR->GyroO, mt->X4_GPS_PhR->VrelO, pRT->/*aut_output_*/Sensor_FailE);
	       // End Init
	       //End write Port-Event
	       if (mt->SemaphoreGive(Init_ok)==ERROR) return;
	       if (mt->SemaphoreTake(Compute,WAIT_FOREVER)==ERROR) return;
	       Current_Iteration=0;
	       for (;;) {
	            if (mt->SemaphoreTake(Synchro,WAIT_FOREVER)==ERROR) return;
	            if (mt->CondEnd) break;
	            status_rt =  mt->RT->GetState();
	            // Take Unlock Parameters
	            if (mt->IsReparam()) {
	                mt->ReParam();
	                mt->EndReparam();
	            }
	            // End re-Parameters
	            if (mt->Filter_Iteration == Current_Iteration)
	                mt->SemaphoreGive(Filter_ok);
	                Current_Iteration++;
	                access_drv = (mt->RT->GetPhR())->GetState();
	                //Begin computation
	                if ((access_drv == PHR_ACCESS) &&
	                   ((status_rt==ACTIVE)||(status_rt==TRANSITE)) &&
	                   (do_reinit == FALSE)) {
	                   mt->X4_GPS_PhR->reinit();
	                   do_reinit = TRUE;
	                }
	               // Compute QuaternionT1_28
	               if (access_drv == PHR_ACCESS) {
	                   mt->X4_GPS_PhR->GET_AccO();
	                   mt->X4_GPS_PhR->GET_MagO();
	                   mt->X4_GPS_PhR->GET_GyroO();
	                   mt->X4_GPS_PhR->GET_VrelO();
	              }
	               if ((mt->Clock)->IsClkActive(_mi_QuaternionT1_28) == 0) {
			 mt->QuaternionT1_28.compute(mt->X4_GPS_PhR->AccO, mt->X4_GPS_PhR->MagO, mt->X4_GPS_PhR->GyroO, mt->X4_GPS_PhR->VrelO, pRT->/*aut_output_*/Sensor_FailE);
	   pRT->WriteLink_1(mt->QuaternionT1_28.QO);
	   mt->SetT1Exceptions();
	               } 

	               status_rt =  mt->RT->GetState();
	               mt->MTEndCompute();
	        }
	       // Begin end
	      mt->QuaternionT1_28.end();
 	      // End end
 	      if (mt->SemaphoreGive(End_ok)==ERROR) return;
 	      if (mt->SemaphoreTake(Continue,WAIT_FOREVER)==ERROR) return;
 	 }
 	 if (mt->SemaphoreTake(Kill,WAIT_FOREVER)==ERROR) return; 
 	 mt->CondKill = mt->CondEnd = mt->RequestParam = FALSE; 

 	 if (mt->SemaphoreGive(Kill_ok)==ERROR) return; 

}