#include "MT6701.h"
#include "ti_msp_dl_config.h"
#include "uart_redircet.h"
#include "stdio.h"
#include "config.h"

volatile int16_t angle;
volatile float angle_f;
volatile float angle_f_rad;

volatile bool gIsI2cError = false;

#define DEBUG_I2C	false

/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
{
    0x03
};

/* Data received from Target */
volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0};

/* I2C clock configuration */
DL_I2C_ClockConfig gI2CclockConfig;
/* Frequency of selected I2C clock*/
volatile uint32_t gClockSelFreq;

/* Cycles to delay after controller transfer initiated */
volatile uint32_t gDelayCycles;

/* I2C Target address */
#define I2C_TARGET_ADDRESS (0x06)

void MT6701_iic_read_angel(void)
{
    /* Get I2C clock source and clock divider to use for delay cycle calculation */
    DL_I2C_getClockConfig(I2C_1_INST, &gI2CclockConfig);
    switch(gI2CclockConfig.clockSel)
    {
        case DL_I2C_CLOCK_BUSCLK:
            gClockSelFreq = 32000000;
            break;
        case DL_I2C_CLOCK_MFCLK:
            gClockSelFreq = 4000000;
            break;
        default:
            break;
    }
    /*
     * Calculate number of clock cycles to delay after controller transfer initiated
     * gDelayCycles = 3 I2C functional clock cycles
     * gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock freq)
     */
    gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
                   (CPUCLK_FREQ / gClockSelFreq);
    if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
    {
        printf("i2c before writing -------\n");

    }

    /*
     * Fill FIFO with data. This example will send a MAX of 8 bytes since it
     * doesn't handle the case where FIFO is full
     */
    DL_I2C_fillControllerTXFIFO(I2C_1_INST, &gTxPacket[0], I2C_TX_PACKET_SIZE);

    /* Wait for I2C to be Idle */
    while(!(
                DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
        ;

    /* Send the packet to the controller.
     * This function will send Start + Stop automatically.
     */
    DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
                                   DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);

    if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
    {
        printf("i2c writing done -------\n");
    }

    /* Workaround for errata I2C_ERR_13 */
    delay_cycles(gDelayCycles);

    /* Poll until the Controller writes all bytes */
    while(
        DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY)
        ;

    /* Trap if there was an error */
    if(DL_I2C_getControllerStatus(I2C_1_INST) &
            DL_I2C_CONTROLLER_STATUS_ERROR)
    {
        gIsI2cError = true;

        if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
        {
            printf("i2c error ------------------------------------------\n");
        }
        /* LED will remain high if there is an error */
        __BKPT(0);
        return;
    }

    /* Wait for I2C to be Idle */
    while(!(
                DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
        ;

    /* Add delay between transfers */
    delay_cycles(1000);

    if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
    {
        printf("i2c before reading -------\n");
    }

    /* Send a read request to Target */
    DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
                                   DL_I2C_CONTROLLER_DIRECTION_RX, I2C_RX_PACKET_SIZE);

    /*
     * Receive all bytes from target. LED will remain high if not all bytes
     * are received
     */
    for(uint8_t i = 0; i < I2C_RX_PACKET_SIZE; i++)
    {
        while(DL_I2C_isControllerRXFIFOEmpty(I2C_1_INST))
            ;
        gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
    }

    if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
    {
        printf("i2c reading done -------\n");
    }


}
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;

// 单圈值
float GetAngle_NoTrack(void)
{
    MT6701_iic_read_angel();
    angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
    angle_f_rad = (float)angle * _2PI / 16384;
    if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
    {
        printf("angle_rad read back is %f \n", angle_f_rad);
    }
    return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle_rad = 0.0;
//多圈值
float GetAngle(void)
{
    volatile  float D_Angle_rad = 0.0;
    volatile float Angle_rad = GetAngle_NoTrack();
    D_Angle_rad = Angle_rad - Last_Angle_rad;

    if(fabs(D_Angle_rad) > (0.8f * 2 * PI))
    {
        full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
    }

    Last_Angle_rad = Angle_rad;

    return (full_rotations * 2 * PI + Last_Angle_rad);
}

volatile float Last_Vel_ts = 0.0;
volatile float Vel_Last_Angle = 0.0;
float GetVelocity(void)
{
    volatile float dt = 0.0;
    volatile float Vel_ts = SysTick -> VAL;
    if(Vel_ts < Last_Vel_ts)
    {
        dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f;
    }
    else
    {
        dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
    }

    if(dt < 0.0001)
    {
        dt = 10000;
    }

    float Vel_Angle = GetAngle();

    float dv = Vel_Angle - Vel_Last_Angle;

    float velocity = (Vel_Angle - Vel_Last_Angle) / dt;

    Last_Vel_ts = Vel_ts;
    Vel_Last_Angle = Vel_Angle;


    return velocity;
}

void MT6701_get_angle_degree(void)
{
    MT6701_iic_read_angel();
    angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
    angle_f = (float)angle * 360 / 16384;
}