from machine import Pin, ADC
import utime

import machine
import uos
import sdcard

cs_pin = machine.Pin(17, machine.Pin.OUT)
spi = machine.SPI(0, baudrate=1000000, polarity=0, phase=0,bits=8, firstbit=machine.SPI.MSB,
                  sck=machine.Pin(18), mosi=machine.Pin(19), miso=machine.Pin(16))

sd = sdcard.SDCard(spi, cs_pin)

vfs = uos.VfsFat(sd)
uos.mount(vfs, "/sd")
fn = "/sd/sdtest.txt"

with open(fn, "w") as f:
    print("Writing data to file sdtest.txt...")
    f.write("This is a test for micro SD card\r\n")
    print("Writing to file completed")

with open(fn, "r") as f:
    print("Reading data from file sdtest.txt...")
    data = f.read()
    print("Data read completed")
    print("Data:",data)

# === A4988 (STEP-Motor Treiber) Pins ===
step_pin = Pin(21, Pin.OUT)
dir_pin = Pin(6, Pin.OUT)
enable_pin = Pin(20, Pin.OUT)
enable_pin.value(0)  # Treiber aktivieren

# === Joystick (X-Achse an GP26 / ADC0) ===
joystick_x = ADC(26)

# === Sicherheits-Taster ===
taste_aus = Pin(2, Pin.IN, Pin.PULL_UP)     # Blau
taste_notaus = Pin(3, Pin.IN, Pin.PULL_UP)      # Grün
taste_reset = Pin(4, Pin.IN, Pin.PULL_UP)       # Braun
schluesselschalter = Pin(5, Pin.IN, Pin.PULL_UP) # Gelb
tuer_schalter = Pin(6, Pin.IN, Pin.PULL_UP)     # Grau
sensor_schalter = Pin(7, Pin.IN, Pin.PULL_UP)   # Rosa

# === Steuerungsvariablen ===
motor_freigegeben = False
motor_aktiv = False
umkehrzone = 2000
max_delay = 2000
min_delay = 300

# === Sicherheitszustände ===
notaus_aktiv = False
sicherheitsfehler = False
reset_erforderlich = False

# === Aktivierungsvariablen ===
aktivierungs_timer = 0
aktivierungs_dauer = 3
letzter_ein_aus_zustand = True
letzter_reset_zustand = True

# === Längebegrenzung

aktuelle_position=0
schritte_gesamt=0
max_position=500
min_position=-500
limit_aktiv=0

# === Mapping-Funktion ===
def map_adc_to_delay(adc_val):
    abstand = abs(adc_val - 32768)
    if abstand < umkehrzone:
        return None
    skala = max_delay - min_delay
    delay = max_delay - int((abstand / 32768) * skala)
    return delay


# === Sicherheitsprüfungen ===
def pruefe_sicherheit():
    global notaus_aktiv, sicherheitsfehler, motor_aktiv, reset_erforderlich
    
    # NOTAUS - höchste Priorität
    if not taste_notaus.value():
        if not notaus_aktiv:
            notaus_aktiv = True
            motor_aktiv = False
            reset_erforderlich = True
            print("*** NOTAUS AKTIVIERT - RESET ERFORDERLICH! ***")
        return False
    
    # Schlüsselschalter muss EIN sein
    #if not schluesselschalter.value():
        #if not sicherheitsfehler:
            #sicherheitsfehler = False
            #motor_aktiv = False
            #print("SICHERHEIT: Schlüssel muss eingeschaltet sein!")
       # return False
    
    # Tür muss geschlossen sein
    if not tuer_schalter.value():
        if not sicherheitsfehler:
            sicherheitsfehler = True
            motor_aktiv = False
            print("SICHERHEIT: Tür muss geschlossen sein!")
        return False
    
    # Sensor darf nicht ausgelöst sein
    if not sensor_schalter.value():
        if not sicherheitsfehler:
            sicherheitsfehler = True
            motor_aktiv = False
            print("SICHERHEIT: Sensor ausgelöst!")
        return False
    
    # Wenn alle Sicherheitsbedingungen erfüllt sind
    if sicherheitsfehler and not reset_erforderlich:
        sicherheitsfehler = False
        print("Sicherheitsbedingungen wieder erfüllt.")
    
    return True


# === Reset-Funktion ===
def handle_reset():
    global notaus_aktiv, sicherheitsfehler, motor_aktiv, reset_erforderlich
    global motor_freigegeben, aktivierungs_timer, letzter_reset_zustand
    
    aktueller_reset_zustand = taste_reset.value()
    
    # Reset-Taste wurde gedrückt (fallende Flanke)
    if letzter_reset_zustand and not aktueller_reset_zustand:
        notaus_aktiv = False
        sicherheitsfehler = False
        motor_aktiv = False
        reset_erforderlich = False
        motor_freigegeben = False
        aktivierungs_timer = 0
        print("*** SYSTEM ZURÜCKGESETZT ***")
        utime.sleep(0.5)
    
    letzter_reset_zustand = aktueller_reset_zustand


# === Ein/Aus-Taster ===
def handle_ein_aus():
    global motor_aktiv, letzter_ein_aus_zustand
    
    aktueller_ein_aus_zustand = schluesselschalter.value()
    
    # Ein/Aus-Taste wurde gedrückt (fallende Flanke)
    if letzter_ein_aus_zustand and not aktueller_ein_aus_zustand:
        if not sicherheitsfehler and not notaus_aktiv:
            motor_aktiv = not motor_aktiv
            print("Motor:", "EIN" if motor_aktiv else "AUS")
        else:
            print("Motor kann nicht aktiviert werden - Sicherheitsfehler!")
        utime.sleep(0.05)
    
    if motor_aktiv and not taste_aus:
        motor_aktiv = False
        print("Motor:", "EIN" if motor_aktiv else "AUS")

    letzter_ein_aus_zustand = aktueller_ein_aus_zustand


# === Hauptschleife ===
print("System gestartet. Sicherheitsprüfung aktiv.")

while True:
    try:
        # Reset-Behandlung (hat höchste Priorität)
        handle_reset()
        
        # Sicherheitsprüfungen
        sicherheit_ok = pruefe_sicherheit()
        
        # Wenn Reset erforderlich ist, nur Reset-Meldung anzeigen
        if reset_erforderlich:
            print("RESET ERFORDERLICH - Drücken Sie die Reset-Taste!")
            utime.sleep(1)
            continue
        
        # Wenn Sicherheitsfehler vorliegt, warten
        if not sicherheit_ok:
            utime.sleep(0.5)
            continue
        
        # Ein/Aus-Taster behandeln
        handle_ein_aus()
        
        # === Erstmalige Aktivierung durch Joystick ===
        if not motor_freigegeben and motor_aktiv:
            x_val = joystick_x.read_u16()
            if x_val <= (32768 - umkehrzone):  # Joystick nach hinten
                aktivierungs_timer += 1
                restzeit = aktivierungs_dauer - aktivierungs_timer / 10
                if aktivierungs_timer % 10 == 0:  # Nur jede Sekunde ausgeben
                    print("Aktivierung in: {:.1f}s".format(restzeit))
                
                if aktivierungs_timer >= aktivierungs_dauer * 10:
                    motor_freigegeben = True
                    print("*** MOTOR DAUERHAFT FREIGEGEBEN ***")
            else:
                if aktivierungs_timer > 0:
                    aktivierungs_timer = 0
                    print("Aktivierung abgebrochen.")
            
            utime.sleep_ms(100)
            continue
        
        # === Motorsteuerung ===
        if motor_aktiv and sicherheit_ok:
            x_val = joystick_x.read_u16()
            delay = map_adc_to_delay(x_val)
            # Check: limit erreicht?
            if aktuelle_position == max_position:
                limit_aktiv = 1
            else:
                limit_aktiv = 0

            if delay:
                # Joystick Richrung
                richtung = 1 if x_val > 32768 else 0

                move_allowed = True
                #Limit erreicht? (Max oder min)
                if richtung ==1:
                    if aktuelle_position >= max_position:
                        move_allowed = False
                        print("MAX LIMIT erreicht! Nur Rückwärtsbewegung möglich.")
                else:
                    if aktuelle_position <= min_position:
                        move_allowed = False
                        print("MIN LIMIT erreicht! Nur Vorwärtsbewegung möglich.")
                
                
                if move_allowed:
                    dir_pin.value(richtung)
                    step_pin.value(1)
                    utime.sleep_us(delay)
                    step_pin.value(0)
                    if richtung == 1:
                        aktuelle_position +=1
                    else:
                        aktuelle_position -=1

                    print (aktuelle_position)

                

        
                utime.sleep_us(delay)
            else:
                utime.sleep_ms(10)
        else:
            utime.sleep_ms(50)
    
    except KeyboardInterrupt:
        print("*** PROGRAMM BEENDET ***")
        enable_pin.value(1)  # Motor-Treiber deaktivieren
        break