#include "Accelerometer.h"

#include "stm32f103xb.h" 
#include "stm32f1xx_ll_adc.h" 
#include "stm32f1xx_ll_bus.h"
#include "stm32f1xx_ll_rcc.h"
#include "stm32f1xx_ll_gpio.h"
#include <math.h>

#ifndef M_PI
  #define M_PI 3.14159265358979323846
#endif

#define ANGLE_LIMIT_DEG 40
#define ANGLE_LIMIT_RAD ANGLE_LIMIT_DEG * (180.0 / M_PI)

#define ZERO_G_VOLTAGE 1.65
#define ZERO_G_READING (ZERO_G_VOLTAGE / 3.3) * 4095

#define VOLT_PER_G 0.48

void ACCELEROMETER_Init(void){
  RCC -> CFGR |= (0x1<<15);
  RCC-> CFGR &= ~ (0x1<<14);
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC1);
  LL_APB1_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOC);

  LL_GPIO_InitTypeDef pc0, pc1;
  LL_ADC_InitTypeDef adc;
  LL_ADC_REG_InitTypeDef adcReg;

  LL_GPIO_StructInit(&pc0);
  pc0.Pin = LL_GPIO_PIN_0;
  pc0.Mode = LL_GPIO_MODE_ANALOG;
  LL_GPIO_Init(GPIOA, &pc0);

  LL_GPIO_StructInit(&pc1);
  pc1.Pin = LL_GPIO_PIN_1;
  pc1.Mode = LL_GPIO_MODE_ANALOG;
  LL_GPIO_Init(GPIOA, &pc1);


  adc.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT;
  adc.SequencersScanMode = LL_ADC_SEQ_SCAN_DISABLE;
  LL_ADC_Init(ADC1, &adc);

  adcReg.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE ;
  adcReg.SequencerLength = LL_ADC_REG_SEQ_SCAN_DISABLE;
  adcReg.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE;
  adcReg.ContinuousMode = LL_ADC_REG_CONV_SINGLE;
  adcReg.DMATransfer = LL_ADC_REG_DMA_TRANSFER_NONE;    

  LL_ADC_REG_Init(ADC1, &adcReg);
  LL_ADC_Enable(ADC1);
}

double ACCELEROMETER_GetX(void){
  LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_1);
  LL_ADC_REG_StartConversionSWStart(ADC1);
  while (LL_ADC_IsActiveFlag_EOS(ADC1) != 1);
  double x= (LL_ADC_REG_ReadConversionData12(ADC1) - ZERO_G_READING) * VOLT_PER_G;
  return x;
}

double ACCELEROMETER_GetY(void){
  LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_11);
  LL_ADC_REG_StartConversionSWStart(ADC1);
  while (LL_ADC_IsActiveFlag_EOS(ADC1) != 1);
  double y = (LL_ADC_REG_ReadConversionData12(ADC1)-ZERO_G_READING) * VOLT_PER_G;
  return y;
}

int ACCELEROMETER_AngleGood(void){
  double x = ACCELEROMETER_GetX();
  double y = ACCELEROMETER_GetY();
  double angle = atan(x/y);

  if (fabs(angle)>ANGLE_LIMIT_RAD){
    return 0;
  }else {
    return 1;
  }
  //le flag EOC n'est jamais mis à un ....
  // Soit la conversion est mal faite soit on n'utilise pas bien la simulation
  //soit on n'utilise pas bien isActiveFlag dans la boucle
}