#include <stdio.h>
#include <stdlib.h>  
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"
#include <math.h> 
#include <stdint.h>
/*电机各相引脚*/
#define A1 8  // 电机A1相
#define A2 9  // 电机A2相
#define B1 6  // 电机B1相
#define B2 7  // 电机B2相

/*板载LED引脚*/
#define LED_PIN 25

/*各按钮引脚*/
#define LOCK 17 // 主开关引脚
#define ACCEL_POSITIVE 21  // 正向加速按钮引脚
#define ACCEL_NEGATIVE 27  // 反向加速按钮引脚

/*物理参数*/
#define STEPS 720  // 1308电机每转一圈的步数
#define RATE_BOX 500  // 减速箱减速比
#define SIDEREAL_DAY_SECONDS 86164.0 // 地球自转一周的秒数，没必要

/*电机步进帧间隔*/
#define STEP_per_SEC (STEPS * RATE_BOX / SIDEREAL_DAY_SECONDS)// 恒星速下,电机每秒的步数(跟踪频率)
#define SPEED_STAR (1000000 / STEP_per_SEC)// 恒星速, 每步间隔时间(微秒) 239234um
#define SPEED_HIGH (1000000 / STEP_per_SEC/200)//恒星速64倍, 每步间隔时间(微秒)
#define SPEED_LOW (1000000 / STEP_per_SEC/37)//恒星速32倍, 每步间隔时间(微秒)

/*电机各相的通电序列，接下来，每行的通电状态叫做“步进帧”，每执行一个步进帧，就会步进一次*/
const int step_sequence[4][4] = {
  {1, 0, 1, 0},  // 步骤1: A1=高, A2=低, B1=高, B2=低 (a+ B+) snnssnnssn
  {0, 1, 1, 0},  // 步骤2: A1=低, A2=高, B1=高, B2=低 (a- b+) nnssnnssnn
  {0, 1, 0, 1},  // 步骤3: A1=低, A2=高, B1=低, B2=高 (a- b-) nssnnssnns
  {1, 0, 0, 1}   // 步骤4: A1=高, A2=低, B1=低, B2=高 (a+ b-) ssnnssnnss
};

/*状态标记*/
int motorIsRunning = 0;  // 电机是否运行
int fastMode = 0;      // 快速模式（0-普通模式，1快速模式， -1反向快速模式）
int dir = 1;        // 运转方向
int isHighLoad = 0; //是否是高负载（默认不开启）

/*步进间隔时间，全局可修改*/
uint64_t step_delay = SPEED_STAR;

/*步进帧序号*/
int step_counter = 0;

/*计时用*/
int flag1 = 0;
int flag2 = 0;
uint64_t prev_time;
uint64_t current_time;

/*反向步进函数, 输入:步进帧序号，输出：一次反向步进*/
void set_motor_reverse_step(int step_index) {
    gpio_put(B1, step_sequence[step_index][0]);
    gpio_put(B2, step_sequence[step_index][1]);
    gpio_put(A1, step_sequence[step_index][2]);
    gpio_put(A2, step_sequence[step_index][3]);
}

/*正向步进函数, 输入:步进帧序号，输出：一次正向步进*/
void set_motor_step(int step_index) {
    gpio_put(A1, step_sequence[step_index][0]);
    gpio_put(A2, step_sequence[step_index][1]);
    gpio_put(B1, step_sequence[step_index][2]);
    gpio_put(B2, step_sequence[step_index][3]);
}

/*根据dir和step_counter调用单步步进函数，根据参数delay执行sleepus步进帧间隔，执行Led闪烁*/
void systemStep(uint64_t delayus){
  if (dir == 1)
    set_motor_step(step_counter);
  else
    set_motor_reverse_step(step_counter);
  gpio_put(LED_PIN, step_counter % 2);
  step_counter = (step_counter + 1) % 4;
  sleep_us(delayus);
}

void accelerate_to(uint64_t target_delay, int accel_time_ms) {


    int64_t current = (int64_t)step_delay;
    int64_t target = (int64_t)target_delay;
    int64_t total_diff = target - current;

    if (total_diff == 0) {
        systemStep((uint64_t)current);
        printf("[加速状态] 已达到目标速度: %llu\n", current);
        return;
    }

    // ===== 可调参数 =====
    int total_accel_ms = accel_time_ms;  // 总加速时间
    int accel_boost = 1;                 // 前期额外增量
    // ===================

    uint64_t accel_us = (uint64_t)total_accel_ms * 1000;
    int step_count = 2 * accel_us / (current + target);
    if (step_count <= 0) step_count = 1;

    // 分段：前、中、后、收尾 4 段
    int seg1 = step_count / 4;
    int seg2 = step_count / 4;
    int seg3 = step_count / 4;
    int seg4 = step_count - seg1 - seg2 - seg3;

    int64_t delta_base = total_diff / step_count;
    if (delta_base == 0) delta_base = (total_diff > 0 ? 1 : -1);

    for (int i = 0; i < step_count; i++) {
        int64_t delta;
        if (i == 0) {
            // 第一次跃迁 1/3 delta，加快响应
            delta = (delta_base / 3) + (total_diff > 0 ? accel_boost : -accel_boost);
        } else if (i < seg1) {
            // 前段加权
            delta = delta_base + (total_diff > 0 ? accel_boost : -accel_boost);
        } else if (i < seg1 + seg2) {
            // 中段正常增量
            delta = delta_base;
        } else if (i < seg1 + seg2 + seg3) {
            // 后段略减小，平滑
            delta = delta_base / 2;
            if (delta == 0) delta = (total_diff > 0 ? 1 : -1);
        } else {
            // 收尾段再减小，稳定到目标
            delta = delta_base / 3;
            if (delta == 0) delta = (total_diff > 0 ? 1 : -1);
        }

        current += delta;

        // 防止超调
        if ((total_diff > 0 && current >= target) ||
            (total_diff < 0 && current <= target)) {
            current = target;
            systemStep((uint64_t)current);
            break;
        }

        systemStep((uint64_t)current);
    }

    step_delay = (uint64_t)current;

    uint64_t t_end = time_us_64(); // 结束计时
}

/*轮询函数, 检查三个按钮的引脚电平，切换状态。引脚电平为0 == 开关按下*/
void turns(){
  if(gpio_get(LOCK) == 1){
    motorIsRunning = 0;
  }
  else if(gpio_get(LOCK) == 0){
    motorIsRunning = 1;
  }

  if(gpio_get(ACCEL_POSITIVE) == 0 && gpio_get(ACCEL_NEGATIVE) == 1){ //只按下正向加速
    dir = 1;
    fastMode = 1; 
    flag1 = 0;
    flag2 = 0;
  }
  else if(gpio_get(ACCEL_POSITIVE) == 1  && gpio_get(ACCEL_NEGATIVE) == 0){ //只按下反向加速
    dir = -1;
    fastMode = 1; 
    flag1 = 0;
    flag2 = 0;
  }
  else if(gpio_get(ACCEL_POSITIVE) == 1 && gpio_get(ACCEL_NEGATIVE) == 1){  //正向加速与反向加速都没有按下
    dir = 1;
    fastMode = 0; 
    flag1 = 0;
    flag2 = 0;
  }
  else if(gpio_get(ACCEL_POSITIVE) == 0 && gpio_get(ACCEL_NEGATIVE) == 0){  //正向加速与反向加速都有按下
    if(flag1 == 0){
      prev_time = time_us_64();
      flag1 = 1;//上锁
    }
    if((time_us_64() - prev_time)/1000000 > 3 && flag2 == 0){//设置高载重模式触发时长为5s，如果时间不足，在切换到其它模式后，flag会解锁。
      isHighLoad = 1 - isHighLoad;
      printf("切换状态");//首次执行关闭高负载模式。之后是在低负载与高负重之间切换
      for(int i = 0 ; i< 15 ;i ++){
        gpio_put(LED_PIN, i%2);
        sleep_ms(100);
      }//切换后会快速闪烁3秒。
      flag2 = 1;
    }
  }
}

/*初始化开发板GPIO引脚电气状态*/
void init_gpio() {
    // 初始化四项引脚为输出（set_dir:设置输出的方向）
    gpio_init(A1);
    gpio_init(A2);
    gpio_init(B1);
    gpio_init(B2);
    gpio_set_dir(A1, GPIO_OUT);
    gpio_set_dir(A2, GPIO_OUT);
    gpio_set_dir(B1, GPIO_OUT);
    gpio_set_dir(B2, GPIO_OUT);

    // 初始化按钮引脚为输入
    gpio_init(LOCK);
    gpio_init(ACCEL_POSITIVE);
    gpio_init(ACCEL_NEGATIVE);
    gpio_set_dir(LOCK, GPIO_IN);
    gpio_set_dir(ACCEL_POSITIVE, GPIO_IN);
    gpio_set_dir(ACCEL_NEGATIVE, GPIO_IN);

    //对按钮引脚启用内部上拉电阻
    gpio_pull_up(LOCK);
    gpio_pull_up(ACCEL_POSITIVE);
    gpio_pull_up(ACCEL_NEGATIVE);
  
    // 初始化板载LED为输出
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
}

int main() {
    stdio_init_all();
    init_gpio();
    printf("初始化完毕\n");
    printf("--------------------------------------\n");

    while (true) {
      turns();//引脚状态轮询，修改三个按钮状态
      if (motorIsRunning) {
        //printf("电机开启：%d,step_counter:%d \n", motorIsRunning, step_counter);
        uint64_t target_delay;
        switch (fastMode) {
          case 1:
            if (isHighLoad == 1)
              target_delay = SPEED_LOW;
            else if (isHighLoad == 0)
              target_delay = SPEED_HIGH;
            break;
          case 0:
            target_delay = SPEED_STAR;
            break;
        }

        // 若目标速度与当前速度不同，则执行加速过程
        if (step_delay != target_delay) {
            accelerate_to(target_delay,100); //平滑变速函数
        } else {
            systemStep(step_delay);
        }
        
      } else {
        //printf("电机未开启");
        // 电机未运行时，四相归0，关闭LED
        gpio_put(A1, 0);
        gpio_put(A2, 0);
        gpio_put(B1, 0);
        gpio_put(B2, 0);
        gpio_put(LED_PIN, 0);
        // 短暂睡眠，避免CPU过度占用
        sleep_ms(50);
      }
    }
    return 0;
}