#!/usr/bin/env python3
"""
Linienfolgeroboter für Raspberry Pi 4 mit AlphaBot2 und TRSensor
Verwendet einen PID-Regler und eine Suchfunktion bei Linienverlust.
(Version 3: Korrektur für Stopp nach Liniensuche)
"""
import time
import RPi.GPIO as GPIO
from AlphaBot2 import AlphaBot2
from TRSensors import TRSensor

# Geschwindigkeits- und PID-Einstellungen
MAX_SPEED = 40 # 50
BASE_SPEED = 30 # 40
SEARCH_SPEED = 30  # Geschwindigkeit während der Liniensuche

# PID-Konstanten - DIESE MÜSSEN EVENTUELL ANGEPASST WERDEN!
Kp = 0.045 # 0.025
Ki = 0.0001 # 0.0005
Kd = 0.3

# Zielwert der Sensoren (Mitte der Linie)
TARGET_POSITION = 2000

# Schwelle, ab der ein Sensor die Linie als "verloren" betrachtet
LINE_LOST_THRESHOLD = 950
# Zeitlimit für die Suche in Sekunden
SEARCH_TIMEOUT = 3.0

IR = 17

def setup_gpio():
    """Initialisiert die GPIO-Pins."""
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)
    GPIO.setup(IR, GPIO.IN)


def calibrate_sensors(tr, robot):
    """Führt eine automatische Kalibrierung der Sensoren durch."""
    print("Starte Sensor-Kalibrierung...")
    robot.setPWMA(20)
    robot.setPWMB(20)
    # Drehung zur Kalibrierung auf weißem und schwarzem Untergrund
    for _ in range(2):
        robot.right()
        for _ in range(50):
            tr.calibrate()
            time.sleep(0.02)
        robot.left()
        for _ in range(100):
            tr.calibrate()
            time.sleep(0.02)
        robot.right()
        for _ in range(50):
            tr.calibrate()
            time.sleep(0.02)
    robot.stop()
    print("Kalibrierung abgeschlossen.")
    print("  Min:", tr.calibratedMin)
    print("  Max:", tr.calibratedMax)


def search_for_line(robot, tr, last_error):
    """
    Sucht die Linie, indem der Roboter sich in die Richtung dreht,
    in der die Linie zuletzt war.
    """
    print("Linie verloren! Suche...")
    robot.stop()
    time.sleep(0.1)

    # Setze zuerst die Geschwindigkeit für beide Motoren
    robot.setPWMA(SEARCH_SPEED)
    robot.setPWMB(SEARCH_SPEED)

    # Bestimme dann die Suchrichtung
    if last_error > 0:
        print("Suche nach links...")
        robot.left()
    else:
        print("Suche nach rechts...")
        robot.right()

    start_time = time.time()
    while time.time() - start_time < SEARCH_TIMEOUT:
        # Lies kontinuierlich die Sensoren, während du dich drehst
        _, sensors = tr.readLine(white_line=False)

        # Prüfe, ob einer der Sensoren die Linie wieder sieht
        if any(s < LINE_LOST_THRESHOLD for s in sensors):
            print("Linie wiedergefunden!")
            robot.stop()
            time.sleep(0.1)
            return True  # Suche war erfolgreich

        time.sleep(0.01)

    # Schleife beendet, ohne die Linie zu finden (Timeout)
    print(f"Suche nach {SEARCH_TIMEOUT}s erfolglos. Roboter stoppt.")
    robot.stop()
    return False  # Suche war erfolglos


def line_follow_loop(tr, robot):
    """Hauptschleife für die Linienverfolgung mittels PID-Regelung."""
    # Setzt die anfängliche Richtung auf vorwärts
    robot.forward()

    last_error = 0
    integral = 0

    while True:
        # Lese die Position der Linie. False für schwarze Linie auf weißem Grund.
        position, sensors = tr.readLine(white_line=False)

        # Wenn alle Sensoren weiß sehen, starte die Liniensuche
        if all(s > LINE_LOST_THRESHOLD for s in sensors):
            search_successful = search_for_line(robot, tr, last_error)
            if search_successful:
                # Setze PID-Werte zurück, um einen Sprung zu vermeiden
                last_error = 0
                integral = 0
                # WICHTIG: Setze die Richtung wieder auf vorwärts, bevor die Schleife weitergeht
                robot.forward()
                continue
            else:
                # Beende die Hauptschleife, wenn die Suche fehlschlägt
                break

                # --- PID-Berechnung ---
        error = position - TARGET_POSITION
        integral += error
        derivative = error - last_error
        last_error = error

        correction = (Kp * error) + (Ki * integral) + (Kd * derivative)

        # Anpassung der Motor-Geschwindigkeiten
        left_speed = BASE_SPEED + correction
        right_speed = BASE_SPEED - correction

        # Geschwindigkeiten auf den gültigen Bereich begrenzen
        left_speed = max(0, min(MAX_SPEED, left_speed))
        right_speed = max(0, min(MAX_SPEED, right_speed))

        # print(f"Pos: {position:.0f} | Err: {error:.0f} | Corr: {correction:.2f} | L: {left_speed:.0f} | R: {right_speed:.0f}")

        robot.setPWMA(int(left_speed))
        robot.setPWMB(int(right_speed))

        time.sleep(0.01)


def follow(tr , robot):
    """Hauptfunktion des Programms."""
    #robot = AlphaBot2()
    #tr = TRSensor()
    #robot.stop()

    try:
        calibrate_sensors(tr, robot)
        print("Kalibrierung abgeschlossen")
        time.sleep(5.0)
        #input("Kalibrierung abgeschlossen. Drücke Enter zum Starten...")
        line_follow_loop(tr, robot)
    except KeyboardInterrupt:
        print("\nProgramm gestoppt.")
    finally:
        robot.stop()

if __name__ == "__main__":
    setup_gpio()
    robot = AlphaBot2()
    tr = TRSensor()
    robot.stop()
    follow(tr, robot)
    print("Räume GPIO auf.")
    GPIO.cleanup()