// ============================================================
// OPEN CHALLENGE -- fully autonomous run, ported from
// robot_autonomous_dm.ino.
//
// Runs only while GPIO2 is HIGH (see loop() in combined_robot.ino,
// which owns and resets the openXxx state variables below on
// every LOW->HIGH transition).
//
// Reuses the SAME ToF sensors (tofRight/tofLeft/tofFront, from
// tof.ino) and the SAME IMU (find_angle()/angleDifference(), from
// IMU.ino) that obstacle_challenge.ino also uses -- no separate
// sensor init here, tof_init()/imu_init() in setup() already
// cover both modes.
//
// Steering logic is unchanged from robot_autonomous_dm.ino:
//   - DRIVING: ToF left/right centering is the normal signal,
//     the IMU is a resistance/guard rail against drift.
//   - TURNING: IMU counts degrees turned toward a ~85 deg corner
//     turn, same as before.
// ============================================================

#include <VL53L1X.h>

extern VL53L1X tofRight;
extern VL53L1X tofLeft;
extern VL53L1X tofFront;

// ---- centering ----
#define OPEN_CENTER_KP     0.08     // deg of servo trim per mm of (left-right)
#define OPEN_CENTER_TOL    15       // mm, within this -> go straight

// ---- IMU straight-line resistance ----
#define OPEN_YAW_RESIST_DEG   8.0
#define OPEN_YAW_KP           1.2
#define OPEN_YAW_MAX_TRIM     18

// ---- front turn trigger ----
#define OPEN_FRONT_TURN_MM        500
#define OPEN_FRONT_TURN_CONFIRM   3
#define OPEN_FRONT_SAFETY_MM      120

// ---- turn ----
#define OPEN_TURN_TARGET_DEG    85.0
#define OPEN_TURN_TOLERANCE_DEG 5.0
#define OPEN_TURN_COOLDOWN_MS   600


void updateDrivingSteeringOpen(uint16_t dLeft, bool timeoutLeft, uint16_t dRight, bool timeoutRight, float yaw)
{
  float yawError = angleDifference(openStraightHeading, yaw); // + = need to steer right to get back

  if (abs(yawError) > OPEN_YAW_RESIST_DEG)
  {
    float trimF = OPEN_YAW_KP * yawError;
    if (trimF >  OPEN_YAW_MAX_TRIM) trimF =  OPEN_YAW_MAX_TRIM;
    if (trimF < -OPEN_YAW_MAX_TRIM) trimF = -OPEN_YAW_MAX_TRIM;
    steer_proportional((int)trimF);
    return;
  }

  if (timeoutLeft || timeoutRight || dLeft == 0 || dRight == 0)
  {
    turn_center();
    return;
  }

  int error = (int)dLeft - (int)dRight; // + = closer to RIGHT wall

  if (abs(error) <= OPEN_CENTER_TOL)
  {
    turn_center();
    return;
  }

  int trim = (int)(-OPEN_CENTER_KP * error);
  steer_proportional(trim);
}


int decideTurnDirectionOpen(uint16_t dLeft, bool timeoutLeft, uint16_t dRight, bool timeoutRight)
{
  bool haveL = !timeoutLeft  && dLeft  > 0;
  bool haveR = !timeoutRight && dRight > 0;

  if (haveL && haveR) return (dRight > dLeft) ? +1 : -1;
  if (haveR) return +1;
  if (haveL) return -1;
  return +1;
}

void startTurnOpen(int dir)
{
  openState = OPEN_TURNING;
  openTurnDirection = dir;
  openTurnAccumulatedDeg = 0.0;

  stop_motor();

  if (dir < 0) { turn_left();  Serial.println(">>> OPEN LEFT TURN"); }
  else         { turn_right(); Serial.println(">>> OPEN RIGHT TURN"); }

  delay(150);
  move_forward(turn_speed);
}

void updateTurnOpen(float yaw)
{
  if (openTurnDirection < 0) turn_left(); else turn_right();

  float step = angleDifference(yaw, openPreviousYaw);
  openTurnAccumulatedDeg += step;

  float progress = openTurnAccumulatedDeg * openTurnDirection;

  if (progress >= (OPEN_TURN_TARGET_DEG - OPEN_TURN_TOLERANCE_DEG))
  {
    turn_center();

    openTurnCount++;
    Serial.print(">>> OPEN TURN DONE (count = ");
    Serial.print(openTurnCount);
    Serial.println(")");

    if (openTurnCount >= 12)
    {
      stop_motor();
      Serial.println(">>> 12 TURNS REACHED - STOPPING");
      openState = OPEN_TURNING;
      openTurnDirection = 0;
      return;
    }

    move_forward(base_speed);

    openState = OPEN_DRIVING;
    openTurnDirection = 0;
    openLastTurnFinishedMs = millis();
    openFrontTurnConfirm = 0;
    openStraightHeading = yaw;
  }
}


void run_open_autonomous()
{
  uint16_t dRight = tofRight.read();
  uint16_t dLeft  = tofLeft.read();
  uint16_t dFront = tofFront.read();

  bool timeoutRight = tofRight.timeoutOccurred();
  bool timeoutLeft  = tofLeft.timeoutOccurred();
  bool timeoutFront = tofFront.timeoutOccurred();

  float yaw = find_angle();

  Serial.print("R: "); Serial.print(timeoutRight ? -1 : dRight);
  Serial.print(" L: "); Serial.print(timeoutLeft ? -1 : dLeft);
  Serial.print(" F: "); Serial.print(timeoutFront ? -1 : dFront);
  Serial.print(" Yaw: "); Serial.print(yaw, 1);
  Serial.print(" TgtHd: "); Serial.print(openStraightHeading, 1);
  Serial.print(" State: "); Serial.println(openState == OPEN_DRIVING ? "DRIVING" : "TURNING");

  bool frontDanger = !timeoutFront && dFront > 0 && dFront < OPEN_FRONT_SAFETY_MM;

  if (frontDanger)
  {
    stop_motor();
    openFrontBlocked = true;
  }
  else
  {
    if (openFrontBlocked) move_forward(openState == OPEN_TURNING ? turn_speed : base_speed);
    openFrontBlocked = false;
  }

  if (openFrontBlocked)
  {
    openPreviousYaw = yaw;
    delay(100);
    return;
  }

  if (openState == OPEN_TURNING)
  {
    updateTurnOpen(yaw);
  }
  else
  {
    bool inCooldown = (millis() - openLastTurnFinishedMs) < OPEN_TURN_COOLDOWN_MS;
    bool frontClose = !timeoutFront && dFront > 0 && dFront < OPEN_FRONT_TURN_MM;

    openFrontTurnConfirm = frontClose ? openFrontTurnConfirm + 1 : 0;

    if (!inCooldown && openFrontTurnConfirm >= OPEN_FRONT_TURN_CONFIRM)
    {
      stop_motor();
      uint16_t dL = tofLeft.read();
      uint16_t dR = tofRight.read();
      bool tL = tofLeft.timeoutOccurred();
      bool tR = tofRight.timeoutOccurred();

      int dir = decideTurnDirectionOpen(dL, tL, dR, tR);
      startTurnOpen(dir);
    }
    else
    {
      updateDrivingSteeringOpen(dLeft, timeoutLeft, dRight, timeoutRight, yaw);
    }
  }

  openPreviousYaw = yaw;
  delay(100);
}