#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.0 - ANGLE_DEBUT)) * (vabs - ANGLE_DEBUT));
  }

}

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