#pragma once
#include <Arduino.h>
#include <CAN.h>
#include <ESP32SJA1000.h>
#define MOTO_REVERSED -1 // 正反转1为正转，-1为反转
#define MOTO_REDUCTION 36
#define MOTO_MAXANG 8192 // 0-8191
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2

#define OUTPUT_MAX 400  // 最大输出转速rpm
#define SACCEL_TIME 100 // 使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就  (200*SACCEL_TIME)/OUTPUT_MAX  秒

typedef struct _PID_TypeDef
{
  float target; // 目标值====
  float lastNoneZeroTarget;
  float kp;
  float ki;
  float kd;
  float measure;  // 测量值
  float err;      // 误差
  float last_err; // 上次误差
  float pout;
  float iout;
  float dout;
  float output; // 本次输出====
  // float last_output;			//上次输出
  float MaxOutput;     // 输出限幅=====
  float IntegralLimit; // 积分限幅=====
  float DeadBand;      // 死区（绝对值）=====
  // float ControlPeriod;		//控制周期====
  float Max_Err; // 最大误差====

  uint32_t thistime;
  uint32_t lasttime;
  uint8_t dtime;

} PID_TypeDef;

typedef struct
{
  int16_t speed_rpm;
  int16_t real_current;
  uint8_t temperature;
  uint16_t angle;      // abs angle range:[0,8191]
  uint16_t last_angle; // abs angle range:[0,8191]
  uint16_t offset_angle;
  int32_t round_cnt;
  int32_t total_angle;
  uint32_t msg_cnt;

  unsigned long starttime;

  float output_speed_rpm; // 输出轴速度 ---rpm
  float output_round_cnt; // 输出轴 总圈数
} moto_measure_t;

/// PID控制器
void pid_param_init(PID_TypeDef *pid,
                    uint16_t maxout,
                    uint16_t intergral_limit,
                    float deadband,
                    int16_t max_err,
                    int16_t target,
                    float kp,
                    float ki,
                    float kd);
inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd);
inline void pid_target(PID_TypeDef *pid, float new_target);
float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget);
void pid_init();

class moto
{
private:
  bool _closed;
  float _ds;
  int16_t _start_speed;
  float _acctime;
  void set_moto_current(int16_t iq1);
  static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr);
  static void onReceive(int packetSize);

public:
  moto();  // 构造函数
  ~moto(); // 析构函数
  bool init();
  void update();
  // 设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
  // 否则setspeedtarget(0)会按照settime_acc的时间停止
  void setspeedtarget(float new_target);
  void settime_acc(float acctime = -1); // 设置加速和减速时间
  void close();                         // 关闭电机供电
  moto_measure_t getmotoinfo();
};