import sys
import select
from machine import Pin
from neopixel import NeoPixel
from time import sleep_ms, ticks_ms, ticks_diff

# --- HARDWARE SETUP ---
pin1 = Pin(26, Pin.OUT)
np1 = NeoPixel(pin1, 8) # FRONT wagen

pin2 = Pin(27, Pin.OUT)
np2 = NeoPixel(pin2, 8) # REAR wagen

# --- KLEUREN (R, G, B) ---
UIT = (0, 0, 0)
WIT = (255, 255, 255)
DIM_WIT = (40, 40, 40)
ROOD = (255, 0, 0)
DIM_ROOD = (30, 0, 0)
BLAUW = (0, 0, 255)
ORANJE = (255, 100, 0) # Neopixels hebben vaak wat minder groen nodig voor mooi oranje

# --- STATUS VARIABELEN (Wat staat er aan?) ---
status_dimlicht = 0
status_remlicht = 0
status_zwaailicht = 0
status_pinker_l = 0
status_pinker_r = 0

# --- TIMERS VOOR ANIMATIES (Blink without delay) ---
laatste_tijd_zwaai = 0
zwaai_toggle = False
zwaai_snelheid = 150 # milliseconden

laatste_tijd_pinker = 0
pinker_toggle = False
pinker_snelheid = 400 # milliseconden

# --- FUNCTIES ---

def parse_commando(tekst):
    """Vertaalt inkomende tekst (bijv 'zwaailicht/1') naar acties"""
    global status_dimlicht, status_remlicht, status_zwaailicht, status_pinker_l, status_pinker_r
    
    try:
        # Splits de tekst op de '/'
        delen = tekst.split('/')
        if len(delen) == 2:
            commando = delen[0].lower()
            waarde = int(delen[1])
            
            print(f"Ontvangen -> Commando: {commando}, Waarde: {waarde}")
            
            # Pas statussen aan
            if commando == "dimlicht":
                status_dimlicht = waarde
            elif commando == "remlicht":
                status_remlicht = waarde
            elif commando == "zwaailicht":
                status_zwaailicht = waarde
            elif commando == "pinker_l":
                status_pinker_l = waarde
            elif commando == "pinker_r":
                status_pinker_r = waarde
            elif commando == "gevaren":
                status_pinker_l = waarde
                status_pinker_r = waarde
            else:
                print("Onbekend commando!")
    except:
        print("Fout in commando formaat. Gebruik: commando/waarde")

def update_leds():
    """Berekent welke LED welke kleur moet hebben o.b.v. de statussen en timers"""
    global zwaai_toggle, laatste_tijd_zwaai, pinker_toggle, laatste_tijd_pinker
    
    huidige_tijd = ticks_ms()
    
    # 1. Update Timers
    if ticks_diff(huidige_tijd, laatste_tijd_zwaai) > zwaai_snelheid:
        zwaai_toggle = not zwaai_toggle
        laatste_tijd_zwaai = huidige_tijd
        
    if ticks_diff(huidige_tijd, laatste_tijd_pinker) > pinker_snelheid:
        pinker_toggle = not pinker_toggle
        laatste_tijd_pinker = huidige_tijd

    # 2. Maak strips leeg (basislaag)
    for i in range(8):
        np1[i] = UIT
        np2[i] = UIT
        
    # 3. Bouw de verlichting op (Lagen over elkaar heen)
    
    # -- DIMLICHT --
    if status_dimlicht == 1:
        # Front: koplampen op positie 1,2 en 5,6
        np1[1], np1[2], np1[5], np1[6] = DIM_WIT, DIM_WIT, DIM_WIT, DIM_WIT
        # Rear: achterlichten op positie 1,2 en 5,6
        np2[1], np2[2], np2[5], np2[6] = DIM_ROOD, DIM_ROOD, DIM_ROOD, DIM_ROOD

    # -- REMLICHT --
    if status_remlicht == 1:
        # Overschrijft achterlichten met FEL rood
        np2[1], np2[2], np2[5], np2[6] = ROOD, ROOD, ROOD, ROOD

    # -- ZWAAILICHT --
    if status_zwaailicht == 1:
        # Positie 3 en 4 worden gebruikt voor zwaailicht op beide strips
        if zwaai_toggle:
            np1[3], np1[4] = BLAUW, UIT
            np2[3], np2[4] = BLAUW, UIT
        else:
            np1[3], np1[4] = UIT, BLAUW
            np2[3], np2[4] = UIT, BLAUW

    # -- PINKERS --
    # Pinkers overschrijven de buitenste LEDS (0 en 7)
    if status_pinker_l == 1 and pinker_toggle:
        np1[0] = ORANJE
        np2[0] = ORANJE
        
    if status_pinker_r == 1 and pinker_toggle:
        np1[7] = ORANJE
        np2[7] = ORANJE

    # 4. Schrijf de data naar de echte LEDS
    np1.write()
    np2.write()

# --- MAIN LOOP ---
print("--- Politie LED Simulator Gestart ---")
print("Typ commando's in de terminal (bijv: dimlicht/1, zwaailicht/1, pinker_l/1)")

# Maak een 'poll' object om de terminal te lezen zonder te blokkeren
poll_obj = select.poll()
poll_obj.register(sys.stdin, select.POLLIN)

while True:
    # Controleer of er iets is ingetypt in de terminal
    poll_results = poll_obj.poll(0)
    if poll_results:
        inkomende_tekst = sys.stdin.readline().strip()
        if inkomende_tekst:
            parse_commando(inkomende_tekst)
            
    # Update de LEDs continu (zorgt voor vloeiend knipperen)
    update_leds()
    
    # Kleine pauze om de processor niet te overbelasten (is heel kort, dus stoort timers niet)
    sleep_ms(500)
    print(">>>>> in loop")