#include "motor_controller.h"
#include <math.h>
#include "putil.h"
#include "stm32l4xx_hal_tim.h"

static TIM_HandleTypeDef* _motor_timer = NULL;
void mc_init(TIM_HandleTypeDef* tim)
{
	_motor_timer = tim;
}

// degrees is anything from 0-359
// speed is anything from 0-99
void mc_service(uint16_t degrees, uint8_t speed)
{
	float right_pwm = cos(((float)degrees*(3.1415 / 180.0f)));
	float left_pwm = right_pwm; 
	PDEBUG("| DEGREES | COS    | SIN    | \n");
	PDEBUG("| %7d | %-5.3f | %-5.3f | \n",
		(int)degrees,
		right_pwm,
		right_pwm);
	
	HAL_GPIO_WritePin(m1_dir_GPIO_Port, m1_dir_Pin, right_pwm >= 0 ? 0 : 1);
	HAL_GPIO_WritePin(m2_dir_GPIO_Port, m2_dir_Pin, left_pwm >= 0 ? 0 : 1);
	setPWM(_motor_timer, TIM_CHANNEL_2, abs((int)(right_pwm * 100.0)));
	setPWM(_motor_timer, TIM_CHANNEL_4, abs((int)(left_pwm * 100.0)));
}