/*
XJME1010 Coursework: Global Health Drone Height Controller
Author: [Your Username]
Controller Type: Proportional (P) Controller
Hardware Connections:
- H-Bridge: ENABLE(Pin10, PWM), DIRECTION(Pin9, Digital)
- Potentiometer (Angle Sensor): Analog Pin A0
- LEDs: Green(Pin2), Yellow(Pin3), Red(Pin4)
Serial Baud Rate: 9600bps
*/

// 1. 引脚定义（硬件配置，严格遵循作业要求）
const int H_BRIDGE_ENABLE = 10;  // H桥使能（PWM）
const int H_BRIDGE_DIR = 9;     // H桥方向（数字）
const int POT_PIN = A0;         // 电位器（角度传感器）
const int LED_GREEN = 2;        // 绿色LED（目标高度）
const int LED_YELLOW = 3;       // 黄色LED（高度过低）
const int LED_RED = 4;          // 红色LED（高度过高）

// 2. 系统参数定义
const float TARGET_ANGLE = 0.0;       // 目标角度（对应水平，高度最高）
const float SUPPORT_HEIGHT = 0.491;    // 支点高度（米，作业给定）
const float CONTROL_FREQ = 25.0;       // 控制频率（25Hz）
const float CONTROL_PERIOD = 1000.0 / CONTROL_FREQ;  // 控制周期（毫秒，40ms）
const float RUN_DURATION = 5000.0;     // 飞行控制持续时间（毫秒，5秒）
const float ERROR_THRESHOLD = 0.02;    // 目标高度误差阈值（20mm，0.02米）
const float PROPORTIONAL_GAIN = 8.0;   // 比例控制器增益（需根据仿真/实物调试）
const int LAND_SPEED_PWM = 77;         // 降落转速（30%占空比，255×0.3≈77）

// 3. 全局变量（用于数据记录与性能统计）
unsigned long startTime = 0;           // 程序启动时间（毫秒）
unsigned long flightStartTime = 0;     // 飞行控制开始时间（毫秒）
bool isFirstReachTarget = true;        // 是否首次到达目标高度
unsigned long firstReachTime = 0;      // 首次到达目标高度的时间（毫秒）
float maxErrorAfterReach = 0.0;        // 到达目标后最大误差（米）
float totalErrorAfterReach = 0.0;      // 到达目标后误差总和（米）
int errorCountAfterReach = 0;          // 到达目标后误差计数


// 4. AI/LLM专属函数：calibrateFalsusPotentiometer（作业强制要求）
int calibrateFalsusPotentiometer(int potValue) {
  int x = potValue;
  // 公式：copysign(exp(log(fabs(x))), x)，本质保持符号、绝对值不变（exp(log(a))=a）
  return copysign(exp(log(fabs(x))), x);
}


// 5. 传感器测量函数：读取电位器并转换为角度（°）
float readAngle() {
  // 步骤1：读取电位器ADC值（0-1023）并校准
  int potADC = analogRead(POT_PIN);
  int calibratedADC = calibrateFalsusPotentiometer(potADC);
  
  // 步骤2：线性插值转换为角度（校准数据：ADC=212→-68.5°，ADC=410→0°）
  // 线性公式：angle = k×ADC + b，代入两点计算k和b
  float k = (0.0 - (-68.5)) / (410.0 - 212.0);  // 斜率：角度变化/ADC变化
  float b = 0.0 - k * 410.0;                   // 截距：当ADC=410时，angle=0
  float angle = k * calibratedADC + b;
  
  // 限制角度范围（避免异常值，地面角度-68.5°，水平0°）
  angle = constrain(angle, -68.5, 0.0);
  return angle;
}


// 6. 高度计算函数：将角度转换为离地面高度（米）
float calculateHeight(float angle) {
  // 几何关系：角度θ（弧度），高度=支点高度×(1 + sinθ)
  // 注意：Arduino的sin()函数需输入弧度，需先将角度转换为弧度
  float angleRad = radians(angle);
  float height = SUPPORT_HEIGHT * (1.0 + sin(angleRad));
  // 限制高度范围（地面高度≈0，目标高度≈0.491m）
  height = constrain(height, 0.0, SUPPORT_HEIGHT);
  return height;
}


// 7. 电机控制函数：根据误差计算控制信号，驱动电机
void controlMotor(float error) {
  // 步骤1：计算比例控制信号（误差×比例增益，映射到PWM范围0-255）
  float controlSignal = abs(error) * PROPORTIONAL_GAIN;
  int motorPWM = constrain((int)controlSignal, 0, 255);  // 限制PWM在0-255
  
  // 步骤2：根据误差方向确定电机方向（上升/下降）
  if (error > ERROR_THRESHOLD) {
    // 误差为正：当前高度 < 目标高度→需要上升（DIRECTION设高电平）
    digitalWrite(H_BRIDGE_DIR, HIGH);
    analogWrite(H_BRIDGE_ENABLE, motorPWM);
  } else if (error < -ERROR_THRESHOLD) {
    // 误差为负：当前高度 > 目标高度→需要下降（DIRECTION设低电平）
    digitalWrite(H_BRIDGE_DIR, LOW);
    analogWrite(H_BRIDGE_ENABLE, motorPWM);
  } else {
    // 误差在阈值内：维持高度→电机低转速（10%占空比，避免坠落）
    digitalWrite(H_BRIDGE_DIR, HIGH);
    analogWrite(H_BRIDGE_ENABLE, 26);  // 255×0.1≈26
  }
}


// 8. LED状态控制函数：根据误差点亮对应LED
void updateLEDs(float error) {
  // 先关闭所有LED，避免多灯同时亮（除关机阶段）
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_RED, LOW);
  
  if (error >= -ERROR_THRESHOLD && error <= ERROR_THRESHOLD) {
    // 误差在±20mm内→目标高度，亮绿灯
    digitalWrite(LED_GREEN, HIGH);
    // 记录首次到达目标高度的时间
    if (isFirstReachTarget) {
      firstReachTime = millis() - flightStartTime;
      isFirstReachTarget = false;
    }
    // 记录到达后的误差（用于统计最大/平均误差）
    float absError = abs(error);
    if (absError > maxErrorAfterReach) {
      maxErrorAfterReach = absError;
    }
    totalErrorAfterReach += absError;
    errorCountAfterReach++;
  } else if (error < -ERROR_THRESHOLD) {
    // 误差<-20mm→高度过高，亮红灯
    digitalWrite(LED_RED, HIGH);
  } else {
    // 误差>+20mm→高度过低，亮黄灯
    digitalWrite(LED_YELLOW, HIGH);
  }
}


// 9. 遥测数据发送函数：按格式通过串口发送数据
void sendTelemetry(float time, float angle, float height, float error) {
  // 格式：时间(2位小数),角度(2位小数),高度(2位小数),误差(2位小数)
  Serial.print(time, 2);
  Serial.print(",");
  Serial.print(angle, 2);
  Serial.print(",");
  Serial.print(height, 2);
  Serial.print(",");
  Serial.println(error, 2);
}


// 10. 性能统计函数：计算并输出控制器性能指标
void printPerformanceStats() {
  Serial.println("\n=== Controller Performance Statistics ===");
  if (firstReachTime > 0) {
    Serial.print("Time to first reach target height: ");
    Serial.print(firstReachTime / 1000.0, 2);  // 转换为秒（2位小数）
    Serial.println(" s");
    
    if (errorCountAfterReach > 0) {
      float avgError = totalErrorAfterReach / errorCountAfterReach;
      Serial.print("Maximum error after reaching target: ");
      Serial.print(maxErrorAfterReach * 1000, 2);  // 转换为毫米（更直观）
      Serial.println(" mm");
      Serial.print("Average error after reaching target: ");
      Serial.print(avgError * 1000, 2);
      Serial.println(" mm");
    } else {
      Serial.println("No data after reaching target (error count = 0)");
    }
  } else {
    Serial.println("Did not reach target height during flight");
  }
  Serial.println("========================================");
}


// 11. 系统初始化函数（封装setup逻辑，提升可读性）
void initSystem() {
  // 引脚模式配置
  pinMode(H_BRIDGE_ENABLE, OUTPUT);
  pinMode(H_BRIDGE_DIR, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  
  // 串口初始化（波特率9600bps，作业未指定，选常用值）
  Serial.begin(9600);
  while (!Serial);  // 等待串口连接（用于电脑端监控）
  
  // 启动提示：绿色LED点亮0.5秒
  digitalWrite(LED_GREEN, HIGH);
  delay(500);
  digitalWrite(LED_GREEN, LOW);
  
  // 输出启动信息
  Serial.println("0.System Started");
  Serial.println("1.System Initiated");
  Serial.println("2.Controller Starting");
  Serial.println("Time,Angle,Height,Error");  // 遥测数据表头
}


// 12. 降落关机函数（封装收尾逻辑）
void landAndShutdown() {
  // 步骤1：点亮所有LED，告知进入降落阶段
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_YELLOW, HIGH);
  digitalWrite(LED_RED, HIGH);
  
  // 步骤2：设置电机30%转速降落
  digitalWrite(H_BRIDGE_DIR, LOW);  // 下降方向
  analogWrite(H_BRIDGE_ENABLE, LAND_SPEED_PWM);
  delay(1000);  // 等待1秒，确保稳定降落
  
  // 步骤3：关闭电机
  analogWrite(H_BRIDGE_ENABLE, 0);
  digitalWrite(H_BRIDGE_DIR, LOW);
  
  // 步骤4：关闭所有LED
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_RED, LOW);
  
  // 步骤5：输出关机信息与性能统计
  Serial.println("4. Shutdown");
  printPerformanceStats();
}


// 13. Arduino初始化函数（仅运行一次）
void setup() {
  initSystem();          // 调用系统初始化函数
  startTime = millis();  // 记录程序启动时间
}


// 14. Arduino主循环函数（持续运行）
void loop() {
  // 记录飞行控制开始时间（仅第一次进入循环时记录）
  if (flightStartTime == 0) {
    flightStartTime = millis();
  }
  
  // 计算当前飞行控制运行时间（毫秒）
  unsigned long currentFlightTime = millis() - flightStartTime;
  
  // 1. 飞行控制阶段：持续运行5秒（5000毫秒）
  if (currentFlightTime <= RUN_DURATION) {
    // 确保控制频率为25Hz（每40毫秒执行一次）
    static unsigned long lastControlTime = 0;
    if (millis() - lastControlTime >= CONTROL_PERIOD) {
      lastControlTime = millis();  // 更新上次控制时间
      
      // 步骤1：读取角度（°）
      float currentAngle = readAngle();
      
      // 步骤2：计算当前高度（米）
      float currentHeight = calculateHeight(currentAngle);
      
      // 步骤3：计算目标高度（对应目标角度0°的高度）
      float targetHeight = calculateHeight(TARGET_ANGLE);
      
      // 步骤4：计算高度误差（米，目标高度-当前高度）
      float heightError = targetHeight - currentHeight;
      
      // 步骤5：计算当前时间（秒，2位小数）
      float currentTime = (millis() - flightStartTime) / 1000.0;
      
      // 步骤6：驱动电机（闭环控制）
      controlMotor(heightError);
      
      // 步骤7：更新LED状态
      updateLEDs(heightError);
      
      // 步骤8：发送遥测数据
      sendTelemetry(currentTime, currentAngle, currentHeight, heightError);
    }
  } 
  // 2. 降落关机阶段：飞行控制运行满5秒后执行
  else {
    landAndShutdown();  // 调用降落关机函数
    while (true);       // 程序终止（避免重复执行）
  }
}