// TODO 16.01
#include "MotionApp.h"


// TODO 16.02
#define ICM42670P_ADDRESS   0x68
uint8_t ICM42670P_WriteData[2];
uint8_t ICM42670P_ReadData[1];
volatile uint8_t I2C3_IsDataReady = 0;
uint8_t AxisOutReg_Accel[6] = { 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10 };
uint8_t AxisOutReg_Gyro[6]  = { 0x11, 0x12, 0x13, 0x14, 0x15, 0x16 };
uint8_t AxisOutByte_Accel[6];
uint8_t AxisOutByte_Gyro[6];
short ax, ay, az, gx, gy, gz;

void I2C3_TransferCallback( uintptr_t contextHandle )
{
    if( ICM42670P_ReadData[0] & 0x1 )
    {
        I2C3_IsDataReady = 1;
    }
}


// TODO 16.03
void MotionSensorInit( void )
{
    ICM42670P_WriteData[0] = 0x02;
    ICM42670P_WriteData[1] = 0x10;
    SERCOM3_I2C_Write( ICM42670P_ADDRESS, ICM42670P_WriteData, 2 );
    myprintf( "@1,8;ICM42670P Software Reset Completed" );
    ICM42670P_WriteData[0] = 0x75;
    SERCOM3_I2C_WriteRead( ICM42670P_ADDRESS, ICM42670P_WriteData, 1, ICM42670P_ReadData, 1 );
    while( SERCOM3_I2C_IsBusy() );
    myprintf( "@1,9;ICM42670P WHO AM I : 0x%X", ICM42670P_ReadData[0]);
    ICM42670P_WriteData[0] = 0x20;
    ICM42670P_WriteData[1] = 0x0C;
    SERCOM3_I2C_Write( ICM42670P_ADDRESS, ICM42670P_WriteData, 2 );
    ICM42670P_WriteData[0] = 0x21;
    ICM42670P_WriteData[1] = 0x0C;
    SERCOM3_I2C_Write( ICM42670P_ADDRESS, ICM42670P_WriteData, 2 );
    ICM42670P_WriteData[0] = 0x1F;
    ICM42670P_WriteData[1] = 0x0F;
    SERCOM3_I2C_Write( ICM42670P_ADDRESS, ICM42670P_WriteData, 2 );
    myprintf( "@1,10;ICM42670P Initial Completed" );
    SERCOM3_I2C_CallbackRegister( I2C3_TransferCallback, 0 );
}


// TODO 16.04
void MotionSensorCheckDataReady( void )
{
    ICM42670P_WriteData[0] = 0x39;
    SERCOM3_I2C_WriteRead( ICM42670P_ADDRESS, ICM42670P_WriteData, 1, ICM42670P_ReadData, 1 );
}


// TODO 16.05
void MotionSensorRead( void )
{
    for( int i=0 ; i<6 ; i++ )
    {
       SERCOM3_I2C_WriteRead( ICM42670P_ADDRESS, &AxisOutReg_Accel[i], 1, &AxisOutByte_Accel[i], 1 );
       while( SERCOM3_I2C_IsBusy() );
       SERCOM3_I2C_WriteRead( ICM42670P_ADDRESS, &AxisOutReg_Gyro[i],  1, &AxisOutByte_Gyro[i],  1 );
       while( SERCOM3_I2C_IsBusy() );
    }
    ax = (short)(AxisOutByte_Accel[0]<<8|AxisOutByte_Accel[1]);
    ay = (short)(AxisOutByte_Accel[2]<<8|AxisOutByte_Accel[3]);
    az = (short)(AxisOutByte_Accel[4]<<8|AxisOutByte_Accel[5]);
    gx = (short)(AxisOutByte_Gyro[0]<<8|AxisOutByte_Gyro[1]);
    gy = (short)(AxisOutByte_Gyro[2]<<8|AxisOutByte_Gyro[3]);
    gz = (short)(AxisOutByte_Gyro[4]<<8|AxisOutByte_Gyro[5]);
}


// TODO 16.06
void MotionSensorGUI( void )
{
    static uint8_t ToggleGUI = 0;
    bool Reset =false;

    if( !BT1_Get() )
    {
        while( !BT1_Get() ){} // Button debounce
        ToggleGUI=(ToggleGUI+1)%4;
    }

    if( !BT2_Get() )
    {
        while( !BT2_Get() ) {} // Button debounce
        if( ToggleGUI==2 )  ax=ay=TC2_Timer16bitCounterGet()%2048;
        else                Reset=true;
    }

    switch( ToggleGUI )
    {
    case 0: DrawLevelBubble ( ax, ay, Reset, LAYER_GRAPHIC ); break;
    case 1: DrawBalanceBall ( ax, ay, Reset, LAYER_GRAPHIC ); break;
    case 2: DrawRotationCube( ax, ay, Reset, LAYER_GRAPHIC ); break;
    case 3: ViewRotationCube( ax, ay, az, gx, gy, gz, Reset, LAYER_GRAPHIC ); break;
    }

    GPL_ScreenUpdate();
}


// TODO 16.07
    MotionSensorInit();


// TODO 16.08
            MotionSensorCheckDataReady();


// TODO 16.09
        if( I2C3_IsDataReady )
        {
            MotionSensorRead();

            myprintf( "@1,6;Accel = (%+5d, %+5d, %+5d)", ax, ay, az );
            myprintf( "@1,7;Gyro  = (%+5d, %+5d, %+5d)", gx, gy, gz );

            MotionSensorGUI();

            I2C3_IsDataReady = 0;
        }