#if 1
#include "ztmc4361A.hpp"

#ifdef HAL_SPI_MODULE_ENABLED

#include <stdarg.h>

#include "../basic/basic.hpp"
#include "./TMC4361A/TMC4361A.h"
#include "sdk/os/zos.hpp"

using namespace iflytop;
#define PRV_FIELD_WRITE(address, mask, shift, value) (writeInt(address, FIELD_SET(readInt(address), mask, shift, value)))
#define PRV_FIELD_READ(address, mask, shift)         FIELD_GET(readInt(address), mask, shift)

#define SET_PIN(pin, val) \
  if (pin) {              \
    pin->setState(val);   \
  }

#if 1
void TMC4361A::readWriteArray(uint8_t *data, size_t length) {
  m_csgpio->setState(false);
  // zchip_clock_early_delayus(10);
  zchip_clock_early_delayus(10);
  HAL_SPI_TransmitReceive(m_spi, data, data, length, 1000);
  m_csgpio->setState(true);
}
void    TMC4361A::writeInt(uint8_t address, int32_t value) { writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
int32_t TMC4361A::readInt(uint8_t address) {
  CriticalContext cc;

  int     value;
  uint8_t data[5];

  address = TMC_ADDRESS(address);

  if (!TMC_IS_READABLE(m_registerAccessTable[address])) return m_defaultRegisterResetState[address];

  data[0] = address;
  readWriteArray(&data[0], 5);

  data[0] = address;
  readWriteArray(&data[0], 5);

  m_status = data[0];
  value    = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];

  return value;
}
void TMC4361A::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
  CriticalContext cc;

  int     value;
  uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC4361A_WRITE_BIT)), x1, x2, x3, x4};

  readWriteArray(&data[0], 5);

  m_status = data[0];

  value = ((uint32_t)x1 << 24) | ((uint32_t)x2 << 16) | (x3 << 8) | x4;

  // Write to the shadow register and mark the register dirty
  address                 = TMC_ADDRESS(address);
  shadowRegister[address] = value;
}

void TMC4361A::readWriteCover(uint8_t *data, size_t length) {
  CriticalContext cc;
  // Buffering old values to not interrupt manual covering
  int32_t old_high = shadowRegister[TMC4361A_COVER_HIGH_WR];
  int32_t old_low  = shadowRegister[TMC4361A_COVER_LOW_WR];

  // Check if datagram length is valid
  if (length == 0 || length > 8) return;

  uint8_t  bytes[8] = {0};
  uint32_t tmp;
  size_t   i;

  // Copy data into buffer of maximum cover datagram length (8 bytes)
  for (i = 0; i < length; i++) bytes[i] = data[length - i - 1];

  // Send the datagram
  if (length > 4) writeDatagram(TMC4361A_COVER_HIGH_WR, bytes[7], bytes[6], bytes[5], bytes[4]);
  writeDatagram(TMC4361A_COVER_LOW_WR, bytes[3], bytes[2], bytes[1], bytes[0]);

  zos_delay(10);

  // Read the reply
  if (length > 4) {
    tmp      = readInt(TMC4361A_COVER_DRV_HIGH_RD);
    bytes[4] = BYTE(tmp, 0);
    bytes[5] = BYTE(tmp, 1);
    bytes[6] = BYTE(tmp, 2);
    bytes[7] = BYTE(tmp, 3);
  }
  tmp      = readInt(TMC4361A_COVER_DRV_LOW_RD);
  bytes[0] = BYTE(tmp, 0);
  bytes[1] = BYTE(tmp, 1);
  bytes[2] = BYTE(tmp, 2);
  bytes[3] = BYTE(tmp, 3);

  // Write the reply to the data array
  for (i = 0; i < length; i++) {
    data[length - i - 1] = bytes[i];
  }

  // Rewriting old values to prevent interrupting manual covering. Imitating unchanged values and state.
  writeInt(TMC4361A_COVER_HIGH_WR, old_high);
  shadowRegister[TMC4361A_COVER_LOW_WR] = old_low;
}

TMC4361A::TMC4361A(/* args */) {
  m_driver_ic_type          = IC_TMC2130;
  m_lastCallPeriodicJobTick = 0;
  // m_reachtarget             = false;
}
/**
 * @brief
 *     当调用TMC-API中的tmc4361A_reset/restore时候，TMC-API中的方法会初始化整个芯片的寄存器
 * 当寄存器初始化完成之后，会调用这个方法??
 *     我们可以在这个方法中，对芯片的部分寄存器进行初始化??
 * @param state
 */
void TMC4361A::tmc4361AConfigCallback(ConfigState state) {}

void TMC4361A::writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) {  //
  PRV_FIELD_WRITE(address, mask, shift, value);
}
void TMC4361A::setAcceleration(float accelerationpps2) {
  /**
   * @brief
   *  TMC4361A_AMAX:有两种模式，这里使用的是频率模式
   *
   *  Frequency mode: [pulses per sec2]

   *      22 digits and 2 decimal places: 250 mpps^2 ?? AMAX ?? 4 Mpps^2
   *  Direct mode: [?v per clk cycle]
   *      a[?v per clk_cycle]= AMAX / 2^37
   *      AMAX [pps2] = AMAX / 237 ?? fCLK^2
   */

  accelerationpps2 = to_motor_acc(accelerationpps2);
  int32_t acc      = (int32_t)accelerationpps2;
  writeInt(TMC4361A_AMAX, acc << 2);
}
void TMC4361A::setDeceleration(float accelerationpps2) {
  /**
   * @brief
   *  TMC4361A_DMAX:有两种模式，这里使用的是频率模式
   *
   *  Frequency mode: [pulses per sec2]
   *      22 digits and 2 decimal places: 250 mpps^2 ?? AMAX ?? 4 Mpps^2
   *  Direct mode: [?v per clk cycle]
   *      a[?v per clk_cycle]= AMAX / 2^37
   *      AMAX [pps2] = AMAX / 237 ?? fCLK^2
   */
  accelerationpps2 = to_motor_acc(accelerationpps2);
  int32_t acc      = (int32_t)accelerationpps2;
  writeInt(TMC4361A_DMAX, acc << 2);
}
#define INIT_GPIO(m_pin, pin, ...)         \
  if (pin != PinNull) {                    \
    m_pin = new ZGPIO();                   \
    ZASSERT(m_pin != nullptr);             \
    m_pin->initAsOutput(pin, __VA_ARGS__); \
  } else {                                 \
    m_pin = nullptr;                       \
  }
void TMC4361A::initialize(cfg_t *cfg) {
  m_spi = cfg->spi;
  INIT_GPIO(m_csgpio, cfg->csgpio, ZGPIO::kMode_nopull, false, true);
  INIT_GPIO(m_resetPin, cfg->resetPin, ZGPIO::kMode_nopull, false, true);
  INIT_GPIO(m_fREEZEPin, cfg->fREEZEPin, ZGPIO::kMode_nopull, false, true);
  INIT_GPIO(m_ennPin, cfg->ennPin, ZGPIO::kMode_nopull, false, true);
  INIT_GPIO(m_driverIC_resetPin, cfg->driverIC_resetPin, ZGPIO::kMode_nopull, false, true);
  INIT_GPIO(m_driverIC_ennPin, cfg->driverIC_ennPin, ZGPIO::kMode_nopull, false, true);

  m_driver_ic_type            = IC_TMC2160;
  m_registerAccessTable       = &tmc4361A_defaultRegisterAccess[0];
  m_defaultRegisterResetState = &tmc4361A_defaultRegisterResetState[0];

  memset(shadowRegister, 0, sizeof(shadowRegister));

  SET_PIN(m_resetPin, true);
  SET_PIN(m_fREEZEPin, true);
  SET_PIN(m_ennPin, true);

  reset();

  driverIC_reset();

  setAcceleration(500000);
  setDeceleration(500000);

  enableIC(true);
  zchip_clock_early_delayus(300 * 1000);
  zchip_clock_early_delayus(300 * 1000);

  driverIC_setIHOLD_IRUN(1, 3, 0);
}

void TMC4361A::setScale(int32_t scale) { m_scale = scale; }

uint8_t TMC4361A::reset() {
  // Pulse the low-active hardware reset pin
  stop();
  SET_PIN(m_resetPin, false);
  zchip_clock_early_delayus(1000);
  SET_PIN(m_resetPin, true);

  /**
   * @brief 重置芯片镜像寄存??
   *
   */
  for (uint32_t add = 0; add < TMC4361A_REGISTER_COUNT; add++) {
    if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) {
      continue;
    }
    writeInt(add, m_defaultRegisterResetState[add]);
  }

  uint8_t  driver, dataLength;
  uint32_t value;
  // Setup SPI
  switch (m_driver_ic_type) {
    case IC_TMC2130:
    case IC_TMC2160:
      driver     = 0x0C;
      dataLength = 0;
      break;
    case IC_TMC2660:
      driver     = 0x0B;
      dataLength = 0;
      break;
    default:
      driver     = 0x0F;
      dataLength = 40;
      break;
  }
  value = 0x44400040 | (dataLength << 13) | (driver << 0);
  writeInt(TMC4361A_SPIOUT_CONF, value);
  writeInt(TMC4361A_CURRENT_CONF, 0x00000003);
  writeInt(TMC4361A_SCALE_VALUES, 0x00000000);
  return 1;
}
uint8_t TMC4361A::restore() { return 1; }

int32_t TMC4361A::getXACTUAL() { return to_user_pos(readInt(TMC4361A_XACTUAL)); }
void    TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); }
int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); }
int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); }
int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); }
void    TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); }
int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); }
void    TMC4361A::enableIC(bool enable) {
  SET_PIN(m_ennPin, !enable);
  SET_PIN(m_driverIC_ennPin, !enable);
}

int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
  if (abs(value) > 8000000) {
    value = (value < 0) ? -8000000 : 8000000;
  }
  return value << 8;
}
void TMC4361A::rotate(int32_t velocity) {
  // velocity *= m_scale;
  velocity     = to_motor_vel(velocity);
  m_motor_mode = kvelmode;
  PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
  writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
}
void TMC4361A::stop() { rotate(0); }
void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
  // position *= m_scale;
  // velocityMax *= m_scale;

  position    = to_motor_pos(position);
  velocityMax = to_motor_vel(velocityMax);

  m_motor_mode = kposmode;
  PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
  writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
  writeInt(TMC4361A_X_TARGET, position);
}
void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) {
  relativePosition += readInt(TMC4361A_XACTUAL);
  moveTo(relativePosition, velocityMax);
}

int32_t TMC4361A::readICVersion() {
  int32_t value = readInt(TMC4361A_VERSION_NO_RD);
  return (value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT;
}

int32_t TMC4361A::readSubICVersion() { return driverIC_readICVersion(); }
/*******************************************************************************
 *                              2160 function end                              *
 *******************************************************************************/
uint32_t TMC4361A::readEVENTS() {
  uint32_t value = readInt(TMC4361A_EVENTS);
  return value;
}
uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
  if (now >= last) {
    return now - last;
  } else {
    return 0xFFFFFFFF - last + now;
  }
}
bool TMC4361A::isReachTarget() {
  uint32_t value = readInt(TMC4361A_STATUS);
  // printf("TMC4361A_STATUS:%08x\n", value);
  if (m_motor_mode == kposmode) {
    return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0;
  } else {
    return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0);
  }
}
/*******************************************************************************
 *                                  DRIVER_IC                                  *
 *******************************************************************************/
#define DRIVER_ID_FIELD_READ(address, mask, shift)         FIELD_GET(driverIC_readInt(address), mask, shift)
#define DRIVER_ID_FIELD_WRITE(address, mask, shift, value) (driverIC_writeInt(address, FIELD_SET(driverIC_readInt(address), mask, shift, value)))
void TMC4361A::driverIC_reset() {
  SET_PIN(m_driverIC_resetPin, false);
  zchip_clock_early_delayus(1000);
  SET_PIN(m_driverIC_resetPin, true);
  // Reset the dirty bits
  int index = 0;
  while (true) {
    while ((index < TMC2160_REGISTER_COUNT) && !TMC_IS_RESTORABLE(tmc2160_defaultRegisterAccess[index])) {
      index++;
    }
    if (index >= TMC2160_REGISTER_COUNT) {
      break;
    }
    // printf("Resetting register %d\n", index);
    driverIC_writeInt(index, tmc2160_defaultRegisterResetState[index]);
    index++;
  }

  driverIC_writeInt(TMC2160_PWMCONF, 0xC40C001E);
  driverIC_writeInt(TMC2160_DRV_CONF, 0x00080400);
}

void TMC4361A::driverIC_enableIC(bool enable) { SET_PIN(m_driverIC_ennPin, !enable); }
void TMC4361A::driverIC_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
  uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC2160_WRITE_BIT)), x1, x2, x3, x4};
  readWriteCover(&data[0], 5);
}
void    TMC4361A::driverIC_writeInt(uint8_t address, int32_t value) { driverIC_writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
int32_t TMC4361A::driverIC_readInt(uint8_t address) {
  address = TMC_ADDRESS(address);
  // register not readable -> shadow register copy
  if (!TMC_IS_READABLE(tmc2160_defaultRegisterAccess[address])) return 0;

  uint8_t data[5];

  data[0] = address;
  readWriteCover(&data[0], 5);

  data[0] = address;
  readWriteCover(&data[0], 5);

  return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
}
void TMC4361A::driverIC_setMotorShaft(bool reverse) {
  //
  int32_t val = reverse ? 1 : 0;
  DRIVER_ID_FIELD_WRITE(TMC2160_GCONF, TMC2160_SHAFT_MASK, TMC2160_SHAFT_SHIFT, val);
}

uint32_t TMC4361A::driverIC_readICVersion() {
  int32_t value = driverIC_readInt(TMC2160_IOIN___OUTPUT);
  return (value & TMC2160_VERSION_MASK) >> TMC2160_VERSION_SHIFT;
}
void TMC4361A::driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT)); }

// Left Virtual Limit Switch  XACTUAL ≤ VIRT_STOP_LEFT 时触发
void TMC4361A::setLeftVirtualLimitSwitch(bool enable, int32_t position) {
  PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_SHIFT, enable ? 1 : 0);
  writeInt(TMC4361A_VIRT_STOP_LEFT, position);
}

// Right Virtual Limit Switch  XACTUAL ≥ VIRT_STOP_RIGHT 时触发
void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) {
  PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_SHIFT, enable ? 1 : 0);
  writeInt(TMC4361A_VIRT_STOP_RIGHT, position);
}

int32_t TMC4361A::to_motor_acc(int32_t acc) {  //
  int32_t val = acc / 60.0 * 51200;
  return val;
}
int32_t TMC4361A::to_motor_vel(int32_t vel) {  //
  int32_t val = vel / 60.0 * 51200;
  return val;
}  // rpm
int32_t TMC4361A::to_motor_pos(int32_t pos) {  //
  int32_t val = pos * 1.0 / m_scale * 51200.0;
  return val;
}  //

int32_t TMC4361A::to_user_pos(int32_t pos) {  //
  int32_t val = pos / 51200.0 * m_scale;
  return val;
}  //
int32_t TMC4361A::to_user_vel(int32_t vel) {  //
  int32_t val = vel * 60.0 / 51200.0;
  return val;
}

#endif
#endif

#endif