Smartwave/orchestrateur/main.py

import base64
import json
import time
import asyncio
import requests

from shared.microwave_state import MicrowaveState, MicrowaveStateFields
from orchestrateur.sensors import gps
from shared import get_lora, get_mqtt_client, deviceTypes, config, payloads, db
from shared.logging import log
from shared.cookingState import CookingStates
from shared.lora_device import LoraCommands
from shared.alerts import AlertManager, Alert, AlertType
from sensors import ultrasonicRanger, temp_hum, button, camera, buzzer, rgb_lcd
from lib.systemd_logs import get_systemd_logs
from external_display_manager import DisplayManager
from rgb_lcd_manager import RGBLCDManager

rgb_lcd.setRGB(255, 255, 255, brightness=0.8)
rgb_lcd.setText("SmartWave\nOrchestrateur")

# --- CONFIGURATION CONSTANTS ---
DISPLAY_DEBOUNCE_SECONDS = 3  # Seconds to wait after first change before broadcasting

# --- DB SETUP ---
DB_PATH = "orchestrateur/db.sqlite"

def init_db():
    """Ensures the connected_components table exists on startup."""
    sql = """
    CREATE TABLE IF NOT EXISTS connected_components (
        id TEXT PRIMARY KEY,
        type TEXT,
        timestamp INTEGER
    );
    """
    db.execute(DB_PATH, sql)
    print(f"[DB] Initialized database table at {DB_PATH}")

def save_connected_component(component_id: str, component_type: str):
    """Upserts component information into the database (blocking sync worker)."""
    current_time = int(time.time())
    sql = """
    INSERT INTO connected_components (id, type, timestamp)
    VALUES (?, ?, ?)
    ON CONFLICT(id) DO UPDATE SET
        type = excluded.type,
        timestamp = excluded.timestamp;
    """
    db.execute(DB_PATH, sql, (str(component_id), str(component_type), current_time))
    print(f"[DB] Component saved/updated -> ID: {component_id}, Type: {component_type}, Timestamp: {current_time}")
    
def get_connected_components():
    """Returns a list of all connected components from the database."""
    sql = "SELECT id, type, timestamp FROM connected_components;"
    rows = db.fetchall(DB_PATH, sql)
    return [{"id": row[0], "type": row[1], "timestamp": row[2]} for row in rows]

# --- Read Unique Device ID ---
def get_device_id():
    for path in ["device_id.txt", "/home/pi/SmartWave/orchestrateur/device_id.txt"]:
        try:
            with open(path, "r") as f:
                return f.read().strip()
        except Exception:
            pass
    return "RPI_Orchestrateur_Default"

DEVICE_ID = get_device_id()

# --- DISPLAY QUEUE & NOTIFICATION HELPERS ---
display_update_queue = None

def notify_display_update():
    """Safely adds an update event timestamp to the display update queue."""
    
    if display_update_queue is not None:
        try:
            loop = asyncio.get_running_loop()
            loop.call_soon_threadsafe(display_update_queue.put_nowait, time.time())
        except RuntimeError:
            pass

def on_microwave_change(state: MicrowaveState, field: MicrowaveStateFields):
    if field != MicrowaveStateFields.STATE: # No external screen or lcd uses this field, so we can skip updates for it
        notify_display_update()
    
    if field == MicrowaveStateFields.STATE and state.state in (MicrowaveState.DONE, MicrowaveState.ANALYZING, MicrowaveState.WAITING_FOR_CLOUD):
        # Send to LoRa device if state changed
        lora.send_reliable(payloads.lora_microwave_state(state.state), max_retries=5)
        

def set_cloud_alert(state: bool):
    """Setter for cloud_alert that triggers a display update when changed."""
    global cloud_alert
    
    if cloud_alert != state:
        cloud_alert = state
        notify_display_update()
        update_lcd_cloud_alert()

# Global state trackers
microwave_states = {"2": MicrowaveState("2", on_change_callback=on_microwave_change)}
button_state = False
cloud_alert = None  # Global status flag for screen / UI display
async_event_queue = None

# Display Manager instance
display_manager = DisplayManager(endpoint_path="/api/display")
rgb_lcd_manager = RGBLCDManager(rgb_lcd, brightness=0.8, interval=5.0)

# Async synchronization trackers for MQTT IR sensors responses
ir_data_cache = {}   # mw_id -> dict of IR readings
ir_data_events = {}  # mw_id -> asyncio.Event()

# --- HARDWARE & MQTT SETUP ---
lora = get_lora()
lora.configure()

# Local MQTT Broker (ESP32 IR & Sensors)
mqtt_client = get_mqtt_client(
    host="192.168.50.1",
    client_id="smartwave-orchestrateur-" + DEVICE_ID,
    use_tls=config.USE_TLS,
    cafile="/home/pi/SmartWave/orchestrateur/mqtt/certs/ca.crt",
    keepalive=config.MQTT_KEEPALIVE,
)
mqtt_client.connect()
mqtt_client.subscribe(config.MQTT_TOPIC_SENSOR, qos=config.MQTT_QOS)
mqtt_client.subscribe(config.MQTT_TOPIC_HELLO, qos=config.MQTT_QOS)

# DHT sensor warmup
time.sleep(2)  # Allow GrovePi MCU and DHT sensor circuits to settle post-service start
temp_hum.get_temperature_and_humidity()  # Dummy read to flush initial NaN state

if hasattr(mqtt_client._client, "loop_start"):
    mqtt_client._client.loop_start()
    print("[MQTT Local] Paho background loop started.")

# Cloud MQTT Broker (Remote Commands)
cloud_mqtt_client = get_mqtt_client(
    host="mqtt.matthiasg.dev",
    client_id=DEVICE_ID,
    use_tls=False,
    username="microwave_device",
    password="myMicrowaveVerySecret",
    keepalive=config.MQTT_KEEPALIVE,
)
try:
    cloud_mqtt_client.connect()
    cloud_mqtt_client.subscribe("cmd/all", qos=config.CLOUD_MQTT_QOS)
    cloud_mqtt_client.subscribe(f"cmd/{DEVICE_ID}", qos=config.CLOUD_MQTT_QOS)
except Exception as e:
    print(f"[MQTT Cloud] Error connecting to cloud MQTT broker: {e}")

if hasattr(cloud_mqtt_client._client, "loop_start"):
    cloud_mqtt_client._client.loop_start()
    print("[MQTT Cloud] Paho background loop started.")


# --- BACKGROUND TASKS (PRODUCERS) ---
async def lora_listener_task():
    """Polls LoRa and pushes to the async queue."""
    print("[LoRa] Async listener started.")
    while True:
        # Run blocking lora receive in a thread to not block asyncio loop
        paquet = await asyncio.to_thread(lora.receive_reliable, timeout_ms=100)
        if paquet:
            await async_event_queue.put({"source": "LoRa", "data": paquet})
        await asyncio.sleep(0.05)

async def mqtt_listener_task():
    """Polls local MQTT cache and pushes to the async queue."""
    print("[MQTT Local] Async listener started.")
    while True:
        message = mqtt_client.get_message()
        if message:
            try:
                payload = json.loads(message['payload'])
            except Exception:
                payload = message['payload']
            
            # --- SAFE TOPIC DECODING ---
            topic = message['topic']
            if isinstance(topic, bytes):
                topic = topic.decode('utf-8')

            await async_event_queue.put({
                "source": "MQTT",
                "topic": topic,
                "data": payload
            })
        await asyncio.sleep(0.1)

async def cloud_mqtt_listener_task():
    """Polls Cloud MQTT cache and pushes commands to the async queue."""
    print("[MQTT Cloud] Async listener started.")
    while True:
        message = cloud_mqtt_client.get_message()
        if message:
            try:
                payload = json.loads(message['payload'])
            except Exception:
                payload = message['payload']
            
            topic = message['topic']
            if isinstance(topic, bytes):
                topic = topic.decode('utf-8')

            await async_event_queue.put({
                "source": "CloudMQTT",
                "topic": topic,
                "data": payload
            })
        await asyncio.sleep(0.1)
# === LCD DISPLAY ===
def on_screens_change(screens):
    """Callback for when screens are added or removed."""
    log(f"[DisplayManager] Screens changed. Current screens: {list(screens.keys())}")
    rgb_lcd_manager.set_external_screen_count(len(screens))
display_manager.set_on_screens_change_callback(on_screens_change)

def update_lcd_microwave_count():
    """Updates the LCD with the current number of connected microwaves."""
    global microwave_states
    count = len(microwave_states)
    rgb_lcd_manager.set_microwave_count(count)
    
def update_lcd_cloud_alert():
    """Updates the LCD with the current cloud connectivity status."""
    global cloud_alert
    rgb_lcd_manager.set_cloud_alert(cloud_alert)

async def display_broadcast_worker_task():
    """
    Debounced display broadcast task.
    Waits until DISPLAY_DEBOUNCE_SECONDS have passed since the first queued update
    before sending state to all connected screens.
    """
    global display_update_queue, microwave_states, cloud_alert
    
    print("[Display Worker] Started debounced display broadcast worker.")
    while True:
        # Block until the FIRST update reminder arrives in the queue
        first_event_time = await display_update_queue.get()

        # Compute wait time relative to the first event's timestamp
        elapsed = time.time() - first_event_time
        remaining_wait = DISPLAY_DEBOUNCE_SECONDS - elapsed
        if remaining_wait > 0:
            await asyncio.sleep(remaining_wait)

        # Clear any subsequent events that arrived during the wait period
        while not display_update_queue.empty():
            try:
                display_update_queue.get_nowait()
            except asyncio.QueueEmpty:
                break

        # Broadcast state ONCE
        await display_manager.broadcast_state(
            microwave_states=microwave_states,
            cloud_alert=cloud_alert
        )

def button_callback():
    """Button physical interrupt callback."""
    global button_state
    if microwave_states.get("2").cooking_state == CookingStates.COOKING or microwave_states.get("2").cooking_state == CookingStates.DONE or microwave_states.get("2").cooking_state == CookingStates.STIRRING_REQUIRED:
        print("[Button] Toggling pause/resume for microwave '2'.")
        lora.send_reliable({"id": DEVICE_ID, "microwave_id": "2", "action": LoraCommands.TOGGLE_PAUSE}, max_retries=6)
    else:
        button_state = not button_state
        print(f"[Button] Defrost state toggled to: {button_state}")
        lora.send_reliable({"id": DEVICE_ID, "microwave_id": "2", "action": LoraCommands.TOGGLE_DEFROST, "defrost_state": button_state}, max_retries=5)

button.set_callback(button_callback)
button.start_button_monitoring_thread()

# --- Alert manager ---
alert_manager = None

def on_new_alert(alert):
    # Sends to cloud server
    alert_dict = alert.to_dict()
    alert_dict["orchestrator_id"] = DEVICE_ID
    
    response = requests.post(
        "https://smartwave.matthiasg.dev/alert", 
        json=alert_dict,
    )

    response.raise_for_status()
    res_json = response.json()
    print(f"[Alert Handling] Alert sent to cloud: {alert_dict}, Response: {res_json}")
    
    # Send the alert to the lora device
    lora.send_reliable(payloads.lora_new_alert(alert))
    
    microwave_states["2"].set_cooking_state(CookingStates.ALERT)

# --- HARDWARE CONTROLLERS ---
def _stop_hardware(microwave_id: str):
    print(f"[{microwave_id}] /!\\ Emergency stop issued to hardware.")
    # TODO: Add LoRa STOP command here

# --- ASYNC COOKING LOGIC ---
def read_local_sensors(microwave_id, initial_dish_height):
    """Blocking function to read local I2C/SPI sensors. Runs in a thread."""
    print(f"[{microwave_id}] Reading local physical sensors...")
    sensor_data = {
        "microwave_id": microwave_id,
        "defrost_mode": button_state,
        "ultrasonic_distance": initial_dish_height  # Reuse height from trigger
    }
    
    # Temp / Hum (handles DHT error safely)
    try:
        temp, hum = temp_hum.get_temperature_and_humidity_with_retry()
        if temp is not None:
            sensor_data["temperature"] = temp
            sensor_data["humidity"] = hum
    except Exception as e:
        log(f"[{microwave_id}] DHT read warning: {e}")
        
    # Camera
    try:
        sensor_data["camera_image"] = camera.get_picture()
    except Exception as e:
        log(f"[{microwave_id}] Camera read failed: {e}")
        
    return sensor_data


async def handle_new_dish(microwave_id, detected_height):
    """Triggered when a new dish is placed inside."""
    microwave_states[microwave_id].set_state(MicrowaveState.ANALYZING)
    print(f"\n[{microwave_id}] 🍽️ Dish detected at {detected_height:.1f} cm! Requesting IR from microwave...")

    # 1. Setup synchronization event and clear previous cache for this microwave
    event = asyncio.Event()
    ir_data_events[microwave_id] = event
    ir_data_cache.pop(microwave_id, None)

    # 2. Send IR request to ESP32 via MQTT immediately
    mqtt_client.publish(
        config.MQTT_TOPIC_COOKING, 
        payloads.mqtt_cooking_init(microwave_id), 
        qos=config.MQTT_QOS
    )

    # 3. Start local sensor reading in parallel
    sensor_task = asyncio.create_task(asyncio.to_thread(read_local_sensors, microwave_id, detected_height))

    # 4. Wait for local sensors to finish reading
    sensors_data = await sensor_task

    # Check if dish was removed while reading sensors
    if microwave_states.get(microwave_id).state != MicrowaveState.ANALYZING:
        print(f"[{microwave_id}] Dish removed during sensor read. Aborting.")
        ir_data_events.pop(microwave_id, None)
        return

    # 5. Wait for MQTT IR data
    try:
        await asyncio.wait_for(event.wait(), timeout=10.0)
        ir_payload = ir_data_cache.get(microwave_id, {})
        sensors_data["ir_initial_temp"] = ir_payload.get("dish_temp")
        sensors_data["ir_ambient_temp"] = ir_payload.get("ambient_temp")
        print(f"[{microwave_id}] IR data synchronized successfully: {ir_payload}")
        # 6. Dispatch cloud request task
        asyncio.create_task(request_cloud_cooking_plan(microwave_id, sensors_data))
    except asyncio.TimeoutError:
        print(f"[{microwave_id}] Timeout waiting for MQTT IR data from ESP32.")
        sensors_data["ir_initial_temp"] = None
        sensors_data["ir_ambient_temp"] = None
    finally:
        ir_data_events.pop(microwave_id, None)

async def request_cloud_cooking_plan(microwave_id, sensors_data):
    """Sends all data to the cloud with up to 3 retries (330s timeout for AI generation)."""
    global cloud_alert, alert_manager
    microwave_states[microwave_id].set_state(MicrowaveState.WAITING_FOR_CLOUD)
    URL = "https://smartwave.matthiasg.dev/cooking-params"
    
    # Format image
    if isinstance(sensors_data.get("camera_image"), bytes):
        sensors_data["camera_image"] = base64.b64encode(sensors_data["camera_image"]).decode("utf-8")
        
    print(f"[{microwave_id}] Requesting cooking plan from cloud app...")

    max_retries = 3
    retry_delay_seconds = 10  # Reduced delay since each request already waits up to 5 mins

    # Tuple format: (connect_timeout_seconds, read_timeout_seconds)
    HTTP_TIMEOUT = (10, 330)

    for attempt in range(1, max_retries + 1):
        if microwave_states.get(microwave_id).state != MicrowaveState.WAITING_FOR_CLOUD:
            print(f"[{microwave_id}] Dish removed or state changed. Aborting API request.")
            return

        try:
            print(f"[{microwave_id}] Connection attempt {attempt}/{max_retries} (Waiting up to 5.5 min)...")
            
            response = await asyncio.to_thread(
                requests.post, 
                URL, 
                json=sensors_data, 
                timeout=HTTP_TIMEOUT
            )

            # Abort if state changed while waiting for cloud AI response
            if microwave_states.get(microwave_id).state != MicrowaveState.WAITING_FOR_CLOUD:
                print(f"[{microwave_id}] Dish removed during API request. Discarding API plan.")
                return

            response.raise_for_status()
            
            set_cloud_alert(False)  # Clear any previous cloud alert

            # JSON extraction
            res_json = response.json()
            
            # Error handling
            if "error" in res_json:
                print(f"[{microwave_id}] Cloud API returned error: {res_json['error']}")
                microwave_states[microwave_id].set_state(MicrowaveState.DONE)
                
                if not res_json['is_safe']: # The cooking area is not safe, raise an alert
                    alert = Alert(AlertType.COOKING_SAFETY, res_json['warning_message'])
                    alert_manager.add_alert(alert)
                else:
                    alert = Alert(AlertType.ERROR, f"Cloud API error for microwave {microwave_id}: {res_json['error']}")
                return
            
            
            plan = res_json.get("cook_plan", res_json)

            c_time = plan.get("cook_time_seconds")
            c_power = plan.get("effective_power_watts")
            c_temp = plan.get("target_temp")
            
            if c_time is None or c_power is None or c_temp is None:
                print(f"[{microwave_id}] Invalid plan received: {res_json}")
                microwave_states[microwave_id].set_state(MicrowaveState.DONE)
                return

            print(f"[{microwave_id}] Cloud Plan Received! Starting microwave: {c_time}s @ {c_power}W")
            microwave_states[microwave_id].set_state(MicrowaveState.COOKING)

            if config.DEBUG:
                c_time = 20  # Set to 20s for debug
                c_temp = 50  # Set to 50°C for debug

            mqtt_client.publish(
                config.MQTT_TOPIC_COOKING, 
                payloads.mqtt_cooking_config(microwave_id, c_time, c_power, c_temp), 
                qos=config.MQTT_QOS
            )
            
            microwave_states[microwave_id].set_cooking_start_time(time.time())
            return

        except Exception as e:
            # Print stacktrace
            import traceback
            traceback.print_exc()
            print(f"[{microwave_id}] Cloud API Error (Attempt {attempt}/{max_retries}): {e}")
            
            if attempt < max_retries:
                print(f"[{microwave_id}] Retrying in {retry_delay_seconds} seconds...")
                # Interruptible wait loop in case state changes mid-wait
                for _ in range(retry_delay_seconds):
                    if microwave_states.get(microwave_id).state != MicrowaveState.WAITING_FOR_CLOUD:
                        print(f"[{microwave_id}] State changed during retry wait. Aborting retries.")
                        return
                    await asyncio.sleep(1)

    # Executed only if all 3 retries failed
    print(f"[{microwave_id}] All cloud retries failed. Setting global alert flag.")
    set_cloud_alert(True)
    microwave_states[microwave_id].set_state(MicrowaveState.DONE)

# --- MAIN LOGIC TASKS ---
async def process_messages_task():
    """Consumes the unified event queue."""
    # Helper to normalize config topics to str
    def to_str(val):
        return val.decode('utf-8') if isinstance(val, bytes) else val
    
    while True:
        msg = await async_event_queue.get()
        source = msg["source"]
        data = msg["data"]
        
        if source == "LoRa":
            if "new_cooking_state" in data.get("data", {}):
                mw_id = data["data"].get("id")
                n_state = data["data"].get("new_cooking_state")
                print(f"[LoRa] Microwave {mw_id} state changed to: {n_state}")
                
                microwave_states[mw_id].set_cooking_state(n_state)
                update_lcd_microwave_count()    # Update the LCD with new microwave count
                
                if n_state in (CookingStates.DONE, CookingStates.STIRRING_REQUIRED, CookingStates.ALERT):
                    beats = 5 if n_state == CookingStates.ALERT else (1 if n_state == CookingStates.STIRRING_REQUIRED else 3)
                    buzzer.buzzer_siren(beatsNb=beats)
                if n_state == CookingStates.IDLE and microwave_states.get(mw_id).state == MicrowaveState.COOKING:
                    microwave_states[mw_id].set_state(MicrowaveState.DONE)
                    print(f"[{mw_id}] Cooking finished. Waiting for user to remove dish.")
            if "action" in data.get("data", {}):
                action = data["data"]["action"]
                if action == LoraCommands.COOKING_UPDATE:
                    mw_id = data["data"].get("id")
                    if mw_id in microwave_states:
                        state_info = data["data"]
                        microwave_states[mw_id].set_cooking_state(state_info.get("cooking_state", microwave_states[mw_id].cooking_state))
                        microwave_states[mw_id].set_cooking_estimated_remaining_time(state_info.get("estimated_remaining_time", microwave_states[mw_id].cooking_estimated_remaining_time))
                        microwave_states[mw_id].set_paused(state_info.get("paused", microwave_states[mw_id].paused))
                else:
                    print(f"[LoRa] Unhandled action received: {action} with data: {data['data']}")
        elif source == "MQTT":
            topic = msg["topic"]

            hello_topic = to_str(config.MQTT_TOPIC_HELLO)
            sensor_topic = to_str(config.MQTT_TOPIC_SENSOR)
            
            if topic == hello_topic:
                if data.get("id_orchestrator") != DEVICE_ID:
                    component_id = data.get("id_microwave")
                    component_type = data.get("type", deviceTypes.DEVICE_TYPES["MICROWAVE"])
                    
                    if component_id:
                        print(f"[MQTT] Hello received from '{component_id}' ({component_type}). Updating DB & sending ACK.")
                        # Offload DB insertion to async thread execution pool
                        await asyncio.to_thread(save_connected_component, component_id, component_type)
                        
                        # Pass change callback to newly connected microwave
                        microwave_states[component_id] = MicrowaveState(
                            component_id, 
                            on_change_callback=on_microwave_change
                        )
                        notify_display_update()         # Update the external screens
                        update_lcd_microwave_count()    # Update the LCD with new microwave count
                        
                        mqtt_client.publish(
                            config.MQTT_TOPIC_HELLO,
                            payloads.mqtt_hello_ack(DEVICE_ID, component_id),
                            qos=config.MQTT_QOS
                        )
            
            elif topic == sensor_topic:
                mw_id = str(data.get("id_microwave"))
                print(f"[MQTT] Sensor data received for microwave {mw_id}: {data}")
                
                # Store IR data and notify the waiting dish handler
                ir_data_cache[mw_id] = data
                if mw_id in ir_data_events:
                    ir_data_events[mw_id].set()

        elif source == "CloudMQTT":
            topic = msg["topic"]
            print(f"[CloudMQTT] Command received on '{topic}': {data}")
            
            action = data.get("action")
            
            if action == "request_telemetry":
                endpoint = data.get("endpoint")
                if endpoint:
                    # Schedule as a background task so processing the queue isn't stalled
                    asyncio.create_task(handle_telemetry_request(endpoint))
                else:
                    print("[CloudMQTT] Received 'request_telemetry' but missing 'endpoint' field.")
            else:
                print(f"[CloudMQTT] Unknown action: {action}")

async def handle_telemetry_request(endpoint: str):
    """
    Task to gather telemetry data (including structured systemd logs) 
    and post it to the target HTTP endpoint.
    """
    print(f"[Telemetry] Starting telemetry collection for endpoint: {endpoint}")
    try:
        # 1. Fetch systemd logs asynchronously
        logs_list = await get_systemd_logs(lines=200, service_name="smartwave")
        
        # 2. Unpack temperature and humidity
        ambient_temp, ambient_humidity = temp_hum.get_temperature_and_humidity_with_retry()
        
        # 3. Build the telemetry payload (returns a JSON string)
        telemetry_payload = payloads.telemetry_payload(
            device_id=DEVICE_ID,
            microwave_states=microwave_states.to_dict(),
            button_state=button_state,
            cloud_alert=cloud_alert,
            gps_data=gps.get_gps_data(),
            ambient_temp=ambient_temp,
            ambient_humidity=ambient_humidity,
            connected_components=get_connected_components(),
            logs=logs_list
        )
                
        # 4. Wait for he microwave turn to send the telemetry data to the cloud endpoint
        timeout = int(DEVICE_ID.split("_")[-1]) * config.TELEMETRY_SEND_INTERVAL
        print(f"[Telemetry] Waiting for {timeout}s before sending telemetry to avoid collisions...")
        await asyncio.sleep(timeout)
        
        # 5. Offload blocking HTTP POST to thread pool
        print(f"[Telemetry] Sending payload with {len(logs_list)} log entries...")
        response = await asyncio.to_thread(
            requests.post, 
            endpoint, 
            data=telemetry_payload,  # <-- Changed from json=telemetry_payload
            headers={"Content-Type": "application/json"},
            timeout=15
        )
        
        response.raise_for_status()
        print(f"[Telemetry] Successfully dispatched telemetry (HTTP {response.status_code}).")

    except requests.exceptions.RequestException as req_err:
        print(f"[Telemetry] HTTP Request failed to {endpoint}: {req_err}")
    except Exception as e:
        print(f"[Telemetry] Error handling telemetry request: {e}")

async def get_filtered_dish_height(samples=3, delay=0.04):
    """Reads ultrasonic sensor multiple times and returns the median, discarding invalid zeros."""
    valid_samples = []
    for _ in range(samples):
        h = await asyncio.to_thread(ultrasonicRanger.get_dish_height)
        # Discard negative glitches
        if h is not None and h >= 0.0:
            valid_samples.append(h)
        await asyncio.sleep(delay)
    
    if valid_samples:
        valid_samples.sort()
        return valid_samples[len(valid_samples) // 2]  # Median sample
    return None  # All reads failed or out of range


async def monitor_dish_height_task():
    """Monitors presence of dish with hysteresis and debouncing."""
    global cloud_alert
    mw_id = "2"
    consecutive_present = 0
    consecutive_absent = 0
    REQUIRED_STABLE_READS = 3  # Must see 3 stable states in a row (~1 second)

    while True:
        dist = await get_filtered_dish_height()
        try:
            current_state = microwave_states.get(mw_id).state
        except AttributeError:
            current_state = MicrowaveState.IDLE

        if dist is not None:
            # Hysteresis Thresholds:
            # - Must be > 2.5 cm to detect dish insertion
            # - Must be < 1.2 cm to detect dish removal
            if dist > 2.5:
                consecutive_present += 1
                consecutive_absent = 0
            elif dist < 1.2:
                consecutive_absent += 1
                consecutive_present = 0
            else:
                # Dead-zone (1.2cm to 2.5cm) -> Noise buffer
                consecutive_present = 0
                consecutive_absent = 0

            # --- DISH INSERTED CONFIRMED ---
            if consecutive_present >= REQUIRED_STABLE_READS and current_state == MicrowaveState.IDLE:
                consecutive_present = 0
                asyncio.create_task(handle_new_dish(mw_id, dist))

            # --- DISH REMOVED CONFIRMED ---
            elif consecutive_absent >= REQUIRED_STABLE_READS and current_state != MicrowaveState.IDLE:
                consecutive_absent = 0
                print(f"\n[{mw_id}] Dish Removed! Resetting state to IDLE.")
                microwave_states[mw_id].set_state(MicrowaveState.IDLE)
                set_cloud_alert(False)
                
                if current_state == MicrowaveState.COOKING:
                    _stop_hardware(mw_id)
                # Remove from IR cache and events
                ir_data_cache.pop(mw_id, None)
                ir_data_events.pop(mw_id, None)
        
        await asyncio.sleep(0.3)

# --- BOOTSTRAP ---
async def main():
    global async_event_queue, display_update_queue
    print("Orchestrateur prêt. Lancement des tâches...")
    
    # Initialize the alert manager and set the callback
    global alert_manager
    alert_manager = AlertManager()
    alert_manager.set_on_alert_callback(on_new_alert)
    
    # Initialize SQLite database table
    init_db()
    
    # Start mDNS Display discovery
    await display_manager.start()
    
    async_event_queue = asyncio.Queue()
    display_update_queue = asyncio.Queue()
    
    await asyncio.gather(
        lora_listener_task(),
        mqtt_listener_task(),
        cloud_mqtt_listener_task(),
        process_messages_task(),
        monitor_dish_height_task(),
        display_broadcast_worker_task()
    )

if __name__ == "__main__":
    try:
        asyncio.run(main())
    except KeyboardInterrupt:
        print("\nArrêt manuel.")
    finally:
        asyncio.run(display_manager.stop())
        if hasattr(mqtt_client._client, "loop_stop"):
            mqtt_client._client.loop_stop()
        if hasattr(cloud_mqtt_client._client, "loop_stop"):
            cloud_mqtt_client._client.loop_stop()
        mqtt_client.close()
        cloud_mqtt_client.close()