print 'hello world!'-- Script d'évitement d'obstacle sécurisé pour ArduRover (Chenilles + TFmini)

local SEUIL_OBSTACLE_M = 1.0  -- Distance de détection en mètres
local TEMPS_RECUL_MS = 2000    -- Temps de recul (2 secondes)
local TEMPS_PIVOT_MS = 2000    -- Temps de pivot (2 secondes)

-- Constantes d'orientation et de modes d'ArduPilot
local ROTATION_FORWARD = 0     -- 0 correspond à l'orientation Face Avant
local MODE_MANUAL = 0
local MODE_AUTO = 10

-- Constantes des fonctions Servo d'ArduPilot (Indépendant du numéro de pin physique)
local PIVOT_STEERING = 26      -- Fonction 26 : GroundSteering (Direction)
local PIVOT_THROTTLE = 70      -- Fonction 70 : Throttle (Moteur/Gaz)

-- Variables d'état du script
local etat_evitement = 0
local minuterie = 0
local direction_pivot = 0

function update()
    -- 1. Vérification et lecture sécurisée du LiDAR pointé vers l'avant
    if not rangefinder:has_data_orient(ROTATION_FORWARD) then
        return update, 100 -- Attend 100ms si le LiDAR n'a pas encore de données valides
    end
    
    local distance = rangefinder:distance_orient(ROTATION_FORWARD)

    -- 2. Lecture du mode actuel du Rover
    local mode_actuel = vehicle:get_mode()

    -- ÉTAT 0 : Surveillance en mode AUTO
    if etat_evitement == 0 then
        if mode_actuel == MODE_AUTO and distance < SEUIL_OBSTACLE_M then
            gcs:send_text(4, string.format("Obstacle à %.2fm ! Recul Manuel.", distance))
            vehicle:set_mode(MODE_MANUAL)
            etat_evitement = 1
            minuterie = millis()
        end

    -- ÉTAT 1 : Recul en ligne droite
    elseif etat_evitement == 1 then
        if (millis() - minuterie) < TEMPS_RECUL_MS then
            -- On envoie des valeurs scalées entre -1 et 1 (ArduPilot gère la conversion PWM)
            SRV_Channels:set_output_scaled(PIVOT_THROTTLE, -0.4) -- Recule à 40% de puissance
            SRV_Channels:set_output_scaled(PIVOT_STEERING, 0.0)  -- Direction droite / Neutre
        else
            -- Choix aléatoire : 0 = Gauche, 1 = Droite
            direction_pivot = math.random(0, 1)
            if direction_pivot == 0 then
                gcs:send_text(6, "Pivotement à gauche...")
            else
                gcs:send_text(6, "Pivotement à droite...")
            end
            etat_evitement = 2
            minuterie = millis()
        end

    -- ÉTAT 2 : Pivot sur place (Le mixage Skid-Steering s'occupe des chenilles)
    elseif etat_evitement == 2 then
        if (millis() - minuterie) < TEMPS_PIVOT_MS then
            if direction_pivot == 0 then
                SRV_Channels:set_output_scaled(PIVOT_STEERING, -0.8) -- Tourne à Gauche
            else
                SRV_Channels:set_output_scaled(PIVOT_STEERING, 0.8)  -- Tourne à Droite
            end
            SRV_Channels:set_output_scaled(PIVOT_THROTTLE, 0.0) -- Pas de gaz vers l'avant
        else
            gcs:send_text(4, "Zone dégagée. Reprise de la mission AUTO.")
            vehicle:set_mode(MODE_AUTO)
            etat_evitement = 0 -- Réinitialisation
        end
    end

    return update, 50 -- Boucle toutes les 50ms
end

gcs:send_text(6, "Script d'évitement chargé.")
return update()