#include "Sail.h"
#include "ServoMotor.h"
#include "IncrementalEncoder.h"

TIM_TypeDef * MOTOR_TIMER = TIM4;
const int MOTOR_CHANNEL = LL_TIM_CHANNEL_CH3;
GPIO_TypeDef * MOTOR_GPIO = GPIOB;
const int MOTOR_PIN = LL_GPIO_PIN_8;

TIM_TypeDef * ENCODER_TIMER = TIM3;
GPIO_TypeDef * ENCODER_GPIO = GPIOA;
const int ENCODER_PIN = LL_GPIO_PIN_5;

const int RESET_ANGLE = 90;

int Sail_isEmergencyState = 0;

void Sail_conf()
{
	ServoMotor_conf(MOTOR_TIMER, MOTOR_CHANNEL, MOTOR_GPIO, MOTOR_PIN);
	IncrementalEncoder_conf(ENCODER_TIMER, ENCODER_GPIO, ENCODER_PIN);
}

void Sail_start()
{
	Sail_isEmergencyState = 0;
	ServoMotor_start(MOTOR_TIMER);
	IncrementalEncoder_start(ENCODER_TIMER);
}

int windToSailAngle(int windAngle)
{
	if (windAngle < 180)
		return 90 * (windAngle - 45) / 135;
	else
		return 90 * ((360 - windAngle) - 45) / 135;
}

int Sail_getSailAngle(void) 
{
	return ServoMotor_getAngle(MOTOR_TIMER, MOTOR_CHANNEL);
}

void Sail_setEmergency(int state)
{
	Sail_isEmergencyState = state;
	if (Sail_isEmergencyState)
		ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, RESET_ANGLE);
}

int Sail_getEmergencyState()
{
	return Sail_isEmergencyState;
}

void Sail_background()
{
	if (Sail_isEmergencyState)
		return;
	
	const int windAngle = IncrementalEncoder_getAngle(ENCODER_TIMER);
	
	if (windAngle < 45 || windAngle > 315) 
			ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, 0);
	else
			ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, windToSailAngle(windAngle));
}