#include "mt6701.h"
#include "config.h"
#include "soft_i2c.h"
#include "stdio.h"
#include "ti_msp_dl_config.h"
#include "uart_redircet.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 << 1)

void MT6701_iic_read_angel(void) {

  I2C_Start();
  I2C_SendByte(I2C_TARGET_ADDRESS);
  I2C_RecviveAck();
  I2C_SendByte(0X03);
  I2C_RecviveAck();

  I2C_Start();
  I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
  I2C_RecviveAck();
  gRxPacket[0] = I2C_RecviveData();
  I2C_SendAck(1);

  I2C_Start();
  I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
  I2C_RecviveAck();
  gRxPacket[1] = I2C_RecviveData();
  I2C_SendAck(1);
  I2C_Stop();
}

// 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(struct AS5600_Sensor *AS5600) {
  Set_Ang_Sensor(AS5600->Mot_num);
  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;
}

// 多圈值
float GetAngle(struct AS5600_Sensor *AS5600) {
  //	float D_Angle = 0.0;
  AS5600->Angle = GetAngle_NoTrack(AS5600);
  float D_Angle = AS5600->Angle - AS5600->Last_Angle;

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

  AS5600->Last_Angle = AS5600->Angle;

  AS5600->Angle = (AS5600->full_rotations * _2PI + AS5600->Last_Angle);

  return AS5600->Angle;
}

float GetVelocity(struct AS5600_Sensor *AS5600_Vel) {
  float dt = 0.0;
  float Vel_ts = DL_SYSTICK_getValue();
  if (Vel_ts < AS5600_Vel->Last_Vel_ts)
    dt = (AS5600_Vel->Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
  else
    dt = (0xFFFFFF - Vel_ts + AS5600_Vel->Last_Vel_ts) / 9 * 1e-6f;

  if (dt < 0.0001)
    dt = 10000;

  float Vel_Angle = GetAngle(AS5600_Vel);

  float dv = Vel_Angle - AS5600_Vel->Vel_Last_Angle;

  AS5600_Vel->velocity = (Vel_Angle - AS5600_Vel->Vel_Last_Angle) / dt;

  AS5600_Vel->Last_Vel_ts = Vel_ts;
  AS5600_Vel->Vel_Last_Angle = Vel_Angle;

  return AS5600_Vel->velocity;
}