#include "by_stepper.h"

#include <math.h>
#include <string.h>
#include <dwt_delay.h>
#include "by_debug.h"

#define GEAR_INDEXING_DIAMETER       (16.0f)                                               // 齿轮分度圆直径
#define GEAR_MODULUS                 (2.0f)                                                // 齿轮模数
#define ENCODER_PULSE_PER_CIRCLE     (2048.0f)                                             // 编码器每圈脉冲数
#define MILLIMETER_PER_CIRCLE        (GEAR_INDEXING_DIAMETER * GEAR_MODULUS)               // 齿轮每圈行走长度
#define ENCODER_PULSE_PER_MILLIMETER (ENCODER_PULSE_PER_CIRCLE / MILLIMETER_PER_CIRCLE)    // 编码器每毫米脉冲数
#define STEPPER_STEP_ANGLE           (1.8f)                                                // 步进角度
#define DRIVER_PULSE_PER_STEP        (16.0f)                                               // 驱动器细分
#define DRIVER_PULSE_PER_CIRCLE      (360.0f / STEPPER_STEP_ANGLE * DRIVER_PULSE_PER_STEP) // 驱动器每圈脉冲数
#define DRIVER_PULSE_PER_MILLIMETER  (DRIVER_PULSE_PER_CIRCLE / MILLIMETER_PER_CIRCLE)     // 驱动器每毫米脉冲数

#define ENCODER_ERROR_THRESHOLD      (20)

int64_t position        = 0;
int64_t position_aim    = 0;
int64_t position_offset = 0;

uint8_t running_flag = 0;
uint8_t reset_flag   = 0;

// TODO 是否增加一个动作列表？一般都比较简单吧

struct by_stepper_t {
  uint8_t dir;
  stepper_speed_t speed;
};

struct by_stepper_t by_stepper;

static int64_t abs_s64(int64_t x)
{
  return x > 0 ? x : -x;
}

void by_stepper_set_dir(uint8_t dir)
{
  by_stepper.dir = dir;
}

void by_stepper_set_speed(stepper_speed_t speed)
{
  by_stepper.speed = speed;
}

void by_stepper_run(void)
{
  // 根据预设条件发送脉冲
  gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN

  gpio_bits_write(GPIOA, GPIO_PINS_8, by_stepper.dir ? TRUE : FALSE);                    // DIR
  gpio_bits_write(GPIOB, GPIO_PINS_15, !gpio_output_data_bit_read(GPIOB, GPIO_PINS_15)); // CLK
  DWT_Delay(40U * (uint16_t)by_stepper.speed);
}

void by_stepper_stop(uint8_t hold)
{
  gpio_bits_write(GPIOA, GPIO_PINS_9, hold ? FALSE : TRUE); // EN

  gpio_bits_write(GPIOA, GPIO_PINS_8, FALSE);  // DIR
  gpio_bits_write(GPIOB, GPIO_PINS_15, FALSE); // CLK
}

void by_stepper_set_position_millimeter(float distance)
{
  if (!running_flag) {
    // 设置距离较小时直接复位，减少累计定位误差
    if (distance < 3 && distance > -3) {
      reset_flag   = 1;
      running_flag = 1;
      return;
    }

    gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN
    position_aim    = (int64_t)(distance * ENCODER_PULSE_PER_MILLIMETER);
    position_offset = position_aim - position;
    running_flag    = 1;
  }
}

void by_stepper_add_position_millimeter(float distance)
{
  if (!running_flag) {
    gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE); // EN
    position_aim    = position + (int64_t)(distance * ENCODER_PULSE_PER_MILLIMETER);
    position_offset = position_aim - position;
    running_flag    = 1;
  }
}

void by_stepper_init(void)
{
  uint32_t slow_start_cnt = 30000;
  LOGD("by_stepper init...");

  memset(&by_stepper, 0x00, sizeof(by_stepper));

  gpio_bits_write(GPIOB, GPIO_PINS_12, TRUE);  // MODE2
  gpio_bits_write(GPIOB, GPIO_PINS_13, FALSE); // MODE1
  gpio_bits_write(GPIOB, GPIO_PINS_14, FALSE); // MODE0
  gpio_bits_write(GPIOA, GPIO_PINS_9, FALSE);  // EN
  gpio_bits_write(GPIOA, GPIO_PINS_8, TRUE);   // DIR
  gpio_bits_write(GPIOB, GPIO_PINS_15, FALSE); // CLK

  by_stepper_set_dir(0);
  // by_stepper_set_speed(STEPPER_SPEED_DIV2);

  // while (gpio_input_data_bit_read(GPIOB, GPIO_PINS_10) == SET && slow_start_cnt--) {
  //   gpio_bits_write(GPIOA, GPIO_PINS_8, by_stepper.dir ? TRUE : FALSE);                    // DIR
  //   gpio_bits_write(GPIOB, GPIO_PINS_15, !gpio_output_data_bit_read(GPIOB, GPIO_PINS_15)); // CLK
  //   DWT_Delay(40U * (uint16_t)by_stepper.speed);
  // }

  by_stepper_set_speed(STEPPER_SPEED_DIV1);

  while (gpio_input_data_bit_read(GPIOB, GPIO_PINS_10) == SET) {
    gpio_bits_write(GPIOA, GPIO_PINS_8, by_stepper.dir ? TRUE : FALSE);                    // DIR
    gpio_bits_write(GPIOB, GPIO_PINS_15, !gpio_output_data_bit_read(GPIOB, GPIO_PINS_15)); // CLK
    DWT_Delay(40U * (uint16_t)by_stepper.speed);
  }

  position        = 0; // 位置归零
  position_aim    = 0;
  position_offset = 0;
  tmr_counter_value_set(TMR3, 0);

  LOGD("by_stepper init ok");

  by_stepper_set_position_millimeter(160.0f);
}

void by_stepper_loop(void)
{
  int16_t temp = (int16_t)tmr_counter_value_get(TMR3);
  position -= (int64_t)temp;
  tmr_counter_value_set(TMR3, 0);

  if (running_flag) {

    if (reset_flag) // 复位模式
    {
      by_stepper_init();
      running_flag = 0;
      reset_flag   = 0;
    } else // 正常模式
    {
      if (position_offset > 0) {
        by_stepper_set_dir(1);
        by_stepper_run();
      } else if (position_offset < 0) {
        by_stepper_set_dir(0);
        by_stepper_run();
      }

      // 当定位误差小于阈值时，停止运动。否则重新计算定位误差
      if (abs_s64(position_aim - position) < ENCODER_ERROR_THRESHOLD) {
        running_flag    = 0;
        position_offset = 0;
      } else {
        position_offset = position_aim - position;
      }
    }

  } else {
    by_stepper_stop(1);
  }

  // if (position_offset) {
  //   if (position_offset > 0) {
  //     by_stepper_set_dir(0);
  //     by_stepper_run();
  //     position_offset -= 1;
  //   } else if (position_offset++ < 0) {
  //     by_stepper_set_dir(1);
  //     by_stepper_run();
  //     position_offset += 1;
  //   }
  // } else {
  //   by_stepper_stop(0);
  // }
}

void by_stpepper_int(void)
{
  // position += (int16_t)(tmr_counter_value_get(TMR3));
  // tmr_counter_value_set(TMR3, 0);
}