#include "Accelerometer.h"
#include "ADC.h"
#include "math.h"
#include "stdio.h"

#define M_PI 3.14159265358979323846

// g-sel1 et g-sel1 ŕ 0 => range de 2.5g
// Donc sensibilité de +- 480 mv/g


const float ZERO_G = 1.27; 	// 0 g
const float SENSITIVITY = 1.35-0.9;
// max: 1.75
// min: 0.935

void Accelerometer_conf(ADC_TypeDef *adc, GPIO_TypeDef * gpio, int pinx, int piny)
{
	ADC_conf(adc);
	GPIO_conf(gpio, pinx, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
	GPIO_conf(gpio, piny, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
}

void Accelerometer_start(ADC_TypeDef *adc)
{
	ADC_start(adc);
}

float voltsToG(float volts)
{
	float readG = (volts - ZERO_G) / SENSITIVITY;
	if (readG > 1.0)
		readG = 1.0;
	else if (readG < -1.0)
		readG = -1.0;
	return readG;
}

int Accelerometer_getAngle(ADC_TypeDef *adc, int channel)
{
	const float readV = ADC_readVolt(adc, channel);
	const float readG = voltsToG(readV);
	
	float angleRad = asin(readG);
	int angleDeg = angleRad * (180/M_PI);
		
	return angleDeg;
}