// KO
// test calcul en point fixe



#include <p33Fxxxx.h>
#include <stdio.h>
#include <timer.h>
#include <uart.h>
#include <math.h>

#include "..\..\drivers\delay.h"
#include "..\..\drivers\drv6DOF.h"
#include "..\..\drivers\drvI2C.h"
#include "IMUfloat_DCM.h"
#include "../Utils/MathFloat.h"

// tick is 2ms
#define drift_DeltaTime 50  // xxx*5ms
#define Euler_DeltaTime 50  // xxx*5ms 


//config oscillateur
_FOSCSEL(FNOSC_PRIPLL);								// Choix du oscillateur + PLL
_FOSC(FCKSM_CSDCMD & OSCIOFNC_OFF  & POSCMD_XT);	// Pas de basculement d'horloge
													// pas de OSC2
													// type d'oscillateur XT
_FWDT(FWDTEN_OFF); // watchdog off;

void InitTimer1();
void InitPORT_AND_PIN() ;
void InitClock2();



// create global
Dof6Accelero accelero;
Dof6Gyro gyro;
extern EulerAttitude attitude;


volatile unsigned long tick=0;
unsigned long next_drift_Time=0; //drift_DeltaTime;
unsigned long next_EulerCompute=Euler_DeltaTime;
volatile char flagIT=0;

volatile float wmeasure[3]; // gyro measure array


unsigned char buffTX[50]={'i','u','t','\n','\0'};
unsigned char buffKOI2C[5]={'K','0','\n','\0'};

unsigned int identvar=0;
unsigned char toogledelta=0;
int delta,delta2;

int main (void)
{	int idAcc,idGyro;
	
	InitClock2() ; //init oscillateur
	InitPORT_AND_PIN(); // init broche remapable
	InitUART1(); // 
	Init6DOF();
	
	// OFFSET CALIBRATION
	CalcAverageOffsetAccelerometer(&accelero); // calculate offset accelo from measure
	CalcAverageOffsetGyro(&gyro);// calculate offset gyro from measure
	
	idAcc=GetWhoAmIAcc(); // must return 0xE5
	idGyro=GetWhoAmIGyro(); // must return 0x69
	sprintf(buffTX,"ACC Id:%d GYRO Id:%d  \n",idAcc,idGyro);
	sprintf(buffTX,"ACC Id:%d GYRO Id:%d  \n",idAcc,idGyro);

		
	InitTimer1();
	initDCM();

	for (;;)
	{
	
	if (flagIT)
		{
		flagIT=0;
		
		// drift task
		if (tick>next_drift_Time)
			{
			identvar++;
			
			if (identvar==10)
				toogledelta=!toogledelta;
			if (toogledelta)		
				delta=23;
			else
				delta=0;
		//	LATAbits.LATA1=1;	
		// 	getAccelerometerData(&accelero);
		//	accelero.data[0]=0;
		//	accelero.data[1]=delta;
		//	accelero.data[2]=256;
		
		//	DriftCompMahony(accelero); //300us environ
			
		
			
			next_drift_Time+=drift_DeltaTime; 
		//	LATAbits.LATA1=0;
			//sprintf(buffTX,"wx:%d   wy:%d      wz:%d       \n\r",wmeasure[0],wmeasure[1],wmeasure[2]);
			//sprintf(buffTX,"phi_accel:%d   teta_accel:%d   \n\r",attitude.phiAccelero,attitude.tetaAccelero);
			//UART_puts(buffTX);
			}
		
		if (tick==next_EulerCompute)
			{
			LATAbits.LATA1=1;
			DCMtoEuler(); // 150us avec atan simplifié  200us environ avec atan2f
			LATAbits.LATA1=0;
			//getAccelerometerData(&accelero);
			//EulerEstimatorAccelero(accelero,&attitude);	
			next_EulerCompute+=Euler_DeltaTime; 
			float phi,teta,psi;
			phi=(attitude.phi*180.0)/PI;
			teta=(attitude.teta*180.0)/PI;
			psi=(attitude.psi*180.0)/PI;
			//sprintf(buffTX,"wx:%d   wy:%d      wz:%d       \n\r",wmeasure[0],wmeasure[1],wmeasure[2]);
			//sprintf(buffTX,"phi;%d;teta;%d;psi:%d\n\r",attitude.phi,attitude.teta,attitude.psi);
			sprintf(buffTX,"delta %d;phi;%.1f;teta;%.1f;psi:%.1f\n\r",identvar,phi,teta,psi);
			//sprintf(buffTX,"accx:%d   accy:%d      accz:%d       \n\r",accelero.data[0],accelero.data[1],accelero.data[2]);
			//sprintf(buffTX,"phi_accel:%d   teta_accel:%d   \n\r",attitude.phiAccelero,attitude.tetaAccelero);
			UART_puts(buffTX);
		
			}
		
		} 
		
 
	}
}

// IT takes 700 us 
void __attribute__((__interrupt__)) _T1Interrupt(void)
{
	LATBbits.LATB5=1; // IT takes 200 us 
	tick++;
	flagIT=1; // Pour traitement différé

	
	getAccelerometerData(&accelero);
	accelero.data[0]=0;
	accelero.data[1]=delta;
	accelero.data[2]=256;
	DriftCompMahony(accelero);  // 300us environ
	
	int resultI2C=0;
	resultI2C=getGyroData(&gyro); // data bias  compensated
	if (resultI2C<0) UART_puts(buffKOI2C);
	wmeasure[0]=KgyroScale*gyro.data[0]; 
	wmeasure[1]=KgyroScale*gyro.data[1];
	wmeasure[2]=KgyroScale*gyro.data[2];
	
/*
	if (identvar==100000)
		{delta2=14;}

	wmeasure[0]=0; 
	wmeasure[1]=KgyroScale*14;
	wmeasure[2]=0;
	
	*/

	
	DCMupdate(wmeasure);  //700us environ
	
    WriteTimer1(0);
    IFS0bits.T1IF = 0;    /* Clear Timer interrupt flag */
    
    LATBbits.LATB5=0;
}  

void InitTimer1()
{
	/* Enable Timer1 Interrupt and Priority to "1" */
    ConfigIntTimer1(T1_INT_PRIOR_1 & T1_INT_ON);
    WriteTimer1(0);
	tick=0;
    OpenTimer1(T1_ON & T1_GATE_OFF & T1_IDLE_STOP &
               T1_PS_1_8 & T1_SYNC_EXT_OFF &
               T1_SOURCE_INT, 22000);  //TIMER1 à 25000
	// prinicpe timer: comptage au rythme de Tcy*PreDiv
	// Si comptage=PER= 39960 alors TM1RIF ON
	//=> xx *Tcy*PreDiv
	// ici Fcy=80M/2=40Mhz =>25ns , PreDiv=T1_PS_1_8 =8
	// 10000*25ns*8=2ms
	// T1_PS_1_64 12500

}

void InitPORT_AND_PIN() 
{  // depend du schema de la carte
	// Rx : RP15
	// Tx:RP14
	_U1RXR=15;	                    //RX sur RP15/RB15
	_RP14R=3;                        //TX sur RP14/RB14
	
	TRISBbits.TRISB15=0; //RP15/RB15 en sortie
	
	AD1PCFGLbits.PCFG1=1; //AN1/RA1 is digital
	TRISAbits.TRISA1=0;  // RA1 out
	TRISBbits.TRISB5=0;
	LATAbits.LATA1=0;
}

	void InitClock2() 
{   // CHOIX DES COEF DE LA PLL : voir notice OSCILLATOT
	// FOSC = FIN •(M/(N1*N2))  et Fcy=Fosc/2 = fréquence de fct des périphriques
	// ex: quartz 8Mhz et Fosc=80Mhz => Fosc=8M*(40/4)
	PLLFBD = 38;				// M = 40       
	CLKDIVbits.PLLPOST = 0;		// N1 = 2
	CLKDIVbits.PLLPRE = 0;		// N2 = 2
	OSCTUN = 0;
	RCONbits.SWDTEN = 0;

	while(OSCCONbits.LOCK != 1) {}; // attente verrouillage PLL
}