#include <stm32f10x.h>
#include <Horloge.h>
#include <MYGPIO.h>
#include <MySPI.h>
#include <Accelerometre.h>
#include <stdio.h>
#include <Servo.h>

uint16_t * Melding;
volatile uint16_t X;
volatile uint16_t Y;
volatile uint16_t Z;
volatile uint32_t moy;
//volatile uint16_t Angle_lim = 0x1500;

volatile uint16_t Angle_lim = 0x1E20 - 60*ANGLE; //#define ANGLE_LIMITE 0x0E38
int main (void) {
	#if MIN 
	int i = 0;
	
	uint32_t moyenne[LONGUEUR_MOY];
	for (int p = 0; p<LONGUEUR_MOY; p++){moyenne[p]=0xFFFF;}

	
	RCC->APB2ENR |= RCC_APB2ENR_IOPBEN; //Broder!!! Hvorfor var ikke den linjen kode her! Var Brage btw som fjernet den. Ingenting skjer hvis selve clocken ikke er på!
	initAccelo();
	//initLacheur();
	initServo(TIM4, 3);

	while(1){
		Melding = KattRecupAccelo(); //Recuperation des données de l'acceleromètre
		moyenne[i] = Melding[2]; //Ajout de la valeur recuperée à la moyenne
		i++; if (i>LONGUEUR_MOY) {i=0;} //itération
		moy = 0; for(int j = 0; j<LONGUEUR_MOY;j++){moy += moyenne[j];} moy = moy/LONGUEUR_MOY; //Calcul de la moyenne glissante
		//LacheVoile((uint16_t)moy); //Envoi de valeur
		//if (moy<Angle_lim){LacheVoileNew();}
		
		//################
		LacheVoileSmart(50, (uint16_t)moy);
		
		//Juste pour essayer de regarder ce que ça affiche
		X=Melding[0];
		Y=Melding[1];
		Z=Melding[2];
	}
		
#else

while(1){
		RCC->APB2ENR |= RCC_APB2ENR_IOPBEN; //Broder!!! Hvorfor var ikke den linjen kode her! Var Brage btw som fjernet den. Ingenting skjer hvis selve clocken ikke er på!
		initAccelo();
		initLacheur();
	
		uint32_t moyenne[LONGUEUR_MOY] = {0}; // initialize to 0
		int i = 0;
		
    Melding = KattRecupAccelo();
    
    moyenne[i] = Melding[2]; // add newest Z
    i++;
    if (i >= LONGUEUR_MOY)i = 0; // wrap around properly
    
    uint32_t sum = 0;
    for (int j = 0; j < LONGUEUR_MOY; j++){
        sum += moyenne[j];
    }
    moy = sum / LONGUEUR_MOY; // moving average
    
    LacheVoile((uint16_t)moy);
    
    X = Melding[0];
    Y = Melding[1];
    Z = Melding[2];
}
#endif
	while(1);
}


/*
	RCC->APB2ENR |= (0x01 << 2) | (0x01 << 3) | (0x01 << 4) ;
	Timer_Init(TIM1, ARR_VAL, PSC_VAL);

//Test du PWM
//PORTA & 0x00000400)>>10
MyTimer_PWM(TIM1, 3);
Set_DutyCycle_PWM(TIM1, 3, 10);
*/
/*
	//DEL INTERNE
#if INTERNE
	initGPIO_Interne();
	MyTimer_ActiveIT(TIM3, 2, commuterDEL_Interne);
#else
	//DEL EXTERNE BRANCHE SUR PB8 et GND (D14 & D8 AVEC LA TEXTE BLEUE SUR LA CARTE)
	initGPIO_Externe();
	MyTimer_ActiveIT(TIM2, 2, commuterDEL_Externe);
#endif
	*/