/*
 * 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[SINE_STEPS];
void ap_init(void)
{
	uint16_t i;
	for(i=0;i<SINE_STEPS;i++)
	{
	  sintab[i]=sin(i/(SINE_STEPS*2.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+= SINE_STEPS * 4;
	}
while(ang>=SINE_STEPS * 4)
	{
		ang-= SINE_STEPS * 4;
	}

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

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