#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "../basic/tmc_ic_interface.hpp"
#include "sdk/os/zos.hpp"

extern "C" {
#include "TMC5130\TMC5130.h"
}
#ifdef HAL_SPI_MODULE_ENABLED
namespace iflytop {
#define TMC5130_LISTENER_MAX 5
/**
 * @brief
 *
 * 注意事项：
 *  1. 速度单位
 *      velocity的单位是:p/t
 *      t = 2^24/fin (fin为外接时钟，当时钟频率为16MHz时，t = 1.048576s)
 *      此时 ppt约等于pps
 *
 *      参考TMC5130A_datasheet_rev1.20%20(1)%20(3).pdf 38页 蓝框中的字
 *
 *  2. 芯片复位方法
 *      reset(); 复位芯片，同时会将芯片的寄存器设置为默认值。一般情况下不需要用户主动调用该方法，因为在初始化时会自动调用该方法。
 *
 */

class Tmc5130RampStat {
 public:
  uint32_t m_state;
  // 参考TMC5130A_datasheet_rev1.20%20(1).pdf 40页
  typedef enum {
    /**
     * @brief
     *  R  ：只读
     *  R+C：只读，但读后会自动清零
     */
    ktmc5130_rs_stopl         = TMC5130_RS_STOPL,          // (R  )
    ktmc5130_rs_stopr         = TMC5130_RS_STOPR,          // (R  )
    ktmc5130_rs_latchl        = TMC5130_RS_LATCHL,         // (R+C)
    ktmc5130_rs_latchr        = TMC5130_RS_LATCHR,         // (R+C)
    ktmc5130_rs_ev_stopl      = TMC5130_RS_EV_STOPL,       // (R  )
    ktmc5130_rs_ev_stopr      = TMC5130_RS_EV_STOPR,       // (R  )
    ktmc5130_rs_ev_stop_sg    = TMC5130_RS_EV_STOP_SG,     // (R+C)
    ktmc5130_rs_ev_posreached = TMC5130_RS_EV_POSREACHED,  // (R+C)
    ktmc5130_rs_velreached    = TMC5130_RS_VELREACHED,     // (R  )
    ktmc5130_rs_posreached    = TMC5130_RS_POSREACHED,     // (R  ) 是否到达目标位置
    ktmc5130_rs_vzero         = TMC5130_RS_VZERO,          // (R  )
    ktmc5130_rs_zerowait      = TMC5130_RS_ZEROWAIT,       // (R  )
    ktmc5130_rs_secondmove    = TMC5130_RS_SECONDMOVE,     // (R+C)
    ktmc5130_rs_sg            = TMC5130_RS_SG,             // (R  )
  } ramp_stat_bit_t;

  Tmc5130RampStat(uint32_t state) : m_state(state) {}
  bool isSetted(ramp_stat_bit_t bit) { return (m_state & bit) != 0; }
};

class TMC5130 : public IStepperMotor {
 public:
  typedef struct {
    SPI_HandleTypeDef *spi;
    Pin_t              csgpio          = PinNull;  //
    Pin_t              resetPin        = PinNull;  //
    Pin_t              fREEZEPin       = PinNull;  //
    Pin_t              ennPin          = PinNull;  //
    Pin_t              spi_mode_select = PinNull;  //
  } cfg_t;

 protected:
  // TMC5130Port     *m_port;
  // TMC5130Config_t *m_config;

  cfg_t m_cfg;

  ZGPIO             *m_csnpin               = NULL;
  ZGPIO             *m_ennpin               = NULL;
  ZGPIO             *m_spi_mode_select_gpio = NULL;
  SPI_HandleTypeDef *m_hspi                 = NULL;

  // uint8_t  m_channel;
  uint32_t m_lastCallPeriodicJobTick;
  int32_t  m_shadowRegister[TMC_REGISTER_COUNT];

  const uint8_t *m_registerAccessTable;
  const int32_t *m_defaultRegisterResetState;

  int32_t m_scale              = 10000;
  int32_t m_scale_deceleration = 1;

  mres_type_t m_MRES           = kmres_256;
  double      m_onecirclepulse = 51200;

 public:
  TMC5130(/* args */);

  // static void createDeafultTMC5130Config(TMC5130Config_t *config, TMC5130Port *m_port);
  void initialize(cfg_t *cfg);

  void         enableIC(bool enable);
  virtual void enable(bool enable) { enableIC(enable); }

  uint8_t reset();

  /*******************************************************************************
   *                             常用寄存器读写方法                              *
   *******************************************************************************/

  uint32_t        readICVersion();            // 5130:0x11 5160:0x30
  virtual int32_t getXACTUAL();               // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
  virtual void    setXACTUAL(int32_t value);  // 设置电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置

  virtual int32_t getVACTUAL();   // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
  uint32_t        readXTARGET();  // 读取驱动器目标位置

  virtual int32_t getENC_POS() { return 0; }    // TODO impl it
  virtual void    setENC_POS(int32_t value) {}  // TODO impl it

  virtual void setScale(int32_t scale);
  virtual void setScaleDenominator(int32_t scale);

  virtual void setAcceleration(float accelerationpps2);
  virtual void setDeceleration(float accelerationpps2);

  void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
  void setGlobalScale(uint8_t globalscale);
  // void setSubdivision(uint8_t subdivision);

  void setMotorShaft(bool reverse);  // 设置电机旋转方向

  uint32_t        getTMC5130_RAMPSTAT();  // 这个寄存器读取的是TMC5130的状态寄存器
  Tmc5130RampStat getTMC5130_RAMPSTAT2();

  virtual void rotate(int32_t velocity);
  virtual void right(int32_t velocity);
  virtual void left(int32_t velocity);

  virtual void moveTo(int32_t position, uint32_t velocityMax);
  virtual void moveBy(int32_t relativePosition, uint32_t velocityMax);

  virtual void stop();
  virtual bool isReachTarget();  // 是否到达目标位置
  virtual bool isStoped() { return isReachTarget(); }

  /*******************************************************************************
   *                             驱动器寄存器读写方法                              *
   *******************************************************************************/
  // void    writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value);
  void    readWriteArray(uint8_t *data, size_t length);
  void    writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
  void    writeInt(uint8_t address, int32_t value);
  int32_t readInt(uint8_t address);

  void setNoAccLimit(bool enable) override;

  /**
   * @brief 设置电机细分
   *
   * @param value
   */
  void setMRES(mres_type_t value);

 private:
  uint32_t haspassedms(uint32_t now, uint32_t last);

  int32_t to_motor_acc(int32_t acc);  // rpm/s^2
  int32_t to_motor_vel(int32_t vel);  // rpm
  int32_t to_motor_pos(int32_t pos);  //

  int32_t to_user_pos(int32_t pos);  //
  int32_t to_user_vel(int32_t vel);
};
}  // namespace iflytop
#endif