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

#include "IncrEncoder.h"
#include "stm32f1xx_ll_gpio.h" 
#include "stm32f1xx_ll_bus.h"
#include "stm32f1xx_ll_exti.h"
#include "stm32f1xx_ll_tim.h"

#define ZERO_POSITION 871
#define TICKS_PER_REVOLUTION 360*4
#define DEGREE_PER_TICKS 0.25

int index_passed = 0;

void INCR_ENCODER_Init(void){
  
  // GPIO initialization for channels a and b of the encoder
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
  LL_GPIO_InitTypeDef  channel_a_pin_conf, channel_b_pin_conf;
	LL_GPIO_StructInit(&channel_a_pin_conf);
  LL_GPIO_StructInit(&channel_b_pin_conf);
	
	channel_a_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
  channel_a_pin_conf.Pin = LL_GPIO_PIN_6;
  LL_GPIO_Init(GPIOA, &channel_a_pin_conf);
  
  channel_b_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
  channel_b_pin_conf.Pin = LL_GPIO_PIN_7;
  LL_GPIO_Init(GPIOA, &channel_b_pin_conf);

  // timer init
  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
  LL_TIM_InitTypeDef tim3_init_struct;
  tim3_init_struct.Autoreload= TICKS_PER_REVOLUTION-1;
  tim3_init_struct.Prescaler=0;
  tim3_init_struct.ClockDivision=LL_TIM_CLOCKDIVISION_DIV1;
  tim3_init_struct.CounterMode=LL_TIM_COUNTERMODE_UP;
  tim3_init_struct.RepetitionCounter=0;

  LL_TIM_Init(TIM3, &tim3_init_struct);

  // timer as encoder init
  LL_TIM_ENCODER_InitTypeDef encoder_init_struct;
  encoder_init_struct.EncoderMode = LL_TIM_ENCODERMODE_X4_TI12;
  encoder_init_struct.IC1Polarity =  LL_TIM_IC_POLARITY_RISING;
  encoder_init_struct.IC2Polarity = LL_TIM_IC_POLARITY_RISING;
  encoder_init_struct.IC1ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
  encoder_init_struct.IC2ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
  encoder_init_struct.IC1Prescaler = LL_TIM_ICPSC_DIV1;
  encoder_init_struct.IC2Prescaler= LL_TIM_ICPSC_DIV1;
  encoder_init_struct.IC1Filter = LL_TIM_IC_FILTER_FDIV1 ;
  encoder_init_struct.IC2Filter = LL_TIM_IC_FILTER_FDIV1 ;

  LL_TIM_ENCODER_Init(TIM3, &encoder_init_struct);

  LL_TIM_EnableCounter(TIM3);
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_AFIO);


  // gpio init for index (interrup) pin
  LL_GPIO_InitTypeDef index_pin_conf;
	LL_GPIO_StructInit(&index_pin_conf);
  index_pin_conf.Pin = LL_GPIO_PIN_5;
  index_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
  
  LL_GPIO_Init(GPIOC, &index_pin_conf);

  // exti init (interrupt)
  LL_EXTI_InitTypeDef exti;

  exti.Line_0_31 = LL_EXTI_LINE_5;
  exti.LineCommand = ENABLE;
  exti.Mode = LL_EXTI_MODE_IT;
  exti.Trigger = LL_EXTI_TRIGGER_RISING;

  LL_EXTI_Init(&exti);
  LL_GPIO_AF_SetEXTISource(LL_GPIO_AF_EXTI_PORTA, LL_GPIO_AF_EXTI_LINE5);

  // enable interrupt from exti in NVIC
  NVIC_SetPriority(EXTI9_5_IRQn, 12); // prio??
  NVIC_EnableIRQ(EXTI9_5_IRQn);
}

void EXTI9_5_IRQHandler(void){
  index_passed = 1;
  // reset counter = encoder position to 0 position
  LL_TIM_WriteReg(TIM3,CNT,0);
  // clear pending (EXTI necessary ?)
  LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_5);
  NVIC_ClearPendingIRQ(EXTI9_5_IRQn);
}

int INCR_ENCODER_IsAbsolute(void)
{
  return index_passed;
};
float INCR_ENCODER_GetAngle(void)
{
	int a,b;
  int counter_value = LL_TIM_ReadReg(TIM3, CNT);
	a = counter_value;
  // center
	int x = counter_value-ZERO_POSITION;
	int m = TICKS_PER_REVOLUTION;
  int centered_counter_value = (x%m + m)%m;
	b = centered_counter_value * 0.25;
	
	if (b >= 0 && b < (TICKS_PER_REVOLUTION * 0.25*0.5))
		return +b;
	else
		return b-360;
  // translate ticks to angle
  //float angle = centered_counter_value * 0.25;
  //return angle;
};