"""
================================================================================
PROJECT: Autonomous High-Speed Swerving Vehicle
CORE LOGIC: Phased S-Curve Obstacle Avoidance
--------------------------------------------------------------------------------
CODE MAP:
[Lines 18 - 30]  I.  Setup & Configuration (GPIO Mapping & Logic Thresholds)
[Lines 32 - 50]  II. Hardware Initialization (Lidar Sensor & Pin Claiming)
[Lines 52 - 76]  III. Motor Actuation Functions (Direct Hardware Control)
[Lines 78 - 98]  IV.  Recovery & Escape Logic (Emergency Pivot Maneuvers)
[Lines 100 - 173] V.  Main Control Loop (S-Curve State Machine & Perception)
[Lines 175 - 182] VI. Teardown & Safety Cleanup
================================================================================
"""
import os
import time
import numpy as np
import lgpio
import signal
from vl53l5cx_ctypes import VL53L5CX

# Force X11 backend for OpenCV compatibility on Pi 5
os.environ["QT_QPA_PLATFORM"] = "xcb"

# ==============================================================================
# I. SETUP & CONFIGURATION
# ==============================================================================
LPN_PIN = 23               # Lidar Low-Power/Reset pin
IN1, IN2 = 17, 27          # Left Side Motor Direction
IN3, IN4 = 22, 24          # Right Side Motor Direction
ENA, ENB = 18, 19          # Motor Speed PWM Pins

# Navigation Logic Thresholds (measured in mm)
THRESHOLD_CRITICAL = 120   # Immediate panic stop/reverse distance
THRESHOLD_SWERVE   = 400   # Distance to trigger the S-Curve maneuver
THRESHOLD_GROUND   = 130   # Ground clutter/floor noise filter
MAX_DIST           = 1000  # Cap sensor readings to ignore distant objects

# Speed Duty Cycles (0 - 100)
BASE_SPEED  = 100          # Maximum cruising velocity
SWERVE_LOW  = 50           # Inner wheel speed during a swerving arc
BACK_SPEED  = 70           # Power for reversing maneuvers

# ==============================================================================
# II. HARDWARE INITIALIZATION
# ==============================================================================
running = True
h = lgpio.gpiochip_open(0) # Open GPIO chip 0 (standard for Pi 5)

def signal_handler(sig, frame):
    """Safely breaks the loop and stops motors on Ctrl+C."""
    global running
    running = False
signal.signal(signal.SIGINT, signal_handler)

# Claim GPIO pins for output
for pin in [IN1, IN2, IN3, IN4, ENA, ENB, LPN_PIN]:
    lgpio.gpio_claim_output(h, pin)

# Wake up and boot the VL53L5CX Lidar
lgpio.gpio_write(h, LPN_PIN, 1)
time.sleep(0.1)

vl53 = VL53L5CX()
vl53.set_resolution(64)          # Configure for 8x8 multizone matrix
vl53.set_ranging_frequency_hz(15) # Set sampling rate to 15 frames/sec
vl53.start_ranging()

# ==============================================================================
# III. MOTOR ACTUATION FUNCTIONS
# ==============================================================================
def set_motors(l_speed, r_speed, d1, d2, d3, d4):
    """Base function to translate logic into physical motor movement."""
    l_speed = max(0, min(100, int(l_speed)))
    r_speed = max(0, min(100, int(r_speed)))
    
    lgpio.tx_pwm(h, ENA, 1000, l_speed) # Frequency 1000Hz
    lgpio.tx_pwm(h, ENB, 1000, r_speed)
    lgpio.gpio_write(h, IN1, d1)
    lgpio.gpio_write(h, IN2, d2)
    lgpio.gpio_write(h, IN3, d3)
    lgpio.gpio_write(h, IN4, d4)

def drive_forward(): 
    set_motors(BASE_SPEED, BASE_SPEED, 1, 0, 1, 0)
def drive_back():    
    set_motors(BACK_SPEED, BACK_SPEED, 0, 1, 0, 1)
def stop_car():      
    set_motors(0, 0, 0, 0, 0, 0)

# ==============================================================================
# IV. RECOVERY & ESCAPE LOGIC
# ==============================================================================
def pivot_recovery(left_clr, right_clr):
    """Used when the car is trapped or an object is too close to swerve."""
    stop_car()
    time.sleep(0.2)
    drive_back() # Back up to gain clearance
    time.sleep(0.5)
    
    # Decide turn direction based on side-zone clearance
    if left_clr > right_clr:
        set_motors(80, 80, 0, 1, 1, 0) # Pivot Left
    else:
        set_motors(80, 80, 1, 0, 0, 1) # Pivot Right
    time.sleep(0.5)
    stop_car()

# ==============================================================================
# V. MAIN CONTROL LOOP
# ==============================================================================
current_action = "INITIALIZING"
last_forward_time = time.time()

# S-Curve State Variables
maneuver_phase = 0    # 0: Cruise, 1: Swerve Away, 2: Return to Heading
maneuver_end_time = 0
bypass_dir = ""       
phase_duration = 1.7  # Duration of each half of the "S" maneuver

try:
    while running:
        now = time.time()
        
        if vl53.data_ready():
            data = vl53.get_data()
            # Reshape 1D sensor data into 8x8 matrix for spatial analysis
            z = np.array(data.distance_mm).reshape((8, 8))
            z[z <= 0] = MAX_DIST
            z = np.rot90(z, k=1)
            
            # Zone Analysis: Extract specific matrix regions
            min_center = np.min(z[0:5, 2:6])    # Obstacles directly in front
            left_side_avg = np.mean(z[:, 0:3])  # Average clearance on left
            right_side_avg = np.mean(z[:, 5:8]) # Average clearance on right
            ground_near = np.min(z[6:8, 2:6])   # Low obstacles/ground noise
            
            # 1. EMERGENCY STOP (Priority #1)
            if ground_near < THRESHOLD_GROUND or min_center < THRESHOLD_CRITICAL:
                current_action = "EMERGENCY RECOVERY"
                maneuver_phase = 0
                pivot_recovery(left_side_avg, right_side_avg)
                last_forward_time = now
                continue
                
            # 2. S-CURVE EXECUTION (Priority #2)
            if maneuver_phase == 1:
                # Phase 1: Arcing away toward the clear side
                if now < maneuver_end_time:
                    if bypass_dir == "LEFT": set_motors(SWERVE_LOW, BASE_SPEED, 1, 0, 1, 0)
                    else: set_motors(BASE_SPEED, SWERVE_LOW, 1, 0, 1, 0)
                    continue
                else:
                    # Timer expired, transition to Phase 2 (the return arc)
                    maneuver_phase = 2
                    maneuver_end_time = now + phase_duration
                    current_action = f"PHASE 2: RETURN {bypass_dir}"
                    
            if maneuver_phase == 2:
                # Phase 2: Arcing back to original forward heading
                if now < maneuver_end_time:
                    if bypass_dir == "LEFT": set_motors(BASE_SPEED, SWERVE_LOW, 1, 0, 1, 0)
                    else: set_motors(SWERVE_LOW, BASE_SPEED, 1, 0, 1, 0)
                    continue
                else:
                    # Maneuver successfully completed
                    maneuver_phase = 0
                    current_action = "BYPASS COMPLETE"
                    
            # 3. DETECTION & INITIATION (Priority #3)
            if min_center < THRESHOLD_SWERVE and maneuver_phase == 0:
                # Determine bypass direction based on side clearance
                bypass_dir = "LEFT" if left_side_avg > right_side_avg else "RIGHT"
                current_action = f"PHASE 1: SWERVE {bypass_dir}"
                maneuver_phase = 1
                maneuver_end_time = now + phase_duration
                last_forward_time = now
                
            # 4. NORMAL NAVIGATION (Priority #4)
            elif maneuver_phase == 0:
                current_action = "CRUISING"
                drive_forward()
                last_forward_time = now
                
            # 5. STUCK DETECTION
            if (now - last_forward_time) > 4.0:
                current_action = "TRAP ESCAPE"
                maneuver_phase = 0
                pivot_recovery(left_side_avg, right_side_avg)
                last_forward_time = now
                
            # Console Telemetry
            print("\033[H", end="") # Refresh terminal without scrolling
            print(f"--- ACTIVE MODE: {current_action} ---")
            print(f"Center Path: {int(min_center):4}mm | Phase: {maneuver_phase}")
            
        time.sleep(0.01)
        
except Exception as e:
    print(f"\nCRITICAL ERROR: {e}")
    
finally:
    # ==========================================================================
    # VI. TEARDOWN & SAFETY CLEANUP
    # ==========================================================================
    stop_car()
    vl53.stop_ranging()
    lgpio.gpiochip_close(h)
    print("\n[!] System Shutdown Cleanly.")