import time
import pyads

# ==========================================
# KONFIGURATION
# ==========================================
AMS_NET_ID = "5.17.17.136.1.1"
PLC_PORT = 851  # Standard SPS-Port (Port 851)

# Positionen & Parameter
POS_1 = 110.0      # Erster Zielpunkt [mm]
POS_2 = 10.0       # Zweiter Zielpunkt [mm]
VELOCITY = 50.0    # Geschwindigkeit [mm/s]
OVERRIDE = 100.0   # Override in %
PAUSE_TIME = 30.0  # Wartezeit im ausgeschalteten Zustand [s]

# ==========================================
# HILFSFUNKTIONEN
# ==========================================
def set_axis_power(plc, enable: bool):
    """Schaltet fbPower über die GVL-Variablen ein oder aus."""
    state_str = "EIN" if enable else "AUS"
    print(f"--> Schalte Achsfreigabe (Power): {state_str}")
    
    plc.write_by_name('GVL.Axis1_Override', float(OVERRIDE), pyads.PLCTYPE_LREAL)
    plc.write_by_name('GVL.Axis1_FeedFw', True, pyads.PLCTYPE_BOOL)
    plc.write_by_name('GVL.Axis1_FeedBw', True, pyads.PLCTYPE_BOOL)
    plc.write_by_name('GVL.Axis1_CmdPower', enable, pyads.PLCTYPE_BOOL)
    
    # Einschwingzeit für Bestromung / Bremsenlüftung
    time.sleep(0.3)

def move_to_position(plc, target_pos: float, velocity: float):
    """Setzt Zielparameter, erzeugt eine Flanke auf CmdMoveAbs und wartet auf Axis1_MoveDone."""
    print(f"--> Starte Positionierung auf {target_pos:.1f} mm (Speed: {velocity:.1f} mm/s)...")
    
    # 1. Zielwerte in die GVL schreiben
    plc.write_by_name('GVL.Axis1_TargetPos', float(target_pos), pyads.PLCTYPE_LREAL)
    plc.write_by_name('GVL.Axis1_Velocity', float(velocity), pyads.PLCTYPE_LREAL)

    # 2. Positive Flanke für rTrigMove in der SPS erzeugen (HIGH -> LOW)
    plc.write_by_name('GVL.Axis1_CmdMoveAbs', True, pyads.PLCTYPE_BOOL)
    time.sleep(0.1)
    plc.write_by_name('GVL.Axis1_CmdMoveAbs', False, pyads.PLCTYPE_BOOL)

    # 3. Warten auf das gelatchte Signal GVL.Axis1_MoveDone
    while True:
        is_busy = plc.read_by_name('GVL.Axis1_Busy', pyads.PLCTYPE_BOOL)
        is_move_done = plc.read_by_name('GVL.Axis1_MoveDone', pyads.PLCTYPE_BOOL)
        is_error = plc.read_by_name('GVL.Axis1_Error', pyads.PLCTYPE_BOOL)

        # Erfolgreicher Abschluss: Achse nicht mehr Busy UND MoveDone ist TRUE
        if not is_busy and is_move_done:
            print(f"    Zielposition {target_pos:.1f} mm erfolgreich erreicht.")
            break

        if is_error:
            err_id = plc.read_by_name('GVL.Axis1_ErrorID', pyads.PLCTYPE_UDINT)
            raise RuntimeError(f"SPS/NC-Fehler bei der Fahrt! ErrorID: {err_id}")

        time.sleep(0.05)

# ==========================================
# HAUPTPROGRAMM (ENDLOSSCHLEIFE)
# ==========================================
def main():
    plc = pyads.Connection(AMS_NET_ID, PLC_PORT)

    try:
        plc.open()
        print(f"Verbindung zu TwinCAT PLC ({AMS_NET_ID}:{PLC_PORT}) erfolgreich hergestellt.")
        print("Starte Ablaufsequenz. Beenden jederzeit mit Strg+C.\n")

        cycle = 1
        while True:
            print(f"--- ZYKLUS {cycle} ---")

            # === TEIL 1: Fahrt auf 110 mm ===
            set_axis_power(plc, enable=True)
            move_to_position(plc, target_pos=POS_1, velocity=VELOCITY)
            set_axis_power(plc, enable=False)
            
            print(f"--> Warte {PAUSE_TIME:.0f} Sekunden...")
            time.sleep(PAUSE_TIME)

            # === TEIL 2: Fahrt auf 10 mm ===
            set_axis_power(plc, enable=True)
            move_to_position(plc, target_pos=POS_2, velocity=VELOCITY)
            set_axis_power(plc, enable=False)
            
            print(f"--> Warte {PAUSE_TIME:.0f} Sekunden...\n")
            time.sleep(PAUSE_TIME)

            cycle += 1

    except KeyboardInterrupt:
        print("\n[ABBRUCH] Sequenz durch Benutzer gestoppt.")
    except Exception as e:
        print(f"\n[FEHLER] Laufzeitfehler: {e}")
    finally:
        # Sicherheitsschaltung beim Beenden oder Abbrechen
        try:
            plc.write_by_name('GVL.Axis1_CmdPower', False, pyads.PLCTYPE_BOOL)
            plc.write_by_name('GVL.Axis1_CmdMoveAbs', False, pyads.PLCTYPE_BOOL)
            print("Sicherheitszustand hergestellt (Achsfreigabe entzogen).")
        except Exception:
            pass
        
        plc.close()
        print("ADS-Verbindung geschlossen.")

if __name__ == '__main__':
    main()