#include "Cpu.h"
#include "Events.h"
#include "Park.h"

extern int d1;									/* Q15 */
extern int q1;									/* Q15 */
extern int alfa;								/* Q15 */
extern int beta;								/* Q15 */
extern int theta;								/* from 0 to 360 */
extern int sina[180];							/* Q15 */								
extern int cosa[180];							/* Q15 */
extern bool negative;	

void Park(void)
{
	long int temp32;
	long int temp33;
	
	if(theta>=180)
 	{
  		theta=theta-180;
  		negative=1;	
 	}

	temp32=(L_mult(alfa,cosa[theta]))>>16;		/* Q15 */
	temp33=(L_mult(beta,sina[theta]))>>16;		/* Q15 */
	d1=(int)(temp32+temp33);
	temp32=(L_mult(-alfa,sina[theta]))>>16;		/* Q15 */
	temp33=(L_mult(beta,cosa[theta]))>>16;		/* Q15 */
	q1=(int)(temp32+temp33);
	
	if(negative==1)
	{
		d1=-d1;
		q1=-q1;
		negative=0;
		theta=theta+180;
	}
	
}