import time
import sys
import machine
from machine import Pin, SPI, I2C

try:
    import network
    has_network = True
except ImportError:
    has_network = False

if sys.platform == 'rp2':
    import st7796 as display_module
else:
    import rlcd as display_module

class DummyMCP:
    def __init__(self):
        self.override_active = False
        self.active_led_mode = "off"
    def update(self): pass
    def start(self, port=80): pass

class DummyVStream:
    def __init__(self):
        self.active = False
    def update(self): pass
    def start(self): pass

# Import our utility classes
from shtc3_util import SHTC3
from rtc_util import PCF85063
from battery_util import BatteryMonitor
from button_util import BoardButtons
from rgb_led_util import BoardLED
from ble_util import BLEUART
from mcp_server import MCPServer
from video_stream import VideoStreamServer

# LED mode options for manual cycling
led_modes = [
    ("Red (Breathing)", lambda led: led.set_color(40, 0, 0), "breath"),
    ("Green (Breathing)", lambda led: led.set_color(0, 40, 0), "breath"),
    ("Blue (Breathing)", lambda led: led.set_color(0, 0, 40), "breath"),
    ("Cyan (Breathing)", lambda led: led.set_color(0, 30, 30), "breath"),
    ("Magenta (Breathing)", lambda led: led.set_color(30, 0, 30), "breath"),
    ("Rainbow Cycle", lambda led: led.set_color(30, 30, 30), "rainbow"),
    ("LED Off", lambda led: led.off(), "off")
]
local_led_mode_idx = 1 # Green breathing

def main():
    global local_led_mode_idx
    print("=== Starting ESP32-S3-RLCD-4.2 Main Boot ===")
    
    # 1. Initialize shared buses and peripherals
    i2c = None
    if sys.platform != 'rp2':
        try:
            i2c = I2C(0, sda=Pin(13), scl=Pin(14))
        except Exception as e:
            print("Failed to initialize I2C0:", e)
    
    if sys.platform == 'rp2':
        spi = SPI(1, baudrate=32000000, polarity=0, phase=0, sck=Pin(10), mosi=Pin(11))
        display = display_module.ST7796(spi, cs=Pin(7), dc=Pin(4), rst=Pin(9), bl=Pin(6))
        
        # Touch controller
        touch = None
        try:
            touch_i2c = I2C(1, sda=Pin(2), scl=Pin(3), freq=400000)
            from ft6336u import FT6336U
            touch = FT6336U(touch_i2c, rst_pin=28, int_pin=25)
        except Exception as te:
            print("Failed to initialize touch:", te)
    else:
        spi = SPI(1, baudrate=20000000, polarity=0, phase=0, sck=Pin(11), mosi=Pin(12))
        display = display_module.RLCD(spi, cs=Pin(40), dc=Pin(5), rst=Pin(41))
        touch = None
    
    if sys.platform != 'rp2':
        # Configure Audio Amp control pin to save power
        amp_pin = Pin(46, Pin.OUT, value=0)
    
    # 2. Initialize utility objects
    sensor = None
    rtc_chip = None
    if i2c is not None:
        try:
            sensor = SHTC3(i2c)
        except Exception as e:
            print("Failed to initialize SHTC3:", e)
        try:
            rtc_chip = PCF85063(i2c)
        except Exception as e:
            print("Failed to initialize PCF85063:", e)
            
    battery = BatteryMonitor()
    led = BoardLED()
    buttons = None
    if sys.platform != 'rp2':
        buttons = BoardButtons()
    ble_uart = BLEUART(name="ESP32-S3-RLCD")
    
    # Sync system clock from RTC chip
    if rtc_chip:
        try:
            rtc_chip.sync_to_system()
        except Exception as e:
            print("Failed to sync clock:", e)
    
    # 3. Connect to Wi-Fi status check
    ip_addr = "Offline (USB)"
    mcp = DummyMCP()
    vstream = DummyVStream()
    
    if has_network:
        try:
            wlan = network.WLAN(network.STA_IF)
            ip_addr = wlan.ifconfig()[0] if wlan.isconnected() else "Disconnected"
            
            # 4. Start background TCP/UDP Video Streaming Server
            from video_stream import VideoStreamServer
            vstream = VideoStreamServer(display, tcp_port=8081, udp_port=8082)
            vstream.start()
            
            # 4b. Start MCP Server
            from mcp_server import MCPServer
            mcp = MCPServer(display, led, battery, sensor, rtc_chip, ble_uart, vstream=vstream)
            mcp.start(port=80)
        except Exception as ne:
            print("Failed to start network services:", ne)
            
    # Default to Green Breathing
    led_modes[local_led_mode_idx][1](led)
    mcp.active_led_mode = led_modes[local_led_mode_idx][2]
    
    # Last user actions
    last_action_str = "Boot finished."
    force_dashboard_redraw = True
    
    # 5. Register Button Handlers
    def on_key_click():
        global local_led_mode_idx
        local_led_mode_idx = (local_led_mode_idx + 1) % len(led_modes)
        mode_name, color_fn, mode_type = led_modes[local_led_mode_idx]
        color_fn(led)
        mcp.active_led_mode = mode_type
        print(f"Local Button: Cycle LED -> {mode_name}")
        nonlocal last_action_str, force_dashboard_redraw
        last_action_str = f"Local Button: {mode_name}"
        # Disable override when manual button is pressed
        mcp.override_active = False
        force_dashboard_redraw = True

    def on_boot_click():
        print("Local Button: Force Dashboard refresh.")
        nonlocal last_action_str, force_dashboard_redraw
        last_action_str = "Dashboard Refreshed"
        # Disable override when manual button is pressed
        mcp.override_active = False
        force_dashboard_redraw = True

    if buttons:
        buttons.key.on_click(on_key_click)
        buttons.boot.on_click(on_boot_click)
    
    # Loop state
    last_dashboard_update = 0
    dashboard_update_interval_ms = 5000
    last_led_update = 0
    
    print("ESP32 MCP loop running...")
    
    # 6. Main execution loop
    while True:
        now = time.ticks_ms()
        
        # Check touch interaction if available
        if touch and touch.is_touched():
            pt = touch.read_touch()
            if pt:
                tx, ty = pt
                print(f"Touch detected at: ({tx}, {ty})")
                last_action_str = f"Touch: ({tx}, {ty})"
                mcp.override_active = False
                force_dashboard_redraw = True
        
        # A. Handle non-blocking MCP client connection updates
        mcp.update()
        
        # B. Handle non-blocking background video stream updates
        vstream.update()
        
        # C. Draw local dashboard (if not overridden by MCP draw text commands or active video stream)
        if not mcp.override_active and not vstream.active:
            if force_dashboard_redraw or time.ticks_diff(now, last_dashboard_update) >= dashboard_update_interval_ms:
                force_dashboard_redraw = False
                last_dashboard_update = now
                
                # Fetch sensor data
                t, h = sensor.read_sensor() if sensor else (None, None)
                t_str = f"{t} C" if t is not None else "Error"
                h_str = f"{h} %" if h is not None else "Error"
                
                # Fetch battery status
                bat_v = battery.read_voltage()
                bat_p = battery.read_percentage()
                bat_str = f"{bat_v:.2f}V ({bat_p}%)" if bat_v is not None else "Error"
                
                # Fetch current time
                dt = rtc_chip.get_datetime() if rtc_chip else None
                time_str = f"{dt[0]:04d}-{dt[1]:02d}-{dt[2]:02d} {dt[4]:02d}:{dt[5]:02d}:{dt[6]:02d}" if dt else "RTC Error"
                
                # Draw standard status dashboard layout
                display.clear(0)
                title_text = "RP2350-TFT MCP SERVER" if sys.platform == 'rp2' else "ESP32-S3-RLCD MCP SERVER"
                line_w = 470 if sys.platform == 'rp2' else 390
                
                display.text(title_text, 10, 10, 1)
                display.line(10, 20, line_w, 20, 1)
                
                display.text_large("ENVIRONMENT", 15, 30, scale=2, c=1)
                display.text(f"Temp        : {t_str}", 25, 55, 1)
                display.text(f"Humid       : {h_str}", 25, 70, 1)
                
                display.line(10, 95, line_w, 95, 1)
                
                display.text_large("MCP NET CONNECTION", 15, 105, scale=2, c=1)
                display.text(f"IP Address  : {ip_addr}", 25, 130, 1)
                display.text(f"Port / Path : 80 /api/mcp", 25, 145, 1)
                display.text(f"BLE Name    : ESP32-S3-RLCD", 25, 160, 1)
                
                display.line(10, 185, line_w, 185, 1)
                
                display.text_large("SYSTEM STATUS", 15, 195, scale=2, c=1)
                display.text(f"Battery     : {bat_str}", 25, 220, 1)
                display.text(f"Time        : {time_str}", 25, 235, 1)
                
                display.line(10, 255, line_w, 255, 1)
                display.text(f"Status: {last_action_str}", 15, 265, 1)
                display.show()
                
        # D. Update NeoPixel animation smoothly (runs every 50ms)
        if time.ticks_diff(now, last_led_update) >= 50:
            last_led_update = now
            mode = mcp.active_led_mode
            if mode == "breath":
                led.update_breathing(1.5)
            elif mode == "rainbow":
                led.update_rainbow(0.4)
            elif mode == "off":
                led.off()
            
        # Poll rapidly if stream is active, otherwise sleep 50ms to save power
        if vstream.active:
            time.sleep_ms(2)
        else:
            time.sleep_ms(50)

if __name__ == "__main__":
    main()