#include "by_motion.h"

#include "pid.h"
#include "dwt_delay.h"
#include "by_utils.h"
#include "by_debug.h"

#define DRV_ENABLE()  gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)

#define MU            (2.854f) // speed = \mu * speed_in_pulse

by_motor_param param_m1;
by_motor_param param_m2;
PID_TypeDef pid_m1;
PID_TypeDef pid_m2;
uint8_t motion_enable_flag;
uint8_t motion_busy_flag;
int16_t time_via;
float position_abs;

void by_motion_set_pwm_m1(int32_t pwm_duty)
{
  pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
  pwm_duty += 499;

  // 互补 pwm 输出，499 为中值
  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}

// void by_motion_set_pwm_m2(int32_t pwm_duty)
// {
//   pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
//   pwm_duty += 499;

//   // 互补 pwm 输出，499 为中值
//   tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
// }

int16_t by_motion_get_speed_m1(void)
{
#define alpha (0.1f)
  static float last_speed = 0.0f;
  param_m1.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(+1 * (int16_t)tmr_counter_value_get(TMR2));
  last_speed              = param_m1.real_speed;
  tmr_counter_value_set(TMR2, 0);
  return (int16_t)param_m1.real_speed;
#undef alpha
}

// int16_t by_motion_get_speed_m2(void)
// {
// #define alpha (0.1f)
//   static float last_speed = 0.0f;
//   param_m2.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
//   last_speed              = param_m2.real_speed;
//   tmr_counter_value_set(TMR3, 0);
//   return (int16_t)param_m2.real_speed;
// #undef alpha
// }

void by_motion_set_speed_m1(int16_t speed)
{
  param_m1.target_speed = speed;
}

// void by_motion_set_speed_m2(int16_t speed)
// {
//   param_m2.target_speed = speed;
// }

void by_motion_set_distance(float distance, int16_t speed)
{
  if (motion_busy_flag) {
    return;
  }

  if (speed < 0) {
    speed = -speed;
  }

  if (distance < 0.0f) {
    by_motion_set_speed_m1(-1 * speed);
    distance = -distance;
  } else {
    by_motion_set_speed_m1(speed);
  }

  motion_busy_flag = 1;
  position_abs += distance;
  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
  // lwprintf("time_via = %d\r\n", time_via);
}

void by_motion_set_distance2(float distance, int16_t speed)
{
  if (motion_busy_flag) {
    return;
  }

  if (speed < 0) {
    speed = -speed;
  }

  if (distance < 0.0f) {
    distance = -distance;
  }

  distance = position_abs - distance;

  if (distance < 0.0f) {
    distance = -distance;
    by_motion_set_speed_m1(-1 * speed);
  } else {
    by_motion_set_speed_m1(speed);
  }

  motion_busy_flag = 1;
  position_abs += distance;
  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
  // lwprintf("time_via = %d\r\n", time_via);
}

void by_motion_init(void)
{
  motion_enable_flag = 0;
  motion_busy_flag   = 0;
  time_via           = 0;
  position_abs       = 0.0f; // TODO 待添加复位操作

  by_motion_set_pwm_m1(0);
  // by_motion_set_pwm_m2(0);

  /* set default parameters */
  param_m1.Kp = BY_MOTION_DEFAULT_KP_M1;
  param_m1.Ki = BY_MOTION_DEFAULT_KI_M1;
  param_m1.Kd = BY_MOTION_DEFAULT_KD_M1;
  // param_m2.Kp = BY_MOTION_DEFAULT_KP_M2;
  // param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
  // param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;

  /* load parameters form ee */
  // TODO

  PID(&pid_m1, &param_m1.real_speed, &param_m1.out_pwm, &param_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
  PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC);
  PID_SetSampleTime(&pid_m1, 10);
  PID_SetOutputLimits(&pid_m1, -400, 400);

  // PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
  // PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
  // PID_SetSampleTime(&pid_m2, 10);
  // PID_SetOutputLimits(&pid_m2, -400, 400);

  motion_enable_flag = 1;
  DRV_ENABLE();
}

void by_motion_run(void)
{
  if (motion_enable_flag) {
    by_motion_get_speed_m1();
    // by_motion_get_speed_m2();

    PID_Compute(&pid_m1);
    // PID_Compute(&pid_m2);

    by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
    // by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
  }
}

void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
{

  if (0x08 == stdd_id) {
    uint8_t mode  = data[0];
    int16_t speed = (int16_t)data[1];
    // float distance = (float)(data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24));
    float distance;
    memcpy(&distance, &data[2], 4);
    if (0 == mode) {
      by_motion_set_distance2(distance, speed);
    } else if (1 == mode) {
      by_motion_set_distance(distance, speed);
    }
  }

  // #define BC2D_MODEL1

  //   if (0x01 == stdd_id) {
  // #if defined(BC2D_MODEL1)
  //     int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8));
  //     int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8));
  //     by_motion_set_speed_m1(speed_m1_temp);
  //     by_motion_set_speed_m2(speed_m2_temp);
  // #elif defined(BC2D_MODEL2)
  //     int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8));
  //     int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8));
  //     by_motion_set_speed_m1(speed_m1_temp);
  //     by_motion_set_speed_m2(speed_m2_temp);
  // #endif
  // }

  LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
  LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm);
}

void by_motion_tmr_handle(void)
{
  if (!motion_busy_flag) {
    return;
  }

  if (time_via > 0) {
    time_via--;
  } else {
    by_motion_set_speed_m1(0);
    motion_busy_flag = 0;
  }
}