#include <stm32f10x.h>
#include <Horloge.h>
#include <MYGPIO.h>
#include <stdlib.h>
#include <MySPI.h>
#include <stdint.h>

//Pin GPIOA_9 et GPIOA_10 sont pris par USART

/*
I2C SDA IN/OUT
I2C SCL OUT 
*/

	//il faut recuperer le data qui sort 
	
	/*
	SPI1_NSS PA4 - Utilisé 
	NSS = 0 -> slave active  
	NSS = 1 -> slave inactive
	
	SPI1_SCK 	 PA5
	SPI1_MISO  PA6  
	SPI1_MOSI  PA7  
	
	TIM3 CH3 	 PB0
	
0x32 50 DATAX0 R 00000000 X-Axis Data 0 
0x33 51 DATAX1 R 00000000 X-Axis Data 1 
0x34 52 DATAY0 R 00000000 Y-Axis Data 0 
0x35 53 DATAY1 R 00000000 Y-Axis Data 1 
0x36 54 DATAZ0 R 00000000 Z-Axis Data 0 
0x37 55 DATAZ1 R 00000000 Z-Axis Data 1
	
	*/


void initAccelo(void)
{
    MySPI_Init(SPI1);
    // Power_CTL register = 0x2D ? write 0x08 (MEASURE = 1)
    MySPI_Clear_NSS();
    MySPI_Send(0x2D & 0x3F);   // write address (no read bit!)
    MySPI_Send(0x08);          // set MEASURE bit
    MySPI_Set_NSS();

    for (volatile int i = 0; i < 10000; i++); // small delay
}

// send bits, les bits inclus en char envoyés: RW MB A5 A4 A3 A2 A1 A0 
//RW: R = 1 et W = 0
//MB à 1 veut measurement et MB à 0 Standby
uint16_t * RecupAccelo(void){ //Renvoie 48 bits en forme des chars
	static uint16_t Messias[3];

	//On lit X0
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x32); //Lecture de X0 et MB à 1 pour garder les valeurs 0b11110010: (R/W|MB|Adress)
	//Faktisk dritsmart det katten gjør, setter MB=1 som sier multiple byte read, så leser den alle 6 bytes samtidig istedenfor en og en
	uint16_t X0 = MySPI_Read();
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	//On lit X1
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x33); //Lecture de X1
	Messias[0] = X0 | (MySPI_Read() << 8);
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	//On lit Y0
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x34); //Lecture de Y0 
	uint16_t Y0 = MySPI_Read();
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	//On lit Y1
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x35); //Lecture de Y1
	Messias[1] =  Y0 | (MySPI_Read() << 8);
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	//On lit Z0
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x36); //Lecture de Z0
	uint16_t Z0 = MySPI_Read();
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	//On lit Z1
	MySPI_Clear_NSS();//Mettre la broche PA4 à 0
	MySPI_Send(0x80|0x00|0x37); //Lecture de Z1
	Messias[2] =  Z0 | (MySPI_Read() << 8);
	MySPI_Set_NSS(); //Mettre la broche PA4 à 1
	
	return Messias;
}


uint16_t * KattRecupAccelo(void) //Beaucoup plus smart
{
    static uint16_t Messias[3];
    uint8_t buf[6];
    // Multi-byte read from 0x32 (X0..Z1)
    MySPI_Clear_NSS();
    // Send READ + MB + address
    MySPI_Send(0x80 | 0x40 | 0x32);  // 0xF2
    // Read 6 sequential registers
    for (int i = 0; i < 6; i++) {
        buf[i] = (uint8_t)MySPI_Read();
    }
    MySPI_Set_NSS();
    // Convert little-endian to 16-bit signed values
    Messias[0] = (uint16_t)(buf[1] << 8 | buf[0]); // X
    Messias[1] = (uint16_t)(buf[3] << 8 | buf[2]); // Y
    Messias[2] = (uint16_t)(buf[5] << 8 | buf[4]); // Z

    return Messias;
}

void initLacheur(void){
		GPIOB->CRH &= ~(0xF << (0 * 4));
		GPIOB->CRH |=  (0xA << (0 * 4)); //On met GPIOB.8 en mode output 2Mhz, alternate pp	
		Timer_Init(TIM4, 20000 - 1, 71);	//Claire m'a aidé
}

	//Recuperer le DATA en X, Z, Y
void LacheVoile(uint16_t * DATA){
	//uint16_t X = DATA[0]; //Z le longe du mât (masten)
	//uint16_t Z = DATA[2];//	//X le long du sense de voilier
	uint16_t Y = DATA[1]; ////Y vers les bords (Tribord/Babord)
	if (Y>=0x007B){// exatement à 40 degrés, on lache le 40%. 0xFF*(40deg/90deg)
		//Le PWM du moteur est gère par PB7
		MyTimer_PWM(TIM4, 3); //TIM4 CH3 pour PB8
		Set_DutyCycle_PWM(TIM4, 3, 2); //On met Duty cycle à 2% et il reste autour de 90 deg
	}
}