/**
 * 第十二届全国大学生机械创新设计大赛 - 仿生蝴蝶仿真程序
 * 专为Wokwi ESP32仿真环境优化
 * 功能：上电执行规定动作，随后可通过虚拟遥控器控制扑翼。
 */

#include "butterfly_config.h" // 自定义配置文件
#include <ESP32Servo.h>

// 创建舵机对象
Servo leftWingServo;
Servo rightWingServo;

// 虚拟遥控信号变量 (在Wokwi中用按钮模拟)
int virtualThrottle = 1500; // 默认中位值
int virtualYaw = 1500;

// 系统状态
bool startupCompleted = false;
unsigned long lastFlapTime = 0;
int flapPhase = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("=== 仿生蝴蝶仿真启动 ===");

  // 1. 初始化舵机
  leftWingServo.attach(PIN_SERVO_LEFT);
  rightWingServo.attach(PIN_SERVO_RIGHT);
  delay(300);

  // 2. 执行比赛规定动作
  executeStartupSequence();

  Serial.println("规定动作完成。");
  Serial.println("虚拟遥控说明：");
  Serial.println("  - 'A'键: 增大油门 (加速扑翼)");
  Serial.println("  - 'S'键: 减小油门 (减速扑翼)");
  Serial.println("  - 'D'键: 左转偏置");
  Serial.println("  - 'F'键: 右转偏置");
  Serial.println("  - 'G'键: 回中");
  Serial.println("=========================");
}

void loop() {
  // 1. 检查键盘输入，更新虚拟遥控信号（仿真环境用）
  updateVirtualRemote();

  // 2. 解析信号，生成控制量
  int flapSpeed = map(virtualThrottle, RC_MIN, RC_MAX, 0, 100);
  int turnBias = map(virtualYaw, RC_MIN, RC_MAX, -TURN_MAX, TURN_MAX);

  // 3. 生成并执行翅膀动作
  if (startupCompleted) {
    controlWings(flapSpeed, turnBias);
  }

  // 4. 定期打印状态到串口监视器
  static unsigned long lastPrintTime = 0;
  if (millis() - lastPrintTime > 1000) {
    lastPrintTime = millis();
    Serial.print("状态: 油门=");
    Serial.print(virtualThrottle);
    Serial.print(" (速度=");
    Serial.print(flapSpeed);
    Serial.print("%) | 方向=");
    Serial.print(virtualYaw);
    Serial.print(" (偏置=");
    Serial.print(turnBias);
    Serial.println(")");
  }

  delay(50); // 控制循环周期
}

// ============== 核心功能函数 ==============
void executeStartupSequence() {
  Serial.println("[动作] 四翅并拢竖立...");
  leftWingServo.write(ANGLE_WING_FOLDED);
  rightWingServo.write(ANGLE_WING_FOLDED);
  delay(2000); // 保持2秒，满足比赛要求

  Serial.println("[动作] 双翅下摆 (>=90度)...");
  leftWingServo.write(ANGLE_WING_DOWN);
  rightWingServo.write(ANGLE_WING_DOWN);
  delay(500);
  
  startupCompleted = true;
}

void controlWings(int speed, int bias) {
  if (speed < 5) { // 油门过低，停止扑动
    leftWingServo.write(ANGLE_WING_DOWN);
    rightWingServo.write(ANGLE_WING_DOWN);
    return;
  }

  // 计算扑动周期（速度越快，周期越短）
  unsigned long period = map(speed, 0, 100, 800, 150); // 单位：毫秒
  if (millis() - lastFlapTime > period) {
    lastFlapTime = millis();
    flapPhase = !flapPhase; // 在上下状态间切换
  }

  // 计算基础角度（上下切换）
  int baseAngle = flapPhase ? ANGLE_WING_UP : ANGLE_WING_DOWN;

  // 应用转向偏置（差速）
  int leftAngle = constrain(baseAngle + bias, 0, 180);
  int rightAngle = constrain(baseAngle - bias, 0, 180);

  leftWingServo.write(leftAngle);
  rightWingServo.write(rightAngle);
}

// ============== 虚拟遥控器函数 (Wokwi专用) ==============
void updateVirtualRemote() {
  // 读取Wokwi虚拟按键（需配合butterfly_config.h中的键盘定义）
  if (digitalRead(KEY_THROTTLE_UP) == LOW) {
    virtualThrottle = min(virtualThrottle + 50, RC_MAX);
    delay(50);
  }
  if (digitalRead(KEY_THROTTLE_DOWN) == LOW) {
    virtualThrottle = max(virtualThrottle - 50, RC_MIN);
    delay(50);
  }
  if (digitalRead(KEY_YAW_LEFT) == LOW) {
    virtualYaw = max(virtualYaw - 50, RC_MIN);
    delay(50);
  }
  if (digitalRead(KEY_YAW_RIGHT) == LOW) {
    virtualYaw = min(virtualYaw + 50, RC_MAX);
    delay(50);
  }
  if (digitalRead(KEY_CENTER) == LOW) {
    virtualYaw = RC_NEUTRAL;
    delay(50);
  }
}