#include "menu.h"
#include <Bounce2.h>

#define gpioTriggerPin 13// GPIO触发引脚
Bounce2::Button gpioTrigger = Bounce2::Button();



#define M1_PWM 3    // PWM输出引脚
#define M2_PWM 5    // PWM输出引脚
#define M3_PWM 7    // PWM输出引脚
#define M4_PWM 9    // PWM输出引脚


#define M1_DIR 2    // DIR输出引脚
#define M1_DIR 4    // DIR输出引脚
#define M1_DIR 6    // DIR输出引脚
#define M1_DIR 8    // DIR输出引脚

const int maxSpeed = 255;      // 最大速度
float accel = 100.0;     // 加速度 (单位: m/s^2)
float decel = 100.0;     // 减速度 (单位: m/s^2)
float totalTime = 10.0;  // 总时间 (单位: 秒)
float accelTime = maxSpeed / accel;  // 加速时间
float decelTime = maxSpeed / decel;  // 减速时间
float constantTime = totalTime - accelTime - decelTime;  // 恒速时间
int timeSteps = 100;     // 时间步数 (越多越平滑)
float timeStep = totalTime / timeSteps;    // 每步时间间隔

void setup() {
  // put your setup code here, to run once:
  Serial1.begin(9600);
  Serial1.println("Hello, Raspberry Pi Pico!");
  initmenu();

  pinMode(M1_PWM, OUTPUT);         // 设置PWM引脚为输出模式
  pinMode(M2_PWM, OUTPUT);         // 设置PWM引脚为输出模式
  pinMode(M3_PWM, OUTPUT);         // 设置PWM引脚为输出模式
  pinMode(M4_PWM, OUTPUT);         // 设置PWM引脚为输出模式
  gpioTrigger.attach(gpioTriggerPin,INPUT_PULLUP);
  analogWriteFreq(18000);
  analogWriteRange(maxSpeed);
}

// 生成速度曲线
void generateSpeedCurve(float speedArray[]) {
  float time = 0.0;  // 初始化时间变量

  for (int i = 0; i < timeSteps; i++) {
    if (time <= accelTime) {
      // 加速阶段
      speedArray[i] = accel * time;
    } else if (time <= accelTime + constantTime) {
      // 恒速阶段
      speedArray[i] = maxSpeed;
    } else {
      // 减速阶段
      speedArray[i] = maxSpeed - decel * (time - accelTime - constantTime);
    }

    // 限制速度为非负值
    if (speedArray[i] < 0) {
      speedArray[i] = 0;
    }

    // 强制设置最后一个时间点的速度为零
    if (i == timeSteps - 1) {
      speedArray[i] = 0;
    }

    time += timeStep;  // 更新时间
  }
}

// 执行PWM程序
void runPWM(float speedArray[]) {
  for (int i = 0; i < timeSteps; i++) {
    // 如果检测到GPIO3再次按下，中断程序
    gpioTrigger.update();
    if (gpioTrigger.rose()){
      Serial1.println("GPIO3检测到中断，停止PWM...");
      analogWrite(M1_PWM, 0);  // 将PWM输出设置为0
      return;                 // 停止运行
    }

    // 输出PWM信号
    analogWrite(M1_PWM, (int)speedArray[i]);  // 转换速度值为整数并发送到PWM

    // 串口打印当前速度
    Serial1.print("时间点: ");
    Serial1.print(i);
    Serial1.print(", 当前速度: ");
    Serial1.println(speedArray[i]);

    delay((int)(timeStep * 1000));  // 根据时间步长设置延迟
  }
  Serial1.println("PWM程序完成。");
}

void loop() {
  gpioTrigger.update();
  if (gpioTrigger.rose()){// 检测GPIO3触发
    Serial1.println("启动PWM程序...");
    float speedArray[timeSteps];  // 存储速度曲线的数组
    generateSpeedCurve(speedArray);  // 生成速度曲线
    runPWM(speedArray);              // 执行PWM程序
  } else {
    //Serial1.println("等待GPIO3触发...");
    delay(50);  // 防止连续打印，设置延迟
  }
  updatemenu();
}