import time
import threading
import cv2
import numpy as np
import statistics
import os
#  PIN CONFIGURATION  —  change these to match your wiring
#  All numbers are BCM (GPIO) numbers, NOT physical pin numbers

# RGB LED  (3 separate pins, one per colour)
# If your LED is common-anode, change active_high=False in test_rgb_led()
RGB_RED_PIN   = 17
RGB_GREEN_PIN = 27
RGB_BLUE_PIN  = 22


# HC-SR04 ultrasonic distance sensor
TRIGGER_PIN = 23
ECHO_PIN    = 24

# L298N motor driver  (connect ENA to 5V or leave the jumper on for full speed)
MOTOR_IN1 = 8   # forward control pin
MOTOR_IN2 = 7   # backward control pin

# USB camera  (0 = first detected camera; try 1, 2 or 8 if this fails)
CAMERA_INDEX = 0

#  gpiozero setup — Pi 5 needs the lgpio pin factory

try:
    from gpiozero.pins.lgpio import LGPIOFactory
    from gpiozero import Device, RGBLED, DistanceSensor, Motor, Buzzer, Button
    Device.pin_factory = LGPIOFactory()
    GPIO_AVAILABLE = True
except Exception:
    GPIO_AVAILABLE = False

_led = None        # owned exclusively by automation.py — never claimed here
_led_strips = None # owned exclusively by automation.py — never claimed here


#  TEST FUNCTIONS

def set_rgb(color):
    if _led is None:
        return
    colors = {"red": (1, 0, 0), "green": (0, 1, 0), "yellow": (1, 1, 0), "blue": (0, 0, 1)}
    _led.color = colors.get(color, (0, 0, 0))


def test_rgb_led():
    print("\n--- RGB LED Test ---")
    print("Watch the LED: RED (1 s) → GREEN (1 s) → BLUE (1 s)")
    for colour in ["red", "green", "blue"]:
        print(f"  {colour.capitalize()}...")
        set_rgb(colour)
        time.sleep(1)
    set_rgb("off")
    print("RGB LED test done!")



def test_distance_sensor():
    print("\n Robust Distance Sensor Test")
    print("Hold an object in front of the sensor. Reading for 10 seconds")

    sensor = DistanceSensor(echo=ECHO_PIN, trigger=TRIGGER_PIN, max_distance=2)

    start = time.time()
    while time.time() - start < 10:
        valid_readings = []
        
        # Try up to 10 times quickly to get 3 valid readings
        attempts = 0
        while len(valid_readings) < 3 and attempts < 10:
            raw_distance = sensor.distance * 100
            
            # Ignore timeouts (200.0) and blind-spot noise (< 2.0cm)
            if 2.0 < raw_distance < 190.0:
                valid_readings.append(raw_distance)
            
            attempts += 1
            time.sleep(0.01) # Ultra-short pause
        
        # If we got at least 1 valid reading after filtering, output the median
        if len(valid_readings) >= 1:
            reliable_distance = statistics.median(valid_readings)
            print(f"  Crate detected at: {reliable_distance:.1f} cm")
        else:
            print("  Tunnel is empty (or signal lost)")
            
        time.sleep(0.5) # Wait half a second before the next main check

    sensor.close()
    print("Distance sensor test done!")


def test_motor():
    print("\n--- Motor Test ---")
    print("Motor will run forward for 2 seconds, then stop.")
    print("Make sure the L298N ENA jumper is on (or ENA is wired to 5V).")

    motor = Motor(forward=MOTOR_IN1, backward=MOTOR_IN2)

    print("  Motor ON (forward)...")
    motor.forward()
    time.sleep(10)

    motor.stop()
    motor.close()
    print("Motor test done!")


def test_camera():
    print("\n--- Camera Test ---")
    # Open de camera (index 0 is meestal de USB camera)
    cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_EXPOSURE, -1) # Waarden variëren per camera, probeer -1 tot -10
    if not cap.isOpened():
        print("Error: Could not open camera. Check USB connection.")
        return

    ret, frame = cap.read()
    
    if ret:
        cv2.imwrite('test_image.jpg', frame)
        print("Camera check successful! Image saved as 'test_image.jpg'.")
        print("Check the file to see if the view is correct.")
    else:
        print("Error: Could not capture frame.")
        
    cap.release()


def test_lcd_screen():
    print("\n--- LCD / HDMI Screen Test ---")
    height, width = 720, 1280
    image = np.zeros((height, width, 3), dtype=np.uint8)
    image[:] = (80, 60, 20)          # dark teal background (BGR)
    text       = "LCD Screen Test Successful"
    font       = cv2.FONT_HERSHEY_SIMPLEX
    font_scale = 2.0
    thickness  = 3
    (text_w, text_h), _ = cv2.getTextSize(text, font, font_scale, thickness)
    cv2.putText(image, text, ((width - text_w) // 2, (height + text_h) // 2),
                font, font_scale, (255, 255, 255), thickness)
    show_results_on_lcd(image)       # routes through persistent thread — no blocking
    print("LCD screen test done!")

def test_buzzer():
    print("\n--- Buzzer Test ---")
    
    # active_high=False vertelt de Pi dat deze buzzer 'omgekeerd' werkt!
    buzzer = Buzzer(16, active_high=False)
    
    try:
        # Zodra de code hier is, moet de buzzer direct stil zijn
        print("Systeem opgestart: Buzzer is stil.")
        time.sleep(1)
        
        print("Korte piep (Test: Succesvolle scan)...")
        buzzer.on()
        time.sleep(0.1)
        buzzer.off()
        
        time.sleep(1)
        
        print("Lange piep (Test: Foutmelding/Alarm)...")
        buzzer.on()
        time.sleep(1)
        buzzer.off()
        
        print("Test succesvol voltooid!")
        
    except KeyboardInterrupt:
        print("Test afgebroken door gebruiker.")
    finally:
        # Laat hem expliciet uit staan
        buzzer.off()
        # Bij Active Low buzzers laten we close() soms weg om te voorkomen
        # dat hij weer begint te gillen als het script stopt.


def beep_structural():
    if not GPIO_AVAILABLE:
        return
    buzzer = Buzzer(16, active_high=False)
    try:
        for i in range(3):
            buzzer.on()
            time.sleep(1.0)
            buzzer.off()
            if i < 2:
                time.sleep(0.5)
    finally:
        buzzer.off()


_lcd_frame             = None          # the BGR numpy array currently on screen
_lcd_lock              = threading.Lock()
_lcd_thread            = None          # the persistent display thread
_lcd_refresh_callback  = None          # set by app.py via set_lcd_refresh_callback()
_factory_mode_callback = None          # set by app.py via set_factory_mode_callback()


def set_lcd_refresh_callback(fn):
    """Register the function app.py wants called when the LCD Refresh button is tapped."""
    global _lcd_refresh_callback
    _lcd_refresh_callback = fn


def set_factory_mode_callback(fn):
    """Register the function app.py wants called when the Auto/Manual toggle is tapped."""
    global _factory_mode_callback
    _factory_mode_callback = fn


def _on_lcd_click(event, x, y, flags, param):
    """OpenCV mouse/touch callback — fires on every tap of the LCD screen."""
    if event != cv2.EVENT_LBUTTONDOWN:
        return
    # Use normalised (0-1) coordinates so this works on any display resolution.
    # getWindowImageRect returns (img_x, img_y, img_w, img_h) in window pixels.
    try:
        rect = cv2.getWindowImageRect("Crate-Scan Pro")
        ox, oy, w, h = rect[0], rect[1], rect[2], rect[3]
    except Exception:
        ox, oy, w, h = 0, 0, 1280, 720
    if w <= 0 or h <= 0:
        ox, oy, w, h = 0, 0, 1280, 720
    rel_x = (x - ox) / w
    rel_y = (y - oy) / h

    # Auto/Manual toggle — top-left (matches fig.text(0.02, 0.95) in generate_dashboard_image)
    if rel_x < 0.22 and rel_y < 0.14 and _factory_mode_callback is not None:
        threading.Thread(target=_factory_mode_callback, daemon=True).start()
        return

    # Refresh button — bottom-left (matches fig.text(0.02, 0.03))
    if rel_x < 0.20 and rel_y > 0.85 and _lcd_refresh_callback is not None:
        threading.Thread(target=_lcd_refresh_callback, daemon=True).start()


def _lcd_loop():
    os.environ.setdefault('DISPLAY', ':0')
    cv2.namedWindow("Crate-Scan Pro", cv2.WINDOW_NORMAL)
    cv2.setWindowProperty("Crate-Scan Pro", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
    cv2.setMouseCallback("Crate-Scan Pro", _on_lcd_click)
    while True:
        with _lcd_lock:
            frame = _lcd_frame
        if frame is not None:
            cv2.imshow("Crate-Scan Pro", frame)
        cv2.waitKey(33)     # ~30 fps event pump — keeps the window responsive


def show_results_on_lcd(image):
    global _lcd_thread, _lcd_frame
    try:
        if isinstance(image, np.ndarray):
            frame = image                                           # already BGR numpy
        else:
            frame = cv2.cvtColor(np.array(image), cv2.COLOR_RGB2BGR)  # PIL image
        with _lcd_lock:
            _lcd_frame = cv2.resize(frame, (1280, 720))
        # Start the display thread once; it keeps running forever (daemon)
        if _lcd_thread is None or not _lcd_thread.is_alive():
            _lcd_thread = threading.Thread(target=_lcd_loop, daemon=True)
            _lcd_thread.start()
    except Exception as e:
        print(f"LCD display error: {e}")


def get_single_distance():
    sensor = DistanceSensor(echo=ECHO_PIN, trigger=TRIGGER_PIN, max_distance=2)
    readings = []
    for _ in range(10):
        raw = sensor.distance * 100
        if 2.0 < raw < 190.0:
            readings.append(raw)
        time.sleep(0.01)
    sensor.close()
    return statistics.median(readings) if readings else None

def test_button():
    print("\n--- Button Test ---")
    button = Button(5)
    try:
        while True:
            button.wait_for_press()
            print("Pressed")
            
            button.wait_for_release()
            print("Released")
            
    except KeyboardInterrupt:
        print("\nstopped")
    finally:
        button.close()

def test_led_strips():
    print("      HARDWARE TEST: 12V LED STRIPS      ")

    try:
        print("ON")
        led_strips_on()
        time.sleep(10)

        print("OUT")

    except KeyboardInterrupt:
        print("\nstopped")
    finally:
        led_strips_off()

def led_strips_on():
    if _led_strips is None:
        return
    _led_strips.backward(speed=1.0)


def led_strips_off():
    if _led_strips is None:
        return
    _led_strips.stop()


def test_camera_with_leds():

    print("\n" + "="*40)
    print("   HARDWARE TEST: CAMERA + LED LIGHTING   ")
    print("="*40)
    
    try:
        print("💡 LEDs AAN (Productielicht starten)...")
        led_strips_on()

        time.sleep(1)
        
        print("📸 Camera initialiseren...")
        cap = cv2.VideoCapture(0)
        
        cap.set(cv2.CAP_PROP_EXPOSURE, -1) 
        
        if not cap.isOpened():
            print("no camera")
            return
        for _ in range(5):
            cap.read()
            time.sleep(0.1)

        ret, frame = cap.read()
        
        if ret:
            cv2.imwrite('test_image_lit.jpg', frame)
            print("succes")
        else:
            print("error")
            
        cap.release()

    except KeyboardInterrupt:
        print("\nstop")
    finally:
        led_strips_off()
# ──────────────────────────────────────────────────────────────
#  MAIN MENU
# ──────────────────────────────────────────────────────────────

if __name__ == "__main__":
    print("╔════════════════════════════════════════════╗")
    print("║  Crate-Scan Pro  —  Hardware Component Tester  ║")
    print("╚════════════════════════════════════════════╝")
    print()
    print("  1  →  RGB LED")
    print("  2  →  Distance Sensor (HC-SR04)")
    print("  3  →  Motor (L298N)")
    print("  4  →  USB Camera")
    print("  5  →  LCD / HDMI Screen")
    print("  6  →  Test Buzzer")
    print("  7  →  Test Button")
    print("  8  →  Test Leds")
    print("  9  →  Test Camera + Leds")
    print("  r  →  RGB LED (Red only)")
    print("  0  →  Quit")
    print()

    MENU = {
        "1": test_rgb_led,
        "2": test_distance_sensor,
        "3": test_motor,
        "4": test_camera,
        "5": test_lcd_screen,
        "6": test_buzzer,
        "7": test_button,
        "8": test_led_strips,
        "9": test_camera_with_leds,
        "r": test_rgb_led_red,
    }

    while True:
        choice = input("Enter choice: ").strip()

        if choice == "0":
            print("Goodbye!")
            break
        elif choice in MENU:
            MENU[choice]()
        else:
            print("  Invalid choice. Enter 1–5 or 0 to quit.")

        print()