#include "moto.h"
#include "Arduino.h"
#include "config.h"

uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis;
PID_TypeDef moto_pid;

moto::moto()
{
  _closed = true;
}
bool moto::init()
{
  Serial.println("init moto");
  pid_init();
  CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
  // start the CAN bus at 500 kbps
  if (!CAN.begin(1000E3))
  {
    Serial.println("Starting CAN failed!");
    return false;
  }
  CAN.onReceive(onReceive);
  return true;
}
moto::~moto()
{
}

void moto::update()
{
  if (!_closed)
  {
    unsigned long dt = millis() - moto_chassis.starttime; // 时间差
    float x = 0.0f;
    float mspeed = 0.0f;
    float _msoftspeed = moto_pid.target;
    if (dt < _acctime)
    {
      x = float(dt) / _acctime; // 归一化时间
      // 公式参考 https://mp.weixin.qq.com/s/bhdvA3Ex6lAWVmBril-Sag
      mspeed = (-2 * x * x * x + 3 * x * x); // S曲线速度公式--计算归一化速度
      _msoftspeed = _start_speed + mspeed * _ds;
    }
    pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
    set_moto_current(moto_pid.output);
    // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
    // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
  }
}
void moto::settime_acc(float acctime)
{
  if (acctime == -1)
    _acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间
  else
    _acctime = acctime;

  printf("settime_acc:%.2f\n", _acctime);
}

void moto::setspeedtarget(float new_target)
{
  moto_pid.target = new_target * MOTO_REDUCTION;
  moto_chassis.starttime = millis(); // 换算为绝对时间
  _start_speed = moto_chassis.speed_rpm;
  _ds = moto_pid.target - _start_speed; // 速度差
  _closed = false;
  printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
}

void moto::set_moto_current(int16_t iq1)
{
  iq1 = MOTO_REVERSED * iq1;
  CAN.beginPacket(0x200, 8);
  CAN.write((uint8_t)((iq1 >> 8) & 0xFF));
  CAN.write((uint8_t)(iq1 & 0xFF));
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.endPacket();
}
void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
{
  // 处理数据
  ptr->last_angle = ptr->angle;
  ptr->angle = (uint16_t)MOTO_REVERSED * (message_ptr[0] << 8 | message_ptr[1]);       // 角度
  ptr->speed_rpm = (int16_t)MOTO_REVERSED * (message_ptr[2] << 8 | message_ptr[3]);    // 速度
  ptr->real_current = (int16_t)MOTO_REVERSED * (message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流
  ptr->temperature = message_ptr[6];                                                   // 电机温度，2006没有，为0

  if (ptr->angle - ptr->last_angle > MOTO_MAXANG_HALF)
    ptr->round_cnt--;
  else if (ptr->angle - ptr->last_angle < -MOTO_MAXANG_HALF)
    ptr->round_cnt++;
  ptr->total_angle = ptr->round_cnt * MOTO_MAXANG + ptr->angle - ptr->offset_angle;
}

moto_measure_t moto::getmotoinfo()
{
  // 在中断里面计算时间太长会导致重启,在取信息时计算浮点
  moto_chassis.output_speed_rpm = (float)moto_chassis.speed_rpm / MOTO_REDUCTION;
  moto_chassis.output_round_cnt = (moto_chassis.round_cnt + (float)(moto_chassis.angle - moto_chassis.offset_angle) / MOTO_MAXANG) / MOTO_REDUCTION;
  return moto_chassis;
}
// 关闭电机输出，防止堵转
void moto::close()
{
  pid_init();
  set_moto_current(0);
  _closed = true;
}
void moto::onReceive(int packetSize)
{
  if (!CAN.packetRtr())
  {
    for (int i = 0; i < packetSize; i++)
    {
      CaninBuffer[i] = CAN.read();
    }
    get_moto_measure(&moto_chassis, CaninBuffer);
  }
}
/////////////PID控制
void pid_init()
{
  pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 10.0f, 1.0f, 0.5f); //
  // pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 14.0f, 0.01f, 0.0f); ////
}
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)
{

  memset(pid, 0, sizeof(PID_TypeDef));

  pid->DeadBand = deadband;
  pid->IntegralLimit = intergral_limit;
  pid->MaxOutput = maxout;
  pid->Max_Err = max_err;
  pid->target = target;

  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;

  pid->output = 0;
}

inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd)
{
  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;
}

inline void pid_target(PID_TypeDef *pid, float new_target)
{
  pid->target = new_target;
}

float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget)
{

  pid->lasttime = pid->thistime;
  pid->thistime = xTaskGetTickCount() * portTICK_RATE_MS; // 换算为绝对时间
  pid->dtime = pid->thistime - pid->lasttime;             // 更新两次计算的时间

  pid->last_err = pid->err;
  pid->err = pidtarget - measure;
  // pid->err = pid->target - measure;

  if (abs(pid->err) > pid->DeadBand)
  {
    pid->pout = pid->kp * pid->err;
    pid->iout += (pid->ki * pid->err);
    pid->dout = pid->kd * (pid->err - pid->last_err); // 除以dtime？？

    // 积分限幅
    if (pid->iout > pid->IntegralLimit)
      pid->iout = pid->IntegralLimit;
    if (pid->iout < -pid->IntegralLimit)
      pid->iout = -pid->IntegralLimit;

    pid->output = pid->pout + pid->iout + pid->dout;

    if (pid->output > pid->MaxOutput)
    {
      pid->output = pid->MaxOutput;
    }
    if (pid->output < -(pid->MaxOutput))
    {
      pid->output = -(pid->MaxOutput);
    }
  }

  return pid->output; // 进入死区维持原值
}