#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 TIM2 since it has pa1 at channel 2 output
	
	// setup timer 2
	LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
	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(TIM2, &tim_init);
	
	// setup gpio
	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
	LL_GPIO_InitTypeDef servo_gpio_init;
	servo_gpio_init.Pin = LL_GPIO_PIN_1;
  servo_gpio_init.Mode = LL_GPIO_MODE_ALTERNATE;
  servo_gpio_init.Speed = LL_GPIO_SPEED_FREQ_LOW;
  servo_gpio_init.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	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 = 1500; // todo find good init value (maybe -1)
	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(TIM2, LL_TIM_CHANNEL_CH2, &oc_init);
	LL_TIM_EnableCounter(TIM2);
	
	// period of timer should be 20 ms
	// input= 72mhz
	// per 
}

void SERVO_SetAngle(int angle){
	// set ccr2 register to alter pwm output between 1 and 2 ms
	
}