/*
下一版常速中要加入步数补偿机制，抵消代码自身运行的时间损耗。
*/

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"

// 引脚定义
#define A1 4  // 电机驱动A1相
#define A2 5  // 电机驱动A2相
#define B1 2  // 电机驱动B1相
#define B2 3  // 电机驱动B2相

#define Lock 17 // 自锁按钮引脚
#define Accel_Positive 21  // 正向加速按钮引脚
#define Accel_Negative 27  // 反向加速按钮引脚

#define STEPS 720  // 电机每转一圈的步数，不确定，店家的数据，官网已经没有这款电机的数据了
#define RATE_BOX 500  // 减速箱比例

// 地球每自转一周秒数
#define SIDEREAL_DAY_SECONDS 86164
// 每秒需要的步数,编译阶段就会算出float具体值
#define STEP_per_SEC (STEPS * RATE_BOX / SIDEREAL_DAY_SECONDS)
// 计算每步延迟时间(微秒)
#define SPEED_1 (1000000 / STEP_per_SEC)
// 64倍速时的延迟时间
#define SPEED_32 (1000000 / STEP_per_SEC/64)
#define SPEED_8 (1000000 / STEP_per_SEC/8)
// 按钮防抖动延迟(毫秒)
#define DEBOUNCE_TIME 99

// 电机运行状态
int motor_running = 0;  // 电机是否运行
int fast_mode = 0;     
int dir = 1;

// 步进电机序列 - 四步序列驱动方式
const int step_sequence[4][4] = {
  {1, 0, 1, 0},  // 步骤1: A1=高, A2=低, B1=高, B2=低
  {0, 1, 1, 0},  // 步骤2: A1=低, A2=高, B1=高, B2=低
  {0, 1, 0, 1},  // 步骤3: A1=低, A2=高, B1=低, B2=高
  {1, 0, 0, 1}   // 步骤4: A1=高, A2=低, B1=低, B2=高
};

// 按钮中断处理函数的最后触发时间
volatile uint64_t last_time = 0;

// 板载LED引脚
#define LED_PIN 25

//步进函数
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]);
}

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 turns(){
  if(gpio_get(Lock) == 1){//高电平 (3.3V),关闭电机
    motor_running = 0;
  }
  else if(gpio_get(Lock) == 0){
    motor_running = 1;
  }

  if(gpio_get(Accel_Positive) == 0){
    fast_mode = 1; 
  }

  else if(gpio_get(Accel_Negative) == 0){
    fast_mode = -1; 
  }
  else if(gpio_get(Accel_Positive) == 1 && gpio_get(Accel_Negative) == 1){
    fast_mode = 0; 
  }
}

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();
    int step_counter = 0;

    while (true) {
        turns();//轮询
        if (motor_running) {
            // 根据当前模式选择延迟时间
            uint64_t step_delay;
            switch(fast_mode){
              case 1:
                dir = 1;
                step_delay = SPEED_32;
                break;
              case -1:
                dir = -1;
                step_delay = SPEED_32;
                break;
              case 0:
                dir = 1;
                step_delay = SPEED_1;
                break;
            }

            if(dir == 1)
              set_motor_step(step_counter);
            else if(dir == -1)
              set_motor_reverse_step(step_counter);

            // 控制板载LED
            if (step_counter % 2 == 1) {
                gpio_put(LED_PIN, 1);  // 奇数步亮起LED
            } 
            else {
                gpio_put(LED_PIN, 0);  // 偶数步熄灭LED
            }

            step_counter++;
            if (step_counter >= 4) {
                step_counter = 0;
            }
            sleep_us(step_delay);//控制电机频率
        } 
        else {
            // 电机未运行时，关闭所有输出以节省能源
            gpio_put(A1, 0);
            gpio_put(A2, 0);
            gpio_put(B1, 0);
            gpio_put(B2, 0);
            // 关闭LED
            gpio_put(LED_PIN, 0);
            // 短暂睡眠，避免CPU过度占用
            sleep_ms(50);
        }
    }
    return 0;
}