#include "Config.h"
#include "Motors.h"
#include "Sensors.h"
#include "BluetoothRC.h"

// Initialisation des objets Core
MotorController motors;
SensorManager sensors;
BluetoothRC bt;

// --- GESTION D'ÉTAT ---
RobotMode currentMode = RC_MODE;
unsigned long lastModeSwitchTime = 0;

// --- DÉCLARATIONS ---
void handleModeSwitch();
void handleRCMode(RCData data);
void handleLineFollowMode();
void applyJoystickMovement(int X_raw, int Y_raw, int P_raw);
void applyLineFollowPControl(int error); // Fonction de contrôle P

void setup() {
    // Initialisation des systèmes
    motors.begin();
    sensors.begin();
    bt.begin();
    
    Serial.println("SYSTEM: Robot Initialized. Ready for commands.");
    Serial.println("SYSTEM: Initial Mode -> RC_MODE");
    
    delay(1000); 
}

void loop() {
    // Vérifie la pression du bouton pour changer de mode
    handleModeSwitch();
    
    // Dispatcher de Logique Principal
    switch (currentMode) {
        case RC_MODE:
            // Mode 1: Contrôle à distance (utiliser le Moniteur Série pour envoyer les données RC)
            handleRCMode(bt.readData());
            break;
        case LINE_FOLLOW_MODE:
            // Mode 2: Suivi de ligne autonome (Utiliser les boutons de capteurs L/C/R)
            handleLineFollowMode();
            break;
    }
}

// --- CORE MODE FUNCTIONS ---

void handleModeSwitch() {
    if (sensors.isModeButtonPressed() && (millis() - lastModeSwitchTime > DEBOUNCE_DELAY)) {
        currentMode = (currentMode == RC_MODE) ? LINE_FOLLOW_MODE : RC_MODE;
        
        Serial.print("MODE SWITCHED: ");
        if (currentMode == RC_MODE) Serial.println("RC_MODE");
        else if (currentMode == LINE_FOLLOW_MODE) Serial.println("LINE_FOLLOW_MODE");
        
        motors.stop(); 
        motors.setLed(LOW, LOW);
        lastModeSwitchTime = millis();
    }
}

void handleRCMode(RCData data) {
    if (data.new_data) {
        applyJoystickMovement(data.X, data.Y, data.Power);
    }
}

void handleLineFollowMode() {
    LineState state;
    sensors.readLineSensors(state);
    // Renommé getLinePattern -> getLineError
    int error = sensors.getLineError(state); 
    
    applyLineFollowPControl(error);
}

// --- LOGIQUE DE CONTRÔLE P POUR LA VITESSE ET LA STABILITÉ ---

void applyLineFollowPControl(int error) {
    
    // 1. Ligne Perdue : Exécuter la stratégie de recherche (Spin)
    if (error == ERROR_LINE_LOST) {
        // Spin sur place pour retrouver rapidement la ligne (par exemple, tourner à gauche)
        motors.move(-BASE_SPEED_LINE, BASE_SPEED_LINE); 
        return;
    }

    // 2. Calcul de la Correction (Erreur * Kp)
    int correction = error * KP_COEFFICIENT;
    
    // 3. Application du P-Control
    // Erreur positive (trop à droite) -> correction positive -> Vitesse Droite augmente, Vitesse Gauche diminue (Tourne à Gauche)
    // Erreur négative (trop à gauche) -> correction négative -> Vitesse Droite diminue, Vitesse Gauche augmente (Tourne à Droite)
    int left_speed = BASE_SPEED_LINE - correction;
    int right_speed = BASE_SPEED_LINE + correction;
    
    // 4. Limiter les vitesses
    left_speed = constrain(left_speed, 0, MAX_PWM_VALUE);
    right_speed = constrain(right_speed, 0, MAX_PWM_VALUE);
    
    // Exécuter le mouvement
    motors.move(left_speed, right_speed);
// Ajouté à la fin de applyLineFollowPControl
if (correction > 5) {
    // Tourne à Gauche (Erreur positive)
    motors.setLed(LOW, HIGH); // LED Droite allumée pour signaler le virage à Gauche
} else if (correction < -5) {
    // Tourne à Droite (Erreur négative)
    motors.setLed(HIGH, LOW); // LED Gauche allumée pour signaler le virage à Droite
} else {
    // Centré (Erreur faible)
    motors.setLed(LOW, LOW);
}
}


// --- JOYSTICK LOGIC IMPLEMENTATION (Non-modifié) ---

void applyJoystickMovement(int X_raw, int Y_raw, int P_raw) {
    int left_speed = 0;
    int right_speed = 0;
    
    int p_reduction = map(P_raw, 100, 0, 0, 255);
    int max_speed_limit = 255 - p_reduction;
    
    if (X_raw == DEAD_CENTER_Y && Y_raw == DEAD_CENTER_Y) {
        motors.stop();
        motors.setLed(LOW, LOW);
        return;
    }

    // 1. Forward Motion (Y <= 124)
    if (Y_raw <= 124) { 
        int Y = Y_raw;
        left_speed = map(Y, 0, 124, 255, 0); 
        right_speed = map(Y, 0, 124, 255, 0); 

        if (X_raw >= DEAD_CENTER_X_HIGH) { 
            int turn_factor = map(X_raw, DEAD_CENTER_X_HIGH, 250, 0, 255);
            right_speed = right_speed - turn_factor * (1/2); 
            left_speed = constrain(left_speed + turn_factor, 0, 255); 
            motors.setLed(LOW, HIGH); 
        } else if (X_raw <= DEAD_CENTER_X_LOW) { 
            int turn_factor = map(X_raw, 0, DEAD_CENTER_X_LOW, 255, 0);
            left_speed = left_speed - turn_factor * (1/2); 
            right_speed = constrain(right_speed + turn_factor, 0, 255); 
            motors.setLed(HIGH, LOW); 
        } else {
            motors.setLed(LOW, LOW);
        }

        left_speed = map(left_speed, 0, 255, 0, max_speed_limit);
        right_speed = map(right_speed, 0, 255, 0, max_speed_limit);
        motors.move(left_speed, right_speed);
    }

    // 2. Reverse Motion (Y >= 126)
    else if (Y_raw >= 126) {
        int Y = Y_raw;
        left_speed = map(Y, 126, 250, 0, 255);
        right_speed = map(Y, 126, 250, 0, 255);

        if (X_raw >= DEAD_CENTER_X_HIGH) { 
            int turn_factor = map(X_raw, DEAD_CENTER_X_HIGH, 250, 0, 255);
            right_speed = right_speed + turn_factor * (1/2); 
            left_speed = constrain(left_speed - turn_factor, 0, 255); 
        } else if (X_raw <= DEAD_CENTER_X_LOW) {
            int turn_factor = map(X_raw, 0, DEAD_CENTER_X_LOW, 255, 0);
            left_speed = left_speed + turn_factor * (1/2);
            right_speed = constrain(right_speed - turn_factor, 0, 255);
        }

        left_speed = -map(left_speed, 0, 255, 0, max_speed_limit);
        right_speed = -map(right_speed, 0, 255, 0, max_speed_limit);
        motors.move(left_speed, right_speed);
        motors.setLed(LOW, LOW);
    } 
    
    // 3. Spin Motion (Y == 125)
    else if (Y_raw == 125) {
        if (X_raw <= DEAD_CENTER_X_LOW) { // Spin Left
            int speed = map(X_raw, 0, DEAD_CENTER_X_LOW, 255, 0);
            speed = map(speed, 0, 255, 0, max_speed_limit);
            motors.move(-speed, speed); 
            motors.setLed(HIGH, LOW);
        } else if (X_raw >= DEAD_CENTER_X_HIGH) { // Spin Right
            int speed = map(X_raw, DEAD_CENTER_X_HIGH, 250, 0, 255);
            speed = map(speed, 0, 255, 0, max_speed_limit);
            motors.move(speed, -speed); 
            motors.setLed(LOW, HIGH);
        } else {
             motors.stop();
             motors.setLed(LOW, LOW);
        }
    } else { motors.stop(); }
}