#include "motor.h"
#include "gpio.h"
#include "tim.h"

#define MAX_SPEED 100
#define MOTOR_L 1
#define MOTOR_R 2
#define MOTOR_TIM (&htim4)
#define MOTOR_L_CHAN TIM_CHANNEL_2
#define MOTOR_R_CHAN TIM_CHANNEL_1

int8_t preProcess(int8_t duty, uint8_t motor);

void MOTOR_Start(void)
{
	HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_L_CHAN);
	HAL_TIM_PWM_Start(MOTOR_TIM, MOTOR_R_CHAN);
}

void MOTOR_Slip(void)
{
	HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, MAX_SPEED);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, MAX_SPEED);
}

void MOTOR_Stop(void)
{
	HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin | L_BACKWARD_Pin | R_FORWARD_Pin | R_BACKWARD_Pin, GPIO_PIN_RESET);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, 0);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, 0);
}

void MOTOR_SetDuty(int8_t leftDuty, int8_t rightDuty)
{
	leftDuty = preProcess(leftDuty, MOTOR_L);
	rightDuty = preProcess(rightDuty, MOTOR_R);

	uint16_t leftSpeed = (uint32_t)MAX_SPEED * leftDuty / 100;
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, leftSpeed);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, leftSpeed);

	uint16_t rightSpeed = (uint32_t)MAX_SPEED * rightDuty / 100;
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_L_CHAN, rightSpeed);
	__HAL_TIM_SetCompare(MOTOR_TIM, MOTOR_R_CHAN, rightSpeed);
}

int8_t preProcess(int8_t duty, uint8_t motor)
{
	if (duty > 100)
		duty = 100;
	else if (duty < -100)
		duty = -100;

	if (duty >= 0)
	{
		if (motor & MOTOR_L)
		{
			HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_RESET);
		}
		if (motor & MOTOR_R)
		{
			HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_RESET);
		}
	}
	else
	{
		if (motor & MOTOR_L)
		{
			HAL_GPIO_WritePin(L_FORWARD_GPIO_Port, L_FORWARD_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(L_BACKWARD_GPIO_Port, L_BACKWARD_Pin, GPIO_PIN_SET);
		}
		if (motor & MOTOR_R)
		{
			HAL_GPIO_WritePin(R_FORWARD_GPIO_Port, R_FORWARD_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(R_BACKWARD_GPIO_Port, R_BACKWARD_Pin, GPIO_PIN_SET);
		}
	}
	return duty;
}