// ============================================================
// COMBINED ROBOT SKETCH -- one project, two challenge modes,
// selected by GPIO2:
//
//   GPIO2 LOW  (default) -> OBSTACLE CHALLENGE
//                           Pi drives steering over Serial.
//                           See obstacle_challenge.ino.
//
//   GPIO2 HIGH            -> OPEN CHALLENGE
//                           Fully autonomous ToF+IMU wall
//                           following / cornering.
//                           See open_challenge_auto.ino.
//
// Both modes share the same ToF sensors (tof.ino), IMU
// (IMU.ino), motors (motors.ino), servo (servo.ino) and colour
// counter (colour_sensor.ino) -- only obstacle_challenge.ino and
// open_challenge_auto.ino are mode-specific.
//
// NOTE: GPIO2 is now the mode-select input, so it's no longer
// free to double as a debug output. The old `pinMode(2, OUTPUT)`
// inside servo_init() and the `digitalWrite(2, HIGH)` inside
// turn_left_90() (both leftover debug lines) have been removed
// from servo.ino.
// ============================================================

int base_speed = 250;
int turn_speed = 180;

extern const int max_left;
extern const int max_right;

bool orientation_set = false;
extern String orientation;

int counter = 0;

#define START_PIN 2

enum ChallengeMode { MODE_OBSTACLE, MODE_OPEN };
ChallengeMode currentMode = MODE_OBSTACLE;

enum OpenState { OPEN_DRIVING, OPEN_TURNING };
OpenState openState = OPEN_DRIVING;

int openTurnDirection = 0;
float openTurnAccumulatedDeg = 0.0;
float openPreviousYaw = 0.0;
float openStraightHeading = 0.0;
int openTurnCount = 0;
unsigned long openLastTurnFinishedMs = 0;
int openFrontTurnConfirm = 0;
bool openFrontBlocked = false;


void setup()
{
    Serial.begin(115200);
    delay(1000);

    pinMode(START_PIN, INPUT_PULLUP);

    motor_init();
    tof_init();
    imu_init();
    servo_init();
    colour_sensor_init();

    turn_center();
    stop_motor();

    Serial.println(">>> READY. GPIO2 LOW = obstacle, HIGH = open <<<");
}

void loop()
{
    bool openHigh = digitalRead(START_PIN) == HIGH;

    if (openHigh)
    {
        if (currentMode != MODE_OPEN)
        {
            currentMode = MODE_OPEN;

            openState = OPEN_DRIVING;
            openTurnDirection = 0;
            openTurnAccumulatedDeg = 0.0;
            openTurnCount = 0;
            openFrontTurnConfirm = 0;
            openFrontBlocked = false;

            float y = find_angle();
            openStraightHeading = y;
            openPreviousYaw = y;
            openLastTurnFinishedMs = millis();

            turn_center();
            move_forward(base_speed);

            Serial.println(">>> GPIO2 HIGH -- OPEN CHALLENGE MODE <<<");
        }

        run_open_autonomous();
    }
    else
    {
        if (currentMode != MODE_OBSTACLE)
        {
            currentMode = MODE_OBSTACLE;
            stop_motor();
            turn_center();
            Serial.println(">>> GPIO2 LOW -- OBSTACLE CHALLENGE MODE <<<");
        }

        obstacle_challenge();
    }
}