#include <QTRSensors.h>
#include <Wire.h>
#include <VL53L1X.h>
// ================== QTR SENSORS ==================
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];


const uint8_t SHUT_1 = 22;
const uint8_t SHUT_2 = 23;
const uint8_t ADDR_1 = 0x30;
const uint8_t ADDR_2 = 0x29;

VL53L1X tof1;
VL53L1X tof2;

// ================== MOTOR PINS ==================
const int ENB = 44;  
const int IN4 = 26;
const int IN3 = 24;
 
const int ENA = 46;
const int IN1 = 28;
const int IN2 = 30;

// ================== LIFT MOTOR (IBT-2 / BTS7960) ==================
const int LIFT_R_EN  = 32;
const int LIFT_L_EN  = 34;
const int LIFT_R_PWM = 11;
const int LIFT_L_PWM = 12;

// Lift parameters
const int LIFT_SPEED = 180;        // PWM (0–255)
const int LIFT_UP_TIME = 11500;     // ms, lift up duration
const int LIFT_DOWN_TIME = 11500;   // ms, lift down duration


// ================== PID ==================
float Kp = 0.15, Ki = 0.0, Kd = 1.0;
int16_t lastError = 0;
float integral = 0;
 
const int BASE_SPEED = 125;
const int MAX_SPEED  = 240;
 
// ================== INTERSECTION ==================
const uint16_t INTERSECTION_THRESHOLD = 600;
const uint8_t  INTERSECTION_MIN_SENSORS = 6;
const int      LINE_CENTER = 3500;
 
// Timings
const int FORWARD_BEFORE_TURN_DELAY = 450; 
// Timings
const int TURN_90_DELAY_EMPTY       = 1200;  // robot without load
const int TURN_pwm_loaded           = 240;  // robot with load
const int TURN_90_DELAY_LOADED      = 850;  // robot with load (to tune)
const int REACQUIRE_DELAY           = 250;

// State: carrying a load?
bool carryingLoad = false;

 
// Anti double-detection
bool          ignoreIntersection = false;
unsigned long ignoreUntil        = 0;

// ================== GRID & ORIENTATION ==================
const int GRID_WIDTH  = 3;
const int GRID_HEIGHT = 3;

int gridX = 0;
int gridY = 0;
int homeX = 0;
int homeY = 0;

enum Orientation {
  NORTH = 0,
  EAST,
  SOUTH,
  WEST
};

Orientation homeOri = NORTH;
// ===== Pickup approach constraint =====
bool pickupRequirePrev = true;   // set false to disable
int pickupPrevX = 2;             // cell required before entering pickup
int pickupPrevY = 2;

bool onPickupApproach = false;   // internal: prev cell reached or not
int lastGridX = 0;               // previous grid position
int lastGridY = 0;
Orientation orientation = NORTH;

// ================== TARGETS & MISSION ==================

// To adapt: pickup and drop zone coordinates
int pickupX = 1;
int pickupY = 2;

int dropX   = 0;
int dropY   = 0;

// Current navigation target
int targetX = 0;
int targetY = 0;

enum MoveAction {
  MOVE_STRAIGHT = 0,
  MOVE_TURN_LEFT,
  MOVE_TURN_RIGHT,
  MOVE_ARRIVED
};

enum MissionPhase {
  PHASE_GOTO_PICKUP = 0,
  PHASE_LIFT_UP,
  PHASE_GOTO_DROPOFF,
  PHASE_LIFT_DOWN,
  PHASE_DONE,
  PHASE_RETURN_HOME,
};

MissionPhase mission = PHASE_GOTO_PICKUP;

// ================== PROTOTYPES ==================
void goLineFollow(uint16_t position);
void handleIntersectionSequence(MoveAction action);
void setMotorSpeeds(int l, int r);
void stopMotors();

void updateGridPosition();
MoveAction computeNextMove();
void rotateOrientationRight();
void rotateOrientationLeft();

void doLiftUp();    // lift up routine
void doLiftDown();  // lift down routine
void setTargetForCurrentMission();

// ================== SETUP ==================
void setup() {
  Serial.begin(9600);
 
  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){3,4,5,6,7,8,9,10}, SensorCount);
  qtr.setEmitterPin(2);
 
  Serial.println("Calibration...");
  for(uint16_t i=0;i<250;i++) qtr.calibrate();
  Serial.println("=> OK");

  // Initial position/orientation
  gridX = 0;
  gridY = 0;
  orientation = NORTH;
  homeX = gridX;
  homeY = gridY;
  homeOri = orientation;

  mission = PHASE_GOTO_PICKUP;
  setTargetForCurrentMission();

  Serial.print("Start grid = (");
  Serial.print(gridX); Serial.print(","); Serial.print(gridY);
  Serial.println(") ORIENT=NORTH");

  Serial.print("Pickup grid = (");
  Serial.print(pickupX); Serial.print(","); Serial.print(pickupY);
  Serial.println(")");

  Serial.print("Drop grid   = (");
  Serial.print(dropX);   Serial.print(","); Serial.print(dropY);
  Serial.println(")");

    // ===== LIFT MOTOR SETUP =====
  pinMode(LIFT_R_EN, OUTPUT);
  pinMode(LIFT_L_EN, OUTPUT);
  pinMode(LIFT_R_PWM, OUTPUT);
  pinMode(LIFT_L_PWM, OUTPUT);

  // Enable IBT-2 driver
  digitalWrite(LIFT_R_EN, HIGH);
  digitalWrite(LIFT_L_EN, HIGH);

  // Lift stopped at startup
  analogWrite(LIFT_R_PWM, 0);
  analogWrite(LIFT_L_PWM, 0);

  Serial.println("Lift motor ready.");
  Wire.begin();
Wire.setClock(400000);

pinMode(SHUT_1, OUTPUT);
pinMode(SHUT_2, OUTPUT);
digitalWrite(SHUT_1, LOW);
digitalWrite(SHUT_2, LOW);
delay(10);

// Sensor 1 -> new address
digitalWrite(SHUT_1, HIGH);
delay(10);
tof1.init();
tof1.setAddress(ADDR_1);
tof1.setDistanceMode(VL53L1X::Long);
tof1.setMeasurementTimingBudget(50000);
tof1.setTimeout(100);
tof1.startContinuous(50);

// Sensor 2 default
digitalWrite(SHUT_2, HIGH);
delay(10);
tof2.init();
tof2.setDistanceMode(VL53L1X::Long);
tof2.setMeasurementTimingBudget(50000);
tof2.setTimeout(100);
tof2.startContinuous(50);


}
 
// ================== MAIN LOOP ==================
void loop() {

  // Phase handling (lift, end)
  if (mission == PHASE_LIFT_UP) {
  Serial.println(">>> PHASE_LIFT_UP: starting lift up");
  stopMotors();

  doLiftUp();   // lift routine

  // After lift: still on pickup cell
  mission = PHASE_GOTO_DROPOFF;
  setTargetForCurrentMission();   // targetX = dropX, targetY = dropY

  Serial.print(">>> Lift UP done, new target = (");
  Serial.print(targetX); Serial.print(","); Serial.print(targetY); Serial.println(")");

  // Compute how to leave this intersection
  MoveAction action = computeNextMove();
  Serial.print("TARGET=("); Serial.print(targetX); Serial.print(","); Serial.print(targetY); Serial.println(")");
  Serial.print("PICKUP=("); Serial.print(pickupX); Serial.print(","); Serial.print(pickupY); Serial.println(")");
  Serial.print("PREV=("); Serial.print(pickupPrevX); Serial.print(","); Serial.print(pickupPrevY); Serial.println(")");
  Serial.print("onPickupApproach="); Serial.println(onPickupApproach);

  if (action == MOVE_TURN_LEFT || action == MOVE_TURN_RIGHT) {
    Serial.println(">>> Turning immediately after pickup");
    handleIntersectionSequence(action);   // pivot logic
  } else {
    Serial.println(">>> Leaving pickup straight");
    // no pivot here
  }

  // Prevent immediate re-detection
  ignoreIntersection = true;
  ignoreUntil = millis() + 1000;

  // Reset PID
  integral = 0;
  lastError = 0;

  return;
  }


  if (mission == PHASE_LIFT_DOWN) {
    Serial.println(">>> PHASE_LIFT_DOWN: starting lift down");
    stopMotors();
    doLiftDown(); // lift routine
    mission = PHASE_DONE;
    Serial.println(">>> Lift DOWN done, mission finished");
    delay(200);
    return;
  }

  if (mission == PHASE_DONE) {
    stopMotors();
    // idle
    return;
  }

  // Navigation (GOTO_PICKUP or GOTO_DROPOFF)

  // If already on current target cell
  if (mission == PHASE_GOTO_PICKUP && gridX == pickupX && gridY == pickupY) {
    
  }
  if (mission == PHASE_GOTO_DROPOFF && gridX == dropX && gridY == dropY) {
    Serial.println(">>> Reached DROPOFF cell (check in loop), switching to LIFT_DOWN");
    mission = PHASE_LIFT_DOWN;
    return;
  }

  // Line read
  uint16_t position = qtr.readLineBlack(sensorValues);
 
  uint8_t blackCount = 0;
  for(uint8_t i=0;i<8;i++){
    if(sensorValues[i] > INTERSECTION_THRESHOLD) blackCount++;
  }
 
  Serial.print("BC="); Serial.print(blackCount);
  Serial.print(" pos="); Serial.print(position);
  Serial.print(" grid=("); Serial.print(gridX); Serial.print(","); Serial.print(gridY); Serial.print(")");
  Serial.print(" mission="); Serial.println(mission);
 
  // Intersection debounce
  if (ignoreIntersection && millis() < ignoreUntil) {
    goLineFollow(position);
    return;
  }
 
  // ======= INTERSECTION =======
  if(blackCount >= INTERSECTION_MIN_SENSORS){
    Serial.println("=== INTERSECTION DETECTED ===");

    ignoreIntersection = true;
    ignoreUntil = millis() + 1500;

    // Update grid position
    updateGridPosition();

    // Choose next action
    MoveAction action = computeNextMove();

    // If arrived on target cell
  if (action == MOVE_ARRIVED) {
  Serial.println(">>> ARRIVED ON TARGET CELL (at intersection)");
  stopMotors();

  if (mission == PHASE_GOTO_PICKUP) {

    // Case 1: target = pickupPrev
    if (pickupRequirePrev && !onPickupApproach &&
        gridX == pickupPrevX && gridY == pickupPrevY) {

      Serial.println(">>> Reached PICKUP PREV CELL. Switching target to PICKUP.");
      onPickupApproach = true;
      setTargetForCurrentMission(); // target = pickup

      MoveAction next = computeNextMove();
      Serial.print(">>> Next after prevPickup: ");
      Serial.println(next);

      if (next == MOVE_TURN_LEFT || next == MOVE_TURN_RIGHT) {
      handleIntersectionSequence(next); // immediate turn from prev cell
      }

      return;
    }

    // Case 2: on PICKUP
if (gridX == pickupX && gridY == pickupY) {

  // Check cameFromPrev if needed
  if (pickupRequirePrev) {
    bool cameFromPrev = (lastGridX == pickupPrevX && lastGridY == pickupPrevY);
    if (!cameFromPrev) {
      Serial.println("!!! PICKUP reached from WRONG side. Going back to pickupPrev.");
      onPickupApproach = false;
      setTargetForCurrentMission(); // target = pickupPrev
      return;
    }
  }

  Serial.println(">>> At PICKUP cell. Checking platform with TOF...");

  if (platformPresentStable()) {
    Serial.println(">>> Platform detected (<100mm). Proceeding to LIFT_UP.");
    mission = PHASE_LIFT_UP;
    return;
  } else {
    Serial.println("!!! No platform detected. Returning HOME.");
    mission = PHASE_RETURN_HOME;
    onPickupApproach = false;

    targetX = homeX;
    targetY = homeY;

    MoveAction next = computeNextMove();
    Serial.print(">>> Next after NO platform (go HOME): ");
    Serial.println(next);

    if (next == MOVE_TURN_LEFT || next == MOVE_TURN_RIGHT) {
      handleIntersectionSequence(next);
    }

    ignoreIntersection = true;
    ignoreUntil = millis() + 1000;
    return;
    }
  }

     

    // Safety: arrived on an unexpected target cell
    Serial.println("!!! ARRIVED but not at pickupPrev or pickup. Check your targets.");
    return;
  }

  if (mission == PHASE_GOTO_DROPOFF) {
    mission = PHASE_LIFT_DOWN;
    return;
  }

  }
  if (mission == PHASE_RETURN_HOME) {
  // If home reached
  if (gridX == homeX && gridY == homeY) {
    Serial.println(">>> HOME reached. Mission done.");
    mission = PHASE_DONE;
    return;
  }
  // Normal navigation to target=(homeX,homeY)
  }


    // Otherwise, execute intersection sequence
    handleIntersectionSequence(action);
    return;
  }
 
  // Otherwise, normal line follow
  goLineFollow(position);
}
const int PLATFORM_MM = 250;  // platform threshold

bool platformSeenOnce() {
  uint16_t d1 = tof1.read();
  bool ok1 = !tof1.timeoutOccurred() && d1 > 0 && d1 < PLATFORM_MM;

  uint16_t d2 = tof2.read();
  bool ok2 = !tof2.timeoutOccurred() && d2 > 0 && d2 < PLATFORM_MM;

  return ok1 || ok2;
}

// Voting filter: 5 reads, if >=2 hits then platform present
bool platformPresentStable() {
  int hits = 0;
  const int N = 5;
  for (int i=0; i<N; i++) {
    if (platformSeenOnce()) hits++;
    delay(50);
  }
  return hits >= 2;
}

// ================== LINE FOLLOW ==================
void goLineFollow(uint16_t position){
  int16_t error = position - LINE_CENTER;
  integral += error;
  float derivative = error - lastError;
  float correction = Kp*error + Kd*derivative;
  lastError = error;
 
  int ls = constrain(BASE_SPEED + correction, 0, MAX_SPEED);
  int rs = constrain(BASE_SPEED - correction, 0, MAX_SPEED);
 
  setMotorSpeeds(ls, rs);
}
 
// ================== INTERSECTION ACTION ==================
void handleIntersectionSequence(MoveAction action){
  
  // Align forward (common for all actions)
  Serial.println(" > Align forward before action");
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  analogWrite(ENA, 100);
  analogWrite(ENB, 85);
  delay(FORWARD_BEFORE_TURN_DELAY);
 
  stopMotors();
  delay(120);

  if (action == MOVE_STRAIGHT) {
    Serial.println(" > Intersection action: GO STRAIGHT");
    // no pivot
  }
  else if (action == MOVE_TURN_RIGHT) {
    Serial.println(" > Intersection action: TURN RIGHT");

    // Turn right
    digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH);
    int turnPwm = carryingLoad ? TURN_pwm_loaded  : 165;
    int turnTime = carryingLoad ? TURN_90_DELAY_LOADED : TURN_90_DELAY_EMPTY;
    analogWrite(ENA, turnPwm);
    analogWrite(ENB, turnPwm);

    
    delay(turnTime);

 
    stopMotors();
    delay(100);

    rotateOrientationRight();
  }
  else if (action == MOVE_TURN_LEFT) {
    Serial.println(" > Intersection action: TURN LEFT");

    // Turn left
    digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH);  // left backward
    digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);   // right forward
    int turnPwm = carryingLoad ? TURN_pwm_loaded  : 165;
    int turnTime = carryingLoad ? TURN_90_DELAY_LOADED : TURN_90_DELAY_EMPTY;
    analogWrite(ENA, turnPwm);
    analogWrite(ENB, turnPwm);
    delay(turnTime);


    stopMotors();
    delay(100);

    rotateOrientationLeft();
  }

  // Advance to reacquire line
  Serial.println(" > Reacquire line");
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  analogWrite(ENA, 120);
  analogWrite(ENB, 120);
  delay(REACQUIRE_DELAY);
 
  stopMotors();
  delay(50);
 
  // Reset PID
  integral = 0;
  lastError = 0;
 
  Serial.println("> Resuming PID");
}
 
// ================== MOTOR HELPERS ==================
void setMotorSpeeds(int l, int r){
  digitalWrite(IN1, l>=0); 
  digitalWrite(IN2, l<0);
  analogWrite(ENA, abs(l));
 
  digitalWrite(IN3, r>=0); 
  digitalWrite(IN4, r<0);
  analogWrite(ENB, abs(r));
}
 
void stopMotors(){
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// ================== NAVIGATION HELPERS ==================
void updateGridPosition() {
  lastGridX = gridX;
  lastGridY = gridY;
  switch (orientation) {
    case NORTH:
      if (gridY < GRID_HEIGHT - 1) gridY++;
      break;
    case SOUTH:
      if (gridY > 0) gridY--;
      break;
    case EAST:
      if (gridX < GRID_WIDTH - 1) gridX++;
      break;
    case WEST:
      if (gridX > 0) gridX--;
      break;
  }

  Serial.print(" -> New grid position = (");
  Serial.print(gridX); Serial.print(","); Serial.print(gridY);
  Serial.println(")");
}
bool isForbiddenCell(int x, int y) {
  // Do not pass through pickup while not in final approach
  if (mission == PHASE_GOTO_PICKUP && pickupRequirePrev && !onPickupApproach) {
    if (x == pickupX && y == pickupY) return true;
  }
  return false;
}
bool bfsNextStep(int sx, int sy, int gx, int gy, int &nx, int &ny) {
  // BFS on 3x3 grid, store parents to reconstruct
  const int W = GRID_WIDTH;
  const int H = GRID_HEIGHT;

  bool vis[3][3] = {{false}};
  int px[3][3];
  int py[3][3];

  // Init parents
  for (int y=0; y<H; y++) for (int x=0; x<W; x++) { px[y][x] = -1; py[y][x] = -1; }

  // Simple queue (max 9 cells)
  int qx[9], qy[9];
  int qs=0, qe=0;

  // If start or goal is forbidden
  if (isForbiddenCell(sx, sy)) return false;
  if (isForbiddenCell(gx, gy)) return false;

  vis[sy][sx] = true;
  qx[qe] = sx; qy[qe] = sy; qe++;

  int dirs[4][2] = {{0,1},{1,0},{0,-1},{-1,0}}; // N,E,S,W steps

  while (qs < qe) {
    int x = qx[qs], y = qy[qs]; qs++;

    if (x == gx && y == gy) break;

    for (int i=0;i<4;i++){
      int xx = x + dirs[i][0];
      int yy = y + dirs[i][1];
      if (xx<0 || xx>=W || yy<0 || yy>=H) continue;
      if (vis[yy][xx]) continue;
      if (isForbiddenCell(xx, yy)) continue;

      vis[yy][xx] = true;
      px[yy][xx] = x;
      py[yy][xx] = y;
      qx[qe] = xx; qy[qe] = yy; qe++;
    }
  }

  if (!vis[gy][gx]) return false; // no path

  // Reconstruct from goal to start to find next step
  int cx = gx, cy = gy;
  int prevx = px[cy][cx];
  int prevy = py[cy][cx];

  // If start == goal
  if (sx == gx && sy == gy) { nx = sx; ny = sy; return true; }

  // Walk back until parent is start: first step found
  while (!(prevx == sx && prevy == sy)) {
    cx = prevx; cy = prevy;
    prevx = px[cy][cx];
    prevy = py[cy][cx];

    // Safety
    if (prevx == -1 || prevy == -1) return false;
  }

  nx = cx; ny = cy; // next cell
  return true;
}

MoveAction computeNextMove() {
  // Arrived?
  if (gridX == targetX && gridY == targetY) return MOVE_ARRIVED;

  int nx, ny;
  bool ok = bfsNextStep(gridX, gridY, targetX, targetY, nx, ny);

  if (!ok) {
    Serial.println("!!! BFS: no path (forbidden blocks). Defaulting to TURN RIGHT.");
    return MOVE_TURN_RIGHT; // fallback (or STOP)
  }

  // Determine direction from (gridX,gridY) to (nx,ny)
  Orientation desiredDir;
  if (nx == gridX && ny == gridY + 1) desiredDir = NORTH;
  else if (nx == gridX + 1 && ny == gridY) desiredDir = EAST;
  else if (nx == gridX && ny == gridY - 1) desiredDir = SOUTH;
  else desiredDir = WEST;

  if (desiredDir == orientation) return MOVE_STRAIGHT;

  Orientation rightOf, leftOf;
  switch (orientation) {
    case NORTH: rightOf = EAST;  leftOf = WEST;  break;
    case EAST:  rightOf = SOUTH; leftOf = NORTH; break;
    case SOUTH: rightOf = WEST;  leftOf = EAST;  break;
    case WEST:  rightOf = NORTH; leftOf = SOUTH; break;
  }

  if (desiredDir == rightOf) return MOVE_TURN_RIGHT;
  if (desiredDir == leftOf)  return MOVE_TURN_LEFT;
  return MOVE_TURN_RIGHT; // 180° -> two rights (or two lefts)
}


void rotateOrientationRight() {
  orientation = static_cast<Orientation>((orientation + 1) % 4);
  Serial.print("   orientation -> ");
  Serial.println(orientation);
}

void rotateOrientationLeft() {
  orientation = static_cast<Orientation>((orientation + 3) % 4);
  Serial.print("   orientation -> ");
  Serial.println(orientation);
}

// ================== TARGET SELECTOR ==================
void setTargetForCurrentMission() {
  if (mission == PHASE_GOTO_PICKUP) {

    // 1) Go to imposed previous cell first
    if (pickupRequirePrev && !onPickupApproach) {
      targetX = pickupPrevX;
      targetY = pickupPrevY;
    }
    // 2) Once on prev cell, go to pickup
    else {
      targetX = pickupX;
      targetY = pickupY;
    }

  } else if (mission == PHASE_GOTO_DROPOFF) {
    targetX = dropX;
    targetY = dropY;
  }
  else if (mission == PHASE_RETURN_HOME) {
  targetX = homeX;
  targetY = homeY;
}

}


// ================== LIFT STUBS ==================
void doLiftUp() {
  Serial.println("   [LIFT UP] starting");

  // Direction 1: L_PWM active, R_PWM = 0
  analogWrite(LIFT_R_PWM, 0);
  analogWrite(LIFT_L_PWM, LIFT_SPEED);

  delay(LIFT_UP_TIME);

  // Stop lift
  analogWrite(LIFT_R_PWM, 0);
  analogWrite(LIFT_L_PWM, 0);
  carryingLoad = true;

  Serial.println("   [LIFT UP] done");
  onPickupApproach = false;   // reset for next pickup mission

}


void doLiftDown() {
  Serial.println("   [LIFT DOWN] starting");

  // Direction 2: R_PWM active, L_PWM = 0
  analogWrite(LIFT_R_PWM, LIFT_SPEED);
  analogWrite(LIFT_L_PWM, 0);

  delay(LIFT_DOWN_TIME);

  // Stop lift
  analogWrite(LIFT_R_PWM, 0);
  analogWrite(LIFT_L_PWM, 0);

  Serial.println("   [LIFT DOWN] done");
  carryingLoad = false;

}