// 定義感測器接腳
const int IR_LEFT = 34;
const int IR_CENTER = 35;
const int IR_RIGHT = 32;

// 定義 L9110S 馬達接腳 (需支援 PWM 的腳位)
const int M_LEFT_A = 25;
const int M_LEFT_B = 26;
const int M_RIGHT_A = 27;
const int M_RIGHT_B = 14;

// PID 參數調校 (根據實際車速調整)
float Kp = 50.0;  // 比例：反應強度
float Ki = 0.01;  // 積分：消除累積偏差
float Kd = 20.0;  // 微分：防止轉向過頭
float lastValidError = 0;

float error = 0, lastError = 0, integral = 0, derivative = 0;
int baseSpeed = 150; // 基礎前進速度 (0~255)

void setup() {
  Serial.begin(115200);
  
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_CENTER, INPUT);
  pinMode(IR_RIGHT, INPUT);

  // 設定 ESP32 PWM (LEDC)
  ledcAttach(M_LEFT_A, 5000, 8);  // 5kHz, 8-bit
  ledcAttach(M_LEFT_B, 5000, 8);
  ledcAttach(M_RIGHT_A, 5000, 8);
  ledcAttach(M_RIGHT_B, 5000, 8);
}

void setMotor(int leftSpeed, int rightSpeed) {
  // 控制左馬達 (正轉)
  if (leftSpeed >= 0) {
    ledcWrite(M_LEFT_A, constrain(leftSpeed, 0, 255));
    ledcWrite(M_LEFT_B, 0);
  } else {
    ledcWrite(M_LEFT_A, 0);
    ledcWrite(M_LEFT_B, constrain(-leftSpeed, 0, 255));
  }
  // 控制右馬達 (正轉)
  if (rightSpeed >= 0) {
    ledcWrite(M_RIGHT_A, constrain(rightSpeed, 0, 255));
    ledcWrite(M_RIGHT_B, 0);
  } else {
    ledcWrite(M_RIGHT_A, 0);
    ledcWrite(M_RIGHT_B, constrain(-rightSpeed, 0, 255));
  }
}

void loop() {
  // 1. 讀取感測器
  int L = !digitalRead(IR_LEFT);
  int C = !digitalRead(IR_CENTER);
  int R = !digitalRead(IR_RIGHT);

  // 2. 判斷感測器狀態並定義 error
  if (L && !C && !R)      error = -2;
  else if (L && C && !R)  error = -1;
  else if (!L && C && !R) error = 0;
  else if (!L && C && R)  error = 1;
  else if (!L && !C && R) error = 2;
  else if (!L && !C && !R) {
    // 【盲區處理】：沒偵測到線時，不跑 PID，直接執行旋轉找線
    if (lastValidError < 0)      setMotor(-baseSpeed, baseSpeed); // 之前偏左，原地左轉
    else if (lastValidError > 0) setMotor(baseSpeed, -baseSpeed); // 之前偏右，原地右轉
    else                         setMotor(0, 0);                  // 初始就沒線，停下
    return; // 跳過後面的 PID 計算
  }

  // 3. 更新最後有效誤差 (用於盲區判斷)
  // 注意：只有在 error != 0 時更新，才能保留「最後偏向哪一邊」的資訊
  if (error != 0) {
    lastValidError = error;
  }

  // 4. PID 計算
  integral += error;
  
  // 【積分飽和預防】：限制積分值範圍，避免轉過頭回不來
  integral = constrain(integral, -100, 100); 
  // 如果回到中心點，快速清空積分（可選）
  if (error == 0) integral = 0;

  derivative = error - lastError;
  float turnOutput = (Kp * error) + (Ki * integral) + (Kd * derivative);
  lastError = error;

  // 5. 輸出控制
  int leftSpeed = baseSpeed + turnOutput;
  int rightSpeed = baseSpeed - turnOutput;
  setMotor(leftSpeed, rightSpeed);

  delay(20); 
}