#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();
}

// 单圈值
float GetAngle_NoTrack(struct MT6701 *mt6701) {
  Set_Ang_Sensor(mt6701->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 MT6701 *mt6701) {
  //	float D_Angle = 0.0;
  mt6701->Angle = GetAngle_NoTrack(mt6701);
  float D_Angle = mt6701->Angle - mt6701->Last_Angle;

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

  mt6701->Last_Angle = mt6701->Angle;

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

  return mt6701->Angle;
}

float GetVelocity(struct MT6701 *mt6701_Vel) {
  float dt = 0.0;
  float Vel_ts = DL_SYSTICK_getValue();
  if (Vel_ts < mt6701_Vel->Last_Vel_ts)
    dt = (mt6701_Vel->Last_Vel_ts - Vel_ts) / MCLK_IN_MHZ * 1e-6f;
  else
    dt = (RELOAD_CYCLES - Vel_ts + mt6701_Vel->Last_Vel_ts) / MCLK_IN_MHZ *
         1e-6f;

  if (dt < 0.0001)
    dt = 10000;

  float Vel_Angle = GetAngle(mt6701_Vel);

  float dv = Vel_Angle - mt6701_Vel->Vel_Last_Angle;

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

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

  return mt6701_Vel->velocity;
}