#include <Servo.h> // 舵机库

// 引脚定义
// 电机
const int LEFT_MOTOR_PIN = 9;  // 左电机PWM控制引脚
const int RIGHT_MOTOR_PIN = 10; // 右电机PWM控制引脚

// TRCT5000 循迹传感器
const int LEFT_TRCT_PIN = 7;  // 左循迹传感器引脚
const int RIGHT_TRCT_PIN = 8; // 右循迹传感器引脚

// 超声波传感器
const int TRIG_PIN = 12; // 超声波Trig引脚
const int ECHO_PIN = 11; // 超声波Echo引脚

// 舵机
const int SERVO_PIN = 3; // 舵机控制引脚

Servo steeringServo; // 创建舵机对象

// 常量定义
const int OBSTACLE_DISTANCE_THRESHOLD_CM = 20; // 障碍物检测阈值 (厘米)
const int SERVO_NEUTRAL_ANGLE = 90;          // 舵机中间位置角度
const int SERVO_LEFT_ANGLE = 75;             // 舵机微调左转角度 (原为60, 调整为更小角度)
const int SERVO_RIGHT_ANGLE = 105;           // 舵机微调右转角度 (原为120, 调整为更小角度)

// 电机速度 (analogWrite PWM值范围 0-255) - 这些常量在新的电机控制逻辑下意义减弱
const int MOTOR_STATE_STOP = LOW;   // 电机停止状态 (使用LOW)
const int MOTOR_STATE_RUN = HIGH; // 电机运行状态 (使用HIGH)
// 保留旧的常量名以便于理解之前的逻辑，但实际传递给电机控制函数的值会被转换为HIGH/LOW
const int MOTOR_SPEED_STOP = 0;         // 逻辑上的停止 (会转换为LOW)
const int MOTOR_SPEED_NORMAL = 1;     // 逻辑上的运行 (会转换为HIGH)
const int MOTOR_SPEED_SLOW = 1;       // 逻辑上的运行 (会转换为HIGH)
const int MOTOR_SPEED_TURN_ASSIST = 1; // 逻辑上的运行 (会转换为HIGH)
// const int MOTOR_SPEED_DIFFERENTIAL_SLOW = 70; // 这个在单边驱动时不再直接使用

// TRCT5000 传感器逻辑 (假设检测到黑线为低电平, 白色地面为高电平)
// 具体根据你的传感器模块实际输出进行调整 (例如，如果模块输出反相，则需要调换定义)
#define ON_LINE LOW  // 在线上 (检测到黑线)
#define OFF_LINE HIGH // 不在线上 (检测到白色地面)

void setup() {
  // 初始化串口通信，波特率9600
  Serial.begin(9600);
  while (!Serial) {
    ; // 等待串口连接 (对于某些板子如Leonardo是必需的，Nano通常不需要但加上无害)
  }
  Serial.println("串口已连接。");

  // 初始化电机引脚为输出模式
  pinMode(LEFT_MOTOR_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN, OUTPUT);

  // 初始化TRCT5000循迹传感器引脚为输入模式
  pinMode(LEFT_TRCT_PIN, INPUT);
  pinMode(RIGHT_TRCT_PIN, INPUT);

  // 初始化超声波传感器引脚
  pinMode(TRIG_PIN, OUTPUT); // Trig引脚为输出
  pinMode(ECHO_PIN, INPUT);  // Echo引脚为输入

  // 舵机初始化，连接到SERVO_PIN，并设置到中间位置
  steeringServo.attach(SERVO_PIN);
  steeringServo.write(SERVO_NEUTRAL_ANGLE);

  // 确保初始状态电机停止
  stopMotors();
  Serial.println("初始状态：电机已停止。");

  Serial.println("Arduino Nano 循迹避障小车 初始化完成，准备就绪。");
  Serial.println("-----------------------------------------");
}

void loop() {
  Serial.println("--- 开始新循环 ---");
  long distance = readUltrasonicDistance(); // 读取超声波测量的距离
  Serial.print("超声波距离: "); Serial.print(distance); Serial.println(" cm");

  if (distance > 0 && distance < OBSTACLE_DISTANCE_THRESHOLD_CM) {
    // 如果检测到障碍物 (距离大于0且小于阈值)
    Serial.println("状态更新：检测到障碍物! 执行停止。");
    stopMotors(); // 停止电机
    steeringServo.write(SERVO_NEUTRAL_ANGLE); // 舵机回中
    Serial.println("动作：舵机回中。");
  } else {
    Serial.println("状态更新：无障碍物，执行循迹逻辑。");
    trackLine();
  }
  Serial.println("--- 循环结束，延时 ---");
  delay(100); // 稍微增加延时，方便观察串口输出，原为50
}

// --- 电机控制函数 ---
void controlMotors(int leftState, int rightState) {
  // 直接使用digitalWrite控制电机，HIGH为全速，LOW为停止
  // leftState 和 rightState 现在应该代表 MOTOR_STATE_RUN (HIGH) 或 MOTOR_STATE_STOP (LOW)
  
  digitalWrite(LEFT_MOTOR_PIN, leftState);
  digitalWrite(RIGHT_MOTOR_PIN, rightState);

  Serial.print("电机控制(直接驱动)：左轮状态="); 
  Serial.print(leftState == HIGH ? "运行(HIGH)" : "停止(LOW)");
  Serial.print(", 右轮状态="); 
  Serial.println(rightState == HIGH ? "运行(HIGH)" : "停止(LOW)");
}

void moveForward() { // 不再需要speed参数，因为总是全速
  Serial.println("动作：向前行驶 (电机全速)。");
  steeringServo.write(SERVO_NEUTRAL_ANGLE); // 确保舵机在中间位置 (直行)
  Serial.println("动作：舵机回中 (配合直行)。");
  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN); // 设置左右电机运行
}

void stopMotors() {
  Serial.println("动作：停止电机。");
  controlMotors(MOTOR_STATE_STOP, MOTOR_STATE_STOP); // 设置左右电机停止
}

void turnLeft() { // 不再需要turnSpeed参数
  Serial.println("动作：左转 (电机全速，舵机控制)。");
  steeringServo.write(SERVO_LEFT_ANGLE); // 舵机打到左转角度
  Serial.print("动作：舵机打到左转角度 ("); Serial.print(SERVO_LEFT_ANGLE); Serial.println("度)。");
  // 两个电机都保持运行，转向仅靠舵机
  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN);
}

void turnRight() { // 不再需要turnSpeed参数
  Serial.println("动作：右转 (电机全速，舵机控制)。");
  steeringServo.write(SERVO_RIGHT_ANGLE); // 舵机打到右转角度
  Serial.print("动作：舵机打到右转角度 ("); Serial.print(SERVO_RIGHT_ANGLE); Serial.println("度)。");
  // 两个电机都保持运行，转向仅靠舵机
  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN);
}

// --- 传感器读取函数 ---
long readUltrasonicDistance() {
  // 读取超声波传感器距离 (返回厘米cm)

  // 清除TRIG_PIN的任何先前状态，确保低电平
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  // 发送一个10微秒的高电平脉冲到TRIG_PIN以触发超声波发送
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // 读取ECHO_PIN返回的脉冲持续时间 (单位：微秒)
  // pulseIn函数会等待引脚变为高电平，然后开始计时，再等待引脚变为低电平，停止计时。
  long duration = pulseIn(ECHO_PIN, HIGH);

  // 计算距离 (单位：厘米)
  // 声速 = 343米/秒 = 0.0343 厘米/微秒
  // 距离 = (时间 * 声速) / 2  (因为声波是往返的)
  long distance = duration * 0.0343 / 2;
  // Serial.print("[内部] 超声波原始时长: "); Serial.println(duration);
  return distance;
}

void trackLine() {
  // 循迹逻辑
  int leftSensorVal = digitalRead(LEFT_TRCT_PIN);  // 读取左循迹传感器状态
  int rightSensorVal = digitalRead(RIGHT_TRCT_PIN); // 读取右循迹传感器状态

  Serial.print("循迹传感器状态: 左="); 
  Serial.print(leftSensorVal == ON_LINE ? "线上(LOW)" : "线下(HIGH)");
  Serial.print(", 右="); 
  Serial.println(rightSensorVal == ON_LINE ? "线上(LOW)" : "线下(HIGH)");

  // 新的循迹逻辑：默认前进，检测到线时微调方向 (电机始终全速)
  if (leftSensorVal == ON_LINE && rightSensorVal == OFF_LINE) {
    Serial.println("循迹判断：左线上，右线下 -> 执行左转 (电机保持运行)");
    turnLeft(); 
  } else if (leftSensorVal == OFF_LINE && rightSensorVal == ON_LINE) {
    Serial.println("循迹判断：左线下，右线上 -> 执行右转 (电机保持运行)");
    turnRight();
  } else if (leftSensorVal == ON_LINE && rightSensorVal == ON_LINE) {
    Serial.println("循迹判断：两侧均线上 -> 执行直行 (电机保持运行)");
    moveForward(); // 两侧都压线，也直行
  } else { // leftSensorVal == OFF_LINE && rightSensorVal == OFF_LINE
    Serial.println("循迹判断：两侧均线下 -> 执行直行 (电机保持运行)");
    moveForward();
  }
}