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

//
#include "../tmc/tmc_type.h"
#include "reg/TMC5130_Type.h"
#include "stm32basic/stm32basic.hpp"
namespace iflytop {
/**
 * @brief
 *
 */
class TMC51X0Cfg {
 public:
  SPI_HandleTypeDef *hspi = NULL;
  Pin_t              csnpin;
  Pin_t              ennpin;

  TMC51X0Cfg(SPI_HandleTypeDef *hspi, Pin_t csnpin, Pin_t ennpin) : hspi(hspi), csnpin(csnpin), ennpin(ennpin) {}
  TMC51X0Cfg() {}
};

class TMC51X0 {
 protected:
  TMC51X0Cfg m_cfg;

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

  int32_t     m_scale              = 10000;
  int32_t     m_scale_deceleration = 1;
  mres_type_t m_MRES               = kmres_256;
  double      m_onecirclepulse     = 51200;
  int32_t     m_enc_resolution     = 0;  // 

  zmutex m_mutex  = {"TMC51X0"};
  bool   m_driErr = false;

 public:
  void initialize(TMC51X0Cfg cfg);

 private:

  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);
  void     writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t value);
  int32_t  readInt(uint8_t address);
  uint32_t readUInt(uint8_t address);

 public:
  bool ping();
  void enable(bool enable);

  void setdriErr(bool val) { m_driErr = val; }
  bool getdriErr() { return m_driErr; }
  /***********************************************************************************************************************
   *                                                        ctrl                                                         *
   ***********************************************************************************************************************/
  void rotate(int32_t velocity);
  void moveTo(int32_t position, uint32_t velocityMax);
  void moveToEnd(int32_t direction, uint32_t velocityMax);
  void moveBy(int32_t relativePosition, uint32_t velocityMax);
  void stop();
  /***********************************************************************************************************************
   *                                                      get state                                                      *
   ***********************************************************************************************************************/
  int32_t               getXACTUAL();  // now pos
  int32_t               getVACTUAL();  // now vel
  int32_t               getEncVal();   // ENCVAL
  TMC5130RampStat       getRampStat();
  TMC5130DevStatusReg_t getDevStatus();  // R
  TMC5130GState_t       getGState();     // R+C read and clear
  int32_t               readICVersion();

  int32_t getXactualRAW();// now pos 51200

  bool isReachTarget(TMC5130RampStat *state);  // is reach target
  bool isTMC5130();

  /***********************************************************************************************************************
   *                                                      set state                                                      *
   ***********************************************************************************************************************/
  void setXACTUAL(int32_t value);  // set pos
  void setMotorShaft(bool reverse);
  void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
  void setGlobalScale(uint8_t globalscale);  // ONLY for 5160
  void setScale(int32_t scale);
  void setScaleDenominator(int32_t scale);
  void setVstart(int32_t val);
  void setA1(int32_t val);
  void setAmax(int32_t val);
  void setV1(int32_t val);
  void setDmax(int32_t val);
  void setD1(int32_t val);
  void setVstop(int32_t val);
  void setTzerowait(int32_t val);
  bool setEncResolution(int32_t enc_resolution);
  void setEncVal(int32_t enc_val);

  /***********************************************************************************************************************
   *                                                    reg operation                                                    *
   ***********************************************************************************************************************/
  void    writeIntExt(uint8_t address, int32_t value);
  int32_t readIntExt(uint8_t address);

 private:
  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