#include "Sail.h"

#include "Servo.h"
#include <stdlib.h>
#include <math.h>

#define SAIL_TRANSFER_FACTOR 1.0
#define SAIL_TRANSFER_OFFSET 0

#define ANGLE_DEBUT 45

void SAIL_Init(void)
{
	SERVO_Init();
}

int SAIL_AngleFromGirouette(float girouette_value){
	float vabs = fabs(girouette_value);

	if(vabs < ANGLE_DEBUT)
	{
		return 0;
	}else{
		// map 45 to 180 -> 0 to 90 and floor it to get an integer
		return floor((90 / (180 - ANGLE_DEBUT)) * (vabs - ANGLE_DEBUT));
	}

}

void SAIL_SetAngle(float angle)
{
	float servo_angle = angle * SAIL_TRANSFER_FACTOR + SAIL_TRANSFER_OFFSET;
	SERVO_SetAngle(servo_angle);
}
