// 使用了SR04超声波传感器、TRCT5000循迹传感器、SG90舵机和直流电机进行循迹避障测试
// 逻辑: 第一次检测到障碍物后开始循迹，第二次检测到障碍物后停止。

#include <Servo.h>

// --- Pin Definitions ---

// Servo
Servo myServo;
const int servoPin = 9;

// Ultrasonic Sensor - Front (for start/stop control)
const int frontTrigPin = 2;
const int frontEchoPin = 3;

// Ultrasonic Sensor - Side (for pausing)
const int sideTrigPin = 8;
const int sideEchoPin = 6;

// LED Pin
const int ledPin = A2;

// Line Following Sensors
const int trct5000_02 = A1; // 左循迹传感器
const int trct5000_01 = A0; // 右循迹传感器

// Motor Control Pins (assuming L298N-like driver)
// Right Motor (User identified pin 5 as right motor)
const int ENA = 4;  // 使能A
const int IN1 = 5;  // 输入1 (速度)
const int IN2 = 10; // 输入2 (方向)
// Left Motor
const int ENB = 7;  // 使能B
const int IN3 = 12; // 输入3 (速度)
const int IN4 = 11; // 输入4 (方向)


// --- Global Variables & Constants ---

float frontDistance;
float sideDistance;
const int OBSTACLE_THRESHOLD = 20; // 障碍物检测阈值 (cm)
int obstacleDetections = 0;
bool obstaclePreviouslySeen = false;
bool ignoreSideSensor = false; // 新增：用于在恢复后忽略侧方传感器
int sideObstacleCounter = 0; // 新增：用于过滤侧方传感器的抖动
int frontStopCounter = 0; // 新增：用于最终停止的防抖

// 定义状态机
enum State {
  IDLE,
  LINE_FOLLOWING,
  PAUSED_BY_SIDE_OBSTACLE, // 新增：被侧方障碍物暂停的状态
  STOPPED
};
State currentState = IDLE;

// --- Function Prototypes ---
void motorStop();
void motorForward(int speed);


void setup() {
  Serial.begin(9600);
  myServo.attach(servoPin, 500, 2500);

  // 初始化超声波传感器引脚
  pinMode(frontTrigPin, OUTPUT);
  pinMode(frontEchoPin, INPUT);
  pinMode(sideTrigPin, OUTPUT);
  pinMode(sideEchoPin, INPUT);

  // 初始化LED引脚
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH); // 初始为高电平，灯灭

  // 初始化循迹传感器引脚
  pinMode(trct5000_01, INPUT);
  pinMode(trct5000_02, INPUT);

  // 初始化电机控制引脚
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // 设置初始状态
  motorStop();
  myServo.write(90); // 舵机居中
  Serial.println("System Initialized. State: IDLE");
  Serial.println("Waiting 3 seconds for sensors to stabilize...");
  delay(3000); // 增加延时以确保传感器稳定
}

void loop() {
  // 1. 读取前方传感器 (用于主流程控制)
  digitalWrite(frontTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(frontTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(frontTrigPin, LOW);
  frontDistance = pulseIn(frontEchoPin, HIGH) / 58.00;

  // 2. 状态机主要逻辑
  switch (currentState) {
    case IDLE:
      motorStop();
      Serial.println("State: IDLE");
      // 检测前方障碍物以启动
      if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0 && !obstaclePreviouslySeen) {
        obstacleDetections++;
        Serial.print("Obstacle detected! Count: ");
        Serial.println(obstacleDetections);
        currentState = LINE_FOLLOWING;
        Serial.println("State change -> LINE_FOLLOWING");
      }
      break;
    
    case LINE_FOLLOWING:
      // 检查前方障碍物以进入最终停止状态 (增加防抖逻辑)
      if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0) {
        frontStopCounter++;
      } else {
        frontStopCounter = 0;
      }

      if (frontStopCounter >= 5) { // 要求连续5次检测到才触发最终停止
        currentState = STOPPED;
        Serial.println("State change -> STOPPED");
        break; // 退出 switch 语句
      }

      // 检查侧方障碍物以进入暂停状态 (增加防抖逻辑)
      if (!ignoreSideSensor) {
        digitalWrite(sideTrigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(sideTrigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(sideTrigPin, LOW);
        sideDistance = pulseIn(sideEchoPin, HIGH) / 58.00;

        if (sideDistance < OBSTACLE_THRESHOLD && sideDistance > 0) {
          sideObstacleCounter++; // 如果检测到，计数器加1
        } else {
          sideObstacleCounter = 0; // 如果没检测到，计数器清零
        }

        if (sideObstacleCounter >= 3) { // 要求连续3次检测到才触发暂停
          currentState = PAUSED_BY_SIDE_OBSTACLE;
          Serial.println("State change -> PAUSED_BY_SIDE_OBSTACLE");
          sideObstacleCounter = 0; // 重置计数器
          break;
        }
      }
      
      // 执行循迹
      motorForward(90); // 采用用户设定的速度

      // 读取循迹传感器
      bool leftOnLine = digitalRead(trct5000_02);
      bool rightOnLine = digitalRead(trct5000_01);
      
      // 使用舵机进行方向控制 (采用用户设定的角度)
      if (leftOnLine && !rightOnLine) {
        myServo.write(75);
      } else if (!leftOnLine && rightOnLine) {
        myServo.write(105);
      }
      break;

    case PAUSED_BY_SIDE_OBSTACLE:
      motorStop();
      digitalWrite(ledPin, LOW); // 亮灯
      Serial.println("State: PAUSED");

      // 在暂停状态下，只检测前方障碍物以恢复运动
      if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0) {
        digitalWrite(ledPin, HIGH); // 灭灯
        ignoreSideSensor = true; // 设置标志位，永久忽略侧方传感器
        currentState = LINE_FOLLOWING;
        Serial.println("Resuming, ignoring side sensor from now on.");
        // 为了防止这次检测被错误地计为第二次停止指令，短暂延时并重置检测标志
        delay(200); 
        obstaclePreviouslySeen = true; 
      }
      break;

    case STOPPED:
      motorStop();
      break;
  }
  
  // 更新前方障碍物的检测标志位，用于识别"新"的障碍物事件
  obstaclePreviouslySeen = (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0);

  delay(10); // 采用用户设定的循环延时
}

// --- Motor Control Functions ---

// 电机停止 (根据L298N标准，将输入引脚设为LOW实现刹车)
void motorStop() {
  // 将方向控制引脚都设为LOW，使电机刹车
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  
  // 为保险起见，也将使能引脚的PWM输出设为0
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// 电机前进 (实现"kick-start"以在低速下启动)
void motorForward(int speed) {
  // 定义电机启动阈值和"kick"参数
  const int START_THRESHOLD = 140; // 电机启动的最小PWM值
  const int KICK_SPEED = 180;      // 用于克服惯性的瞬时启动速度
  const int KICK_DURATION = 20;    // 启动脉冲的持续时间 (ms)

  // 1. 设置电机方向为前进 (根据用户最新配置)
  // 右电机
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  // 左电机 (用户已反转)
  digitalWrite(IN3, LOW); 
  digitalWrite(IN4, HIGH);

  // 2. 使用PWM在Enable引脚上设置速度, 对低速进行"kick-start"
  if (speed > 0 && speed < START_THRESHOLD) {
    // 先用一个较高的速度"kick"一下来克服静摩擦力
    analogWrite(ENA, KICK_SPEED);
    analogWrite(ENB, KICK_SPEED);
    delay(KICK_DURATION);
    
    // 然后迅速降低到目标低速来维持运动
    analogWrite(ENA, speed);
    analogWrite(ENB, speed);
  } else if (speed >= START_THRESHOLD) {
    // 对于高于阈值的速度，直接设置
    analogWrite(ENA, speed);
    analogWrite(ENB, speed);
  } else { // speed == 0
    // 如果目标速度为0，则调用刹车函数
    motorStop();
  }
}