#!/usr/bin/env python3
"""
Line following robot for Raspberry Pi 4 using AlphaBot2 and TRSensor
Ohne LED-Visualisierung
"""
import time
import RPi.GPIO as GPIO
from AlphaBot2 import AlphaBot2
from TRSensors import TRSensor

MAX_SPEED = 25
CORRECTION_STRENGTH = 8
EXTRA_CURVE_CORRECTION = 2
LINE_DETECTED_THRESHOLD = 650

# Liste für gleitenden Durchschnitt
last_outer = []

def setup_gpio():
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)

def calibrate_sensors(tr, robot):
    print("Auto-calibrating sensors...")

    for i in range(5):
        tr.calibratedMin[i] = 1023
        tr.calibratedMax[i] = 0

    for _ in range(3):
        robot.setPWMA(20)
        robot.setPWMB(15)
        robot.forward()
        for _ in range(30):
            raw = tr.AnalogRead()
            for i in range(5):
                tr.calibratedMin[i] = min(tr.calibratedMin[i], raw[i])
                tr.calibratedMax[i] = max(tr.calibratedMax[i], raw[i])
            time.sleep(0.02)

        robot.backward()
        for _ in range(30):
            raw = tr.AnalogRead()
            for i in range(5):
                tr.calibratedMin[i] = min(tr.calibratedMin[i], raw[i])
                tr.calibratedMax[i] = max(tr.calibratedMax[i], raw[i])
            time.sleep(0.02)

    robot.stop()
    print("Calibration complete.")
    print("  Min:", tr.calibratedMin)
    print("  Max:", tr.calibratedMax)

def detect_line_type(tr):
    sensor_values = tr.readCalibrated()
    center_avg = sum(sensor_values[1:4]) / 3
    outer_avg = (sensor_values[0] + sensor_values[4]) / 2
    white_line = outer_avg < center_avg
    print("Detected line type:", "WHITE on BLACK" if white_line else "BLACK on WHITE")
    return white_line

def compute_correction(sensors):
    left = sensors[1]
    right = sensors[3]
    correction = 0

    if abs(left - right) < 50:
        correction = 0
    elif left < LINE_DETECTED_THRESHOLD and right > LINE_DETECTED_THRESHOLD:
        correction = -CORRECTION_STRENGTH
    elif right < LINE_DETECTED_THRESHOLD and left > LINE_DETECTED_THRESHOLD:
        correction = CORRECTION_STRENGTH

    if sensors[0] < LINE_DETECTED_THRESHOLD:
        correction -= EXTRA_CURVE_CORRECTION
    elif sensors[4] < LINE_DETECTED_THRESHOLD:
        correction += EXTRA_CURVE_CORRECTION

    return correction

def perform_turn_if_needed(sensors, robot, position):
    global last_outer
    l = sensors[0]
    r = sensors[4]
    last_outer.append((l, r))
    if len(last_outer) > 5:
        last_outer.pop(0)

    avg_l = sum(x for x, _ in last_outer) / len(last_outer)
    avg_r = sum(y for _, y in last_outer) / len(last_outer)
    delta = avg_r - avg_l

    print(f"Turn-Check: AvgL={avg_l:.0f}, AvgR={avg_r:.0f}, Δ={delta:.0f}, pos={position:.1f}")

    if not (1500 <= position <= 2500):
        return False
    if avg_l > 800 and avg_r > 800:
        return False

    if avg_r > 800 and avg_l < 500 and delta > 400:
        print("→ 90° right-curve detected")
        robot.stop()
        time.sleep(0.1)
        robot.right()
        time.sleep(0.3)
        robot.forward()
        time.sleep(0.35)
        return True
    elif avg_l > 800 and avg_r < 500 and delta < -600 and sensors[2] < LINE_DETECTED_THRESHOLD:
        print("← 90° left-curve detected")
        robot.stop()
        time.sleep(0.1)
        robot.left()
        time.sleep(0.3)
        robot.forward()
        time.sleep(0.35)
        return True

    return False

def line_follow_loop(tr, robot, white_line):
    robot.forward()

    while True:
        position, sensors = tr.readLine(white_line=white_line)

        print(f"Sensors: {sensors}, pos={position:.1f}")

        if perform_turn_if_needed(sensors, robot, position):
            robot.forward()
            time.sleep(0.35)
            robot.stop()
            break

        if all(s > LINE_DETECTED_THRESHOLD for s in sensors):
            time.sleep(0.05)
            robot.backward()
            time.sleep(0.35)
            robot.stop()
            continue

        correction = compute_correction(sensors)
        left_speed = min(MAX_SPEED, max(0, MAX_SPEED + correction))
        right_speed = min(MAX_SPEED, max(0, MAX_SPEED - correction))

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

        time.sleep(0.02)

def line_follow(tr=None, robot=None):
    if tr is None or robot is None:
        setup_gpio()
        robot = AlphaBot2()
        tr = TRSensor()
        robot.stop()
        calibrate_sensors(tr, robot)

    white_line = detect_line_type(tr)

    print("Starting line follow...")
    try:
        line_follow_loop(tr, robot, white_line)
    except KeyboardInterrupt:
        print("Stopping and cleaning up...")
        robot.stop()
        GPIO.cleanup()

if __name__ == "__main__":
    setup_gpio()
    robot = AlphaBot2()
    tr = TRSensor()
    robot.stop()

    calibrate_sensors(tr, robot)
    input("Press Enter to continue...")
    line_follow(tr, robot)