/***********************************************************************//**
 * @file		RobotControl.c
 * @brief		Contains tasks for robot control
 * @version		1.0
 * @date		21. April. 2014
 * @author		Michal Semrad
 **************************************************************************/

#include "RobotControl.h"

/********************************************************************//*
 * @brief		Creation of MainControlTask, AutomatModeTask and SW timer for speed control
 * @param[in]	none
 * @return 		None
 **********************************************************************/

void KinematicInit(){

	xyzabg[0]=200;
	xyzabg[1]=0;
	xyzabg[2]=100;
	xyzabg[3]=90;
	xyzabg[4]=0;
	xyzabg[5]=20;

	/*******************************************PWM init************************************************/
	int temp;

	for (temp = 0; temp < 6; temp++)
	{
		PWMvalue_actual[temp]=37500;//neutral middle
		speedp[temp]=230;
	}
	/***************************************************************************************************/

	SetExecutType(EXCYCLE);
	SetPWMmode(0);
	SetSpeed(30);


	xTaskCreate(MainConrolTask, (signed portCHAR *) "MAIN", 128, NULL, 3, NULL);
	xTaskCreate(AutomatModeTask,(signed portCHAR *) "AUTO", 128, NULL, 2, &xHandleAutomat);

	/*******************************************TIMER INIT********************************************************/
	Timer20ms=xTimerCreate((const signed char *)"TSPE",20,pdTRUE,(void*)1, vSpeedControlTimer );
	if(Timer20ms==NULL){
		LogMessage("Timer was not created",21);
	}
	else{
		if(xTimerStart(Timer20ms,0)!=pdPASS){
			LogMessage("Timer was not started",21);
		}
	}
	/**********************************************************************************************/



}

/********************************************************************//*
 * @brief		Main control task, reacts on received packets
 * @param[in]	none
 * @return 		None
 **********************************************************************/

 void MainConrolTask(){

		InverseKinematic();
		SetLedStatus(LEDOFF);
		SlipSend(ID_ANGLES,(char*)anglesfrac,2*6);
		SlipSend(ID_XYZABG,(char*)xyzabg,2*6);

 	while(1){


 		if( xSemaphoreTake( xSemaphoreCOM, portMAX_DELAY) != pdTRUE ) {//waiting for received packet
 			LogMessage("K xSemaphoreCOM take failed",27);
 		}

 		if(GetKinMode()==MODEXYZ){

 			if( xSemaphoreTake( xAngXyzMutex, 100 ) != pdTRUE ) {
 				LogMessage("K xAngXyzMutex take failed",26);
 			}
 			if(InverseKinematic()!=1){
 		 		if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
 		 			 LogMessage("K xAngXyzMutex give failed",26);
 		 		}
 		 		SetLedStatus(LEDBLINKING); //blinking error
 				LogMessage("Unreachable point",17);
 			}
 			else{
 				SetPWMmode(1);
 				SlipSend(ID_ANGLES,(char*)anglesfrac,2*6);
 		 		if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
 		 			 LogMessage("K xAngXyzMutex give failed",26);
 		 		}
 			}
 		}
 		if(GetKinMode()==MODEANGLES){

 	 		 if( xSemaphoreTake( xAngXyzMutex, 100 ) != pdTRUE ) {
 	 			 LogMessage("K xAngXyzMutex take failed",26);
 	 		 }
 			AnglesToPWM();
 			DirectKinematic();

 			SetPWMmode(2);

 			SlipSend(ID_XYZABG,(char*)xyzabg,2*6);

 	 		if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
 	 			 LogMessage("K xAngXyzMutex give failed",26);
 	 		}
 		}
 		if (GetKinMode()==MODEALPHAGRIP){

 	 		 if( xSemaphoreTake( xAngXyzMutex, 100 ) != pdTRUE ) {
 	 			 LogMessage("K xAngXyzMutex take failed",26);
 	 		 }
 			anglesfrac[4]=DEG2FRAC(xyzabg[3]) ;
 			anglesfrac[5]=xyzabg[5];

 			PWMvalue[4]=15500+2.6245*anglesfrac[4];
 			PWMvalue[5]=-973*anglesfrac[5]+54158;

			SetPWMmode(2);


 	 		if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
 	 			 LogMessage("K xAngXyzMutex give failed",26);
 	 		}
 		}

 		if(GetKinMode()==MODESTARTPOS){

 			GoToStartPosition();
 		}

 		if(GetKinMode()==MODEAUT){

 			if( xSemaphoreGive( xSemaphoreStartAut ) != pdTRUE ){
 				LogMessage("xSemaphoreStartAut give failed",30);
 			}
 		}
 	}
 }
 /********************************************************************//*
  * @brief		Automatic mode of execution
  * @param[in]	none
  * @return 	None
  **********************************************************************/
 void AutomatModeTask(){

	 static int count;

	 while(1){

		 if( xSemaphoreTake( xSemaphoreStartAut, portMAX_DELAY) != pdTRUE ) {   // waiting until MainControlTask gives semaphore
			 LogMessage("xSemaphoreStartAut take failed",30);
		 }
		 count=0;

		 while(1){

			 if( xSemaphoreTake( xAngXyzMutex, 100 ) != pdTRUE ) {
				 LogMessage("A xAngXyzMutex take failed",26);
			 }

			 memcpy(xyzabg,&instructions[count].xyzabg,12);

			 if(InverseKinematic()!=1){

				 if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
					 LogMessage("A xAngXyzMutex give failed",26);
				 }

				 SetLedStatus(LEDBLINKING);  //blinking error
				 LogMessage("Unreachable point",17);

			 }
			 else{
				 SetPWMmode(1);

				 SetSpeed(instructions[count].speed);

				 ComputeTimeToWait();

				 if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
					 LogMessage("A xAngXyzMutex give failed",26);
				 }

				 vTaskDelay(GetDelay()+(int)instructions[count].delay);
			 }

			 if(GetKinMode() != MODEAUT)break;						//check, if automatic mode is still active
			 count++;
			 if (GetInstCount()<=count){							//any instruction exists?
				 if (executingType==EXONCE)break;					// break if ExecutingOnce is active
				 count=0;
			 }
		 }
	 }
 }
 /********************************************************************//*
  * @brief		speed control
  * @param[in]	none
  * @return 	None
  **********************************************************************/

 void vSpeedControlTimer( xTimerHandle Timer20ms )
  {
 	int i,e[6],eMAX;

 	if( xSemaphoreTake( xPWMMutex,  2 ) != pdTRUE ) {	//has to be taken in 2 ms otherwise step out the deadline
 		LogMessage("T PWMmutex was not taken",24);
 	}

 	if(GetPWMmode()==1){								//new PWM values. Have to compute new speeds.

 		for(i=0;i<6;i++){
 			e[i]=PWMvalue[i]-PWMvalue_actual[i];
 			if (e[i]<0) e[i]*=-1;						//absolute value
 		}

 		eMAX=e[0];
 		for(i=1;i<6;i++){								//find maximum
 			if(e[i]>eMAX)eMAX=e[i];
 		}

 		for(i=0;i<6;i++){
 			speedp[i]=e[i]*GetSpeed()/eMAX;				//compute particular speeds
 		}
 		SetPWMmode(0);
 	}

 	if (GetPWMmode()==2) {								//Just one joint should move, so have to reset particular speeds
 		for(i=0;i<6;i++)
 			speedp[i]=GetSpeed();

 		SetPWMmode(0);

 	}

 	for(i=0;i<6;i++){									//compute new PWM values
 		e[i]=PWMvalue[i]-(int)PWMvalue_actual[i];

 		if(e[i]>0)
 			PWMvalue_actual[i]+=speedp[i];
 		if(e[i]<0)
 			PWMvalue_actual[i]-=speedp[i];
 		if((e[i]<2*speedp[i])&&(e[i]>-2*speedp[i]))
 			PWMvalue_actual[i]=PWMvalue[i];

 		if (PWMvalue_actual[i]>MAX_PWM)
 			PWMvalue_actual[i]=MAX_PWM;

 		if (PWMvalue_actual[i]<MIN_PWM)
 			PWMvalue_actual[i]=MIN_PWM;
 	}
 	PWMUpdate(PWMvalue_actual);							//update position

 	if( xSemaphoreGive( xPWMMutex ) != pdTRUE ){
 		LogMessage("T PWMmutex was not given",24);
 	}

  }

 /********************************************************************//*
  * @brief		moves to the start position
  * @param[in]	none
  * @return 	None
  **********************************************************************/
 void GoToStartPosition(void){

	 if( xSemaphoreTake( xAngXyzMutex, 100 ) != pdTRUE ) {
		 LogMessage("K xAngXyzMutex take failed",26);
	 }
	 xyzabg[0]=200;
	 xyzabg[1]=0;
	 xyzabg[2]=100;
	 xyzabg[3]=90;
	 xyzabg[4]=0;
	 xyzabg[5]=20;

	 InverseKinematic(xyzabg,PWMvalue);
	 SetPWMmode(1);
	 SetSpeed(30);

	 xTimerStart(Timer20ms,0);
	 vTaskResume(xHandleAutomat);

	 SlipSend(ID_ANGLES,(char*)anglesfrac,2*6);
	 SlipSend(ID_XYZABG,(char*)xyzabg,2*6);

	 if( xSemaphoreGive( xAngXyzMutex ) != pdTRUE ){
		 LogMessage("K xAngXyzMutex give failed",26);
	 }

	 SetKinMode(MODEXYZ);
 }

 /********************************************************************//*
   * @brief		compute time to finish current movement
   * @param[in]	none
   * @return 	None
   **********************************************************************/
 void ComputeTimeToWait(){

	 int i,e[6],eMAX;

	// speed_=speed_*6+50;

	 if( xSemaphoreTake( xPWMMutex,  2 ) != pdTRUE ) {//has to be taken in 2 ms otherwise step out the deadline
	  		LogMessage("A PWMmutex was not taken",24);
	  	}

	 for(i=0;i<6;i++){
		 e[i]=PWMvalue[i]-PWMvalue_actual[i];
		 if (e[i]<0) e[i]*=-1;//absolute value
	 }

	 if( xSemaphoreGive( xPWMMutex ) != pdTRUE ){
	  		LogMessage("A PWMmutex was not given",24);
	  	}

	 eMAX=e[0];
	 for(i=1;i<6;i++){			//find maximum
		 if(e[i]>eMAX)eMAX=e[i];
	 }

	 SetDelay(20*eMAX/GetSpeed());
 }
