#include <WiFi.h>
// #include <AsyncTCP.h> // No longer needed for ESP32 acting as WS server
// #include <ESPAsyncWebServer.h> // No longer needed for ESP32 acting as WS server
#include <TinyGPS++.h> // Added TinyGPS++ library
#include <WebSocketsClient.h>   // For WebSocket client functionality
#include <TinyGPS++.h>
#include <WiFiClientSecure.h> // For WSS (secure WebSocket)
#include <ArduinoJson.h>
#include <Wire.h>
#include <MPU6050.h>
#include <math.h>
#include <ESP32Servo.h>
#include <PID_v1.h>
#include <SPIFFS.h>
#include <HardwareSerial.h>

// Define a blinking interval for indicators (e.g., 250ms)
const int BLINK_INTERVAL = 250;

// MPU6050 object
MPU6050 mpu;

// Motor control pins for IBT-2 (gyroscope motor)
const int IBT2_IN1 = 25;
const int IBT2_IN2 = 26;
const int IBT2_EN = 14;

// Servo control pin
const int SERVO_PIN = 18;
// Servo Angles
const int SERVO_MIN_ANGLE = 45;
const int SERVO_CENTER_ANGLE = 90;
const int SERVO_MAX_ANGLE = 135;

// Additional motor control pins
const int DRIVE_MOTOR_PWM = 5;    // PWM pin for hub motor speed control
const int DRIVE_MOTOR_DIR = 17;   // Direction pin for hub motor
const int BRAKE_PIN = 16;         // Brake control pin
const int IGNITION_PIN = 4;       // Ignition control pin
const int CLUTCH_PIN = 2;         // Pin for the electric clutch
const int LEFT_INDICATOR_PIN = 15;    // Pin for the left turn indicator
const int RIGHT_INDICATOR_PIN = 13; // Pin for the right turn indicator

// Battery Management Pins
const int BATTERY_1_PIN = 34; // GPIO34 for Battery 1 voltage sensing
const int BATTERY_2_PIN = 35; // GPIO35 for Battery 2 voltage sensing
const int BATTERY_SWITCH_CTRL_1 = 32; // Output to control Battery 1 charging/discharging (e.g., Q1+Q2)
const int BATTERY_SWITCH_CTRL_2 = 33; // Output to control Battery 2 charging/discharging (e.g., Q3+Q4)

// Servo object
Servo steeringServo;

// PID variables for gyroscope motor
double gyroSetpoint = 0;        // We want to stay at 0 degrees (balanced)
double gyroInput, gyroOutput;
PID gyroPID(&gyroInput, &gyroOutput, &gyroSetpoint, 2.0, 5.0, 1.0, DIRECT); // Default PID values

// PID variables for steering servo
double steeringSetpoint = SERVO_CENTER_ANGLE; // Center position (servo angle)
double steeringInputAngle;        // This will be the MPU roll_degrees
double steeringPIDOutputAngle;  // This will be the servo command
PID steeringPID(&steeringInputAngle, &steeringPIDOutputAngle, &steeringSetpoint, 1.5, 0.2, 0.5, DIRECT); // Tuned for servo angle

// WiFi credentials
const char* MY_SSID = "Wokwi-GUEST"; // <<--- PUT YOUR WIFI SSID HERE
const char* MY_PASSWORD = ""; // <<--- PUT YOUR WIFI PASSWORD HERE
// #define WIFI_SSID "Wokwi-GUEST"
// #define WIFI_PASSWORD ""
// // Defining the WiFi channel speeds up the connection:
// #define WIFI_CHANNEL 6
// --- WebSocket Client Setup ---
WebSocketsClient webSocket;
WiFiClientSecure secureClient; // Secure client for WSS

const char* WEBSOCKET_SERVER_HOST = "esp32-autonomous-bicycle.onrender.com"; //deployement URL. we can use filrebase url here if we run servr from agent
const uint16_t WEBSOCKET_SERVER_PORT = 443; // Default for WSS
const char* WEBSOCKET_SERVER_PATH = "/ws?device=esp32"; // Path including query parameter

// AsyncWebServer server(80); // REMOVED - ESP32 is now a client
// AsyncWebSocket ws("/ws"); // REMOVED - ESP32 is now a client

// Dashboard / Status variables
float currentSpeed = 0;
int speedLevel = 1;       // 1=low, 2=medium, 3=high
bool isDriving = false;
bool isReverse = false;
bool isIgnitionOn = false;
String currentAction = "Idle";
bool balanceSystemActive = false;
bool regenerativeBrakingActive = false;

// Speed profiles (PWM values)
const int SPEED_LOW = 85;
const int SPEED_MEDIUM = 170;
const int SPEED_HIGH = 255;

// GPS data
float vehicleLat = 33.6844; // Initial vehicle location
float vehicleLng = 73.0479;
float userLat = 0.0; // User pickup location
float userLng = 0.0;
bool hasUserTargetLocation = false;

// Lidar sensor simulation
float sensorForward = 100.0;
float sensorBackward = 100.0;
float sensorLeft = 100.0;
float sensorRight = 100.0;
float sensorForwardLeft = 100.0;
float sensorForwardRight = 100.0;

// Battery Management
const float VOLTAGE_DIVIDER_R1 = 100000.0;
const float VOLTAGE_DIVIDER_R2 = 3100.0;
const float ADC_MAX_VALUE = 4095.0;
const float ADC_REFERENCE_VOLTAGE = 3.3;
const float BATTERY_LOW_THRESHOLD = 40.0;
const float BATTERY_SWITCH_HYSTERESIS = 2.0;
unsigned long lastBatteryCheckTime = 0;
const long BATTERY_CHECK_INTERVAL = 5000;
int activeBattery = 1;
float battery1Voltage = 0.0;
float battery2Voltage = 0.0;

// Timer for sending WebSocket data
unsigned long lastWsSendTime = 0;
const long wsSendInterval = 500;

// GPS Module (Neo6M)
HardwareSerial neogps(1);
const int GPS_RX_PIN = 12;
TinyGPSPlus gps;
// Note: Your previous code had GPS_TX_PIN as 13. If you are only receiving GPS data,
// you don't strictly need to define or use the TX pin in your begin() call.
const int GPS_TX_PIN = 13; // Connect to GPS RX (if you need to send commands)

// Function Declarations
void applyGyroPID();
void applySteeringPID();
void stopGyroMotor();
void turnLeft();
void turnRight();
void keepStraight();
void driveForward();
void driveReverse();
void stopDriveMotor();
void applyBrake();
void toggleIgnition();
void toggleRegenerativeBraking();
void emergencyStop();
void sendSensorDataToClients(String rearAlertMessage, String rearAlertLevel);
void handleWebSocketMessage(uint8_t *payload, size_t length); // MODIFIED SIGNATURE
void webSocketClientEvent(WStype_t type, uint8_t *payload, size_t length); // NEW WebSocket client event handler
float readBatteryVoltage(int analogPin);
void switchBatteriesIfNeeded();
void engageClutch();
void disengageClutch();
void blinkLeftIndicator();
void blinkRightIndicator();
void readGpsData();

void setup() {
    Serial.begin(115200);
    Serial.println("\nBooting device...");

    Wire.begin();
    mpu.initialize();
    Serial.print("Testing MPU6050 connection...");
    if (!mpu.testConnection()) {
        Serial.println(" FAILED! Halting execution.");
        while (1) { delay(100); }
    }
    Serial.println(" Ready!");

    pinMode(IBT2_IN1, OUTPUT);
    pinMode(IBT2_IN2, OUTPUT);
    pinMode(IBT2_EN, OUTPUT);
    pinMode(DRIVE_MOTOR_PWM, OUTPUT);
    pinMode(DRIVE_MOTOR_DIR, OUTPUT);
    pinMode(BRAKE_PIN, OUTPUT);
    pinMode(IGNITION_PIN, OUTPUT);
    pinMode(CLUTCH_PIN, OUTPUT);
    pinMode(LEFT_INDICATOR_PIN, OUTPUT);
    pinMode(RIGHT_INDICATOR_PIN, OUTPUT);
    pinMode(BATTERY_1_PIN, INPUT);
    pinMode(BATTERY_2_PIN, INPUT);
    pinMode(BATTERY_SWITCH_CTRL_1, OUTPUT);
    pinMode(BATTERY_SWITCH_CTRL_2, OUTPUT);

    steeringServo.attach(SERVO_PIN);

    gyroPID.SetMode(MANUAL);
    gyroPID.SetOutputLimits(-255, 255);
    steeringPID.SetMode(AUTOMATIC);
    steeringPID.SetOutputLimits(SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);

    stopGyroMotor();
    keepStraight();
    stopDriveMotor();
    digitalWrite(BRAKE_PIN, LOW);
    digitalWrite(IGNITION_PIN, LOW);
    disengageClutch();
    digitalWrite(BATTERY_SWITCH_CTRL_1, LOW);
    digitalWrite(BATTERY_SWITCH_CTRL_2, HIGH);

    Serial.print("Mounting SPIFFS...");
    if (!SPIFFS.begin(true)) {
        Serial.println(" FAILED! Halting execution.");
        while (1) { delay(100); }
    }
    Serial.println(" Mounted.");

    Serial.print("Connecting to WiFi: ");
    Serial.println(MY_SSID);
    WiFi.begin(MY_SSID, MY_PASSWORD);

    int maxAttempts = 20;
    while (WiFi.status() != WL_CONNECTED && maxAttempts > 0) {
        delay(500);
        Serial.print(".");
        maxAttempts--;
    }

    if (WiFi.status() == WL_CONNECTED) {
        Serial.println("\nWiFi connected!");
        Serial.print("IP Address: ");
        Serial.println(WiFi.localIP());

        // --- Configure WebSocket Client ---
        // For WSS (SSL/TLS connection to Render)
        // If you encounter SSL handshake errors, you might need to enable insecure mode (less safe)
        // or provide the server's root CA certificate.
        // secureClient.setInsecure(); // Uncomment ONLY if necessary and understand the risks.
        // Example for setting a Root CA (if you have it):
        // secureClient.setCACert(rootCACertificate);
        
        //webSocket.setSSLClient(&secureClient); // Use our secure client instance for WSS
        //webSocket.begin(WEBSOCKET_SERVER_HOST, WEBSOCKET_SERVER_PORT, WEBSOCKET_SERVER_PATH);
        // Use the beginSSL method:
        webSocket.beginSSL(WEBSOCKET_SERVER_HOST, WEBSOCKET_SERVER_PORT, WEBSOCKET_SERVER_PATH);
        webSocket.onEvent(webSocketClientEvent);
        webSocket.setReconnectInterval(5000); // Attempt to reconnect every 5 seconds if connection is lost
        // Optional: Enable PING/PONG heartbeat to keep connection alive through firewalls
        // webSocket.enableHeartbeat(15000, 3000, 2); // Send PING every 15s, timeout 3s, 2 retries

        Serial.println("WebSocket client configured. Attempting to connect to Render server...");
    } else {
        Serial.println("\nFailed to connect to WiFi. WebSocket client connection skipped.");
    }

    // Old server setup - REMOVED
    // ws.onEvent(onEvent);
    // server.addHandler(&ws);
    // server.onNotFound(...);
    // server.begin();
    // Serial.println("HTTP and WebSocket server started.");

    currentAction = "Awaiting Commands";

    neogps.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN); // Initialize with RX and TX pins
    Serial.println("GPS Serial Initialized.");
}

void loop() {
    if (WiFi.status() == WL_CONNECTED) {
        webSocket.loop(); // Must be called regularly to process WebSocket events
    }

    int16_t accelX, accelY, accelZ;
    mpu.getAcceleration(&accelX, &accelY, &accelZ);

    float aX = accelX / 16384.0;
    float aZ = accelZ / 16384.0;
    float roll_radians = atan2(aX, aZ);
    float roll_degrees = roll_radians * (180.0 / PI);

    gyroInput = roll_degrees;
    if (balanceSystemActive) {
        gyroPID.Compute();
    }

    steeringInputAngle = roll_degrees;
    steeringPID.Compute();
    steeringServo.write(steeringPIDOutputAngle);

    if (isIgnitionOn) {
        if (balanceSystemActive) {
            applyGyroPID();
        } else {
            stopGyroMotor();
        }
        if (hasUserTargetLocation && isDriving) {
            float dLat = userLat - vehicleLat;
            float dLng = userLng - vehicleLng;
            float distance = sqrt(dLat * dLat + dLng * dLng);
            // Replace simulation with actual navigation logic if needed later
            if (distance > 0.0001) {
                // Basic steering towards target - implement proper navigation later
            } else {
                currentAction = "Arrived at User Location";
                hasUserTargetLocation = false;
            }
        }
    } else {
        stopGyroMotor();
        stopDriveMotor();
        applyBrake();
    }

    unsigned long currentTime = millis();
    if (currentTime - lastBatteryCheckTime >= BATTERY_CHECK_INTERVAL) {
        battery1Voltage = readBatteryVoltage(BATTERY_1_PIN);
        battery2Voltage = readBatteryVoltage(BATTERY_2_PIN);
        switchBatteriesIfNeeded();
        lastBatteryCheckTime = currentTime;
    }

    readGpsData();

    sensorForward = 50 + (sin(millis() / 1000.0) * 20);
    sensorBackward = 60 + (cos(millis() / 1500.0) * 15);
    sensorLeft = 40 + (sin(millis() / 800.0) * 10);
    sensorRight = 45 + (cos(millis() / 1200.0) * 12);
    sensorForwardLeft = 70 + (sin(millis() / 900.0) * 18);
    sensorForwardRight = 65 + (cos(millis() / 1100.0) * 14);

    String rearAlertMessage = "Clear";
    String rearAlertLevel = "none";
    if (sensorBackward < 20) {
        rearAlertMessage = "Danger! Object very close behind!";
        rearAlertLevel = "danger";
        emergencyStop();
        currentAction = "Emergency Stop (Rear Obstacle)";
    } else if (sensorBackward < 50) {
        rearAlertMessage = "Warning! Object behind!";
        rearAlertLevel = "warning";
    }

    if ((sensorForward < 30 || sensorForwardLeft < 30 || sensorForwardRight < 30) &&
        (sensorLeft < 30 || sensorRight < 30) &&
        (sensorBackward < 30)) {
        currentAction = "Surrounded! Alarm! Stopping!";
        emergencyStop();
    }

    if (currentTime - lastWsSendTime >= wsSendInterval) {
        if (webSocket.isConnected()) { // Only send if connected
            sendSensorDataToClients(rearAlertMessage, rearAlertLevel);
        }
        lastWsSendTime = currentTime;
    }

    delay(10);
}

// --- New WebSocket Client Event Handler ---
void webSocketClientEvent(WStype_t type, uint8_t *payload, size_t length) {
    switch (type) {
        case WStype_DISCONNECTED:
            Serial.printf("[WSc] Disconnected from %s!\n", WEBSOCKET_SERVER_HOST);
            currentAction = "Disconnected from Server";
            break;
        case WStype_CONNECTED:
            Serial.printf("[WSc] Connected to: %s\n", (char*)payload); // Server URL on successful connection
            currentAction = "Connected to Server";
            // Send initial status to the server upon connection
            sendSensorDataToClients("Clear", "none");
            break;
        case WStype_TEXT:
            Serial.printf("[WSc] Received text: %s\n", payload);
            handleWebSocketMessage(payload, length); // Process message from server
            break;
        case WStype_BIN:
            Serial.printf("[WSc] Received binary data of length: %u\n", length);
            // Handle binary data if needed
            break;
        case WStype_ERROR:
            Serial.printf("[WSc] Error: %s\n", payload);
            currentAction = "WebSocket Error";
            break;
        case WStype_FRAGMENT_TEXT_START:
        case WStype_FRAGMENT_BIN_START:
        case WStype_FRAGMENT:
        case WStype_FRAGMENT_FIN:
            Serial.printf("[WSc] Fragment event: %d\n", type);
            // Handle fragmented messages if your server sends them
            break;
        case WStype_PING:
            Serial.printf("[WSc] Received PING\n");
            // Library handles PONG automatically
            break;
        case WStype_PONG:
            Serial.printf("[WSc] Received PONG\n");
            break;
        default:
            Serial.printf("[WSc] Unknown event type: %d\n", type);
            break;
    }
}

// --- Modified WebSocket Message Handler (now for client) ---
// Original: void handleWebSocketMessage(AsyncWebSocketClient *client, AwsFrameInfo *info, uint8_t *data, size_t len)
void handleWebSocketMessage(uint8_t *payload, size_t length) {
    // Payload is from the server. Process it as JSON.
    StaticJsonDocument<1024> doc;
    DeserializationError error = deserializeJson(doc, (const char*)payload, length); // Use const char* and length

    if (error) {
        Serial.print(F("deserializeJson() failed: "));
        Serial.println(error.c_str());
        Serial.print(F("Payload was: "));
        Serial.write(payload, length); // Print the raw payload for debugging
        Serial.println();
        return;
    }

    if (!doc.containsKey("command")) {
        Serial.println("WS Error: No 'command' field in received JSON from server");
        return;
    }
    const char* command = doc["command"];
    Serial.printf("Received command from server: %s\n", command);

    // The rest of your command processing logic remains the same.
    // Ensure all commands your server might send are handled here.
    if (strcmp(command, "request_pickup") == 0) {
        currentAction = "Pickup Requested";
        if (!isIgnitionOn) toggleIgnition();
        balanceSystemActive = true;
        gyroPID.SetMode(AUTOMATIC);

        if (doc.containsKey("userLocation") && doc["userLocation"].is<JsonObject>()) {
            userLat = doc["userLocation"]["lat"];
            userLng = doc["userLocation"]["lng"];
            hasUserTargetLocation = true;
            currentAction = "Navigating to Pickup Location";
            driveForward();
        } else {
            currentAction = "Pickup Requested (No specific location provided)";
        }
    } else if (strcmp(command, "start_ignition") == 0) {
        toggleIgnition();
        currentAction = isIgnitionOn ? "Ignition ON" : "Ignition OFF";
    } else if (strcmp(command, "balance_cycle") == 0) {
        if (isIgnitionOn) {
            balanceSystemActive = !balanceSystemActive;
            if (balanceSystemActive) {
                gyroPID.SetMode(AUTOMATIC);
                currentAction = "Balance System Engaged";
            } else {
                gyroPID.SetMode(MANUAL);
                stopGyroMotor();
                currentAction = "Balance System Disengaged";
            }
        } else {
            currentAction = "Ignition must be ON for Balance";
        }
    } else if (strcmp(command, "toggle_regenerative_braking") == 0) {
        regenerativeBrakingActive = !regenerativeBrakingActive;
        if (regenerativeBrakingActive) {
            engageClutch();
            currentAction = "Regenerative Braking ON";
        } else {
            disengageClutch();
            currentAction = "Regenerative Braking OFF";
        }
    } else if (strcmp(command, "emergency_stop") == 0) {
        emergencyStop();
        currentAction = "Emergency Stop Activated!";
    } else if (strcmp(command, "set_direction") == 0) {
        if (!doc.containsKey("value") || !doc["value"].is<const char*>()) return;
        const char* value = doc["value"];
        if (strcmp(value, "forward") == 0) {
            driveForward(); currentAction = "Driving Forward";
        } else if (strcmp(value, "reverse") == 0) {
            driveReverse(); currentAction = "Driving Reverse";
        } else if (strcmp(value, "left") == 0) {
            turnLeft(); currentAction = "Turning Left";
        } else if (strcmp(value, "right") == 0) {
            turnRight(); currentAction = "Turning Right";
        } else if (strcmp(value, "stop") == 0) {
            stopDriveMotor(); applyBrake(); currentAction = "Stopped";
        } else if (strcmp(value, "forward_left") == 0) {
            driveForward(); turnLeft(); currentAction = "Driving Forward Left";
        } else if (strcmp(value, "forward_right") == 0) {
            driveForward(); turnRight(); currentAction = "Driving Forward Right";
        } else if (strcmp(value, "reverse_left") == 0) {
            driveReverse(); turnLeft(); currentAction = "Driving Reverse Left";
        } else if (strcmp(value, "reverse_right") == 0) {
            driveReverse(); turnRight(); currentAction = "Driving Reverse Right";
        }
    } else if (strcmp(command, "set_speed") == 0) {
        if (!doc.containsKey("value") || !doc["value"].is<int>()) return;
        int level = doc["value"];
        if (level >= 1 && level <= 3) {
            speedLevel = level;
            if (isDriving && !isReverse) driveForward(); // Re-apply speed if driving forward
            else if (isDriving && isReverse) driveReverse(); // Re-apply speed if driving reverse
            currentAction = "Speed set to level " + String(speedLevel);
        }
    } else if (strcmp(command, "set_gyro_pid") == 0) {
        if (doc.containsKey("p") && doc.containsKey("i") && doc.containsKey("d")) {
            if (doc["p"].is<double>() && doc["i"].is<double>() && doc["d"].is<double>()) {
                gyroPID.SetTunings(doc["p"], doc["i"], doc["d"]);
                currentAction = "Gyro PID Updated";
            }
        }
    } else if (strcmp(command, "set_steering_pid") == 0) {
        if (doc.containsKey("p") && doc.containsKey("i") && doc.containsKey("d")) {
            if (doc["p"].is<double>() && doc["i"].is<double>() && doc["d"].is<double>()) {
                steeringPID.SetTunings(doc["p"], doc["i"], doc["d"]);
                currentAction = "Steering PID Updated";
            }
        }
    }
    // Send updated status back to server after processing a command, as in original logic
    sendSensorDataToClients("Clear", "none");
}

void sendSensorDataToClients(String rearAlertMessage, String rearAlertLevel) {
    if (!webSocket.isConnected()) {
        // Serial.println("[WSc] Cannot send data, not connected to server.");
        return;
    }

    StaticJsonDocument<1024> doc;
    doc["gps"]["lat"] = vehicleLat;
    doc["gps"]["lng"] = vehicleLng;
    doc["user_gps"]["lat"] = userLat;
    doc["user_gps"]["lng"] = userLng;
    doc["ignition"] = isIgnitionOn ? "On" : "Off";
    doc["gyroscope"] = balanceSystemActive ? "Active" : "Off";
    doc["balance"] = String(gyroInput, 2);
    doc["currentAction"] = currentAction;
    doc["battery_1_voltage"] = String(battery1Voltage, 2);
    doc["battery_2_voltage"] = String(battery2Voltage, 2);
    doc["active_battery"] = String(activeBattery);
    doc["sensors"]["forward"] = String(sensorForward, 1);
    doc["sensors"]["backward"] = String(sensorBackward, 1);
    doc["sensors"]["left"] = String(sensorLeft, 1);
    doc["sensors"]["right"] = String(sensorRight, 1);
    doc["sensors"]["forwardLeft"] = String(sensorForwardLeft, 1);
    doc["sensors"]["forwardRight"] = String(sensorForwardRight, 1);
    doc["rearAlert"]["message"] = rearAlertMessage;
    doc["rearAlert"]["level"] = rearAlertLevel;

    char jsonBuffer[1024];
    size_t len = serializeJson(doc, jsonBuffer, sizeof(jsonBuffer));
    
    // ws.textAll(jsonBuffer); // REMOVED (was for server mode)
    webSocket.sendTXT(jsonBuffer, len); // NEW: Send data to the connected server
    // Serial.print("[WSc] Sent data to server: "); Serial.println(jsonBuffer); // Optional: for debugging
}

// --- Actuator Control Functions (Unchanged) ---
void applyGyroPID() {
    if (gyroOutput > 0) {
        digitalWrite(IBT2_IN1, HIGH);
        digitalWrite(IBT2_IN2, LOW);
        analogWrite(IBT2_EN, abs(gyroOutput));
    } else if (gyroOutput < 0) {
        digitalWrite(IBT2_IN1, LOW);
        digitalWrite(IBT2_IN2, HIGH);
        analogWrite(IBT2_EN, abs(gyroOutput));
    } else {
        stopGyroMotor();
    }
}

void stopGyroMotor() {
    digitalWrite(IBT2_IN1, LOW);
    digitalWrite(IBT2_IN2, LOW);
    analogWrite(IBT2_EN, 0);
}

void turnLeft() {
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
    blinkLeftIndicator();
    steeringServo.write(SERVO_MIN_ANGLE);
}

void turnRight() {
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
    blinkRightIndicator();
    steeringServo.write(SERVO_MAX_ANGLE);
}

void keepStraight() {
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
    steeringServo.write(SERVO_CENTER_ANGLE);
}

void driveForward() {
    if (!isIgnitionOn) return;
    isDriving = true;
    isReverse = false;
    digitalWrite(BRAKE_PIN, LOW);
    digitalWrite(DRIVE_MOTOR_DIR, HIGH);
    int pwmValue = 0;
    if (speedLevel == 1) pwmValue = SPEED_LOW;
    else if (speedLevel == 2) pwmValue = SPEED_MEDIUM;
    else if (speedLevel == 3) pwmValue = SPEED_HIGH;
    analogWrite(DRIVE_MOTOR_PWM, pwmValue);
    currentAction = "Driving Forward at Speed " + String(speedLevel);
}

void driveReverse() {
    if (!isIgnitionOn) return;
    isDriving = true;
    isReverse = true;
    digitalWrite(BRAKE_PIN, LOW);
    digitalWrite(DRIVE_MOTOR_DIR, LOW);
    int pwmValue = 0;
    if (speedLevel == 1) pwmValue = SPEED_LOW;
    else if (speedLevel == 2) pwmValue = SPEED_MEDIUM;
    else if (speedLevel == 3) pwmValue = SPEED_HIGH;
    analogWrite(DRIVE_MOTOR_PWM, pwmValue);
    currentAction = "Driving Reverse at Speed " + String(speedLevel);
}

void stopDriveMotor() {
    isDriving = false;
    analogWrite(DRIVE_MOTOR_PWM, 0);
    applyBrake();
    currentAction = "Motor Stopped";
}

void applyBrake() {
    digitalWrite(BRAKE_PIN, HIGH);
}

void toggleIgnition() {
    isIgnitionOn = !isIgnitionOn;
    digitalWrite(IGNITION_PIN, isIgnitionOn ? HIGH : LOW);
    if (!isIgnitionOn) {
        stopDriveMotor();
        stopGyroMotor();
        applyBrake();
        balanceSystemActive = false;
        regenerativeBrakingActive = false;
        disengageClutch();
        currentAction = "Ignition OFF";
    } else {
        currentAction = "Ignition ON";
    }
}

void toggleRegenerativeBraking() {
    regenerativeBrakingActive = !regenerativeBrakingActive;
    if (regenerativeBrakingActive) {
        engageClutch();
        currentAction = "Regenerative Braking ON";
    } else {
        disengageClutch();
        currentAction = "Regenerative Braking OFF";
    }
}

void emergencyStop() {
    isIgnitionOn = false;
    digitalWrite(IGNITION_PIN, LOW);
    stopDriveMotor();
    stopGyroMotor();
    applyBrake();
    balanceSystemActive = false;
    regenerativeBrakingActive = false;
    disengageClutch();
    currentAction = "EMERGENCY STOP! ALL SYSTEMS OFF!";
}

void engageClutch() {
    digitalWrite(CLUTCH_PIN, HIGH);
}

void disengageClutch() {
    digitalWrite(CLUTCH_PIN, LOW);
}

float readBatteryVoltage(int analogPin) {
    int adcValue = analogRead(analogPin);
    float voltage = (adcValue * ADC_REFERENCE_VOLTAGE / ADC_MAX_VALUE) * ((VOLTAGE_DIVIDER_R1 + VOLTAGE_DIVIDER_R2) / VOLTAGE_DIVIDER_R2);
    return voltage;
}

void switchBatteriesIfNeeded() {
    if (activeBattery == 1) {
        if (battery1Voltage < (BATTERY_LOW_THRESHOLD - BATTERY_SWITCH_HYSTERESIS)) {
            Serial.println("Battery 1 low. Switching to Battery 2.");
            digitalWrite(BATTERY_SWITCH_CTRL_1, HIGH);
            digitalWrite(BATTERY_SWITCH_CTRL_2, LOW);
            activeBattery = 2;
            currentAction = "Switched to Battery 2 (Driving)";
        }
    } else { // activeBattery == 2
        if (battery2Voltage < (BATTERY_LOW_THRESHOLD - BATTERY_SWITCH_HYSTERESIS)) {
            Serial.println("Battery 2 low. Switching to Battery 1.");
            digitalWrite(BATTERY_SWITCH_CTRL_2, HIGH);
            digitalWrite(BATTERY_SWITCH_CTRL_1, LOW);
            activeBattery = 1;
            currentAction = "Switched to Battery 1 (Driving)";
        }
    }
}

unsigned long lastBlinkTimeLeft = 0;
bool leftIndicatorState = LOW;
void blinkLeftIndicator() {
    if (millis() - lastBlinkTimeLeft >= BLINK_INTERVAL) {
        lastBlinkTimeLeft = millis();
        leftIndicatorState = !leftIndicatorState;
        digitalWrite(LEFT_INDICATOR_PIN, leftIndicatorState);
    }
}

unsigned long lastBlinkTimeRight = 0;
bool rightIndicatorState = LOW;
void blinkRightIndicator() {
    if (millis() - lastBlinkTimeRight >= BLINK_INTERVAL) {
        lastBlinkTimeRight = millis();
        rightIndicatorState = !rightIndicatorState;
        digitalWrite(RIGHT_INDICATOR_PIN, rightIndicatorState);
    }
}

void readGpsData() {
 while (neogps.available()) {
 if (gps.encode(neogps.read())) {
 if (gps.location.isValid()) {
 vehicleLat = gps.location.lat();
 vehicleLng = gps.location.lng();
 }
        }
    }
}