/*
 * ap_math.c

 *
 *  Created on: 31 Jan 2016
 *      Author: Mike
 */
#include <math.h>
#include "stm32l1xx_hal.h"
#include "ap_math.h"
/* this is an approximate maths library where all the answers are accurate enough */
static uint8_t sintab[90];
void ap_init(void)
{
	uint8_t i;
	for(i=0;i<90;i++)
	{
	  sintab[i]=sin(i/180.0*3.14159)*AP_K;
	}
}

/* returns 255 * sin(ang) where ang is in degrees */
int ap_sin(int ang)
{
	/* wrap into range */
	while(ang<0)
	{
		ang+= 360;
	}
	while(ang>=360)
	{
		ang-= 360;
	}

	if(ang>=0 && ang<90)
	{
		return(sintab[ang]);
	}
	else
	if(ang==90)
	{
		return AP_K;
	}
	else
	if(ang>90 && ang<=180)
	{
		return(sintab[180-ang]);
	}
	else
	if(ang==270)
	{
	    return -AP_K;
	}
	if(ang>180 && ang< 270)
	{
		return(-sintab[ang-180]);
	}
	else
    /* ang > 270 and ang < 360 */
	{
	return(-sintab[360-ang]);
	}
}

/* returns 255 * cos(ang) where ang is in degrees */
int ap_cos(int ang)
{
	return ap_sin(90-ang);
}