/***********************************************************************//**
 * @file		kinematic.c
 * @brief		Contains functions for inverse kinematics calculations
 * @version		1.0
 * @date		26. February. 2014
 * @author		Michal Semrad
 **************************************************************************/

#include "kinematic.h"

 /********************************************************************//*
   * @brief		Compute inverse kinematics and updates PWM values
   * @param[in]	none
   * @return 	1 if it is OK
   * 			0 if not
   **********************************************************************/

int InverseKinematic()
{
	int x3=0;
	int y3=0;
	int z3=0;
	int l=0;
	int angle_q0;
	int angle_q1;
	int angle_q2;
	int angle_gamma;
	static int angle_q0_previous;

	ConvertCoordinatesKto3(&x3,&y3,&z3,&angle_q0,&angle_q0_previous);

	if (CheckCoordinates(x3,y3,z3,&l)!=1)
		return 0;

	if(q0(angle_q0)!=1)
		return 0;

	if(q2(l,&angle_gamma,&angle_q2)!=1)		//q2 has to be compute before q1, because q1 needs angle q2
		return 0;

	if(q1(x3,y3,z3,l,angle_gamma,&angle_q1)!=1)
		return 0;

	if(q3(xyzabg[4],angle_q1,angle_q2)!=1)
		return 0;

	if(q4(xyzabg[3])!=1)
		return 0;

	if(q5(xyzabg[5])!=1)
		return 0;

	AnglesToPWM();//PWM values have been modified, so timer thread has access to new values.

	return 1;
}

/********************************************************************//*
  * @brief		Compute coordinates of q3(x3,y3,z3) and angle q0
  * @param[in]	pointer to x3
  * @param[in]	pointer to y3
  * @param[in]	pointer to z3
  * @param[in]	pointer to q0
  * @param[in]	pointer to q0_previous
  * @return 	None
  **********************************************************************/
void ConvertCoordinatesKto3(	int* x3,
								int* y3,
								int* z3,
								int* angle_q0,
								int* angle_q0_previous){

	int cosbeta,cosq0,angle_beta,sinq0;
	float aux;

	if(xyzabg[0]==0 && xyzabg[1]==0){     			//singular point
		*angle_q0=*angle_q0_previous;
	}
	else{
		*angle_q0=atan2Lookup(xyzabg[0],xyzabg[1]);
		*angle_q0_previous=*angle_q0;
	}

	angle_beta=DEG2FRAC(xyzabg[4]);

	cosbeta=cos_q15(angle_beta);
	cosq0=cos_q15(*angle_q0);
	sinq0=arm_sin_q15(*angle_q0);

	aux=(LENGTHARM4*cosbeta*1.0/0x8000)*(cosq0*1.0/0x8000)+(LENGTHARM1*cosq0*1.0/0x8000);//LENGTHARM4*cos(beta)*cos(q0)+LENGTHARM1*cos(q0)
    *x3=xyzabg[0]-aux;
	if (xyzabg[0]<0) *x3=xyzabg[0]+aux;

    aux=(LENGTHARM4*cosbeta*1.0/0x8000)*(sinq0*1.0/0x8000)+(LENGTHARM1*sinq0*1.0/0x8000);//LENGTHARM4*cos(beta)*sin(q0)+LENGTHARM1*sin(q0)
    *y3=xyzabg[1]-aux;
     if (xyzabg[0]<0) *y3=xyzabg[1]+aux;

    *z3=xyzabg[2]-LENGTHARM0+(LENGTHARM4*arm_sin_q15(angle_beta))/0x8000;////z3=zk-LENGTHARM0+LENGTHARM4*sin(beta)

}


/********************************************************************//*
  * @brief		Check if the point is reachable and also compute l for next usage
  * @param[in]	x3
  * @param[in]	y3
  * @param[in]	z3
  * @param[in]	pointer to l
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int CheckCoordinates(int x3,int y3,int z3, int* l){

	float aux;

	arm_sqrt_f32(x3*x3+y3*y3+z3*z3,&aux);

	*l=(int)aux;

	if(*l>LENGTHARM2+LENGTHARM3){
		LogMessage("l>",2);
		return 0;
	}
	if(*l<60){
		LogMessage("l<",2);
		return 0;
	}
	return 1;
}

/********************************************************************//*
  * @brief		store angle q0 to global array of all q angles
  * @param[in]	angle q0
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int q0(int angle_q0){

	anglesfrac[0]=angle_q0;
	return 1;

}
/********************************************************************//*
  * @brief		compute and store angle q1
  * @param[in]	x3
  * @param[in]	y3
  * @param[in]	z3
  * @param[in]	l
  * @param[in]	angle_gamma
  * @param[in]	pointer to angle_q1
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/

int q1(int x3,int y3,int z3,int l,int angle_gamma,int* angle_q1){

	float in;
	in=arm_sin_q15(angle_gamma)*1.0/0x7FFF;//scale sinus down to have it less than one (true value)
    in=in*(LENGTHARM3*1.0/l);

	float aux;
	arm_sqrt_f32((1-in*in),&aux);


	if(l>LENGTHCRITICAL)//!!!!!!!!!!!!!!!!!!LENGTHCRITICAL=sqrt(l2^2-l1^2)!!!!!!!!!!!!!!!!!!!!!!!!!!!
		in=atan2Lookup(0xFFFF*aux,0xFFFF*in);	//in is from 0 to 90 deg
	else
		in=atan2Lookup(-0xFFFF*aux,0xFFFF*in); //in is from 91 to cca 120 deg


	float den=x3*x3+y3*y3;
	arm_sqrt_f32(x3*x3+y3*y3,&den);

	while((den<1000) && (z3 <1000)){
		den*=10;
		z3*=10;
	}

	 uint16_t omega=(atan2Lookup(den,z3));//tady zmenit z

	 omega=0x8000-omega;	//reverse orientation

	 if(x3<0 || (xyzabg[1]<=0 && y3>0)|| (xyzabg[1]>=0 && y3<0)){
		 omega=0x6000-(omega-0x6000);
	 }


	 *angle_q1=omega-in;
	 if (*angle_q1<0)*angle_q1+=0x8000;


	 if((*angle_q1>=0x4000)&&(*angle_q1<0x8000)){		//check if it is in servo range
		 anglesfrac[1]=*angle_q1;
		 return 1;
	 }
	 else{
		 LogMessage("q1",2);
		 return -1;
	 }

}


/********************************************************************//*
  * @brief		compute and store angle q2
  * @param[in]	l
  * @param[in]	pointer to angle_gamma
  * @param[in]	pointer to angle_q2
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int q2(int l,int* angle_gamma,int* angle_q2){

	float a = (LENGTHARM2*LENGTHARM2+LENGTHARM3*LENGTHARM3-l*l*1.0)/(2*LENGTHARM2*LENGTHARM3);

	float num;
	arm_sqrt_f32(1-a*a,&num);

	//variable a and num are always less than 1 so we have to scale them up before we pass them to Atangens like integer values
	num*=0xFFFF;
	a*=0xFFFF;

	*angle_gamma=atan2Lookup(a,num);

	*angle_q2=0x4000-(*angle_gamma);

	if(*angle_q2>=0 && *angle_q2<DEG2FRAC(161)){//has to be in range 0..161 deg
		anglesfrac[2]=*angle_q2;
		return 1;
	}
	else
		LogMessage("q2",2);
		return-1;
}

/********************************************************************//*
  * @brief		compute and store angle q3
  * @param[in]	angle_bet_deg
  * @param[in]	angle_q1
  * @param[in]	angle_q2
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int q3(int angle_beta_deg,int angle_q1,int angle_q2){

	int q3 = DEG2FRAC(angle_beta_deg)-angle_q1-angle_q2;
	if (q3<0)q3+=0x8000;
	if (q3<0)q3+=0x8000;

	 if (q3>=0x6000){
		 anglesfrac[3]=q3-0x6000;
		 return 1;
	 }
	 else if (q3<=0x2000){
		 anglesfrac[3]=q3+0x2000;
		 return 1;
	 }
	 else {
		LogMessage("q3",2);
		return -1;
	}


}

/********************************************************************//*
  * @brief		compute and store angle q4
  * @param[in]	angle_alpha_deg
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int q4(int angle_alpha_deg){

	anglesfrac[4]=DEG2FRAC(angle_alpha_deg);
	return 1;

}

/********************************************************************//*
  * @brief		compute and store angle q5
  * @param[in]	grip_mm
  * @return 	1 if it is OK
  * 			0 if not
  **********************************************************************/
int q5(int grip_mm){

	if (grip_mm<7)grip_mm=7;
	if (grip_mm>37)grip_mm=37;

	anglesfrac[5]=grip_mm;

	return 1;

}

/********************************************************************//*
  * @brief		compute direct kinematics, using global variables
  * @param[in]	None
  * @return 	None
  **********************************************************************/
 void DirectKinematic(){

	 int x,y,z,beta,q3,q0;
	 float part1,part2;

	 if (anglesfrac[3]<0x2000){
		 q3=anglesfrac[3]+0x6000;
	 }
	 else {
		 q3=anglesfrac[3]-0x2000;
	 }

	 if(anglesfrac[0]>0x2000 && anglesfrac[0]<0x6000){
		 q0 = (anglesfrac[0]+0x4000) % 0x8000;
	 }
	 else{
		 q0 = anglesfrac[0];
	 }

	 part1=cos_q15(q0)*1.0/0x8000;
	 part2=(LENGTHARM3*cos_q15(anglesfrac[1]+anglesfrac[2])+LENGTHARM2*cos_q15(anglesfrac[1])+LENGTHARM4*cos_q15(anglesfrac[1]+anglesfrac[2]+q3))*1.0/0x8000;
	 x=part1*(part2+LENGTHARM1);

	 part1=sin_q15(anglesfrac[0])*1.0/0x8000;
	 y=part1*(part2+LENGTHARM1);

	 z= LENGTHARM0+(-LENGTHARM2*sin_q15(anglesfrac[1])-LENGTHARM3*sin_q15(anglesfrac[1]+anglesfrac[2])-LENGTHARM4*sin_q15(anglesfrac[1]+anglesfrac[2]+q3))*1.0/0x8000;

	 beta= (anglesfrac[1]+anglesfrac[2]+q3)%0x8000;

	 xyzabg[0]=x;
	 xyzabg[1]=y;
	 xyzabg[2]=z;
	 xyzabg[4]=FRAC2DEG(beta);

 }

 /********************************************************************//*
   * @brief		convert anglesfrac to PWM_values
   * @param[in]	None
   * @return 	None
   **********************************************************************/
 void AnglesToPWM(){

	 int q0;

	 if (anglesfrac[0]==0x2000){
		 anglesfrac[0]-=1;
	 }

	 q0=(anglesfrac[0]+0x6000)%0x4000;

	 if( xSemaphoreTake( xPWMMutex, 2 ) != pdTRUE ) {
		 LogMessage("R xPWMMutex take failed",23);
	 }

	 PWMvalue[0]=59000-2.7465*q0;

	 PWMvalue[1]=16500+2.5024*(0x8000-(anglesfrac[1]));

	 PWMvalue[2]=16500+2.5635*anglesfrac[2];

	 PWMvalue[3]=61000-anglesfrac[3]*2.6855;

	 PWMvalue[4]=15500+2.6245*anglesfrac[4];

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

	 if( xSemaphoreGive( xPWMMutex ) != pdTRUE ){
		 LogMessage("R xPWMMutex give failed",23);
	 }

	 SetLedStatus(LEDON);

 }
