#include <math.h>

#include "Servo.h"
#include "stm32f1xx_ll_bus.h" // Pour l'activation des horloges
#include "stm32f1xx_ll_tim.h" 
#include "stm32f1xx_ll_gpio.h" 


void SERVO_Init(void){
  // use TIM1 since it has pa8 at channel 1 output

  // setup timer 1
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
  LL_TIM_InitTypeDef tim_init;
  tim_init.Prescaler = 71;
  tim_init.Autoreload = 19999;
  tim_init.ClockDivision=LL_TIM_CLOCKDIVISION_DIV1;
  tim_init.CounterMode=LL_TIM_COUNTERMODE_UP;
  tim_init.RepetitionCounter=0;
  LL_TIM_Init(TIM1, &tim_init);

  // setup main output enabled
  LL_TIM_EnableAllOutputs(TIM1);

  // setup gpio
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
  LL_GPIO_InitTypeDef servo_gpio_init;
  servo_gpio_init.Pin = LL_GPIO_PIN_8;
  servo_gpio_init.Mode = LL_GPIO_MODE_ALTERNATE;
  servo_gpio_init.Speed = LL_GPIO_MODE_OUTPUT_2MHz ;
  servo_gpio_init.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  servo_gpio_init.Pull = LL_GPIO_PULL_DOWN; // dont know why but otherwise it crashes
  LL_GPIO_Init(GPIOA, &servo_gpio_init);

  // setup output compare
  LL_TIM_OC_InitTypeDef oc_init;
  oc_init.OCMode = LL_TIM_OCMODE_PWM1;
  oc_init.OCState = LL_TIM_OCSTATE_ENABLE;
  oc_init.OCNState = LL_TIM_OCSTATE_ENABLE;
  oc_init.CompareValue = 999; 
  oc_init.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
  oc_init.OCNPolarity = LL_TIM_OCPOLARITY_LOW;
  oc_init.OCIdleState = LL_TIM_OCIDLESTATE_LOW;
  oc_init.OCNIdleState = LL_TIM_OCIDLESTATE_HIGH;
  LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &oc_init);


  LL_TIM_EnableCounter(TIM1);
}

void SERVO_SetAngle(int angle){
  // set ccr3	register to alter pwm output between 1 and 2 ms
	angle = 90 - angle;
  float vminCOR = 1250, vmaxCOR = 1950;
  float diffCOR = vmaxCOR - vminCOR;
  int ccr3Value = round(angle*diffCOR/90)+vminCOR; 

  LL_TIM_WriteReg(TIM1, CCR1, ccr3Value);
}