#!/usr/bin/env python3
"""
ugv_hardware.py
Hardware abstraction layer for UGV Chassis.
Supports dry-run mocking for validation on standard PCs.
"""

import logging

class MockBumper:
    def __init__(self):
        self._is_pressed = False
        logging.info("[MOCK] Bumper initialized.")

    @property
    def is_pressed(self):
        return self._is_pressed

    def simulate_press(self, state: bool):
        self._is_pressed = state

class RealBumper:
    def __init__(self, pin: int):
        from gpiozero import Button
        # Internal pull-up: unpressed = True (High), pressed = False (Low)
        self.button = Button(pin, pull_up=True)
        logging.info(f"[HARDWARE] Bumper initialized on GPIO {pin}.")

    @property
    def is_pressed(self):
        return self.button.is_active

class MockMotorController:
    def __init__(self):
        self.left_speed = 0.0
        self.right_speed = 0.0
        logging.info("[MOCK] Motor controller initialized.")

    def set_motors(self, left_speed: float, right_speed: float):
        self.left_speed = max(-1.0, min(1.0, left_speed))
        self.right_speed = max(-1.0, min(1.0, right_speed))
        logging.debug(f"[MOCK] Motors set -> Left: {self.left_speed:.2f}, Right: {self.right_speed:.2f}")

class RealMotorController:
    def __init__(self, i2c_bus=1, pca_addr=0x40):
        import smbus2
        from gpiozero import DigitalOutputDevice

        self.bus = smbus2.SMBus(i2c_bus)
        self.pca_addr = pca_addr

        # Initialize PCA9685
        self.bus.write_byte_data(self.pca_addr, 0x00, 0x10) # Sleep
        self.bus.write_byte_data(self.pca_addr, 0xFE, 0x79) # Set prescaler for ~50Hz
        self.bus.write_byte_data(self.pca_addr, 0x00, 0x20) # Auto-increment

        # Initialize TB6612FNG Direction Pins
        self.ain1 = DigitalOutputDevice(22)
        self.ain2 = DigitalOutputDevice(23)
        self.bin1 = DigitalOutputDevice(24)
        self.bin2 = DigitalOutputDevice(25)

        logging.info("[HARDWARE] Motor controller initialized via PCA9685 and GPIO.")

    def _set_pwm(self, channel: int, duty_cycle: float):
        # Duty cycle from 0.0 to 1.0 mapped to 0-4095
        val = int(duty_cycle * 4095)
        self.bus.write_byte_data(self.pca_addr, 0x06 + 4*channel, 0)
        self.bus.write_byte_data(self.pca_addr, 0x07 + 4*channel, 0)
        self.bus.write_byte_data(self.pca_addr, 0x08 + 4*channel, val & 0xFF)
        self.bus.write_byte_data(self.pca_addr, 0x09 + 4*channel, val >> 8)

    def set_motors(self, left_speed: float, right_speed: float):
        # Constrain speeds
        left_speed = max(-1.0, min(1.0, left_speed))
        right_speed = max(-1.0, min(1.0, right_speed))

        # Left Motor (Motor A)
        if left_speed >= 0:
            self.ain1.on()
            self.ain2.off()
            self._set_pwm(0, left_speed)
        else:
            self.ain1.off()
            self.ain2.on()
            self._set_pwm(0, -left_speed)

        # Right Motor (Motor B)
        if right_speed >= 0:
            self.bin1.on()
            self.bin2.off()
            self._set_pwm(1, right_speed)
        else:
            self.bin1.off()
            self.bin2.on()
            self._set_pwm(1, -right_speed)

class UGVChassis:
    def __init__(self, dry_run: bool):
        self.dry_run = dry_run
        if dry_run:
            self.bumper = MockBumper()
            self.motors = MockMotorController()
        else:
            self.bumper = RealBumper(pin=17)
            self.motors = RealMotorController()

    def halt(self):
        self.motors.set_motors(0.0, 0.0)

#!/usr/bin/env python3
"""
ugv_teleop.py
Main teleoperation node with bumper safety override.
"""

import argparse
import logging
import time
import sys
import select
import termios
import tty
from ugv_hardware import UGVChassis

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')

class TeleopController:
    def __init__(self, chassis: UGVChassis):
        self.chassis = chassis
        self.current_cmd = 'x'
        self.running = True

    def process_command(self, cmd: str):
        speed_forward = 0.8
        speed_turn = 0.5

        left_cmd = 0.0
        right_cmd = 0.0

        if cmd == 'w':
            left_cmd, right_cmd = speed_forward, speed_forward
        elif cmd == 's':
            left_cmd, right_cmd = -speed_forward, -speed_forward
        elif cmd == 'a':
            left_cmd, right_cmd = -speed_turn, speed_turn
        elif cmd == 'd':
            left_cmd, right_cmd = speed_turn, -speed_turn
        elif cmd == 'x':
            left_cmd, right_cmd = 0.0, 0.0
        elif cmd == 'q':
            self.running = False
            return

        # --- SAFETY OVERRIDE LOGIC ---
        # If bumper is pressed, prevent ANY forward motion commands
        if self.chassis.bumper.is_pressed:
            if left_cmd > 0: left_cmd = 0.0
            if right_cmd > 0: right_cmd = 0.0
            logging.warning("BUMPER PRESSED! Forward motion disabled.")

        self.chassis.motors.set_motors(left_cmd, right_cmd)

def get_key_non_blocking():
    """Reads a single character from stdin without blocking."""
    if select.select([sys.stdin], [], [], 0.0)[0]:
        return sys.stdin.read(1)
    return None

def run_self_test(controller: TeleopController):
    """Automated dry-run test sequence proving the safety logic."""
    logging.info("Starting automated self-test sequence...")

    # Test 1: Forward motion logic
    controller.process_command('w')
    assert controller.chassis.motors.left_speed == 0.8, "Left motor failed forward command."
    assert controller.chassis.motors.right_speed == 0.8, "Right motor failed forward command."
    logging.info("Test 1 Passed: Forward command executed correctly.")

    # Test 2: Bumper override
    controller.chassis.bumper.simulate_press(True)
    controller.process_command('w')
    assert controller.chassis.motors.left_speed == 0.0, "Safety override failed on left motor!"
    assert controller.chassis.motors.right_speed == 0.0, "Safety override failed on right motor!"
    logging.info("Test 2 Passed: Bumper successfully halted forward motion.")

    # Test 3: Reverse motion while bumper pressed
    controller.process_command('s')
    assert controller.chassis.motors.left_speed == -0.8, "Reverse failed during bumper press."
    assert controller.chassis.motors.right_speed == -0.8, "Reverse failed during bumper press."
    logging.info("Test 3 Passed: Reverse escape maneuvers remain active.")

    logging.info("All self-tests passed successfully.")

def main():
    parser = argparse.ArgumentParser(description="UGV Teleop Node")
    parser.add_argument('--dry-run', action='store_true', help="Run without hardware")
    parser.add_argument('--self-test', action='store_true', help="Run automated safety validation")
    args = parser.parse_args()

    chassis = UGVChassis(dry_run=args.dry_run or args.self_test)
    controller = TeleopController(chassis)

    if args.self_test:
        run_self_test(controller)
        return

    print("UGV Teleop Started. Keys: W (fwd), S (rev), A (left), D (right), X (stop), Q (quit)")
    old_settings = termios.tcgetattr(sys.stdin)
    try:
        tty.setcbreak(sys.stdin.fileno())
        while controller.running:
            cmd = get_key_non_blocking()
            if cmd:
                controller.current_cmd = cmd.lower()

            controller.process_command(controller.current_cmd)
            time.sleep(0.05) # 20Hz loop
    finally:
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
        chassis.halt()
        print("\nUGV Teleop Stopped.")

if __name__ == "__main__":
    main()

"""
ugv_hardware.py
Hardware Abstraction Layer for 2WD UGV with TB6612FNG and TCRT5000.
Includes mock classes for off-hardware dry-run validation.
"""

import time

try:
    from gpiozero import PWMOutputDevice, DigitalOutputDevice, DigitalInputDevice
    GPIO_AVAILABLE = True
except ImportError:
    GPIO_AVAILABLE = False

class MockMotor:
    """Mock motor class for dry-run validation."""
    def __init__(self, name):
        self.name = name
        self.current_speed = 0.0

    def drive(self, speed):
        # Constrain speed between -1.0 and 1.0
        self.current_speed = max(min(speed, 1.0), -1.0)
        direction = "FORWARD" if self.current_speed > 0 else "REVERSE" if self.current_speed < 0 else "STOPPED"
        print(f"[MOCK] {self.name} Motor -> Speed: {abs(self.current_speed):.2f} | Dir: {direction}")

class MockSensorArray:
    """Mock sensor array that cycles through predefined track states."""
    def __init__(self):
        # (Left, Center, Right) - 1 means line detected, 0 means no line
        self.test_sequence = [
            (0, 1, 0),  # Centered
            (1, 1, 0),  # Drifting slightly right (left sensor hits line)
            (1, 0, 0),  # Drifting hard right
            (0, 1, 0),  # Centered again
            (0, 0, 1),  # Drifting hard left
            (0, 0, 0),  # Line lost
        ]
        self.step = 0

    def read_sensors(self):
        state = self.test_sequence[self.step % len(self.test_sequence)]
        self.step += 1
        print(f"[MOCK] Sensors read (L, C, R): {state}")
        return state

class RealMotor:
    """Real implementation for TB6612FNG motor control using gpiozero."""
    def __init__(self, pwm_pin, in1_pin, in2_pin):
        self.pwm = PWMOutputDevice(pwm_pin)
        self.in1 = DigitalOutputDevice(in1_pin)
        self.in2 = DigitalOutputDevice(in2_pin)

    def drive(self, speed):
        speed = max(min(speed, 1.0), -1.0)
        if speed > 0:
            self.in1.on()
            self.in2.off()
            self.pwm.value = speed
        elif speed < 0:
            self.in1.off()
            self.in2.on()
            self.pwm.value = abs(speed)
        else:
            self.in1.off()
            self.in2.off()
            self.pwm.value = 0.0

class RealSensorArray:
    """Real implementation for TCRT5000 array using gpiozero."""
    def __init__(self, left_pin, center_pin, right_pin):
        self.left = DigitalInputDevice(left_pin)
        self.center = DigitalInputDevice(center_pin)
        self.right = DigitalInputDevice(right_pin)

    def read_sensors(self):
        # Assuming sensor outputs 1 when dark line is detected
        return (self.left.value, self.center.value, self.right.value)

def get_hardware(dry_run=False):
    """Factory function to return hardware interfaces based on execution mode."""
    if dry_run or not GPIO_AVAILABLE:
        if not dry_run:
            print("Warning: gpiozero not found. Defaulting to dry-run mode.")
        left_motor = MockMotor("Left")
        right_motor = MockMotor("Right")
        sensors = MockSensorArray()
        return left_motor, right_motor, sensors

    # Hardware pin mapping based on setup table
    left_motor = RealMotor(pwm_pin=17, in1_pin=27, in2_pin=22)
    right_motor = RealMotor(pwm_pin=18, in1_pin=23, in2_pin=24)
    sensors = RealSensorArray(left_pin=5, center_pin=6, right_pin=13)

    return left_motor, right_motor, sensors

"""
ugv_line_follower.py
Main control loop for the UGV line following prototype.
Reads sensor states and applies differential steering.
"""

import argparse
import time
import sys
from ugv_hardware import get_hardware

# Configuration parameters
BASE_SPEED = 0.5    # Normal forward speed (0.0 to 1.0)
TURN_SPEED = 0.6    # Speed of the outer wheel during a turn
REDUCE_SPEED = 0.2  # Speed of the inner wheel during a turn
LOOP_DELAY = 0.1    # Delay between control loops in seconds

def follow_line(left_motor, right_motor, sensors, max_iterations=None):
    """
    Main control loop. Evaluates sensor states to determine motor speeds.
    """
    print("--- Starting UGV Line Follower ---")
    print("Press Ctrl+C to stop.")

    iterations = 0
    try:
        while True:
            if max_iterations and iterations >= max_iterations:
                print("Max iterations reached. Stopping.")
                break

            left_val, center_val, right_val = sensors.read_sensors()

            # State 1: Centered on the line
            if center_val == 1 and left_val == 0 and right_val == 0:
                left_motor.drive(BASE_SPEED)
                right_motor.drive(BASE_SPEED)

            # State 2: Drifting Right (Left sensor sees line)
            elif left_val == 1 and right_val == 0:
                # Turn Left: Right motor fast, Left motor slow
                left_motor.drive(REDUCE_SPEED)
                right_motor.drive(TURN_SPEED)

            # State 3: Drifting Left (Right sensor sees line)
            elif right_val == 1 and left_val == 0:
                # Turn Right: Left motor fast, Right motor slow
                left_motor.drive(TURN_SPEED)
                right_motor.drive(REDUCE_SPEED)

            # State 4: Intersection or perpendicular line (All sensors see line)
            elif left_val == 1 and center_val == 1 and right_val == 1:
                print("Intersection detected. Stopping for safety.")
                left_motor.drive(0.0)
                right_motor.drive(0.0)
                break

            # State 5: Line completely lost (No sensors see line)
            elif left_val == 0 and center_val == 0 and right_val == 0:
                print("Line lost. Halting.")
                left_motor.drive(0.0)
                right_motor.drive(0.0)
                break

            else:
                # Catch-all for ambiguous states (e.g., 1, 0, 1) - maintain base speed cautiously
                left_motor.drive(BASE_SPEED * 0.5)
                right_motor.drive(BASE_SPEED * 0.5)

            iterations += 1
            time.sleep(LOOP_DELAY)

    except KeyboardInterrupt:
        print("\nManual interrupt received.")
    finally:
        # Failsafe: Ensure motors are stopped on exit
        print("Shutting down motors.")
        left_motor.drive(0.0)
        right_motor.drive(0.0)

def main():
    parser = argparse.ArgumentParser(description="UGV Line Follower Control")
    parser.add_argument("--dry-run", action="store_true", help="Run without hardware using mock interfaces")
    parser.add_argument("--self-test", action="store_true", help="Run a short automated test sequence and exit")
    args = parser.parse_args()

    # Initialize hardware or mocks
    left_motor, right_motor, sensors = get_hardware(dry_run=args.dry_run)

    # Determine execution length
    max_iters = 6 if args.self-test else None

    # Execute control loop
    follow_line(left_motor, right_motor, sensors, max_iterations=max_iters)

if __name__ == "__main__":
    main()

#include <Arduino.h>

// --------------------------------------------------------
// Pin Definitions
// --------------------------------------------------------
const int TOUCH_PAD_1 = 4;  // GPIO 4  (Touch 0)
const int TOUCH_PAD_2 = 2;  // GPIO 2  (Touch 2)
const int TOUCH_PAD_3 = 15; // GPIO 15 (Touch 3)
const int LED_PIN     = 21; // Status LED
const int BUZZER_PIN  = 22; // Piezo Buzzer

// --------------------------------------------------------
// System Configuration & Thresholds
// --------------------------------------------------------
// A typical untouched ESP32 pin reads ~50-80. 
// A touched pin drops below 20. Adjust this if your pads differ.
const int TOUCH_THRESHOLD = 30; 

// Access Control Sequence Configuration
const int SEQUENCE_LENGTH = 4;
const int SECRET_PIN[SEQUENCE_LENGTH] = {1, 2, 3, 2}; // The correct access code
int inputSequence[SEQUENCE_LENGTH];
int inputIndex = 0;

// State Machine Variables
enum SystemState { LOCKED, INPUTTING, UNLOCKED };
SystemState currentState = LOCKED;

unsigned long unlockTimestamp = 0;
const unsigned long UNLOCK_DURATION = 5000; // Keep unlocked for 5 seconds

// Debouncing Variables
bool pad1_wasTouched = false;
bool pad2_wasTouched = false;
bool pad3_wasTouched = false;

// --------------------------------------------------------
// Function Prototypes
// --------------------------------------------------------
void processTouch();
void handleKeyPress(int keyNumber);
void evaluateSequence();
void triggerSuccess();
void triggerFailure();
void lockSystem();
void playTone(int frequency, int duration);

// --------------------------------------------------------
// Setup
// --------------------------------------------------------
void setup() {
    Serial.begin(115200);
    while (!Serial) { delay(10); } // Wait for serial connection

    Serial.println("\n--- Capacitive Touch Access Panel Initialized ---");

    pinMode(LED_PIN, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);

    lockSystem(); // Ensure system starts in locked state
}

// --------------------------------------------------------
// Main Loop
// --------------------------------------------------------
void loop() {
    // Handle state timeouts (Auto-lock)
    if (currentState == UNLOCKED) {
        if (millis() - unlockTimestamp >= UNLOCK_DURATION) {
            Serial.println("Auto-locking system due to timeout.");
            lockSystem();
        }
    } else {
        // Only process touch inputs if the system is not currently unlocked
        processTouch();
    }

    // Small delay to yield to the underlying RTOS
    delay(10); 
}

// --------------------------------------------------------
// Touch Processing & Debouncing
// --------------------------------------------------------
void processTouch() {
    // Read raw capacitance values
    int val1 = touchRead(TOUCH_PAD_1);
    int val2 = touchRead(TOUCH_PAD_2);
    int val3 = touchRead(TOUCH_PAD_3);

    // Evaluate Pad 1
    bool pad1_isTouched = (val1 < TOUCH_THRESHOLD);
    if (pad1_isTouched && !pad1_wasTouched) {
        handleKeyPress(1);
    }
    pad1_wasTouched = pad1_isTouched;

    // Evaluate Pad 2
    bool pad2_isTouched = (val2 < TOUCH_THRESHOLD);
    if (pad2_isTouched && !pad2_wasTouched) {
        handleKeyPress(2);
    }
    pad2_wasTouched = pad2_isTouched;

    // Evaluate Pad 3
    bool pad3_isTouched = (val3 < TOUCH_THRESHOLD);
    if (pad3_isTouched && !pad3_wasTouched) {
        handleKeyPress(3);
    }
    pad3_wasTouched = pad3_isTouched;
}

// --------------------------------------------------------
// Logic Handling
// --------------------------------------------------------
void handleKeyPress(int keyNumber) {
    // Provide immediate acoustic feedback
    playTone(1000, 100); 

    Serial.print("Key Pressed: ");
    Serial.println(keyNumber);

    // Update state
    currentState = INPUTTING;

    // Store the input
    inputSequence[inputIndex] = keyNumber;
    inputIndex++;

    // Check if we have collected enough inputs
    if (inputIndex >= SEQUENCE_LENGTH) {
        evaluateSequence();
    }
}

void evaluateSequence() {
    Serial.println("Evaluating entered sequence...");
    bool isMatch = true;

    for (int i = 0; i < SEQUENCE_LENGTH; i++) {
        if (inputSequence[i] != SECRET_PIN[i]) {
            isMatch = false;
            break;
        }
    }

    if (isMatch) {
        triggerSuccess();
    } else {
        triggerFailure();
    }

    // Reset input index for the next attempt
    inputIndex = 0;
}

// --------------------------------------------------------
// Output & Feedback Generators
// --------------------------------------------------------
void triggerSuccess() {
    Serial.println("ACCESS GRANTED.");
    currentState = UNLOCKED;
    unlockTimestamp = millis();

    // Visual indicator: LED ON
    digitalWrite(LED_PIN, HIGH);

    // Acoustic indicator: Success Melody
    playTone(1200, 150);
    delay(50);
    playTone(1500, 150);
    delay(50);
    playTone(2000, 300);
}

void triggerFailure() {
    Serial.println("ACCESS DENIED. Incorrect PIN.");

    // Acoustic indicator: Error Tone
    playTone(300, 400);
    delay(100);
    playTone(300, 400);

    // Return to locked state immediately
    lockSystem();
}

void lockSystem() {
    currentState = LOCKED;
    inputIndex = 0; // Clear any partial inputs
    digitalWrite(LED_PIN, LOW); // LED OFF indicates locked
    Serial.println("System LOCKED. Ready for input.");
}

// Helper function for the buzzer
void playTone(int frequency, int duration) {
    tone(BUZZER_PIN, frequency, duration);
    // The tone function in Arduino is non-blocking, but for this HMI 
    // we want the beep to complete before proceeding in feedback sequences.
    delay(duration); 
}

/**
 * BLE Room Presence Beacon
 * Device: ESP32 DevKitC + pushbutton/contact input + status LED
 * Framework: Arduino via PlatformIO
 */

#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

// Hardware Pin Definitions
#define BUTTON_PIN 4
#define LED_PIN 5

// State Machine Variables
bool isOccupied = false;
int buttonState = HIGH;
int lastReading = HIGH;

// Non-blocking Debounce Variables
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50; // 50 milliseconds

// BLE Global Pointer
BLEAdvertising *pAdvertising;

/**
 * Updates the BLE Advertisement payload based on the current room state.
 * Connectionless BLE requires us to stop advertising, update the payload,
 * and then restart advertising so scanners see the new data immediately.
 */
void updateBLEAdvertisement() {
    if (pAdvertising != nullptr) {
        pAdvertising->stop();
    }

    BLEAdvertisementData oAdvertisementData = BLEAdvertisementData();

    // Set standard BLE flags. 
    // 0x04 = BR_EDR_NOT_SUPPORTED (Indicates this is a BLE-only device)
    oAdvertisementData.setFlags(0x04); 

    // Dynamically change the advertised device name based on state.
    // This allows scanners to know the room status without connecting.
    if (isOccupied) {
        oAdvertisementData.setName("ROOM_INUSE");
    } else {
        oAdvertisementData.setName("ROOM_AVAIL");
    }

    pAdvertising->setAdvertisementData(oAdvertisementData);
    pAdvertising->start();
}

void setup() {
    // Initialize Serial Monitor for debugging
    Serial.begin(115200);
    while (!Serial) {
        ; // Wait for serial port to connect
    }
    Serial.println("Initializing BLE Room Presence Beacon...");

    // Configure GPIO Pins
    pinMode(BUTTON_PIN, INPUT_PULLUP);
    pinMode(LED_PIN, OUTPUT);

    // Set initial hardware state
    digitalWrite(LED_PIN, LOW); // LED OFF = Available

    // Initialize the BLE environment with a default name
    BLEDevice::init("ROOM_AVAIL");
    pAdvertising = BLEDevice::getAdvertising();

    // Apply our custom advertisement data and start broadcasting
    updateBLEAdvertisement();

    Serial.println("Initialization Complete. Broadcasting as ROOM_AVAIL.");
}

void loop() {
    // Read the current physical state of the pushbutton
    int reading = digitalRead(BUTTON_PIN);

    // If the switch changed (due to noise or pressing)
    if (reading != lastReading) {
        lastDebounceTime = millis(); // Reset the debouncing timer
    }

    // Whatever the reading is at, it's been there for longer than the debounce delay,
    // so take it as the actual current state.
    if ((millis() - lastDebounceTime) > debounceDelay) {

        // If the button state has truly changed
        if (reading != buttonState) {
            buttonState = reading;

            // Only toggle the room state when the button is actively PRESSED (transition to LOW)
            if (buttonState == LOW) {
                isOccupied = !isOccupied;

                // Update the physical Status LED
                digitalWrite(LED_PIN, isOccupied ? HIGH : LOW);

                // Update the BLE Advertisement Payload
                updateBLEAdvertisement();

                // Print to Serial Monitor for validation
                Serial.print("State toggled! Room is now: ");
                Serial.println(isOccupied ? "OCCUPIED" : "AVAILABLE");
            }
        }
    }

    // Save the reading. Next time through the loop, it'll be the lastReading.
    lastReading = reading;
}