#include "M451Series.h"
#include "data.h"
#include "mpu.h"
#include "imu.h"

#define PLL_CLOCK 5000000


void Timer0Init()
{
	CLK_EnableModuleClock(TMR0_MODULE);
	TIMER_Open(TIMER0,TIMER_PERIODIC_MODE,200);
	TIMER_EnableCapture(TIMER0,TIMER_CAPTURE_FREE_COUNTING_MODE,TIMER_CAPTURE_FALLING_AND_RISING_EDGE);
	TIMER_EnableInt(TIMER0);
	TIMER_Start(TIMER0);
	SYS_UnlockReg();
  NVIC_EnableIRQ(TMR0_IRQn);
	SYS_LockReg();
}


void TMR0_IRQHandler()
{
	TIMER_ClearIntFlag(TIMER0);

	static uint8_t cnt_3ms = 0;
	static uint8_t cnt_6ms = 0;	 
	cnt_3ms++;
	cnt_6ms++;
	if (cnt_6ms >= 2){
		cnt_6ms = 0;
		MpuGetData();
		GetAngle(&MPU6050UnFiltered,&Angle,0.010f);
	}
	else{
	}

}
void HalInit(){
	  SYS_UnlockReg();

/*---------------------------------------------------------------------------------------------------------*/
    /* Init System Clock                                                                                       */
    /*---------------------------------------------------------------------------------------------------------*/

    /* Enable Internal RC 22.1184MHz clock */
    CLK_EnableXtalRC(CLK_PWRCTL_HIRCEN_Msk);

    /* Waiting for Internal RC clock ready */
    CLK_WaitClockReady(CLK_STATUS_HIRCSTB_Msk);

    /* Switch HCLK clock source to Internal RC and HCLK source divide 1 */
    CLK_SetHCLK(CLK_CLKSEL0_HCLKSEL_HIRC, CLK_CLKDIV0_HCLK(1));

    /* Enable external XTAL 12MHz clock */
    CLK_EnableXtalRC(CLK_PWRCTL_HXTEN_Msk);

    /* Waiting for external XTAL clock ready */
    CLK_WaitClockReady(CLK_STATUS_HXTSTB_Msk);

    /* Set core clock as PLL_CLOCK from PLL */
    CLK_SetCoreClock(PLL_CLOCK);

    /* Enable UART module clock */

    /* Enable I2C0 module clock */
    CLK_EnableModuleClock(I2C0_MODULE);


    /*---------------------------------------------------------------------------------------------------------*/
    /* Init I/O Multi-function                                                                                 */
    /*---------------------------------------------------------------------------------------------------------*/

		SYS->GPD_MFPL &= ~SYS_GPD_MFPL_PD4MFP_Msk;
		SYS->GPD_MFPL |= SYS_GPD_MFPL_PD4MFP_I2C0_SDA;
		
		SYS->GPD_MFPL &= ~SYS_GPD_MFPL_PD5MFP_Msk;
		SYS->GPD_MFPL |= SYS_GPD_MFPL_PD5MFP_I2C0_SCL;
		I2C_Open(I2C0,50000);
		
    printf("I2C clock %d Hz\n", I2C_GetBusClockFreq(I2C0));

    I2C_SetSlaveAddr(I2C0, 0, 0x78, 0);   /* Slave Address : 0x15 */

    SYS_LockReg();

	//GPIO_SetMode(PB,BIT14,GPIO_PMD_INPUT);
	//GPIO_SetMode(PB,BIT9,GPIO_PMD_OUTPUT);

  //I2C_EnableInt(I2C0);
  //NVIC_EnableIRQ(I2C0_IRQn);
}


int main(){
	HalInit();
	MpuInit();

	Timer0Init();
	while(1){
		
	}
}