#include <Arduino.h>
#include <QTRSensors.h>
#include <NewPing.h>

// --- ĐỊNH NGHĨA CHÂN KẾT NỐI ---
// Động cơ (TB6612FNG)
const int AIN1 = 25, AIN2 = 26, PWMA = 18;
const int BIN1 = 27, BIN2 = 14, PWMB = 19;
const int STBY = 32;

// Siêu âm (SRF04)
NewPing sonarFront(12, 13, 200); 
NewPing sonarLeft(33, 35, 200);
NewPing sonarRight(2, 4, 200);

// Cảm biến Line (QTR-8A)
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];

// Công tắc hành trình (Limit Switch)
const int LIMIT_SWITCH = 15; 

// Cảm biến màu (TCS3200)
const int S2 = 5, S3 = 17, OUT_PIN = 16;

// Biến trạng thái và PID
enum State { WAITING_LIGHT, LINE_FOLLOW, MAZE_SOLVE, FINISHED };
State currentState = WAITING_LIGHT;

int lastError = 0;
const float Kp = 0.08; // Hiệu chỉnh tại sân
const float Kd = 0.25; 
const int maxSpeed = 200;

// --- HÀM ĐIỀU KHIỂN ĐỘNG CƠ ---
void motorControl(int speedA, int speedB) {
  digitalWrite(STBY, HIGH);
  // Motor A
  if (speedA >= 0) { digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); }
  else { digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); speedA = -speedA; }
  // Motor B
  if (speedB >= 0) { digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); }
  else { digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); speedB = -speedB; }
  
  analogWrite(PWMA, constrain(speedA, 0, 255));
  analogWrite(PWMB, constrain(speedB, 0, 255));
}

// --- HÀM NHẬN DIỆN MÀU ĐÈN (VÒNG CHUNG KẾT) ---
bool isGreenLight() {
  digitalWrite(S2, HIGH); digitalWrite(S3, HIGH); // Chọn bộ lọc màu Xanh
  long duration = pulseIn(OUT_PIN, LOW);
  return (duration > 20 && duration < 60); // Tần số mẫu cho màu xanh
}

// --- THUẬT TOÁN DÒ LINE PID ---
void runLineFollow() {
  uint16_t position = qtr.readLineBlack(sensorValues);
  int error = position - 3500;
  int motorSpeed = Kp * error + Kd * (error - lastError);
  lastError = error;

  motorControl(150 + motorSpeed, 150 - motorSpeed);

  // Check Zero-Touch: Chạm trạm C1
  if (digitalRead(LIMIT_SWITCH) == LOW) {
    motorControl(0, 0);
    delay(2000); // Chờ cổng mở
    currentState = MAZE_SOLVE;
  }
}

// --- THUẬT TOÁN GIẢI MÊ CUNG (BÁM TƯỜNG) ---
void runMazeSolve() {
  int distF = sonarFront.ping_cm();
  int distL = sonarLeft.ping_cm();
  int distR = sonarRight.ping_cm();

  if (distF > 0 && distF < 12) { // Có tường trước mặt
    if (distL > distR) motorControl(-120, 120); // Rẽ trái
    else motorControl(120, -120);              // Rẽ phải
  } else {
    motorControl(130, 130); // Đi thẳng tìm C2
  }

  // Check trạm C2 để dừng hoàn toàn
  if (digitalRead(LIMIT_SWITCH) == LOW) {
    motorControl(0, 0);
    currentState = FINISHED;
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(STBY, OUTPUT);
  pinMode(LIMIT_SWITCH, INPUT_PULLUP);
  pinMode(S2, OUTPUT); pinMode(S3, OUTPUT); pinMode(OUT_PIN, INPUT);

  // Cấu hình QTR-8A
  qtr.setTypeAnalog();
  qtr.setSensorPins((const uint8_t[]){36, 39, 34, 35, 32, 33, 25, 26}, SensorCount);

  // Hiệu chuẩn cảm biến (Quay robot tại chỗ trong 5s)
  for (uint16_t i = 0; i < 400; i++) {
    qtr.calibrate();
    motorControl(80, -80);
  }
  motorControl(0, 0);
}

void loop() {
  switch (currentState) {
    case WAITING_LIGHT:
      if (isGreenLight()) currentState = LINE_FOLLOW; // Chỉ chạy khi đèn Xanh
      break;

    case LINE_FOLLOW:
      runLineFollow();
      break;

    case MAZE_SOLVE:
      runMazeSolve();
      break;

    case FINISHED:
      motorControl(0, 0);
      break;
  }
}