using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using System.Net.Http;
using System.Text.Json;
using OpenCvSharp;
using OpenCvSharp.Aruco;

// ================================================================
// LOGGING: komplette Konsolenausgabe zusätzlich in Datei
// ================================================================

// Offline-Modus wird vor Kamera, Bluetooth und Ollama abgefangen.
// --replay [Datei oder Ordner ...]; ohne Pfad: eingebaute Logversuche.
if (args.Length > 0 && args[0].Equals("--strategy-test", StringComparison.OrdinalIgnoreCase))
{
    StrategyRegression.Run();
    return;
}
bool offlineRequested = args.Length > 0 && args[0].Equals("--replay", StringComparison.OrdinalIgnoreCase);
if (args.Length == 0)
{
    Console.WriteLine("EV3 V7: T = Offline-Test der eingebauten Logs, ENTER = Roverbetrieb");
    offlineRequested = string.Equals(Console.ReadLine()?.Trim(), "T", StringComparison.OrdinalIgnoreCase);
}
if (offlineRequested)
{
    try { OfflineReplay.Run(args.Skip(1).ToArray()); }
    catch (Exception ex) { Console.WriteLine("Offline-Test fehlgeschlagen: " + ex.Message); Environment.ExitCode = 1; }
    if (args.Length == 0) { Console.WriteLine("ENTER zum Beenden."); Console.ReadLine(); }
    return;
}

TextWriter originalOut = Console.Out;
TextWriter originalError = Console.Error;

string logFile = Path.Combine(
    AppContext.BaseDirectory,
    $"EV3_Log_{DateTime.Now:yyyyMMdd_HHmmss}.txt");

using StreamWriter logWriter = new(logFile, append: false, Encoding.UTF8)
{
    AutoFlush = true
};

Console.SetOut(new TeeTextWriter(originalOut, logWriter));
Console.SetError(new TeeTextWriter(originalError, logWriter));

Console.WriteLine("EV3 - kamerageregelter autonomer Farbsortierer / Gezielte Paartrennung V7 / Gemma mit Rueckfragen");
Console.WriteLine("=====================================================");
Console.WriteLine($"Logdatei: {logFile}");
Console.WriteLine();

try
{
    List<TargetZone> zones = TargetZoneFactory.CreateDefaultZones();

    PrintTargetZones(zones);

    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("KAMERA STARTEN UND KALIBRIEREN");
    Console.WriteLine("=====================================================");
    Console.WriteLine();

    using VisionSystem vision = new(
        cameraIndex: 0,
        expectedRedBoxes: 2,
        expectedBlueBoxes: 2);

    vision.CalibrateInteractively();

    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("STARTPOSITION FESTLEGEN");
    Console.WriteLine("=====================================================");
    Console.WriteLine(
        "H ist abgeschlossen. Der Rover darf jetzt beliebig umgesetzt " +
        "und gedreht werden.");
    Console.WriteLine(
        "ENTER erst drücken, wenn die gewünschte Startposition erreicht ist.");
    Console.ReadLine();

    Console.WriteLine();
    Console.WriteLine("Erste automatische Weltmessung ...");

    VisionSnapshot firstSnapshot =
        vision.MeasureScene();

    WorldModel world =
        BuildWorldFromVision(
            firstSnapshot,
            zones);

    ValidateCapacity(world);

    Console.WriteLine();
    Console.WriteLine("STARTWELT AUS KAMERA");
    Console.WriteLine("====================");
    PrintWorld(world);

    using Ev3Rover rover = new("COM3");
    rover.Connect();

    int actionNumber = 1;
    int consecutiveFreePushes = 0;

    while (world.Boxes.Any(b => !b.Sorted))
    {
        Console.WriteLine();
        Console.WriteLine("=====================================================");
        Console.WriteLine($"PLANUNG FÜR AKTION {actionNumber}");
        Console.WriteLine("=====================================================");
        Console.WriteLine();
        Console.WriteLine("Verwendet wird die zuletzt real gemessene Kamerawelt.");

        AgentActionCandidate action;
        try
        {
            action = TaskPlanner.ChooseNext(world);
        }
        catch (PlanningUnavailableException ex)
        {
            Console.WriteLine("\nPLANUNGSPAUSE - kein Motorbefehl wird ausgefuehrt.");
            Console.WriteLine(ex.Message);
            Console.WriteLine("ENTER: Kamera neu messen und erneut planen. Q: Lauf beenden.");
            string? answer = Console.ReadLine();
            if (answer == null || answer.Trim().Equals("Q", StringComparison.OrdinalIgnoreCase)) break;
            RefreshWorldFromVision(world, vision.MeasureScene());
            RevalidateAllSortedBoxes(world);
            PrintWorld(world);
            continue;
        }

        PrintAction(action);

        if (action.Kind == AgentActionKind.FreePush)
        {
            consecutiveFreePushes++;

            const int MaximumConsecutiveFreePushes = 12;
            if (consecutiveFreePushes > MaximumConsecutiveFreePushes)
            {
                Console.WriteLine($"PLANUNGSPAUSE: {MaximumConsecutiveFreePushes} Freischuebe erreicht.");
                Console.WriteLine("ENTER: weitere Versuche zulassen und neu planen. Q: Lauf beenden.");
                string? answer = Console.ReadLine();
                if (answer == null || answer.Trim().Equals("Q", StringComparison.OrdinalIgnoreCase)) break;
                consecutiveFreePushes = 0;
                RefreshWorldFromVision(world, vision.MeasureScene());
                RevalidateAllSortedBoxes(world);
                continue;
            }
            Console.WriteLine($"KI-Freischub {consecutiveFreePushes}/{MaximumConsecutiveFreePushes}");
        }
        else
        {
            consecutiveFreePushes = 0;
            FreePushStrategy.Reset();
        }

        Dictionary<int, Point2D> positionsBeforeAction =
            world.Boxes.ToDictionary(
                b => b.Id,
                b => b.Position);

        Console.WriteLine();
        Console.WriteLine("ENTER zum Ausführen dieses Plans.");
        Console.ReadLine();

        VisionSnapshot measuredAfterAction =
            ExecuteAction(
                rover,
                vision,
                action);

        // ------------------------------------------------------------
        // Für die Zuordnung der farbgleichen Kisten setzen wir die
        // bewegte Kiste zunächst auf ihre erwartete Zielposition.
        // Unmittelbar danach überschreibt die Kamera ALLE Positionen
        // mit der real gemessenen Welt.
        // ------------------------------------------------------------

        if (action.Kind != AgentActionKind.FreePush)
        {
            action.Box.Position =
                action.Destination;
        }

        RefreshWorldFromVision(
            world,
            measuredAfterAction);

        Console.WriteLine();
        Console.WriteLine("REAL GEMESSENE WELT NACH DER AKTION");
        Console.WriteLine("-----------------------------------");

        if (action.Kind == AgentActionKind.Sort)
        {
            ConfirmSortResultFromCamera(
                world,
                action);
        }
        else if (action.Kind == AgentActionKind.Relocate)
        {
            Console.WriteLine();
            Console.WriteLine("=====================================================");
            Console.WriteLine("KAMERA-BESTÄTIGUNG DER TEMPORÄREN VERSCHIEBUNG");
            Console.WriteLine("=====================================================");
            Console.WriteLine(
                $"Kiste #{action.Box.Id}: real bei " +
                $"({action.Box.Position.X:F2}, {action.Box.Position.Y:F2})");
            Console.WriteLine(
                $"Zwischenziel war " +
                $"({action.Destination.X:F2}, {action.Destination.Y:F2})");
            Console.WriteLine(
                $"Abweichung = " +
                $"{Geometry2D.Distance(action.Box.Position, action.Destination):F2} cm");
        }
        else
        {
            Console.WriteLine();
            Console.WriteLine("=====================================================");
            Console.WriteLine("KAMERA-AUSWERTUNG DES KI-FREISCHUBS");
            Console.WriteLine("=====================================================");

            foreach (BoxState box in world.Boxes.OrderBy(b => b.Id))
            {
                Point2D before =
                    positionsBeforeAction[box.Id];

                double moved =
                    Geometry2D.Distance(
                        before,
                        box.Position);

                Console.WriteLine(
                    $"Kiste #{box.Id} / {ColorName(box.Color)}: " +
                    $"({before.X:F2}, {before.Y:F2}) -> " +
                    $"({box.Position.X:F2}, {box.Position.Y:F2}), " +
                    $"Verschiebung {moved:F2} cm");
            }

            Console.WriteLine(
                "Danach wird die komplette reale Kamerawelt erneut " +
                "dem normalen TaskPlanner übergeben.");
        }

        // Auch bereits sortierte Kisten werden überwacht.
        // Wird später eine Kiste versehentlich deutlich verschoben,
        // wird sie wieder freigegeben und erneut eingeplant.
        RevalidateAllSortedBoxes(
            world);

        if (action.Kind == AgentActionKind.FreePush)
            OllamaDeadlockSolver.RecordOutcome(world, action, positionsBeforeAction);

        PrintWorld(world);

        actionNumber++;
    }

    Console.WriteLine();
    Console.WriteLine("*****************************************************");
    Console.WriteLine(world.Boxes.All(b => b.Sorted)
        ? "ALLE KISTEN SIND SORTIERT"
        : "LAUF BEENDET - ES SIND NOCH KISTEN OFFEN");
    Console.WriteLine("*****************************************************");
}
catch (Exception ex)
{
    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("FEHLER - PROGRAMM WIRD ANGEHALTEN");
    Console.WriteLine("=====================================================");
    Console.WriteLine();
    Console.WriteLine(ex.ToString());
    Console.WriteLine();
    Console.WriteLine($"Logdatei: {logFile}");
    Console.WriteLine();
    Console.WriteLine("ENTER drücken, wenn die Fehlermeldung notiert wurde.");
    Console.ReadLine();
}

Console.WriteLine();
Console.WriteLine($"Logdatei: {logFile}");
Console.WriteLine("ENTER zum Beenden.");
Console.ReadLine();


// ================================================================
// AKTION AUSFÜHREN
// ================================================================

static VisionSnapshot ExecuteAction(
    Ev3Rover rover,
    VisionSystem vision,
    AgentActionCandidate action)
{
    if (action.Kind == AgentActionKind.FreePush)
    {
        return ExecuteFreePushAction(
            rover,
            vision,
            action);
    }

    PushPlan plan =
        action.Plan;

    Console.WriteLine();
    Console.WriteLine("#####################################################");
    Console.WriteLine("CLOSED-LOOP-AUSFÜHRUNG MIT KAMERA");
    Console.WriteLine("#####################################################");
    Console.WriteLine(
        "Nach jeder Navigationsetappe wird gestoppt, gemessen und korrigiert.");
    Console.WriteLine(
        "Vor dem Kistenkontakt werden Q, P und Schubrichtung aus der Kamera neu berechnet.");


    // ============================================================
    // PHASE 1
    // Jede geplante Navigationetappe wird jetzt kamerageregelt:
    //
    // 1. aktuelle Roverpose messen
    // 2. Drehung zum Wegpunkt aus IST-Pose neu berechnen
    // 3. drehen
    // 4. Kamera kontrolliert den Winkel
    // 5. reale Restdistanz neu bestimmen
    // 6. fahren
    // 7. Kamera kontrolliert den Wegpunkt
    // 8. ggf. kleine Positionskorrektur
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 1 - KAMERAGEFÜHRTE NAVIGATION ZUM Q");
    Console.WriteLine("------------------------------------------");

    for (int i = 0;
         i < plan.Navigation.Legs.Count;
         i++)
    {
        NavigationLeg leg =
            plan.Navigation.Legs[i];

        Console.WriteLine();
        Console.WriteLine(
            $"ETAPPE {i + 1}/{plan.Navigation.Legs.Count}");
        Console.WriteLine(
            $"Plan-Wegpunkt: ({leg.To.X:F2}, {leg.To.Y:F2})");

        ClosedLoopMoveToPoint(
            rover,
            vision,
            leg.To,
            $"NAVIGATION ETAPPE {i + 1}",
            plannedTurnDeg: leg.TurnDeg,
            plannedDistanceCm: leg.DistanceCm,
            // Zwischen-Wegpunkte brauchen keine Millimeterpräzision.
            // Unter 2 cm Restfehler vermeiden wir unnötige große
            // Wendemanöver um einen fast schon erreichten Punkt.
            toleranceCm: 2.0,
            maximumCorrectionCm: 12.0);
    }


    // ============================================================
    // PHASE 1b
    // Am geplanten Q wird die Kiste selbst nochmals gemessen.
    // Daraus berechnen wir einen NEUEN realen Q und P.
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 1b - KAMERA-NEUBERECHNUNG VON Q UND P");
    Console.WriteLine("-------------------------------------------");

    VisionSnapshot qSnapshot =
        vision.MeasureScene(
            printSnapshot: false);

    VisionDetectedBox activeAtQ =
        FindActiveBoxInSnapshot(
            qSnapshot,
            action.Box.Color,
            action.Box.Position);

    CameraPushGeometry cameraGeometry =
        BuildCameraPushGeometry(
            activeAtQ.Position,
            action.Destination);

    PrintCameraPushGeometry(
        action,
        plan,
        qSnapshot,
        cameraGeometry);


    // Falls sich die aus der Kamera neu berechnete Q-Position vom
    // alten Planner-Q unterscheidet oder der Rover nicht exakt dort
    // angekommen ist, wird Q jetzt real angefahren.
    ClosedLoopMoveToPoint(
        rover,
        vision,
        cameraGeometry.PreApproachPoint,
        "Q-FEINKORREKTUR",
        plannedTurnDeg: null,
        plannedDistanceCm: null,
        toleranceCm: 1.2,
        maximumCorrectionCm: 10.0);


    // Nach der Q-Korrektur Kiste nochmals messen. So verwenden wir
    // für die letzten Zentimeter garantiert die aktuellste Geometrie.
    qSnapshot =
        vision.MeasureScene(
            printSnapshot: false);

    activeAtQ =
        FindActiveBoxInSnapshot(
            qSnapshot,
            action.Box.Color,
            activeAtQ.Position);

    cameraGeometry =
        BuildCameraPushGeometry(
            activeAtQ.Position,
            action.Destination);


    // ============================================================
    // PHASE 2
    // Am REALEN Q auf die REAL gemessene Schubrichtung drehen.
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 2 - KAMERAGEFÜHRT AUSRICHTEN");
    Console.WriteLine("----------------------------------");

    AlignHeadingClosedLoop(
        rover,
        vision,
        cameraGeometry.PushHeadingDeg,
        "SCHUBRICHTUNG AM Q",
        plannedTurnDeg: plan.FinalTurnDeg);


    // ============================================================
    // PHASE 2b
    // Q -> P wird nicht mehr blind mit pauschal 5 cm gefahren.
    // P wird aus der aktuellen Kistenposition bestimmt und mit
    // Kamera kontrolliert angefahren.
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 2b - KAMERAGEFÜHRTER ENDANFLUG Q -> P");
    Console.WriteLine("--------------------------------------------");

    ClosedLoopMoveToPoint(
        rover,
        vision,
        cameraGeometry.ApproachPoint,
        "ENDANFLUG ZUM P",
        plannedTurnDeg: 0.0,
        plannedDistanceCm: plan.FinalApproachCm,
        toleranceCm: 1.0,
        maximumCorrectionCm: 8.0);


    // ============================================================
    // KONTROLLPUNKT P
    //
    // WICHTIG:
    // Am Punkt P kann der Rover die farbige Markierung der aktiven
    // Kiste teilweise verdecken. Deshalb verlangen wir hier KEINE
    // erneute Kistenerkennung mehr.
    //
    // Die Kiste wurde am Q zuletzt sicher gemessen und hat sich
    // zwischen Q und P physikalisch noch nicht bewegt. Für P,
    // Ausrichtung und Kontakt verwenden wir daher diese letzte
    // sichere Kistenposition weiter. Nur der Rover wird am P erneut
    // gemessen.
    // ============================================================

    RoverPose pRover =
        vision.MeasureRoverPose();

    VisionDetectedBox activeBeforeContact =
        activeAtQ;

    CameraPushGeometry finalGeometry =
        BuildCameraPushGeometry(
            activeBeforeContact.Position,
            action.Destination);


    double pError =
        Geometry2D.Distance(
            new Point2D(
                pRover.X,
                pRover.Y),
            finalGeometry.ApproachPoint);

    if (pError > 1.0)
    {
        Console.WriteLine();
        Console.WriteLine(
            $"P-NACHKORREKTUR: Kamera sieht noch {pError:F2} cm Positionsfehler.");

        if (pError > 8.0)
        {
            throw new InvalidOperationException(
                $"P-Fehler {pError:F2} cm ist für eine sichere Feinkorrektur zu groß.");
        }

        ClosedLoopMoveToPoint(
            rover,
            vision,
            finalGeometry.ApproachPoint,
            "P-NACHKORREKTUR",
            plannedTurnDeg: null,
            plannedDistanceCm: null,
            toleranceCm: 1.0,
            maximumCorrectionCm: 8.0);

        pRover =
            vision.MeasureRoverPose();
    }


    // Vor Kontakt nochmals exakt die Schubrichtung herstellen.
    RoverPose alignedRover =
        AlignHeadingClosedLoop(
            rover,
            vision,
            finalGeometry.PushHeadingDeg,
            "LETZTE AUSRICHTUNG VOR KONTAKT",
            plannedTurnDeg: null);


    ContactGeometry contact =
        CalculateContactGeometry(
            alignedRover,
            activeBeforeContact.Position,
            finalGeometry.PushHeadingDeg);


    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("KAMERA-KONTROLLPUNKT VOR KONTAKT");
    Console.WriteLine("=====================================================");
    Console.WriteLine(
        $"Kiste #{action.Box.Id} letzte sichere Messung am Q = " +
        $"({activeBeforeContact.Position.X:F2}, {activeBeforeContact.Position.Y:F2})");
    Console.WriteLine(
        $"Rover real = " +
        $"({alignedRover.X:F2}, {alignedRover.Y:F2}), " +
        $"{alignedRover.HeadingDeg:F2}°");
    Console.WriteLine(
        $"Soll-P aus Kamera = " +
        $"({finalGeometry.ApproachPoint.X:F2}, {finalGeometry.ApproachPoint.Y:F2})");
    Console.WriteLine(
        $"Schubrichtung aus Kamera = {finalGeometry.PushHeadingDeg:F2}°");
    Console.WriteLine(
        $"Seitlicher Fehler zur Schublinie = {contact.LateralErrorCm:F2} cm");
    Console.WriteLine(
        $"Kamera-berechneter Restweg bis Kontakt = {contact.ContactTravelCm:F2} cm");
    Console.WriteLine(
        $"Alter Planner-Kontaktweg = {plan.ContactCm:F2} cm");
    Console.WriteLine(
        $"KONTAKT-WEGKORREKTUR = " +
        $"{contact.ContactTravelCm - plan.ContactCm:+0.00;-0.00;0.00} cm");


    if (contact.LateralErrorCm >
        3.0)
    {
        throw new InvalidOperationException(
            $"Seitlicher Fehler vor Kistenkontakt ist mit " +
            $"{contact.LateralErrorCm:F2} cm zu groß.");
    }

    if (contact.ContactTravelCm <
            0.0 ||
        contact.ContactTravelCm >
            12.0)
    {
        throw new InvalidOperationException(
            $"Unplausibler kameraberechneter Kontaktweg: " +
            $"{contact.ContactTravelCm:F2} cm.");
    }


    Console.WriteLine();
    Console.WriteLine(
        "ENTER zum Kontakt und Schieben.");
    Console.ReadLine();


    // ============================================================
    // PHASE 3
    // Kontaktweg jetzt aus der Kamera-Geometrie, nicht blind 6,5 cm.
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 3 - KAMERABERECHNETER KONTAKT");
    Console.WriteLine("-----------------------------------");

    rover.ForwardCm(
        contact.ContactTravelCm);


    // ============================================================
    // PHASE 4
    // Die Schubdistanz wird ebenfalls aus der zuletzt gemessenen
    // realen Kistenposition zum festen Ziel neu bestimmt.
    // ============================================================

    double cameraPushDistance =
        Geometry2D.Distance(
            activeBeforeContact.Position,
            action.Destination);

    Console.WriteLine();
    Console.WriteLine(
        action.Kind == AgentActionKind.Sort
            ? "PHASE 4 - INS ZIEL SCHIEBEN"
            : "PHASE 4 - TEMPORÄR VERSCHIEBEN");

    Console.WriteLine(
        $"Planner-Schubweg = {plan.PushCm:F2} cm");
    Console.WriteLine(
        $"Kamera-Schubweg  = {cameraPushDistance:F2} cm");
    Console.WriteLine(
        $"SCHUB-WEGKORREKTUR = " +
        $"{cameraPushDistance - plan.PushCm:+0.00;-0.00;0.00} cm");

    rover.ForwardCm(
        cameraPushDistance);


    // ============================================================
    // PHASE 5
    // Rückzug bleibt mechanisch definiert.
    // Danach liefert die Kamera den realen Endzustand.
    // ============================================================

    Console.WriteLine();
    Console.WriteLine("PHASE 5 - RÜCKZUG");

    rover.BackwardCm(
        plan.RetreatCm);


    Console.WriteLine();
    Console.WriteLine("KAMERA-ENDKONTROLLE DER AKTION");
    Console.WriteLine("------------------------------");

    VisionSnapshot finalSnapshot =
        vision.MeasureScene(
            printSnapshot: true);

    return finalSnapshot;
}




// ================================================================
// KI-FREISCHUB AUSFÜHREN
//
// Ollama schlägt nur eine Strategie vor.
// C# berechnet und prüft die Geometrie und führt kamerageregelt aus.
// ================================================================

static VisionSnapshot ExecuteFreePushAction(
    Ev3Rover rover,
    VisionSystem vision,
    AgentActionCandidate action)
{
    FreePushData data =
        action.FreePush
        ?? throw new InvalidOperationException(
            "FREE_PUSH ohne FreePushData.");

    PushPlan plan =
        action.Plan;

    Console.WriteLine();
    Console.WriteLine("#####################################################");
    Console.WriteLine("EXPERIMENTELLER KI-FREISCHUB MIT KAMERA");
    Console.WriteLine("#####################################################");
    Console.WriteLine(
        "Ollama liefert nur Strategieparameter; " +
        "C# bleibt Sicherheits- und Ausführungsinstanz.");


    Console.WriteLine();
    Console.WriteLine("PHASE F1 - KAMERAGEFÜHRTE NAVIGATION ZUM Q");
    Console.WriteLine("--------------------------------------------");

    for (int i = 0;
         i < plan.Navigation.Legs.Count;
         i++)
    {
        NavigationLeg leg =
            plan.Navigation.Legs[i];

        Console.WriteLine();
        Console.WriteLine(
            $"ETAPPE {i + 1}/{plan.Navigation.Legs.Count}");
        Console.WriteLine(
            $"Plan-Wegpunkt: ({leg.To.X:F2}, {leg.To.Y:F2})");

        ClosedLoopMoveToPoint(
            rover,
            vision,
            leg.To,
            $"FREE_PUSH NAVIGATION {i + 1}",
            plannedTurnDeg: leg.TurnDeg,
            plannedDistanceCm: leg.DistanceCm,
            toleranceCm: 2.0,
            maximumCorrectionCm: 12.0);
    }


    VisionSnapshot qSnapshot =
        vision.MeasureScene(
            printSnapshot: false);

    VisionDetectedBox primaryAtQ =
        FindActiveBoxInSnapshot(
            qSnapshot,
            action.Box.Color,
            data.PrimaryReferencePosition);

    FreePushGeometry geometry =
        FreePushPlanner.CalculateGeometry(
            primaryAtQ.Position,
            data.PushHeadingDeg,
            data.LateralOffsetCm,
            data.PushDistanceCm);


    Console.WriteLine();
    Console.WriteLine("PHASE F2 - FREE_PUSH-GEOMETRIE AUS KAMERA");
    Console.WriteLine("------------------------------------------");
    Console.WriteLine(
        $"Primärkiste #{action.Box.Id}: " +
        $"({primaryAtQ.Position.X:F2}, {primaryAtQ.Position.Y:F2})");
    Console.WriteLine(
        $"Schubrichtung: {geometry.PushHeadingDeg:F2}°");
    Console.WriteLine(
        $"Seitlicher Schieberversatz: " +
        $"{data.LateralOffsetCm:+0.00;-0.00;0.00} cm");
    Console.WriteLine(
        $"Kontaktzentrum Schieber: " +
        $"({geometry.ContactCenter.X:F2}, {geometry.ContactCenter.Y:F2})");
    Console.WriteLine(
        $"Kamera-Q: ({geometry.PreApproachPoint.X:F2}, {geometry.PreApproachPoint.Y:F2})");
    Console.WriteLine(
        $"Kamera-P: ({geometry.ApproachPoint.X:F2}, {geometry.ApproachPoint.Y:F2})");


    ClosedLoopMoveToPoint(
        rover,
        vision,
        geometry.PreApproachPoint,
        "FREE_PUSH Q-FEINKORREKTUR",
        plannedTurnDeg: null,
        plannedDistanceCm: null,
        toleranceCm: 1.2,
        maximumCorrectionCm: 10.0);


    qSnapshot =
        vision.MeasureScene(
            printSnapshot: false);

    primaryAtQ =
        FindActiveBoxInSnapshot(
            qSnapshot,
            action.Box.Color,
            primaryAtQ.Position);

    geometry =
        FreePushPlanner.CalculateGeometry(
            primaryAtQ.Position,
            data.PushHeadingDeg,
            data.LateralOffsetCm,
            data.PushDistanceCm);


    AlignHeadingClosedLoop(
        rover,
        vision,
        geometry.PushHeadingDeg,
        "FREE_PUSH SCHUBRICHTUNG",
        plannedTurnDeg: plan.FinalTurnDeg);


    ClosedLoopMoveToPoint(
        rover,
        vision,
        geometry.ApproachPoint,
        "FREE_PUSH ENDANFLUG ZUM P",
        plannedTurnDeg: 0.0,
        plannedDistanceCm: geometry.FinalApproachCm,
        toleranceCm: 1.0,
        maximumCorrectionCm: 8.0);


    RoverPose aligned =
        AlignHeadingClosedLoop(
            rover,
            vision,
            geometry.PushHeadingDeg,
            "FREE_PUSH LETZTE AUSRICHTUNG",
            plannedTurnDeg: null);


    double headingRad =
        geometry.PushHeadingDeg *
        Math.PI /
        180.0;

    double ux =
        Math.Cos(headingRad);

    double uy =
        Math.Sin(headingRad);

    double vx =
        -uy;

    double vy =
        ux;

    double dx =
        geometry.ContactRoverPoint.X -
        aligned.X;

    double dy =
        geometry.ContactRoverPoint.Y -
        aligned.Y;

    double contactTravel =
        dx * ux +
        dy * uy;

    double lateralError =
        Math.Abs(
            dx * vx +
            dy * vy);


    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("FREE_PUSH SICHERHEITSKONTROLLPUNKT");
    Console.WriteLine("=====================================================");
    Console.WriteLine(
        $"Rover real: ({aligned.X:F2}, {aligned.Y:F2}), " +
        $"{aligned.HeadingDeg:F2}°");
    Console.WriteLine(
        $"Soll-Roverposition bei Kontakt: " +
        $"({geometry.ContactRoverPoint.X:F2}, {geometry.ContactRoverPoint.Y:F2})");
    Console.WriteLine(
        $"Seitlicher Fehler: {lateralError:F2} cm");
    Console.WriteLine(
        $"Restweg bis Kontakt: {contactTravel:F2} cm");
    Console.WriteLine(
        $"KI-Freischub: {geometry.PushDistanceCm:F2} cm");
    Console.WriteLine(
        $"Sicherheitsrückzug: {geometry.RetreatDistanceCm:F2} cm");
    Console.WriteLine(
        $"Möglicherweise berührte Kisten: " +
        $"{string.Join(", ", data.ExpectedAffectedBoxIds.Select(id => "#" + id))}");


    if (lateralError > 3.0)
    {
        throw new InvalidOperationException(
            $"FREE_PUSH: seitlicher Kontaktfehler " +
            $"{lateralError:F2} cm ist zu groß.");
    }

    if (contactTravel < 0.0 ||
        contactTravel > 12.0)
    {
        throw new InvalidOperationException(
            $"FREE_PUSH: unplausibler Restweg bis Kontakt " +
            $"{contactTravel:F2} cm.");
    }


    Console.WriteLine();
    Console.WriteLine(
        "ACHTUNG: experimenteller KI-Freischub. " +
        "ENTER zum Kontakt und kurzen Schub.");
    Console.ReadLine();


    Console.WriteLine();
    Console.WriteLine("PHASE F3 - KONTAKT");
    rover.ForwardCm(
        contactTravel);


    Console.WriteLine();
    Console.WriteLine("PHASE F4 - KURZER FREISCHUB");
    rover.ForwardCm(
        geometry.PushDistanceCm);


    Console.WriteLine();
    Console.WriteLine("PHASE F5 - GROSSER SICHERHEITSRÜCKZUG");
    rover.BackwardCm(
        geometry.RetreatDistanceCm);


    Console.WriteLine();
    Console.WriteLine("KAMERA-NEUMESSUNG NACH FREE_PUSH");
    Console.WriteLine("--------------------------------");

    return vision.MeasureScene(
        printSnapshot: true);
}


// ================================================================
// CLOSED-LOOP-REGELUNG
// ================================================================

static RoverPose ClosedLoopMoveToPoint(
    Ev3Rover rover,
    VisionSystem vision,
    Point2D target,
    string label,
    double? plannedTurnDeg,
    double? plannedDistanceCm,
    double toleranceCm,
    double maximumCorrectionCm)
{
    const int maxPositionCorrectionAttempts = 3;

    RoverPose pose =
        vision.MeasureRoverPose();

    Console.WriteLine();
    Console.WriteLine($"[{label}]");
    Console.WriteLine(
        $"Kamera-IST: Rover " +
        $"({pose.X:F2}, {pose.Y:F2}), " +
        $"{pose.HeadingDeg:F2}°");
    Console.WriteLine(
        $"Sollpunkt: ({target.X:F2}, {target.Y:F2})");


    for (int attempt = 0;
         attempt <= maxPositionCorrectionAttempts;
         attempt++)
    {
        Point2D actual =
            new(
                pose.X,
                pose.Y);

        double distance =
            Geometry2D.Distance(
                actual,
                target);

        if (distance <= toleranceCm)
        {
            Console.WriteLine(
                $"PUNKT ERREICHT: Restfehler {distance:F2} cm " +
                $"(Toleranz {toleranceCm:F2} cm).");

            return pose;
        }


        if (attempt > 0 &&
            distance > maximumCorrectionCm)
        {
            throw new InvalidOperationException(
                $"{label}: Positionsfehler {distance:F2} cm ist größer " +
                $"als zulässige Feinkorrektur {maximumCorrectionCm:F2} cm.");
        }


        double desiredHeading =
            Geometry2D.HeadingDeg(
                actual,
                target);

        double cameraTurn =
            Geometry2D.ShortestTurn(
                pose.HeadingDeg,
                desiredHeading);


        if (attempt == 0)
        {
            Console.WriteLine(
                $"Kamera-Sollrichtung = {desiredHeading:F2}°");
            Console.WriteLine(
                $"Kamera-Drehung      = {DescribeTurn(cameraTurn)}");

            if (plannedTurnDeg.HasValue)
            {
                Console.WriteLine(
                    $"Planner-Drehung     = {DescribeTurn(plannedTurnDeg.Value)}");
                Console.WriteLine(
                    $"LENKKORREKTUR       = " +
                    $"{cameraTurn - plannedTurnDeg.Value:+0.00;-0.00;0.00}°");
            }

            Console.WriteLine(
                $"Kamera-Distanz      = {distance:F2} cm");

            if (plannedDistanceCm.HasValue)
            {
                Console.WriteLine(
                    $"Planner-Distanz     = {plannedDistanceCm.Value:F2} cm");
                Console.WriteLine(
                    $"WEGKORREKTUR        = " +
                    $"{distance - plannedDistanceCm.Value:+0.00;-0.00;0.00} cm");
            }
        }
        else
        {
            Console.WriteLine();
            Console.WriteLine(
                $"POSITIONSKORREKTUR #{attempt}: " +
                $"Restfehler {distance:F2} cm");
        }


        // --------------------------------------------------------
        // Große Drehungen NICHT mehr in einem einzigen Motorbefehl.
        //
        // Der Test hat gezeigt, dass z.B. ein 110°-Befehl real nur
        // etwa 45° ergeben kann. Daher maximal 25° pro Teilschritt,
        // danach sofort Kamera messen und aus der REALEN Pose die
        // Zielrichtung neu berechnen.
        // --------------------------------------------------------

        pose =
            CameraTurnTowardPoint(
                rover,
                vision,
                target,
                positionToleranceCm: toleranceCm,
                headingToleranceDeg: 1.5,
                label: label);


        Point2D actualBeforeDrive =
            new(
                pose.X,
                pose.Y);

        double correctedDistance =
            Geometry2D.Distance(
                actualBeforeDrive,
                target);


        if (correctedDistance <= toleranceCm)
        {
            Console.WriteLine(
                $"PUNKT DURCH DREHUNG BEREITS ERREICHT: " +
                $"Restfehler {correctedDistance:F2} cm.");

            return pose;
        }


        double desiredBeforeDrive =
            Geometry2D.HeadingDeg(
                actualBeforeDrive,
                target);

        double headingErrorBeforeDrive =
            Geometry2D.ShortestTurn(
                pose.HeadingDeg,
                desiredBeforeDrive);

        Console.WriteLine(
            $"Vor Fahrt kamerabestätigt: " +
            $"Richtung {pose.HeadingDeg:F2}°, " +
            $"Soll {desiredBeforeDrive:F2}°, " +
            $"Restwinkel {headingErrorBeforeDrive:+0.00;-0.00;0.00}°");

        Console.WriteLine(
            $"Reale Restdistanz vor Fahrt = {correctedDistance:F2} cm");


        if (correctedDistance >
            0.20)
        {
            rover.ForwardCm(
                correctedDistance);
        }


        pose =
            vision.MeasureRoverPose();

        double errorAfterDrive =
            Geometry2D.Distance(
                new Point2D(
                    pose.X,
                    pose.Y),
                target);

        Console.WriteLine(
            $"Kamera nach Fahrt: " +
            $"({pose.X:F2}, {pose.Y:F2}), " +
            $"{pose.HeadingDeg:F2}°");
        Console.WriteLine(
            $"Positionsfehler nach Fahrt = {errorAfterDrive:F2} cm");

        if (errorAfterDrive <=
            toleranceCm)
        {
            Console.WriteLine(
                "PUNKT KAMERABESTÄTIGT.");

            return pose;
        }
    }


    double finalError =
        Geometry2D.Distance(
            new Point2D(
                pose.X,
                pose.Y),
            target);

    throw new InvalidOperationException(
        $"{label}: Zielpunkt nach Closed-Loop-Korrekturen nicht erreicht. " +
        $"Restfehler {finalError:F2} cm.");
}


// ================================================================
// CLOSED-LOOP WINKELAUSRICHTUNG
// ================================================================

static RoverPose AlignHeadingClosedLoop(
    Ev3Rover rover,
    VisionSystem vision,
    double targetHeadingDeg,
    string label,
    double? plannedTurnDeg)
{
    RoverPose pose =
        vision.MeasureRoverPose();

    Console.WriteLine();
    Console.WriteLine($"[{label}]");
    Console.WriteLine(
        $"Kamera-IST-Winkel = {pose.HeadingDeg:F2}°");
    Console.WriteLine(
        $"Kamera-SOLL-Winkel = {targetHeadingDeg:F2}°");

    double initialTurn =
        Geometry2D.ShortestTurn(
            pose.HeadingDeg,
            targetHeadingDeg);

    Console.WriteLine(
        $"Kamera-Drehung = {DescribeTurn(initialTurn)}");

    if (plannedTurnDeg.HasValue)
    {
        Console.WriteLine(
            $"Planner-Drehung = {DescribeTurn(plannedTurnDeg.Value)}");
        Console.WriteLine(
            $"LENKKORREKTUR = " +
            $"{initialTurn - plannedTurnDeg.Value:+0.00;-0.00;0.00}°");
    }


    pose =
        CameraTurnToHeading(
            rover,
            vision,
            targetHeadingDeg,
            toleranceDeg: 1.2,
            label: label);

    double residual =
        Geometry2D.ShortestTurn(
            pose.HeadingDeg,
            targetHeadingDeg);

    Console.WriteLine(
        $"WINKEL KAMERABESTÄTIGT: " +
        $"{pose.HeadingDeg:F2}°, " +
        $"Rest {residual:+0.00;-0.00;0.00}°");

    return pose;
}


// ================================================================
// KAMERA-SERVO: ZU EINEM PUNKT DREHEN
//
// Maximal 25° pro Motorbefehl. Nach JEDEM Teilschritt wird die
// tatsächliche Roverpose neu gemessen. Dadurch sind große
// Unterdrehungen kein Abbruchgrund mehr.
// ================================================================

static RoverPose CameraTurnTowardPoint(
    Ev3Rover rover,
    VisionSystem vision,
    Point2D target,
    double positionToleranceCm,
    double headingToleranceDeg,
    string label)
{
    const double maxTurnStepDeg = 25.0;
    const int maxTurnSteps = 24;

    int stalledSteps = 0;

    RoverPose pose =
        vision.MeasureRoverPose();


    for (int step = 1;
         step <= maxTurnSteps;
         step++)
    {
        Point2D actual =
            new(
                pose.X,
                pose.Y);

        double distance =
            Geometry2D.Distance(
                actual,
                target);

        if (distance <=
            positionToleranceCm)
        {
            return pose;
        }


        double desiredHeading =
            Geometry2D.HeadingDeg(
                actual,
                target);

        double error =
            Geometry2D.ShortestTurn(
                pose.HeadingDeg,
                desiredHeading);

        if (Math.Abs(error) <=
            headingToleranceDeg)
        {
            return pose;
        }


        double command =
            Math.Clamp(
                error,
                -maxTurnStepDeg,
                maxTurnStepDeg);

        double beforeHeading =
            pose.HeadingDeg;

        Console.WriteLine(
            $"DREHSCHRITT {step}: " +
            $"IST {pose.HeadingDeg:F2}°, " +
            $"SOLL {desiredHeading:F2}°, " +
            $"Fehler {error:+0.00;-0.00;0.00}°, " +
            $"Befehl {DescribeTurn(command)}");

        Turn(
            rover,
            command);


        RoverPose after =
            vision.MeasureRoverPose();

        double actualTurn =
            Geometry2D.ShortestTurn(
                beforeHeading,
                after.HeadingDeg);

        Point2D actualAfter =
            new(
                after.X,
                after.Y);

        double newDistance =
            Geometry2D.Distance(
                actualAfter,
                target);

        double newDesiredHeading =
            newDistance > 0.001
                ? Geometry2D.HeadingDeg(
                    actualAfter,
                    target)
                : after.HeadingDeg;

        double newError =
            Geometry2D.ShortestTurn(
                after.HeadingDeg,
                newDesiredHeading);

        double efficiency =
            Math.Abs(command) > 0.01
                ? Math.Abs(actualTurn) /
                  Math.Abs(command) *
                  100.0
                : 100.0;


        Console.WriteLine(
            $"  KAMERA: Winkel {after.HeadingDeg:F2}°, " +
            $"reale Drehung {actualTurn:+0.00;-0.00;0.00}°, " +
            $"Wirkung {efficiency:F0} %, " +
            $"neues Soll {newDesiredHeading:F2}°, " +
            $"Rest {newError:+0.00;-0.00;0.00}°");

        Console.WriteLine(
            $"  KAMERA: Position " +
            $"({after.X:F2}, {after.Y:F2}), " +
            $"Distanz zum Punkt {newDistance:F2} cm");


        // Nur bei wirklichem Stillstand abbrechen.
        // Unterdrehung ist erlaubt und wird einfach weitergeregelt.
        if (Math.Abs(command) >=
                8.0 &&
            Math.Abs(actualTurn) <
                1.0)
        {
            stalledSteps++;

            Console.WriteLine(
                $"  WARNUNG: kaum Drehwirkung " +
                $"({stalledSteps}/3).");
        }
        else
        {
            stalledSteps = 0;
        }


        if (stalledSteps >= 3)
        {
            throw new InvalidOperationException(
                $"{label}: Rover reagiert dreimal hintereinander " +
                $"praktisch nicht auf einen Drehbefehl. " +
                $"Mögliche mechanische Blockierung.");
        }


        pose =
            after;
    }


    Point2D finalPosition =
        new(
            pose.X,
            pose.Y);

    double finalDesired =
        Geometry2D.HeadingDeg(
            finalPosition,
            target);

    double finalError =
        Geometry2D.ShortestTurn(
            pose.HeadingDeg,
            finalDesired);

    throw new InvalidOperationException(
        $"{label}: Kamerageregelte Drehung erreicht nach " +
        $"{maxTurnSteps} Teilschritten die Zielrichtung nicht. " +
        $"Restwinkelfehler {finalError:F2}°.");
}


// ================================================================
// KAMERA-SERVO: AUF FESTEN WINKEL DREHEN
// ================================================================

static RoverPose CameraTurnToHeading(
    Ev3Rover rover,
    VisionSystem vision,
    double targetHeadingDeg,
    double toleranceDeg,
    string label)
{
    const double maxTurnStepDeg = 25.0;
    const int maxTurnSteps = 24;

    int stalledSteps = 0;

    RoverPose pose =
        vision.MeasureRoverPose();


    for (int step = 1;
         step <= maxTurnSteps;
         step++)
    {
        double error =
            Geometry2D.ShortestTurn(
                pose.HeadingDeg,
                targetHeadingDeg);

        if (Math.Abs(error) <=
            toleranceDeg)
        {
            return pose;
        }


        double command =
            Math.Clamp(
                error,
                -maxTurnStepDeg,
                maxTurnStepDeg);

        double beforeHeading =
            pose.HeadingDeg;

        Console.WriteLine(
            $"DREHSCHRITT {step}: " +
            $"IST {pose.HeadingDeg:F2}°, " +
            $"SOLL {targetHeadingDeg:F2}°, " +
            $"Fehler {error:+0.00;-0.00;0.00}°, " +
            $"Befehl {DescribeTurn(command)}");

        Turn(
            rover,
            command);


        RoverPose after =
            vision.MeasureRoverPose();

        double actualTurn =
            Geometry2D.ShortestTurn(
                beforeHeading,
                after.HeadingDeg);

        double newError =
            Geometry2D.ShortestTurn(
                after.HeadingDeg,
                targetHeadingDeg);

        double efficiency =
            Math.Abs(command) > 0.01
                ? Math.Abs(actualTurn) /
                  Math.Abs(command) *
                  100.0
                : 100.0;


        Console.WriteLine(
            $"  KAMERA: Winkel {after.HeadingDeg:F2}°, " +
            $"reale Drehung {actualTurn:+0.00;-0.00;0.00}°, " +
            $"Wirkung {efficiency:F0} %, " +
            $"Rest {newError:+0.00;-0.00;0.00}°");


        if (Math.Abs(command) >=
                8.0 &&
            Math.Abs(actualTurn) <
                1.0)
        {
            stalledSteps++;

            Console.WriteLine(
                $"  WARNUNG: kaum Drehwirkung " +
                $"({stalledSteps}/3).");
        }
        else
        {
            stalledSteps = 0;
        }


        if (stalledSteps >= 3)
        {
            throw new InvalidOperationException(
                $"{label}: Rover reagiert dreimal hintereinander " +
                $"praktisch nicht auf einen Drehbefehl. " +
                $"Mögliche mechanische Blockierung.");
        }


        pose =
            after;
    }


    double finalError =
        Geometry2D.ShortestTurn(
            pose.HeadingDeg,
            targetHeadingDeg);

    throw new InvalidOperationException(
        $"{label}: Kamerageregelte Winkelkorrektur erreicht nach " +
        $"{maxTurnSteps} Teilschritten das Ziel nicht. " +
        $"Restwinkelfehler {finalError:F2}°.");
}


// ================================================================
// AKTIVE KISTE IN EINEM KAMERA-SNAPSHOT FINDEN
// ================================================================

static VisionDetectedBox FindActiveBoxInSnapshot(
    VisionSnapshot snapshot,
    BoxColor color,
    Point2D expectedPosition)
{
    IReadOnlyList<VisionDetectedBox> boxes =
        color == BoxColor.Red
            ? snapshot.RedBoxes
            : snapshot.BlueBoxes;

    if (boxes.Count == 0)
    {
        throw new InvalidOperationException(
            $"Keine {ColorName(color)}e Kiste im Kamerabild erkannt.");
    }

    VisionDetectedBox best =
        boxes
            .OrderBy(
                b => Geometry2D.Distance(
                    b.Position,
                    expectedPosition))
            .First();

    double distance =
        Geometry2D.Distance(
            best.Position,
            expectedPosition);

    Console.WriteLine(
        $"Aktive Kiste #{(color == BoxColor.Red ? "ROT" : "BLAU")} " +
        $"über Nähe zu ({expectedPosition.X:F2}, {expectedPosition.Y:F2}) gewählt: " +
        $"({best.Position.X:F2}, {best.Position.Y:F2}), Abstand {distance:F2} cm");

    if (distance >
        15.0)
    {
        throw new InvalidOperationException(
            $"Aktive {ColorName(color)}-Kiste kann nicht sicher zugeordnet werden. " +
            $"Nächste Farbkiste ist {distance:F2} cm von der erwarteten Position entfernt.");
    }

    return best;
}


// ================================================================
// SCHUBGEOMETRIE AUS DER AKTUELLEN KAMERAPOSITION DER KISTE
// ================================================================

static CameraPushGeometry BuildCameraPushGeometry(
    Point2D box,
    Point2D target)
{
    const double approachDistanceCm = 15.0;

    double dx =
        target.X - box.X;

    double dy =
        target.Y - box.Y;

    double pushDistance =
        Math.Sqrt(
            dx * dx +
            dy * dy);

    if (pushDistance <
        0.001)
    {
        throw new InvalidOperationException(
            "Kamera sieht die Kiste bereits exakt am Ziel.");
    }

    double ux =
        dx /
        pushDistance;

    double uy =
        dy /
        pushDistance;

    double preApproachDistanceCm =
        FieldApproachGeometry.GetAdaptivePreApproachDistance(
            box,
            ux,
            uy);

    Point2D p =
        new(
            box.X -
                approachDistanceCm * ux,
            box.Y -
                approachDistanceCm * uy);

    Point2D q =
        new(
            box.X -
                preApproachDistanceCm * ux,
            box.Y -
                preApproachDistanceCm * uy);

    double heading =
        Geometry2D.HeadingDeg(
            dx,
            dy);

    return new CameraPushGeometry(
        BoxPosition: box,
        PreApproachPoint: q,
        ApproachPoint: p,
        PushHeadingDeg: heading,
        PushDistanceCm: pushDistance);
}


static void PrintCameraPushGeometry(
    AgentActionCandidate action,
    PushPlan plan,
    VisionSnapshot snapshot,
    CameraPushGeometry camera)
{
    Console.WriteLine();
    Console.WriteLine("KAMERA-NEUPLANUNG AM Q");
    Console.WriteLine("======================");
    Console.WriteLine(
        $"Rover IST = " +
        $"({snapshot.Rover.X:F2}, {snapshot.Rover.Y:F2}), " +
        $"{snapshot.Rover.HeadingDeg:F2}°");
    Console.WriteLine(
        $"Kiste IST = " +
        $"({camera.BoxPosition.X:F2}, {camera.BoxPosition.Y:F2})");
    Console.WriteLine(
        $"Ziel      = " +
        $"({action.Destination.X:F2}, {action.Destination.Y:F2})");
    Console.WriteLine();
    Console.WriteLine(
        $"Planner-Q = " +
        $"({plan.PreApproachPoint.X:F2}, {plan.PreApproachPoint.Y:F2})");
    Console.WriteLine(
        $"Kamera-Q  = " +
        $"({camera.PreApproachPoint.X:F2}, {camera.PreApproachPoint.Y:F2})");

    double cameraQDistance =
        Geometry2D.Distance(
            camera.BoxPosition,
            camera.PreApproachPoint);

    Console.WriteLine(
        $"Kamera-Q-Abstand = {cameraQDistance:F2} cm" +
        (cameraQDistance < 19.99
            ? "  [FELDRAND: automatisch gekürzt]"
            : ""));

    Console.WriteLine(
        $"Q-KORREKTUR = " +
        $"{Geometry2D.Distance(plan.PreApproachPoint, camera.PreApproachPoint):F2} cm");
    Console.WriteLine();
    Console.WriteLine(
        $"Planner-P = " +
        $"({plan.ApproachPoint.X:F2}, {plan.ApproachPoint.Y:F2})");
    Console.WriteLine(
        $"Kamera-P  = " +
        $"({camera.ApproachPoint.X:F2}, {camera.ApproachPoint.Y:F2})");
    Console.WriteLine(
        $"P-KORREKTUR = " +
        $"{Geometry2D.Distance(plan.ApproachPoint, camera.ApproachPoint):F2} cm");
    Console.WriteLine();
    Console.WriteLine(
        $"Planner-Schubrichtung = {plan.PushHeadingDeg:F2}°");
    Console.WriteLine(
        $"Kamera-Schubrichtung  = {camera.PushHeadingDeg:F2}°");
    Console.WriteLine(
        $"RICHTUNGSKORREKTUR = " +
        $"{Geometry2D.ShortestTurn(plan.PushHeadingDeg, camera.PushHeadingDeg):+0.00;-0.00;0.00}°");
    Console.WriteLine(
        $"Planner-Schubweg = {plan.PushCm:F2} cm");
    Console.WriteLine(
        $"Kamera-Schubweg  = {camera.PushDistanceCm:F2} cm");
}


// ================================================================
// KONTAKTGEOMETRIE AUS KAMERA
// ================================================================

static ContactGeometry CalculateContactGeometry(
    RoverPose rover,
    Point2D box,
    double pushHeadingDeg)
{
    const double bumperOffsetCm = 6.0;
    const double boxLengthCm = 5.0;

    double radians =
        pushHeadingDeg *
        Math.PI /
        180.0;

    double ux =
        Math.Cos(
            radians);

    double uy =
        Math.Sin(
            radians);

    double dx =
        box.X -
        rover.X;

    double dy =
        box.Y -
        rover.Y;

    double along =
        dx * ux +
        dy * uy;

    double lateral =
        Math.Abs(
            dx * uy -
            dy * ux);

    double desiredCenterDistanceAtContact =
        bumperOffsetCm +
        boxLengthCm / 2.0;

    double contactTravel =
        along -
        desiredCenterDistanceAtContact;

    return new ContactGeometry(
        ContactTravelCm: contactTravel,
        LateralErrorCm: lateral,
        AlongDistanceToBoxCm: along);
}


// ================================================================
// SORTIERERGEBNIS ERST NACH KAMERAPRÜFUNG BESTÄTIGEN
// ================================================================

static void ConfirmSortResultFromCamera(
    WorldModel world,
    AgentActionCandidate action)
{
    const double sortToleranceCm = 6.0;

    double error =
        Geometry2D.Distance(
            action.Box.Position,
            action.Destination);

    Console.WriteLine();
    Console.WriteLine("=====================================================");
    Console.WriteLine("KAMERA-ZIELKONTROLLE");
    Console.WriteLine("=====================================================");
    Console.WriteLine(
        $"Kiste #{action.Box.Id}: real " +
        $"({action.Box.Position.X:F2}, {action.Box.Position.Y:F2})");
    Console.WriteLine(
        $"Sollziel: " +
        $"({action.Destination.X:F2}, {action.Destination.Y:F2})");
    Console.WriteLine(
        $"Zielabweichung = {error:F2} cm");
    Console.WriteLine(
        $"Erlaubte Toleranz = {sortToleranceCm:F2} cm");


    if (error <=
        sortToleranceCm)
    {
        action.Box.Sorted =
            true;

        action.Box.SlotId =
            action.Slot!.Id;

        action.Slot.OccupiedByBoxId =
            action.Box.Id;

        Console.WriteLine(
            "ERGEBNIS: KAMERA BESTÄTIGT - KISTE IST SORTIERT.");
    }
    else
    {
        action.Box.Sorted =
            false;

        action.Box.SlotId =
            null;

        if (action.Slot != null &&
            action.Slot.OccupiedByBoxId ==
                action.Box.Id)
        {
            action.Slot.OccupiedByBoxId =
                null;
        }

        Console.WriteLine(
            "ERGEBNIS: ZIEL NICHT GENAU GENUG ERREICHT.");
        Console.WriteLine(
            "Kiste bleibt OFFEN und wird mit ihrer realen Position neu eingeplant.");
    }
}


// ================================================================
// BEREITS SORTIERTE KISTEN NACH JEDER KAMERAMESSUNG ÜBERWACHEN
// ================================================================

static void RevalidateAllSortedBoxes(
    WorldModel world)
{
    const double sortToleranceCm = 6.0;

    foreach (BoxState box
             in world.Boxes
                 .Where(b => b.Sorted)
                 .ToList())
    {
        if (!box.SlotId.HasValue)
            continue;

        TargetSlot? slot =
            world.Zones
                .SelectMany(z => z.Slots)
                .FirstOrDefault(
                    s => s.Id ==
                         box.SlotId.Value);

        if (slot == null)
            continue;

        double error =
            Geometry2D.Distance(
                box.Position,
                slot.Position);

        if (error <=
            sortToleranceCm)
        {
            continue;
        }

        Console.WriteLine();
        Console.WriteLine(
            $"KAMERA-WARNUNG: bereits sortierte Kiste #{box.Id} " +
            $"ist {error:F2} cm von Slot #{slot.Id} entfernt.");
        Console.WriteLine(
            "Status wird wieder auf OFFEN gesetzt; Planner darf nachsortieren.");

        if (slot.OccupiedByBoxId ==
            box.Id)
        {
            slot.OccupiedByBoxId =
                null;
        }

        box.Sorted =
            false;

        box.SlotId =
            null;
    }
}


// ================================================================
// WELTMODELL AUS KAMERA
// ================================================================

static WorldModel BuildWorldFromVision(
    VisionSnapshot snapshot,
    List<TargetZone> zones)
{
    List<BoxState> boxes =
        new();

    int nextId = 1;

    foreach (VisionDetectedBox detected
             in snapshot.RedBoxes
                 .OrderBy(b => b.Position.X)
                 .ThenBy(b => b.Position.Y))
    {
        boxes.Add(
            new BoxState(
                nextId++,
                BoxColor.Red,
                detected.Position));
    }

    foreach (VisionDetectedBox detected
             in snapshot.BlueBoxes
                 .OrderBy(b => b.Position.X)
                 .ThenBy(b => b.Position.Y))
    {
        boxes.Add(
            new BoxState(
                nextId++,
                BoxColor.Blue,
                detected.Position));
    }

    return new WorldModel(
        snapshot.Rover,
        boxes,
        zones);
}


// ================================================================
// BESTEHENDES WELTMODELL MIT NEUER KAMERAMESSUNG AKTUALISIEREN
//
// IDs, Sorted-Status und Slot-Zuordnung bleiben erhalten.
// Nur die physisch gemessenen Positionen werden ersetzt.
// ================================================================

static void RefreshWorldFromVision(
    WorldModel world,
    VisionSnapshot snapshot)
{
    world.Rover =
        snapshot.Rover;

    RefreshBoxesOfColor(
        world,
        BoxColor.Red,
        snapshot.RedBoxes);

    RefreshBoxesOfColor(
        world,
        BoxColor.Blue,
        snapshot.BlueBoxes);
}


static void RefreshBoxesOfColor(
    WorldModel world,
    BoxColor color,
    IReadOnlyList<VisionDetectedBox> detectedBoxes)
{
    List<BoxState> existing =
        world.Boxes
            .Where(b => b.Color == color)
            .OrderBy(b => b.Id)
            .ToList();

    if (existing.Count != detectedBoxes.Count)
    {
        throw new InvalidOperationException(
            $"Kamera meldet für {ColorName(color)} " +
            $"{detectedBoxes.Count} Kisten, " +
            $"Weltmodell erwartet aber {existing.Count}.");
    }

    if (existing.Count == 0)
        return;

    // Die Kisten haben keine dauerhaften visuellen IDs.
    // Deshalb suchen wir die Zuordnung mit der kleinsten gesamten
    // Positionsänderung. Bei 2 Kisten je Farbe ist das trivial,
    // funktioniert aber auch für einige weitere Kisten.
    Point2D[] measured =
        detectedBoxes
            .Select(b => b.Position)
            .ToArray();

    int[] assignment =
        FindBestAssignment(
            existing,
            measured);

    for (int i = 0;
         i < existing.Count;
         i++)
    {
        existing[i].Position =
            measured[assignment[i]];
    }
}


static int[] FindBestAssignment(
    IReadOnlyList<BoxState> existing,
    IReadOnlyList<Point2D> measured)
{
    int count =
        existing.Count;

    int[] current =
        new int[count];

    int[] best =
        new int[count];

    bool[] used =
        new bool[count];

    double bestScore =
        double.PositiveInfinity;


    void Search(
        int index,
        double score)
    {
        if (score >= bestScore)
            return;

        if (index == count)
        {
            bestScore = score;
            Array.Copy(
                current,
                best,
                count);

            return;
        }

        Point2D oldPosition =
            existing[index].Position;

        for (int j = 0;
             j < count;
             j++)
        {
            if (used[j])
                continue;

            double distance =
                Geometry2D.Distance(
                    oldPosition,
                    measured[j]);

            used[j] = true;
            current[index] = j;

            Search(
                index + 1,
                score + distance * distance);

            used[j] = false;
        }
    }


    Search(
        0,
        0.0);

    if (double.IsPositiveInfinity(
            bestScore))
    {
        throw new InvalidOperationException(
            "Kistenzuordnung aus Kameramessung fehlgeschlagen.");
    }

    return best;
}


// ================================================================
// WELT EINLESEN
// ================================================================

// ================================================================
// AUSGABEN
// ================================================================

static void PrintTargetZones(IReadOnlyList<TargetZone> zones)
{
    Console.WriteLine("ZIELGEBIETE");
    Console.WriteLine("===========");

    foreach (TargetZone zone in zones)
    {
        Console.WriteLine();
        Console.WriteLine($"{ColorName(zone.Color)}:");
        Console.WriteLine($"  x = {zone.XMin:F0} ... {zone.XMax:F0}");
        Console.WriteLine($"  y = {zone.YMin:F0} ... {zone.YMax:F0}");
        Console.WriteLine("  Ablageplätze:");

        foreach (TargetSlot slot in zone.Slots)
        {
            Console.WriteLine(
                $"    #{slot.Id}: ({slot.Position.X:F1}, {slot.Position.Y:F1}), " +
                $"Priorität {slot.Priority}");
        }
    }
}

static void PrintWorld(WorldModel world)
{
    Console.WriteLine();
    Console.WriteLine("AKTUELLER WELTZUSTAND");
    Console.WriteLine("---------------------");
    Console.WriteLine(
        $"Rover: ({world.Rover.X:F1}, {world.Rover.Y:F1}), " +
        $"{world.Rover.HeadingDeg:F1}°");

    foreach (BoxState box in world.Boxes)
    {
        string status = box.Sorted
            ? $"SORTIERT / Slot {box.SlotId}"
            : "offen";

        Console.WriteLine(
            $"Kiste #{box.Id}: {ColorName(box.Color)}, " +
            $"({box.Position.X:F1}, {box.Position.Y:F1}) - {status}");
    }
}

static void PrintAction(AgentActionCandidate action)
{
    Console.WriteLine();
    Console.WriteLine("AUSGEWÄHLTE AKTION");
    Console.WriteLine("------------------");

    if (action.Kind == AgentActionKind.Sort)
    {
        Console.WriteLine("Typ:             SORTIEREN");
        Console.WriteLine($"Kiste:           #{action.Box.Id} / {ColorName(action.Box.Color)}");
        Console.WriteLine($"Zielslot:        #{action.Slot!.Id}");

        if (!string.Equals(
                action.Reason,
                "Direkte Sortieraktion",
                StringComparison.Ordinal))
        {
            Console.WriteLine($"Grund:           {action.Reason}");
        }
    }
    else if (action.Kind == AgentActionKind.Relocate)
    {
        Console.WriteLine("Typ:             TEMPORÄR VERSCHIEBEN");
        Console.WriteLine($"Kiste:           #{action.Box.Id} / {ColorName(action.Box.Color)}");
        Console.WriteLine($"Grund:           {action.Reason}");
    }
    else
    {
        FreePushData data =
            action.FreePush
            ?? throw new InvalidOperationException(
                "FREE_PUSH ohne FreePushData.");

        Console.WriteLine("Typ:             KI-FREISCHUB / FREE_PUSH");
        Console.WriteLine(
            $"Primärkiste:      #{action.Box.Id} / {ColorName(action.Box.Color)}");
        Console.WriteLine($"Grund:           {action.Reason}");
        Console.WriteLine(
            $"Schubrichtung:    {data.PushHeadingDeg:F2}°");
        Console.WriteLine(
            $"Schieberversatz:  {data.LateralOffsetCm:+0.00;-0.00;0.00} cm");
        Console.WriteLine(
            $"Freischub:        {data.PushDistanceCm:F2} cm");
        Console.WriteLine(
            $"Möglicherweise betroffen: " +
            $"{string.Join(", ", data.ExpectedAffectedBoxIds.Select(id => "#" + id))}");
    }

    Console.WriteLine(
        $"Von:             ({action.Box.Position.X:F2}, {action.Box.Position.Y:F2})");

    if (action.Kind != AgentActionKind.FreePush)
    {
        Console.WriteLine(
            $"Nach:            ({action.Destination.X:F2}, {action.Destination.Y:F2})");
    }
    Console.WriteLine(
        $"Anfahrpunkt P:   ({action.Plan.ApproachPoint.X:F2}, {action.Plan.ApproachPoint.Y:F2})");
    Console.WriteLine(
        $"Vor-Anfahrpunkt Q: ({action.Plan.PreApproachPoint.X:F2}, {action.Plan.PreApproachPoint.Y:F2})");

    double qDistance =
        action.Kind == AgentActionKind.FreePush
            ? Geometry2D.Distance(
                action.Plan.ApproachPoint,
                action.Plan.PreApproachPoint)
            : Geometry2D.Distance(
                action.Box.Position,
                action.Plan.PreApproachPoint);

    Console.WriteLine(
        action.Kind == AgentActionKind.FreePush
            ? $"Q->P Abstand:    {qDistance:F2} cm"
            : $"Q-Abstand:       {qDistance:F2} cm" +
              (qDistance < 19.99
                  ? "  [FELDRAND: automatisch gekürzt]"
                  : ""));

    Console.WriteLine(
        $"Navigation:      {(action.Plan.Navigation.UsedDetour ? "Umweg" : "direkt")}");
    Console.WriteLine($"Navigationsweg:  {action.Plan.Navigation.DistanceCm:F2} cm");
    Console.WriteLine($"Schubweg:        {action.Plan.PushCm:F2} cm");
    Console.WriteLine($"Gesamtweg:       {action.Plan.TotalTravelCm:F2} cm");
    Console.WriteLine($"Bewertung:       {action.Score:F2}");

    for (int i = 0; i < action.Plan.Navigation.Legs.Count; i++)
    {
        NavigationLeg leg = action.Plan.Navigation.Legs[i];
        Console.WriteLine(
            $"Etappe {i + 1}: ({leg.To.X:F2}, {leg.To.Y:F2}) - " +
            $"{leg.DistanceCm:F2} cm, {DescribeTurn(leg.TurnDeg)}");
    }

    Console.WriteLine($"Ausrichten:      {DescribeTurn(action.Plan.FinalTurnDeg)}");
}


// ================================================================
// HILFSFUNKTIONEN
// ================================================================

static void ValidateCapacity(WorldModel world)
{
    foreach (BoxColor color in Enum.GetValues<BoxColor>())
    {
        int boxes = world.Boxes.Count(b => b.Color == color);
        int slots = world.GetZone(color).Slots.Count;

        if (boxes > slots)
        {
            throw new InvalidOperationException(
                $"Für {ColorName(color)} gibt es {boxes} Kisten, aber nur {slots} Ablageplätze.");
        }
    }
}

static void Turn(Ev3Rover rover, double degrees)
{
    if (Math.Abs(degrees) < 0.05)
        return;

    if (degrees > 0)
    {
        Console.WriteLine($"Links {degrees:F2}°");
        rover.TurnLeft(degrees);
    }
    else
    {
        Console.WriteLine($"Rechts {-degrees:F2}°");
        rover.TurnRight(-degrees);
    }
}

static string DescribeTurn(double degrees)
{
    if (Math.Abs(degrees) < 0.05)
        return "keine Drehung";

    return degrees > 0
        ? $"{degrees:F2}° links"
        : $"{-degrees:F2}° rechts";
}

static string ColorName(BoxColor color) =>
    color == BoxColor.Red ? "ROT" : "BLAU";


// ================================================================
// DATENTYPEN
// ================================================================

public enum BoxColor
{
    Red,
    Blue
}

public enum AgentActionKind
{
    Sort,
    Relocate,
    FreePush
}

public record Point2D(double X, double Y);

public record RoverPose(
    double X,
    double Y,
    double HeadingDeg);

public record NavigationLeg(
    Point2D From,
    Point2D To,
    double HeadingDeg,
    double TurnDeg,
    double DistanceCm);

public record NavigationPlan(
    IReadOnlyList<Point2D> Points,
    IReadOnlyList<NavigationLeg> Legs,
    bool UsedDetour,
    double FinalHeadingDeg,
    double DistanceCm);

public record PushPlan(
    Point2D PreApproachPoint,
    Point2D ApproachPoint,
    double PushHeadingDeg,
    NavigationPlan Navigation,
    double FinalTurnDeg,
    double FinalApproachCm,
    double ContactCm,
    double PushCm,
    double RetreatCm,
    double TotalTravelCm,
    RoverPose EndPose);

public record AgentActionCandidate(
    AgentActionKind Kind,
    BoxState Box,
    TargetSlot? Slot,
    Point2D Destination,
    PushPlan Plan,
    double Score,
    string Reason,
    FreePushData? FreePush = null);


public record FreePushData(
    Point2D PrimaryReferencePosition,
    double PushHeadingDeg,
    double LateralOffsetCm,
    double PushDistanceCm,
    IReadOnlyList<int> ExpectedAffectedBoxIds,
    string AiReason);


public record FreePushGeometry(
    Point2D PrimaryBoxPosition,
    Point2D ContactCenter,
    Point2D ContactRoverPoint,
    Point2D ApproachPoint,
    Point2D PreApproachPoint,
    double PushHeadingDeg,
    double FinalApproachCm,
    double PushDistanceCm,
    double RetreatDistanceCm);


public record FreePushProposal(
    int PrimaryBoxId,
    double PushHeadingDeg,
    double LateralOffsetCm,
    double PushDistanceCm,
    string Reason);


public sealed class FreePushProposalEnvelope
{
    public List<FreePushProposalDto> Proposals { get; set; } = new();
}


public sealed class FreePushProposalDto
{
    public int PrimaryBoxId { get; set; }
    public double PushHeadingDeg { get; set; }
    public double LateralOffsetCm { get; set; }
    public double PushDistanceCm { get; set; }
    public string Reason { get; set; } = "";
}


public record CameraPushGeometry(
    Point2D BoxPosition,
    Point2D PreApproachPoint,
    Point2D ApproachPoint,
    double PushHeadingDeg,
    double PushDistanceCm);

public record ContactGeometry(
    double ContactTravelCm,
    double LateralErrorCm,
    double AlongDistanceToBoxCm);


// ================================================================
// KISTE / ZIELZONE / WELT
// ================================================================

public sealed class BoxState
{
    public int Id { get; }
    public BoxColor Color { get; }
    public Point2D Position { get; set; }
    public bool Sorted { get; set; }
    public int? SlotId { get; set; }

    public BoxState(int id, BoxColor color, Point2D position)
    {
        Id = id;
        Color = color;
        Position = position;
    }

    public BoxState Clone()
    {
        return new BoxState(Id, Color, Position)
        {
            Sorted = Sorted,
            SlotId = SlotId
        };
    }
}

public sealed class TargetSlot
{
    public int Id { get; }
    public BoxColor Color { get; }
    public Point2D Position { get; }
    public int Priority { get; }
    public int? OccupiedByBoxId { get; set; }
    public bool IsFree => OccupiedByBoxId == null;

    public TargetSlot(
        int id,
        BoxColor color,
        Point2D position,
        int priority)
    {
        Id = id;
        Color = color;
        Position = position;
        Priority = priority;
    }

    public TargetSlot Clone()
    {
        return new TargetSlot(Id, Color, Position, Priority)
        {
            OccupiedByBoxId = OccupiedByBoxId
        };
    }
}

public sealed class TargetZone
{
    public BoxColor Color { get; }
    public double XMin { get; }
    public double XMax { get; }
    public double YMin { get; }
    public double YMax { get; }
    public List<TargetSlot> Slots { get; }

    public TargetZone(
        BoxColor color,
        double xMin,
        double xMax,
        double yMin,
        double yMax,
        List<TargetSlot> slots)
    {
        Color = color;
        XMin = xMin;
        XMax = xMax;
        YMin = yMin;
        YMax = yMax;
        Slots = slots;
    }

    public bool Contains(Point2D p, double margin = 0.0)
    {
        return p.X >= XMin - margin &&
               p.X <= XMax + margin &&
               p.Y >= YMin - margin &&
               p.Y <= YMax + margin;
    }

    public TargetZone Clone()
    {
        return new TargetZone(
            Color,
            XMin,
            XMax,
            YMin,
            YMax,
            Slots.Select(s => s.Clone()).ToList());
    }
}

public sealed class WorldModel
{
    public RoverPose Rover { get; set; }
    public List<BoxState> Boxes { get; }
    public List<TargetZone> Zones { get; }

    public WorldModel(
        RoverPose rover,
        List<BoxState> boxes,
        List<TargetZone> zones)
    {
        Rover = rover;
        Boxes = boxes;
        Zones = zones;
    }

    public TargetZone GetZone(BoxColor color) =>
        Zones.Single(z => z.Color == color);

    public WorldModel DeepClone()
    {
        return new WorldModel(
            Rover,
            Boxes.Select(b => b.Clone()).ToList(),
            Zones.Select(z => z.Clone()).ToList());
    }
}


// ================================================================
// FESTE ZIELGEBIETE
// ================================================================

public static class TargetZoneFactory
{
    public static List<TargetZone> CreateDefaultZones()
    {
        // ROT wird von links außen nach innen gefüllt.
        // Dabei jeweils zuerst hinten (y=90),
        // danach davor (y=76).

        TargetZone red = new(
            BoxColor.Red,
            5, 45,
            72, 96,
            new List<TargetSlot>
            {
                new(101, BoxColor.Red, new Point2D(40, 90), 0),
                new(102, BoxColor.Red, new Point2D(40, 76), 1),

                new(103, BoxColor.Red, new Point2D(25, 90), 2),
                new(104, BoxColor.Red, new Point2D(25, 76), 3),

                new(105, BoxColor.Red, new Point2D(10, 90), 4),
                new(106, BoxColor.Red, new Point2D(10, 76), 5)
            });


        // BLAU spiegelbildlich von rechts außen nach innen.

        TargetZone blue = new(
            BoxColor.Blue,
            55, 95,
            72, 96,
            new List<TargetSlot>
            {
                new(201, BoxColor.Blue, new Point2D(60, 90), 0),
                new(202, BoxColor.Blue, new Point2D(60, 76), 1),

                new(203, BoxColor.Blue, new Point2D(75, 90), 2),
                new(204, BoxColor.Blue, new Point2D(75, 76), 3),

                new(205, BoxColor.Blue, new Point2D(90, 90), 4),
                new(206, BoxColor.Blue, new Point2D(90, 76), 5)
            });

        return new List<TargetZone>
        {
            red,
            blue
        };
    }
}


// ================================================================
// GEOMETRIE
// ================================================================

public static class Geometry2D
{
    public static double Distance(Point2D a, Point2D b)
    {
        double dx = b.X - a.X;
        double dy = b.Y - a.Y;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    public static double HeadingDeg(Point2D a, Point2D b) =>
        HeadingDeg(b.X - a.X, b.Y - a.Y);

    public static double HeadingDeg(double dx, double dy)
    {
        double angle = Math.Atan2(dy, dx) * 180.0 / Math.PI;
        return NormalizeAngle(angle);
    }

    public static double NormalizeAngle(double angle)
    {
        angle %= 360.0;
        if (angle < 0)
            angle += 360.0;
        return angle;
    }

    // positiv = links, negativ = rechts
    public static double ShortestTurn(double fromDeg, double toDeg)
    {
        return (toDeg - fromDeg + 540.0) % 360.0 - 180.0;
    }

    public static double DistancePointToSegment(
        Point2D point,
        Point2D a,
        Point2D b)
    {
        double dx = b.X - a.X;
        double dy = b.Y - a.Y;
        double lengthSquared = dx * dx + dy * dy;

        if (lengthSquared < 1e-12)
            return Distance(point, a);

        double t =
            ((point.X - a.X) * dx + (point.Y - a.Y) * dy) /
            lengthSquared;

        t = Math.Max(0.0, Math.Min(1.0, t));

        Point2D nearest = new(
            a.X + t * dx,
            a.Y + t * dy);

        return Distance(point, nearest);
    }
}


// ================================================================
// ADAPTIVER VOR-ANFAHRPUNKT Q AM FELDRAND
//
// Q liegt normalerweise 20 cm hinter der Kiste auf exakt derselben
// Geraden wie P und die spätere Schubrichtung.
//
// Problemfall:
// Eine Kiste steht nahe am Feldrand und muss schräg vom Rand weg
// geschoben werden. Dann kann das starre Q bei 20 cm z.B. x=100,5
// ergeben, obwohl P bei 15 cm noch problemlos im Feld liegt.
//
// Lösung:
// Q wird nur so weit verkürzt, dass es mit kleiner Reserve innerhalb
// des 0..100-Feldes bleibt. Die Richtung bleibt unverändert.
// ================================================================

public static class FieldApproachGeometry
{
    private const double FieldSizeCm = 100.0;
    private const double BoundaryReserveCm = 0.25;

    public const double ApproachDistanceCm = 15.0;
    public const double PreferredPreApproachDistanceCm = 20.0;

    public static double GetAdaptivePreApproachDistance(
        Point2D box,
        double ux,
        double uy,
        double preferredDistanceCm =
            PreferredPreApproachDistanceCm)
    {
        double maximumDistance =
            preferredDistanceCm;

        double minimumCoordinate =
            BoundaryReserveCm;

        double maximumCoordinate =
            FieldSizeCm -
            BoundaryReserveCm;


        // Q = box - t * u
        //
        // Für jede Koordinate bestimmen wir, wie groß t maximal
        // werden darf, bevor Q den Feldrand erreicht.

        if (ux > 1e-9)
        {
            maximumDistance =
                Math.Min(
                    maximumDistance,
                    (box.X - minimumCoordinate) / ux);
        }
        else if (ux < -1e-9)
        {
            maximumDistance =
                Math.Min(
                    maximumDistance,
                    (maximumCoordinate - box.X) / (-ux));
        }


        if (uy > 1e-9)
        {
            maximumDistance =
                Math.Min(
                    maximumDistance,
                    (box.Y - minimumCoordinate) / uy);
        }
        else if (uy < -1e-9)
        {
            maximumDistance =
                Math.Min(
                    maximumDistance,
                    (maximumCoordinate - box.Y) / (-uy));
        }


        // P muss weiterhin bei 15 cm liegen können.
        // Zusätzlich verlangen wir wenigstens 0,25 cm Q->P,
        // damit Q nicht numerisch mit P zusammenfällt.
        double minimumUsefulDistance =
            ApproachDistanceCm +
            0.25;

        if (maximumDistance <
            minimumUsefulDistance)
        {
            throw new InvalidOperationException(
                $"Für diese Schubrichtung ist selbst ein verkürzter " +
                $"Vor-Anfahrpunkt Q nicht sicher innerhalb des Feldes. " +
                $"Maximal möglicher Q-Abstand: {maximumDistance:F2} cm.");
        }


        return Math.Min(
            preferredDistanceCm,
            maximumDistance);
    }
}


// ================================================================
// KREISFÖRMIGE SICHERHEITSZONE UM EINE KISTE
// ================================================================

public sealed class CircularObstacle
{
    public int BoxId { get; }
    public Point2D Center { get; }
    public double Radius { get; }

    public CircularObstacle(int boxId, Point2D center, double radius)
    {
        BoxId = boxId;
        Center = center;
        Radius = radius;
    }

    public bool Contains(
    Point2D point,
    double extraClearance = 0.0)
    {
        return
            Geometry2D.Distance(
                Center,
                point)
            <= Radius + extraClearance;
    }


    public bool SegmentBlocked(
        Point2D a,
        Point2D b,
        double extraClearance = 0.0)
    {
        double distance =
            Geometry2D.DistancePointToSegment(
                Center,
                a,
                b);

        return
            distance <=
            Radius + extraClearance;
    }

    public List<Point2D> GetRoutingPoints(double clearance)
    {
        const int count = 12;
        double radius = Radius + clearance;
        List<Point2D> result = new();

        for (int i = 0; i < count; i++)
        {
            double angle = i * 2.0 * Math.PI / count;

            result.Add(
                new Point2D(
                    Center.X + radius * Math.Cos(angle),
                    Center.Y + radius * Math.Sin(angle)));
        }

        return result;
    }
}


// ================================================================
// NAVIGATION PLANNER
// ================================================================

public static class NavigationPlanner
{
    private const double FieldSizeCm = 100.0;
    // Tatsächlicher zusätzlicher Sicherheitsabstand
    // für ALLE Fahrstrecken.
    private const double PathSafetyMarginCm = 2.0;

    // Wegpunkte liegen noch etwas weiter außen,
    // damit Verbindungssegmente nicht genau
    // auf der Sicherheitsgrenze liegen.
    private const double RoutingPointClearanceCm = 3.0;

    public static NavigationPlan Create(
        Point2D start,
        double startHeadingDeg,
        Point2D goal,
        IReadOnlyList<CircularObstacle> obstacles)
    {
        if (Geometry2D.Distance(start, goal) < 0.001)
        {
            return new NavigationPlan(
                new List<Point2D> { start },
                new List<NavigationLeg>(),
                false,
                startHeadingDeg,
                0.0);
        }

        foreach (CircularObstacle obstacle in obstacles)
        {
            if (obstacle.Contains(start, PathSafetyMarginCm))
            {
                throw new InvalidOperationException(
                    $"Rover startet in der Sicherheitszone von Kiste #{obstacle.BoxId}.");
            }

            if (obstacle.Contains(goal, PathSafetyMarginCm))
            {
                throw new InvalidOperationException(
                    $"Anfahrpunkt liegt in der Sicherheitszone von Kiste #{obstacle.BoxId}.");
            }
        }

        if (SegmentFree(start, goal, obstacles))
        {
            return BuildNavigationPlan(
                new List<Point2D> { start, goal },
                startHeadingDeg,
                usedDetour: false);
        }

        List<Point2D> nodes = new() { start, goal };

        foreach (CircularObstacle obstacle in obstacles)
        {
            foreach (Point2D point in obstacle.GetRoutingPoints(RoutingPointClearanceCm))
            {
                if (!InsideField(point))
                    continue;

                if (InsideAnyObstacle(point, obstacles))
                    continue;

                nodes.Add(point);
            }
        }

        int count = nodes.Count;
        double[,] graph = new double[count, count];

        for (int i = 0; i < count; i++)
        {
            for (int j = 0; j < count; j++)
                graph[i, j] = double.PositiveInfinity;

            graph[i, i] = 0.0;
        }

        for (int i = 0; i < count; i++)
        {
            for (int j = i + 1; j < count; j++)
            {
                if (!SegmentFree(nodes[i], nodes[j], obstacles))
                    continue;

                double d = Geometry2D.Distance(nodes[i], nodes[j]);
                graph[i, j] = d;
                graph[j, i] = d;
            }
        }

        double[] distanceTo =
            Enumerable.Repeat(double.PositiveInfinity, count).ToArray();

        int[] previous =
            Enumerable.Repeat(-1, count).ToArray();

        bool[] visited = new bool[count];
        distanceTo[0] = 0.0;

        for (int iteration = 0; iteration < count; iteration++)
        {
            int u = -1;
            double best = double.PositiveInfinity;

            for (int i = 0; i < count; i++)
            {
                if (!visited[i] && distanceTo[i] < best)
                {
                    best = distanceTo[i];
                    u = i;
                }
            }

            if (u < 0 || u == 1)
                break;

            visited[u] = true;

            for (int v = 0; v < count; v++)
            {
                if (double.IsPositiveInfinity(graph[u, v]))
                    continue;

                double candidate = distanceTo[u] + graph[u, v];

                if (candidate < distanceTo[v])
                {
                    distanceTo[v] = candidate;
                    previous[v] = u;
                }
            }
        }

        if (double.IsPositiveInfinity(distanceTo[1]))
        {
            throw new InvalidOperationException(
                "Kein kollisionsfreier Weg zum Anfahrpunkt gefunden.");
        }

        List<Point2D> reversePath = new();
        int current = 1;

        while (current >= 0)
        {
            reversePath.Add(nodes[current]);

            if (current == 0)
                break;

            current = previous[current];
        }

        reversePath.Reverse();

        return BuildNavigationPlan(
            reversePath,
            startHeadingDeg,
            usedDetour: true);
    }

    private static bool SegmentFree(
        Point2D a,
        Point2D b,
        IReadOnlyList<CircularObstacle> obstacles)
    {
        foreach (CircularObstacle obstacle in obstacles)
        {
            if (obstacle.SegmentBlocked(a, b, PathSafetyMarginCm))
            {
                return false;
            }
        }

        return true;
    }

    private static bool InsideAnyObstacle(
        Point2D point,
        IReadOnlyList<CircularObstacle> obstacles)
    {
        foreach (CircularObstacle obstacle in obstacles)
        {
            if (obstacle.Contains(point))
                return true;
        }

        return false;
    }

    private static NavigationPlan BuildNavigationPlan(
        List<Point2D> points,
        double startHeadingDeg,
        bool usedDetour)
    {
        List<NavigationLeg> legs = new();
        double heading = startHeadingDeg;
        double total = 0.0;

        for (int i = 0; i < points.Count - 1; i++)
        {
            Point2D from = points[i];
            Point2D to = points[i + 1];

            double newHeading = Geometry2D.HeadingDeg(from, to);
            double turn = Geometry2D.ShortestTurn(heading, newHeading);
            double distance = Geometry2D.Distance(from, to);

            legs.Add(
                new NavigationLeg(
                    from,
                    to,
                    newHeading,
                    turn,
                    distance));

            total += distance;
            heading = newHeading;
        }

        return new NavigationPlan(
            points,
            legs,
            usedDetour,
            heading,
            total);
    }

    private static bool InsideField(Point2D point) =>
        point.X >= 0 &&
        point.X <= FieldSizeCm &&
        point.Y >= 0 &&
        point.Y <= FieldSizeCm;
}


// ================================================================
// PUSH PLANNER
// ================================================================

public static class PushPlanner
{
    private const double FieldSizeCm = 100.0;

    // P = eigentlicher Anfahrpunkt
    private const double ApproachDistanceCm = 15.0;

    // Q = Vor-Anfahrpunkt.
    // Normal 20 cm hinter der Kiste; nahe am Feldrand wird Q
    // automatisch entlang derselben Geraden verkürzt.
    private const double PreferredPreApproachDistanceCm = 20.0;

    private const double BumperOffsetCm = 6.0;
    private const double BoxLengthCm = 5.0;

    // Nach dem Schieben weit genug zurück,
    // damit der Rover außerhalb der Sicherheitszone steht.
    private const double RetreatCm = 7.0;

    private const double NavigationObstacleRadiusCm = 12.0;

    // Gleiche zusätzliche Reserve wie im NavigationPlanner.
    private const double PathSafetyMarginCm = 2.0;

    private const double PushCorridorClearanceCm = 12.0;


    public static PushPlan Create(
        RoverPose rover,
        BoxState activeBox,
        Point2D target,
        IReadOnlyList<BoxState> allBoxes)
    {
        Point2D box = activeBox.Position;

        ValidatePoint(
            new Point2D(rover.X, rover.Y),
            "Rover");

        ValidatePoint(
            box,
            $"Kiste #{activeBox.Id}");

        ValidatePoint(
            target,
            "Zielpunkt");


        // ============================================================
        // SCHUBRICHTUNG
        // ============================================================

        double pushDx =
            target.X - box.X;

        double pushDy =
            target.Y - box.Y;

        double pushDistance =
            Math.Sqrt(
                pushDx * pushDx +
                pushDy * pushDy);

        if (pushDistance < 0.001)
        {
            throw new InvalidOperationException(
                "Kiste steht bereits am Zielpunkt.");
        }


        double ux =
            pushDx / pushDistance;

        double uy =
            pushDy / pushDistance;

        double pushHeading =
            Geometry2D.HeadingDeg(
                pushDx,
                pushDy);


        // ============================================================
        // SCHUBKORRIDOR PRÜFEN
        // ============================================================

        foreach (BoxState other in allBoxes)
        {
            if (other.Id == activeBox.Id)
                continue;

            double clearance =
                Geometry2D.DistancePointToSegment(
                    other.Position,
                    box,
                    target);

            if (clearance < PushCorridorClearanceCm)
            {
                throw new InvalidOperationException(
                    $"Schubweg wird durch Kiste #{other.Id} blockiert " +
                    $"({clearance:F1} cm Abstand).");
            }
        }


        // ============================================================
        // P = EIGENTLICHER ANFAHRPUNKT
        //
        // 15 cm hinter dem Mittelpunkt der Kiste.
        // ============================================================

        Point2D approach =
            new(
                box.X -
                    ApproachDistanceCm * ux,

                box.Y -
                    ApproachDistanceCm * uy);


        // ============================================================
        // Q = VOR-ANFAHRPUNKT
        //
        // Normal 20 cm hinter der Kiste.
        // Falls dieser Punkt knapp außerhalb des 100x100-Feldes
        // liegen würde, wird Q entlang der gleichen Schublinie
        // automatisch so weit verkürzt, dass er im Feld bleibt.
        //
        // Bis Q darf der normale NavigationPlanner fahren.
        // Danach wird auf die endgültige Richtung gedreht und nur
        // noch schnurgerade gefahren.
        // ============================================================

        double preApproachDistanceCm =
            FieldApproachGeometry.GetAdaptivePreApproachDistance(
                box,
                ux,
                uy,
                PreferredPreApproachDistanceCm);

        Point2D preApproach =
            new(
                box.X -
                    preApproachDistanceCm * ux,

                box.Y -
                    preApproachDistanceCm * uy);


        ValidatePoint(
            approach,
            "Anfahrpunkt P");

        ValidatePoint(
            preApproach,
            "Vor-Anfahrpunkt Q");


        // ============================================================
        // HINDERNISSE
        // ============================================================

        List<CircularObstacle> obstacles =
            new();

        foreach (BoxState obstacleBox in allBoxes)
        {
            obstacles.Add(
                new CircularObstacle(
                    obstacleBox.Id,
                    obstacleBox.Position,
                    NavigationObstacleRadiusCm));
        }


        // ============================================================
        // NEU:
        // Auch die gerade Endanflugstrecke Q -> P muss sicher sein.
        //
        // Die aktive Kiste darf dabei ebenfalls geprüft werden:
        // P liegt 15 cm entfernt; Q liegt normalerweise 20 cm
        // entfernt und kann am Feldrand etwas näher liegen.
        // Die verbotene Zone reicht nur bis 14 cm.
        // ============================================================

        foreach (CircularObstacle obstacle in obstacles)
        {
            if (obstacle.SegmentBlocked(
                    preApproach,
                    approach,
                    PathSafetyMarginCm))
            {
                throw new InvalidOperationException(
                    $"Gerader Endanflug Q->P wird durch " +
                    $"Kiste #{obstacle.BoxId} blockiert.");
            }
        }


        // ============================================================
        // NORMALE NAVIGATION NUR BIS Q
        // ============================================================

        NavigationPlan navigation =
            NavigationPlanner.Create(
                start:
                    new Point2D(
                        rover.X,
                        rover.Y),

                startHeadingDeg:
                    rover.HeadingDeg,

                goal:
                    preApproach,

                obstacles:
                    obstacles);


        // ============================================================
        // DREHUNG AM PUNKT Q
        // ============================================================

        double finalTurn =
            Geometry2D.ShortestTurn(
                navigation.FinalHeadingDeg,
                pushHeading);


        // ============================================================
        // Q -> P
        //
        // Normal 20 - 15 = 5 cm.
        // Bei randbedingt verkürztem Q entsprechend weniger.
        // ============================================================

        double finalApproachDistance =
            preApproachDistanceCm -
            ApproachDistanceCm;


        // ============================================================
        // P -> KISTENKONTAKT
        //
        // P liegt 15 cm hinter dem Kistenmittelpunkt.
        //
        // Roverachse bis Schieber = 6 cm
        // halbe Kistenlänge     = 2,5 cm
        //
        // 15 - 6 - 2,5 = 6,5 cm
        // ============================================================

        double contactDistance =
            ApproachDistanceCm -
            BumperOffsetCm -
            BoxLengthCm / 2.0;


        // ============================================================
        // ROVERPOSITION NACH SCHUB + RÜCKZUG
        // ============================================================

        double roverBehindTarget =
            BumperOffsetCm +
            BoxLengthCm / 2.0 +
            RetreatCm;


        RoverPose endPose =
            new(
                target.X -
                    roverBehindTarget * ux,

                target.Y -
                    roverBehindTarget * uy,

                pushHeading);


        // ============================================================
        // GESAMTWEG
        // ============================================================

        double totalTravel =
            navigation.DistanceCm +
            finalApproachDistance +
            contactDistance +
            pushDistance +
            RetreatCm;


        return new PushPlan(
            PreApproachPoint:
                preApproach,

            ApproachPoint:
                approach,

            PushHeadingDeg:
                pushHeading,

            Navigation:
                navigation,

            FinalTurnDeg:
                finalTurn,

            FinalApproachCm:
                finalApproachDistance,

            ContactCm:
                contactDistance,

            PushCm:
                pushDistance,

            RetreatCm:
                RetreatCm,

            TotalTravelCm:
                totalTravel,

            EndPose:
                endPose);
    }


    private static void ValidatePoint(
        Point2D point,
        string name)
    {
        if (point.X < 0 ||
            point.X > FieldSizeCm ||
            point.Y < 0 ||
            point.Y > FieldSizeCm)
        {
            throw new InvalidOperationException(
                $"{name} ({point.X:F1}, {point.Y:F1}) " +
                $"liegt außerhalb des Feldes.");
        }
    }
}





// ================================================================
// FREE_PUSH-PLANER
//
// Ollama darf nur Primärkiste, Richtung, seitlichen Versatz und
// kurze Schubstrecke vorschlagen. Alles andere wird hier geprüft.
// ================================================================

public static class FreePushPlanner
{
    private const double FieldSizeCm = 100.0;
    private const double FieldReserveCm = 0.5;

    private const double BoxHalfLengthCm = 2.5;
    private const double BoxHalfWidthCm = 3.0;

    private const double PusherHalfWidthCm = 7.5;
    private const double BumperOffsetCm = 6.0;

    private const double ContactApproachCm = 6.5;
    private const double PreferredQGapCm = 5.0;

    private const double NavigationObstacleRadiusCm = 12.0;
    private const double PathSafetyMarginCm = 2.0;

    private const double MinimumPushCm = 3.0;
    private const double MaximumPushCm = 15.0;
    private const double MaximumLateralOffsetCm = 6.0;
    private const double ExtraRetreatCm = 7.0;

    // Freischub-Verbesserung 2026-09-09, Dr. Erhard Henkes.
    // Konservative Kreisumhuellung: Kamera liefert keine Kistenorientierung.
    private static readonly double BoxEnvelopeRadiusCm =
        Math.Sqrt(BoxHalfLengthCm * BoxHalfLengthCm + BoxHalfWidthCm * BoxHalfWidthCm);
    private const double ContactMarginCm = 0.5;


    public static bool TryCreate(
        WorldModel world,
        FreePushProposal proposal,
        out AgentActionCandidate? candidate,
        out string rejection)
    {
        candidate = null;
        rejection = "";

        BoxState? primary =
            world.Boxes
                .FirstOrDefault(
                    b => b.Id == proposal.PrimaryBoxId);

        if (primary == null)
        {
            rejection =
                $"Primärkiste #{proposal.PrimaryBoxId} existiert nicht.";
            return false;
        }

        if (primary.Sorted)
        {
            rejection =
                $"Primärkiste #{primary.Id} ist bereits sortiert.";
            return false;
        }

        if (!double.IsFinite(proposal.PushHeadingDeg) ||
            !double.IsFinite(proposal.LateralOffsetCm) ||
            !double.IsFinite(proposal.PushDistanceCm))
        {
            rejection =
                "Nicht endliche Zahl in KI-Vorschlag.";
            return false;
        }

        if (proposal.PushDistanceCm < MinimumPushCm ||
            proposal.PushDistanceCm > MaximumPushCm)
        {
            rejection =
                $"Freischub muss zwischen {MinimumPushCm:F0} und " +
                $"{MaximumPushCm:F0} cm liegen.";
            return false;
        }

        if (Math.Abs(proposal.LateralOffsetCm) >
            MaximumLateralOffsetCm)
        {
            rejection =
                $"Seitlicher Schieberversatz darf höchstens " +
                $"{MaximumLateralOffsetCm:F0} cm betragen.";
            return false;
        }


        FreePushGeometry geometry =
            CalculateGeometry(
                primary.Position,
                proposal.PushHeadingDeg,
                proposal.LateralOffsetCm,
                proposal.PushDistanceCm);


        if (!InsideField(geometry.ApproachPoint) ||
            !InsideField(geometry.PreApproachPoint) ||
            !InsideField(geometry.ContactRoverPoint))
        {
            rejection =
                "Q, P oder Rover-Kontaktposition liegt zu nah am Feldrand.";
            return false;
        }


        if (!PusherSweepInsideField(
                geometry.ContactCenter,
                geometry.PushHeadingDeg,
                geometry.PushDistanceCm))
        {
            rejection =
                "Schieber würde während des Freischubs den Feldrand überschreiten.";
            return false;
        }


        List<int> affected =
            GetPotentiallyAffectedBoxes(
                world,
                geometry);

        if (!affected.Contains(primary.Id))
        {
            rejection =
                $"Die vorgeschlagene Schieberlinie trifft Primärkiste #{primary.Id} " +
                "nach dem Sicherheitsmodell nicht.";
            return false;
        }


        foreach (BoxState box in world.Boxes)
        {
            if (!affected.Contains(box.Id))
                continue;

            if (box.Sorted)
            {
                rejection =
                    $"Der Freischub könnte bereits sortierte Kiste #{box.Id} berühren.";
                return false;
            }
        }


        double rad =
            geometry.PushHeadingDeg *
            Math.PI /
            180.0;

        double ux =
            Math.Cos(rad);

        double uy =
            Math.Sin(rad);

        foreach (BoxState box in world.Boxes)
        {
            if (!affected.Contains(box.Id))
                continue;

            Point2D worstCase =
                new(
                    box.Position.X +
                        (geometry.PushDistanceCm + ContactApproachCm) * ux,

                    box.Position.Y +
                        (geometry.PushDistanceCm + ContactApproachCm) * uy);

            if (worstCase.X <
                    BoxEnvelopeRadiusCm + ContactMarginCm + FieldReserveCm ||
                worstCase.X >
                    FieldSizeCm - BoxEnvelopeRadiusCm - ContactMarginCm - FieldReserveCm ||
                worstCase.Y <
                    BoxEnvelopeRadiusCm + ContactMarginCm + FieldReserveCm ||
                worstCase.Y >
                    FieldSizeCm - BoxEnvelopeRadiusCm - ContactMarginCm - FieldReserveCm)
            {
                rejection =
                    $"Kiste #{box.Id} könnte beim Freischub den Feldrand erreichen.";
                return false;
            }
        }


        List<CircularObstacle> obstacles =
            world.Boxes
                .Select(
                    b =>
                        new CircularObstacle(
                            b.Id,
                            b.Position,
                            NavigationObstacleRadiusCm))
                .ToList();


        foreach (CircularObstacle obstacle in obstacles)
        {
            if (obstacle.SegmentBlocked(
                    geometry.PreApproachPoint,
                    geometry.ApproachPoint,
                    PathSafetyMarginCm))
            {
                rejection =
                    $"Gerader FREE_PUSH-Endanflug Q->P wird durch " +
                    $"Kiste #{obstacle.BoxId} blockiert.";
                return false;
            }
        }


        NavigationPlan navigation;

        try
        {
            navigation =
                NavigationPlanner.Create(
                    start:
                        new Point2D(
                            world.Rover.X,
                            world.Rover.Y),

                    startHeadingDeg:
                        world.Rover.HeadingDeg,

                    goal:
                        geometry.PreApproachPoint,

                    obstacles:
                        obstacles);
        }
        catch (Exception ex)
        {
            rejection =
                "Navigation zum FREE_PUSH-Q nicht möglich: " +
                ex.Message;
            return false;
        }


        double finalTurn =
            Geometry2D.ShortestTurn(
                navigation.FinalHeadingDeg,
                geometry.PushHeadingDeg);


        RoverPose endPose =
            CalculateEndPose(
                geometry);


        double totalTravel =
            navigation.DistanceCm +
            geometry.FinalApproachCm +
            ContactApproachCm +
            geometry.PushDistanceCm +
            geometry.RetreatDistanceCm;


        PushPlan pushPlan =
            new(
                PreApproachPoint:
                    geometry.PreApproachPoint,

                ApproachPoint:
                    geometry.ApproachPoint,

                PushHeadingDeg:
                    geometry.PushHeadingDeg,

                Navigation:
                    navigation,

                FinalTurnDeg:
                    finalTurn,

                FinalApproachCm:
                    geometry.FinalApproachCm,

                ContactCm:
                    ContactApproachCm,

                PushCm:
                    geometry.PushDistanceCm,

                RetreatCm:
                    geometry.RetreatDistanceCm,

                TotalTravelCm:
                    totalTravel,

                EndPose:
                    endPose);


        Point2D predictedPrimaryDestination =
            new(
                primary.Position.X +
                    geometry.PushDistanceCm * ux,

                primary.Position.Y +
                    geometry.PushDistanceCm * uy);


        string affectedText =
            string.Join(
                ", ",
                affected.Select(id => "#" + id));

        string reason =
            $"OLLAMA-FREISCHUB: {proposal.Reason} " +
            $"Sicherheitsmodell: möglicherweise betroffen {affectedText}.";


        FreePushData freePush =
            new(
                PrimaryReferencePosition:
                    primary.Position,

                PushHeadingDeg:
                    geometry.PushHeadingDeg,

                LateralOffsetCm:
                    proposal.LateralOffsetCm,

                PushDistanceCm:
                    geometry.PushDistanceCm,

                ExpectedAffectedBoxIds:
                    affected,

                AiReason:
                    proposal.Reason);


        // Der eigentliche Zukunftsvergleich erfolgt nach der Sicherheitspruefung
        // in OllamaDeadlockSolver. Score bleibt ein Wegkosten-Tiebreaker.
        double score = totalTravel + affected.Count * 10.0;



        candidate =
            new AgentActionCandidate(
                AgentActionKind.FreePush,
                primary,
                null,
                predictedPrimaryDestination,
                pushPlan,
                score,
                reason,
                freePush);

        return true;
    }


    public static FreePushGeometry CalculateGeometry(
        Point2D primaryBoxPosition,
        double pushHeadingDeg,
        double lateralOffsetCm,
        double pushDistanceCm)
    {
        double heading =
            NormalizeAngle(
                pushHeadingDeg);

        double rad =
            heading *
            Math.PI /
            180.0;

        double ux =
            Math.Cos(rad);

        double uy =
            Math.Sin(rad);

        double vx =
            -uy;

        double vy =
            ux;


        Point2D contactCenter =
            new(
                primaryBoxPosition.X -
                    BoxHalfLengthCm * ux +
                    lateralOffsetCm * vx,

                primaryBoxPosition.Y -
                    BoxHalfLengthCm * uy +
                    lateralOffsetCm * vy);


        Point2D contactRover =
            new(
                contactCenter.X -
                    BumperOffsetCm * ux,

                contactCenter.Y -
                    BumperOffsetCm * uy);


        Point2D approach =
            new(
                contactRover.X -
                    ContactApproachCm * ux,

                contactRover.Y -
                    ContactApproachCm * uy);


        double qGap =
            MaximumBackwardDistanceInsideField(
                approach,
                ux,
                uy,
                PreferredQGapCm);

        if (qGap < 0.25)
        {
            throw new InvalidOperationException(
                "Für FREE_PUSH ist hinter P kein sicherer Q-Punkt verfügbar.");
        }


        Point2D preApproach =
            new(
                approach.X -
                    qGap * ux,

                approach.Y -
                    qGap * uy);


        double retreat =
            pushDistanceCm +
            ExtraRetreatCm;


        return new FreePushGeometry(
            PrimaryBoxPosition:
                primaryBoxPosition,

            ContactCenter:
                contactCenter,

            ContactRoverPoint:
                contactRover,

            ApproachPoint:
                approach,

            PreApproachPoint:
                preApproach,

            PushHeadingDeg:
                heading,

            FinalApproachCm:
                qGap,

            PushDistanceCm:
                pushDistanceCm,

            RetreatDistanceCm:
                retreat);
    }


    private static List<int> GetPotentiallyAffectedBoxes(
        WorldModel world, FreePushGeometry geometry)
    {
        double rad = geometry.PushHeadingDeg * Math.PI / 180.0;
        double ux = Math.Cos(rad), uy = Math.Sin(rad);
        double vx = -uy, vy = ux;
        double radius = BoxEnvelopeRadiusCm + ContactMarginCm;
        var affected = new HashSet<int>();

        // Gesamter gerade Anflug Q->P->Kontakt->Schubende.
        // Frueher fehlten Kisten hinter dem nominellen Kontaktpunkt.
        double start = -ContactApproachCm - geometry.FinalApproachCm;
        foreach (BoxState box in world.Boxes)
        {
            double dx = box.Position.X - geometry.ContactCenter.X;
            double dy = box.Position.Y - geometry.ContactCenter.Y;
            double along = dx * ux + dy * uy;
            double lateral = Math.Abs(dx * vx + dy * vy);
            if (along >= start - radius &&
                along <= geometry.PushDistanceCm + radius &&
                lateral <= PusherHalfWidthCm + radius)
                affected.Add(box.Id);
        }

        // Folgeberuehrungen: Eine geschobene Kiste kann weitere mitnehmen.
        // Konservative Erreichbarkeit, keine exakte Starrkoerper-Simulation.
        double maxMotion = geometry.PushDistanceCm + ContactApproachCm;
        bool changed;
        do
        {
            changed = false;
            foreach (BoxState moving in world.Boxes.Where(b => affected.Contains(b.Id)).ToList())
                foreach (BoxState other in world.Boxes)
                {
                    if (affected.Contains(other.Id)) continue;
                    double dx = other.Position.X - moving.Position.X;
                    double dy = other.Position.Y - moving.Position.Y;
                    double along = dx * ux + dy * uy;
                    if (along < -ContactMarginCm) continue;
                    double t = Math.Clamp(along, 0.0, maxMotion);
                    double ex = dx - t * ux, ey = dy - t * uy;
                    double clearance = 2.0 * BoxEnvelopeRadiusCm + 0.25;
                    if (ex * ex + ey * ey <= clearance * clearance)
                        changed |= affected.Add(other.Id);
                }
        } while (changed);
        return affected.OrderBy(id => id).ToList();
    }

    public static (int Sortable, int ReleasedPairs, double LocalGain) EvaluateFuture(
        WorldModel world, AgentActionCandidate candidate)
    {
        FreePushData data = candidate.FreePush!;
        double rad = data.PushHeadingDeg * Math.PI / 180.0;
        int sortable = int.MaxValue, released = int.MaxValue;
        double gain = double.PositiveInfinity;
        // Zwei vereinfachte Folgewelten: voller und 70%-Schub nach Anfahrt.
        // Sicherheits-Huellmenge bedeutet NICHT, dass alle Kisten gleich mitfahren.
        // Das getrennte Kontaktmodell berechnet individuelle Bewegungswege.
        // Mindestwert bevorzugt Ergebnisse, die nicht nur bei exakter Laenge entstehen.
        foreach (double fraction in new[] { 0.7, 1.0 })
        {
            WorldModel predicted = world.DeepClone();
            predicted.Rover = candidate.Plan.EndPose;
            Point2D pusherStart = new(
                candidate.Plan.ApproachPoint.X + 6.0 * Math.Cos(rad),
                candidate.Plan.ApproachPoint.Y + 6.0 * Math.Sin(rad));
            var positions = ContactMotionModel.Predict(world.Boxes, pusherStart,
                data.PushHeadingDeg, candidate.Plan.ContactCm + fraction * data.PushDistanceCm);
            foreach (BoxState box in predicted.Boxes)
                box.Position = positions[box.Id];
            sortable = Math.Min(sortable, TaskPlanner.CountDirectlySortableBoxes(predicted));
            int scenarioReleased = 0;
            double scenarioGain = 0.0;
            var open = world.Boxes.Where(b => !b.Sorted).ToList();
            for (int i = 0; i < open.Count; i++)
                for (int j = i + 1; j < open.Count; j++)
                {
                    double before = Geometry2D.Distance(open[i].Position, open[j].Position);
                    double after = Geometry2D.Distance(
                        predicted.Boxes.Single(b => b.Id == open[i].Id).Position,
                        predicted.Boxes.Single(b => b.Id == open[j].Id).Position);
                    // Nur enge Nachbarschaften zaehlen. Bereits getrennte Gruppen
                    // weiter auseinanderzuschieben liefert keinen Fortschrittsbonus.
                    const double clearance = 14.0;
                    if (before < clearance && after >= clearance) scenarioReleased++;
                    if (before >= clearance && after < clearance) scenarioReleased--;
                    scenarioGain += Math.Min(after, clearance) - Math.Min(before, clearance);
                }
            released = Math.Min(released, scenarioReleased);
            gain = Math.Min(gain, scenarioGain);
        }
        return (sortable, released, gain);
    }


    private static bool PusherSweepInsideField(
        Point2D contactCenter,
        double headingDeg,
        double pushDistanceCm)
    {
        double rad =
            headingDeg *
            Math.PI /
            180.0;

        double ux =
            Math.Cos(rad);

        double uy =
            Math.Sin(rad);

        double vx =
            -uy;

        double vy =
            ux;


        foreach (double along
                 in new[] { -ContactApproachCm - PreferredQGapCm, pushDistanceCm })
        {
            foreach (double lateral
                     in new[] { -PusherHalfWidthCm, PusherHalfWidthCm })
            {
                Point2D corner =
                    new(
                        contactCenter.X +
                            along * ux +
                            lateral * vx,

                        contactCenter.Y +
                            along * uy +
                            lateral * vy);

                if (!InsideField(corner))
                    return false;
            }
        }

        return true;
    }


    private static RoverPose CalculateEndPose(
        FreePushGeometry geometry)
    {
        double rad =
            geometry.PushHeadingDeg *
            Math.PI /
            180.0;

        double ux =
            Math.Cos(rad);

        double uy =
            Math.Sin(rad);

        double displacementFromContact =
            geometry.PushDistanceCm -
            geometry.RetreatDistanceCm;


        return new RoverPose(
            geometry.ContactRoverPoint.X +
                displacementFromContact * ux,

            geometry.ContactRoverPoint.Y +
                displacementFromContact * uy,

            geometry.PushHeadingDeg);
    }


    private static double MaximumBackwardDistanceInsideField(
        Point2D from,
        double ux,
        double uy,
        double preferred)
    {
        double result =
            preferred;

        double min =
            FieldReserveCm;

        double max =
            FieldSizeCm -
            FieldReserveCm;


        if (ux > 1e-9)
        {
            result =
                Math.Min(
                    result,
                    (from.X - min) / ux);
        }
        else if (ux < -1e-9)
        {
            result =
                Math.Min(
                    result,
                    (max - from.X) / (-ux));
        }


        if (uy > 1e-9)
        {
            result =
                Math.Min(
                    result,
                    (from.Y - min) / uy);
        }
        else if (uy < -1e-9)
        {
            result =
                Math.Min(
                    result,
                    (max - from.Y) / (-uy));
        }


        return result;
    }


    private static bool InsideField(
        Point2D p)
    {
        return
            p.X >= FieldReserveCm &&
            p.X <= FieldSizeCm - FieldReserveCm &&
            p.Y >= FieldReserveCm &&
            p.Y <= FieldSizeCm - FieldReserveCm;
    }


    private static double NormalizeAngle(
        double angle)
    {
        angle %=
            360.0;

        if (angle < 0.0)
            angle += 360.0;

        return angle;
    }
}


// ================================================================
// LOKALER OLLAMA-DEADLOCK-STRATEGE
// ================================================================

public static class OllamaDeadlockSolver
{
    private static readonly Queue<string> OutcomeHistory = new();

    public static void RecordOutcome(WorldModel world, AgentActionCandidate action,
        IReadOnlyDictionary<int, Point2D> before)
    {
        var moved = world.Boxes.Where(b => Geometry2D.Distance(before[b.Id], b.Position) > 1.0)
            .Select(b => "#" + b.Id).ToList();
        var pairs = new List<string>();
        var open = world.Boxes.Where(b => !b.Sorted).ToList();
        for (int i = 0; i < open.Count; i++)
            for (int j = i + 1; j < open.Count; j++)
            {
                double oldDistance = Geometry2D.Distance(before[open[i].Id], before[open[j].Id]);
                double newDistance = Geometry2D.Distance(open[i].Position, open[j].Position);
                if (oldDistance < 14.0 || newDistance < 14.0)
                    pairs.Add($"#{open[i].Id}/#{open[j].Id}: {oldDistance:F1}->{newDistance:F1} cm");
            }
        FreePushData data = action.FreePush!;
        string summary = $"Primär #{action.Box.Id}, Richtung {data.PushHeadingDeg:F1}, " +
            $"Versatz {data.LateralOffsetCm:F1}, Schub {data.PushDistanceCm:F1}: " +
            $"real bewegt [{string.Join(",", moved)}]; " +
            $"Nahpaare [{string.Join("; ", pairs)}]; " +
            $"danach direkt sortierbar: {TaskPlanner.CountDirectlySortableBoxes(world)}.";
        OutcomeHistory.Enqueue(summary);
        while (OutcomeHistory.Count > 6) OutcomeHistory.Dequeue();
        Console.WriteLine("KI-AKTIONSGEDAECHTNIS: " + summary);
        FreePushStrategy.Observe(world, action, before);
    }

    private const string BaseUrl =
        "http://localhost:11434";

    private const int MaximumProposals =
        8;

    private static readonly HttpClient Http =
        new()
        {
            Timeout =
                TimeSpan.FromSeconds(240)
        };


    public static AgentActionCandidate? TryFindFreePush(
        WorldModel world,
        IReadOnlyList<string> directFailures)
    {
        Console.WriteLine();
        Console.WriteLine("=====================================================");
        Console.WriteLine("OLLAMA-DEADLOCK-STRATEGE");
        Console.WriteLine("=====================================================");
        Console.WriteLine(
            "Klassischer Planner ist ausgeschöpft. " +
            "Lokale KI darf jetzt FREE_PUSH-Strategien vorschlagen.");

        try
        {
            AgentActionCandidate? crossPush = FreePushStrategy.TryCrossPush(world);
            if (crossPush != null)
            {
                Console.WriteLine("Gepruefter Querschub gefunden; keine Ollama-Anfrage erforderlich.");
                return crossPush;
            }
            string? model =
                SelectInstalledModel();

            if (string.IsNullOrWhiteSpace(model))
            {
                Console.WriteLine(
                    "Ollama ist erreichbar, aber kein geeignetes " +
                    "lokales Sprachmodell wurde gefunden.");
                return null;
            }


            Console.WriteLine(
                $"Ollama-Modell: {model}");
            Console.WriteLine(
                "Ollama-Thinking: AUS (schneller strukturierter JSON-Modus)");
            Console.WriteLine(
                "HTTP-Timeout: 240 Sekunden");

            const int MaximumProposalRounds = 3;
            var feedback = new List<string>();
            string basePrompt = BuildPrompt(world, directFailures);
            for (int round = 1; round <= MaximumProposalRounds; round++)
            {
                Console.WriteLine($"\nKI-VORSCHLAGSRUNDE {round}/{MaximumProposalRounds} - Rover bleibt stehen.");
                string prompt = basePrompt + "\n" + BuildRetryFeedback(round, feedback);
                Stopwatch requestClock = Stopwatch.StartNew();
                string rawJson = CallOllama(model, prompt);
                requestClock.Stop();
                Console.WriteLine($"Ollama-Antwortdauer: {requestClock.Elapsed.TotalSeconds:F1} s");
                FreePushProposalEnvelope? envelope;
                try
                {
                    envelope = ParseProposalEnvelope(rawJson);
                }
                catch (JsonException)
                {
                    feedback.Add($"Runde {round}: Antwort war kein lesbares JSON im geforderten Format. " +
                        "Liefere ein Objekt mit proposals als Array und Zahlen als Zahlen.");
                    Console.WriteLine("JSON-Antwort ungueltig; erneute Anfrage innerhalb des Rundenlimits.");
                    continue;
                }
                if (envelope?.Proposals == null || envelope.Proposals.Count == 0)
                {
                    feedback.Add($"Runde {round}: Keine Vorschlaege erhalten. Liefere mehrere verschiedene Alternativen.");
                    Console.WriteLine("Keine Vorschlaege; erneute Anfrage innerhalb des Rundenlimits.");
                    continue;
                }
                Stopwatch checkClock = Stopwatch.StartNew();
                Console.WriteLine($"KI lieferte {envelope.Proposals.Count} Vorschlag/Vorschlaege; maximal {MaximumProposals} werden geprueft.");


                var valid = new List<(AgentActionCandidate Candidate, int Sortable,
                    int ReleasedPairs, double LocalGain, double TargetGain)>();

                int number =
                    0;

                foreach (FreePushProposalDto dto
                         in envelope.Proposals.Take(MaximumProposals))
                {
                    number++;
                    if (dto == null)
                    {
                        feedback.Add($"Runde {round}, Vorschlag {number}: null statt eines Vorschlagsobjekts.");
                        continue;
                    }
                    var rejectionDetails = new List<string>();

                    FreePushProposal proposal =
                        new(
                            dto.PrimaryBoxId,
                            dto.PushHeadingDeg,
                            dto.LateralOffsetCm,
                            dto.PushDistanceCm,
                            dto.Reason ?? "");


                    Console.WriteLine();
                    Console.WriteLine(
                        $"KI-Vorschlag {number}: " +
                        $"Kiste #{proposal.PrimaryBoxId}, " +
                        $"Richtung {proposal.PushHeadingDeg:F1}°, " +
                        $"Versatz {proposal.LateralOffsetCm:+0.0;-0.0;0.0} cm, " +
                        $"Schub {proposal.PushDistanceCm:F1} cm");

                    Console.WriteLine(
                        $"KI-Begründung: {proposal.Reason}");


                    // Richtung, Primärkiste und Schublaenge bleiben KI-Strategie.
                    // Beide Seiten pruefen: positiver Versatz ist links in Fahrtrichtung.
                    foreach (double offset in new[] { proposal.LateralOffsetCm, -6.0, -5.0,
                        -3.0, 0.0, 3.0, 5.0, 6.0 }.Distinct())
                    {
                        FreePushProposal variant = proposal with { LateralOffsetCm = offset };
                        try
                        {
                            if (FreePushPlanner.TryCreate(world, variant,
                                out AgentActionCandidate? candidate, out string rejection))
                            {
                                var future = FreePushPlanner.EvaluateFuture(world, candidate!);
                                if (!FreePushStrategy.IsUseful(world, variant, future, out string strategyReason))
                                {
                                    Console.WriteLine($"  Versatz {offset:F1}: STRATEGISCH VERWORFEN - {strategyReason}");
                                    rejectionDetails.Add(strategyReason);
                                    continue;
                                }
                                string outlook = $"Folgewelt-Prognose (70/100%): {future.Sortable} sortierbar; " +
                                    $"geloeste Nahpaare {future.ReleasedPairs}; Nahabstandsgewinn {future.LocalGain:F1} cm";
                                candidate = candidate! with { Reason = candidate!.Reason + " " + outlook };
                                Console.WriteLine($"  Versatz {offset:+0.0;-0.0;0.0} cm: " +
                                    $"AKZEPTIERT. {outlook}; Wegkosten {candidate.Score:F1}.");
                                valid.Add((candidate, future.Sortable, future.ReleasedPairs, future.LocalGain,
                                    FreePushStrategy.HasPendingGroup ? FreePushStrategy.TargetPairGain(world, variant) : 0.0));
                            }
                            else
                            {
                                Console.WriteLine($"  Versatz {offset:+0.0;-0.0;0.0} cm: VERWORFEN - {rejection}");
                                rejectionDetails.Add($"Versatz {offset:F1}: {rejection}");
                            }
                        }
                        catch (InvalidOperationException ex)
                        {
                            Console.WriteLine($"  Versatz {offset:+0.0;-0.0;0.0} cm: VERWORFEN - {ex.Message}");
                            rejectionDetails.Add($"Versatz {offset:F1}: {ex.Message}");
                        }
                    }
                    if (rejectionDetails.Count > 0)
                        feedback.Add($"Runde {round}, Vorschlag {number}: Kiste #{proposal.PrimaryBoxId}, " +
                            $"Richtung {proposal.PushHeadingDeg:F1} Grad, Schub {proposal.PushDistanceCm:F1} cm. " +
                            string.Join("; ", rejectionDetails.Distinct().Take(8)));
                }
                checkClock.Stop();
                Console.WriteLine($"C#-Pruefung und Folgeweltbewertung: {checkClock.Elapsed.TotalSeconds:F1} s");


                if (valid.Count == 0)
                {
                    Console.WriteLine();
                    Console.WriteLine(
                        "Kein KI-Vorschlag bestand Geometrie- UND Fortschrittspruefung.");
                    if (round < MaximumProposalRounds)
                        Console.WriteLine("Ablehnungsgruende gehen an Gemma; suche andere Kisten und Richtungen.");
                    continue;
                }


                AgentActionCandidate best =
                    valid
                        .OrderByDescending(c => c.Sortable)
                        .ThenByDescending(c => c.TargetGain)
                        .ThenByDescending(c => c.ReleasedPairs)
                        .ThenByDescending(c => Math.Round(c.LocalGain, 1))
                        .ThenBy(c => c.Candidate.Score)
                        .First().Candidate;

                Console.WriteLine();
                Console.WriteLine("Auswahl: Sortierbarkeit > aktives Trennziel > Nahpaare > Abstandsgewinn > Wegkosten.");
                Console.WriteLine($"Bester sicherer KI-Freischub: Primärkiste #{best.Box.Id}. {best.Reason}");

                return best;
            }
            Console.WriteLine("Alle drei KI-Vorschlagsrunden sind ausgeschoepft. Kein zulaessiger Freischub gefunden.");
            return null;
        }
        catch (Exception ex)
        {
            Console.WriteLine();
            Console.WriteLine(
                "OLLAMA konnte nicht als Deadlock-Strategie genutzt werden:");
            Console.WriteLine(
                ex.Message);

            return null;
        }
    }


    private static string? SelectInstalledModel()
    {
        return "qwen3:8b"; 
    }

    private static string BuildRetryFeedback(int round, IReadOnlyList<string> feedback)
    {
        var text = new StringBuilder();
        text.AppendLine($"VORSCHLAGSRUNDE {round} VON 3.");
        text.AppendLine(FreePushStrategy.Context());
        text.AppendLine("Liefere nach Moeglichkeit 6 bis 8 verschiedene Vorschlaege im proposals-Array.");
        text.AppendLine("Variiere Primärkiste UND Richtung. 0 Grad=rechts, 90=oben, 180=links, 270=unten.");
        if (feedback.Count > 0)
        {
            text.AppendLine("FEEDBACK DER C#-PRUEFUNG FUER DIESE UNVERAENDERTE STELLUNG:");
            text.AppendLine("Keiner dieser Vorschlaege wurde ausgefuehrt. Alle Kisten und der Rover stehen weiterhin an den oben genannten Positionen.");
            foreach (string item in feedback.TakeLast(24)) text.AppendLine(item);
            text.AppendLine("Wiederhole nicht dieselben gescheiterten Kombinationen. Wenn alle seitlichen Versätze an einer Kiste scheitern, suche eine andere Schubrichtung oder Primärkiste.");
            text.AppendLine("Sicherheitsabstaende und Feldgrenzen bleiben verbindlich. Antworte ausschliesslich im vorgegebenen JSON-Format.");
        }
        return text.ToString();
    }


    private static string BuildPrompt(
        WorldModel world,
        IReadOnlyList<string> directFailures)
    {
        StringBuilder sb =
            new();

        sb.AppendLine(
            "Du bist der strategische Deadlock-Agent eines realen LEGO-EV3-Roboters.");

        sb.AppendLine(
            "Der normale deterministische Planner und seine Einzelkisten-" +
            "Zwischenbewegungen sind bereits gescheitert.");

        sb.AppendLine();
        sb.AppendLine("DEINE AUFGABE:");
        sb.AppendLine(
            "Schlage bis zu 8 kurze FREE_PUSH-Manöver vor, die die " +
            "Blockadestruktur verändern könnten.");

        sb.AppendLine(
            "Du steuerst KEINE Motoren. Jeder Vorschlag wird danach von " +
            "einem deterministischen C#-Sicherheitsplanner geprüft.");

        sb.AppendLine();
        sb.AppendLine("KOORDINATEN:");
        sb.AppendLine(
            "Feld 100x100 cm. 0° = +X/rechts, 90° = +Y/oben, " +
            "180° = links, 270° = unten.");

        sb.AppendLine(
            "Kisten ungefähr 5 cm in Schubrichtung und 6 cm breit.");

        sb.AppendLine(
            "Der Frontschieber des Rovers ist ungefähr 15 cm breit.");

        sb.AppendLine();
        sb.AppendLine("FREE_PUSH-PARAMETER:");
        sb.AppendLine(
            "primaryBoxId = Kiste, an deren rückwärtiger Fläche der " +
            "Schieber geometrisch orientiert wird.");

        sb.AppendLine(
            "pushHeadingDeg = gewünschte Schubrichtung.");

        sb.AppendLine(
            "lateralOffsetCm = seitlicher Versatz des Schieberzentrums " +
            "relativ zur Primärkiste; erlaubt -6 bis +6 cm.");

        sb.AppendLine(
            "pushDistanceCm = kurzer Stoß; erlaubt 3 bis 15 cm.");

        sb.AppendLine();
        sb.AppendLine("WICHTIG:");
        sb.AppendLine(
            "- Der Roboter kann nur SCHIEBEN, nicht ziehen.");

        sb.AppendLine(
            "- FREE_PUSH darf bewusst mehrere UNSORTIERTE Kisten berühren.");

        sb.AppendLine(
            "- Bereits sortierte Kisten sollen nicht berührt werden.");

        sb.AppendLine(
            "- Bevorzuge verschiedene Richtungen/Versätze und Strategien, " +
            "die einen kompakten Block aufbrechen.");

        sb.AppendLine(
            "- Ein Vorschlag muss nicht sofort sortieren; er soll eine " +
            "neue geometrische Situation schaffen.");

        sb.AppendLine();
        sb.AppendLine("GEOMETRIEHINWEISE FUER TRENNEN STATT BLOCKVERSCHIEBEN:");
        sb.AppendLine("Schieberbreite 15 cm; Kisten etwa 6 x 5 cm. Versatz + = links in Fahrtrichtung, - = rechts.");
        sb.AppendLine("Bei 90 Grad (oben): + versetzt nach Welt-links, - nach Welt-rechts.");
        sb.AppendLine("Bei rechtem Rand eines Blocks und Schub nach oben: eher -5 bis -6 cm; am linken Rand eher +5 bis +6 cm.");
        sb.AppendLine("Bevorzuge eine aeussere Kiste oder Teilgruppe. Ein Verschieben des ganzen Blocks schafft meist keine Trennung.");
        sb.AppendLine("C# prueft zusaetzlich beide seitlichen Versatzrichtungen. Liefere unterschiedliche sinnvolle Schubrichtungen.");
        sb.AppendLine("ZIEL: Jede einzelne Kiste muss anschliessend separat sortierbar werden, auch gleichfarbige Paare trennen.");
        sb.AppendLine("Plane den naechsten sinnvollen Sortierschritt mit. Eine unveraendert blockierte Teilgruppe nur weiter zu verschieben ist kein Erfolg.");
        sb.AppendLine("BISHERIGE REALE FREISCHUB-ERGEBNISSE (dieser Programmlauf):");
        if (OutcomeHistory.Count == 0) sb.AppendLine("Noch keine.");
        foreach (string outcome in OutcomeHistory) sb.AppendLine(outcome);
        sb.AppendLine("Wiederhole keine erfolglose Paarverschiebung; suche stattdessen einen aeusseren Einzelkontakt mit anderer Richtung.");
        sb.AppendLine("AKTUELLER ROVER:");
        sb.AppendLine(
            $"x={world.Rover.X:F2}, y={world.Rover.Y:F2}, " +
            $"heading={world.Rover.HeadingDeg:F2}");

        sb.AppendLine();
        sb.AppendLine("KISTEN:");

        foreach (BoxState box
                 in world.Boxes.OrderBy(b => b.Id))
        {
            sb.AppendLine(
                $"#{box.Id}: color={box.Color}, " +
                $"x={box.Position.X:F2}, y={box.Position.Y:F2}, " +
                $"sorted={box.Sorted}");
        }

        sb.AppendLine();
        sb.AppendLine("ZIELSLOTS:");

        foreach (TargetSlot slot
                 in world.Zones.SelectMany(z => z.Slots))
        {
            sb.AppendLine(
                $"#{slot.Id}: color={slot.Color}, " +
                $"x={slot.Position.X:F2}, y={slot.Position.Y:F2}, " +
                $"free={slot.IsFree}");
        }

        sb.AppendLine();
        sb.AppendLine("WARUM DER NORMALE PLANNER SCHEITERTE:");

        foreach (string failure
                 in directFailures.Take(24))
        {
            sb.AppendLine(
                "- " + failure);
        }

        sb.AppendLine();
        sb.AppendLine(
            "ANTWORTFORMAT: NUR gültiges JSON, kein Markdown, " +
            "keine Erklärung außerhalb JSON.");

        sb.AppendLine(
            "{\"proposals\":[{\"primaryBoxId\":1,\"pushHeadingDeg\":45.0," +
            "\"lateralOffsetCm\":0.0,\"pushDistanceCm\":8.0," +
            "\"reason\":\"kurze Begründung\"}]}");

        return sb.ToString();
    }


    private static string CallOllama(
        string model,
        string prompt)
    {
        var request =
            new
            {
                model,
                prompt,
                stream = false,

                // Qwen3 unterstützt einen Thinking-Modus. Für unsere kleine,
                // streng strukturierte FREE_PUSH-Ausgabe ist der unnötig
                // langsam und kann auf lokaler Hardware leicht in den
                // HTTP-Timeout laufen.
                think = false,

                format = "json",
                options =
                    new
                    {
                        temperature = 0.35,

                        // Acht kurze Vorschläge benötigen keinen langen Output.
                        // Das schützt zusätzlich gegen ausufernde Antworten.
                        num_predict = 1200
                    }
            };

        string body =
            JsonSerializer.Serialize(
                request);

        using StringContent content =
            new(
                body,
                Encoding.UTF8,
                "application/json");

        using HttpResponseMessage response =
            Http.PostAsync(
                    BaseUrl + "/api/generate",
                    content)
                .GetAwaiter()
                .GetResult();

        response.EnsureSuccessStatusCode();

        string responseJson =
            response.Content
                .ReadAsStringAsync()
                .GetAwaiter()
                .GetResult();

        using JsonDocument document =
            JsonDocument.Parse(
                responseJson);

        if (!document.RootElement.TryGetProperty(
                "response",
                out JsonElement responseElement))
        {
            throw new InvalidOperationException(
                "Ollama-Antwort enthält kein 'response'-Feld.");
        }


        return responseElement.GetString()
            ?? "";
    }


    private static FreePushProposalEnvelope? ParseProposalEnvelope(
        string text)
    {
        if (string.IsNullOrWhiteSpace(text))
            return null;

        string json =
            text.Trim();

        int firstBrace =
            json.IndexOf('{');

        int lastBrace =
            json.LastIndexOf('}');

        if (firstBrace >= 0 &&
            lastBrace > firstBrace)
        {
            json =
                json.Substring(
                    firstBrace,
                    lastBrace - firstBrace + 1);
        }


        JsonSerializerOptions options =
            new()
            {
                PropertyNameCaseInsensitive =
                    true
            };


        return JsonSerializer.Deserialize<FreePushProposalEnvelope>(
            json,
            options);
    }
}


// ================================================================
// TASK PLANNER
//
// 1. normale Sortieraktion suchen
// 2. dabei pro Farbe die Slot-Priorität möglichst beibehalten
// 3. ist der nächste freie Slot geometrisch blockiert, den ersten
//    späteren sicher erreichbaren Ausweichslot zulassen
// 4. wenn auch das nicht möglich ist: Deadlock erkennen
// 5. temporäre Verschiebung suchen
// 6. nur Verschiebungen akzeptieren, nach denen sofort mindestens
//    eine normale Sortieraktion möglich wird
// ================================================================

public static class TaskPlanner
{
    // Nur direkte Sortierungen pruefen: keine Relocation-/Ollama-Rekursion,
    // keine Motorbefehle und keine Aenderung der uebergebenen Welt.
    public static int CountDirectlySortableBoxes(WorldModel world)
    {
        int count = 0;
        foreach (BoxState box in world.Boxes.Where(b => !b.Sorted))
        {
            foreach (TargetSlot slot in world.GetZone(box.Color).Slots
                .Where(t => t.IsFree).OrderBy(t => t.Priority))
            {
                try
                {
                    _ = PushPlanner.Create(world.Rover, box, slot.Position, world.Boxes);
                    count++;
                    break;
                }
                catch (InvalidOperationException) { }
            }
        }
        return count;
    }

    private const double TemporaryMinX = 15.0;
    private const double TemporaryMaxX = 85.0;
    private const double TemporaryMinY = 15.0;
    private const double TemporaryMaxY = 65.0;
    private const double TemporaryGridStepCm = 10.0;

    private const double TemporaryMinMoveCm = 15.0;
    private const double TemporaryMinBoxDistanceCm = 16.0;
    private const double TemporaryZoneMarginCm = 3.0;
    private const double RelocationPenalty = 5.0;

    public static AgentActionCandidate ChooseNext(WorldModel world)
    {
        List<string> directFailures = new();

        AgentActionCandidate? direct =
            FindBestDirectSortCandidate(
                world,
                directFailures);

        if (direct != null)
        {
            Console.WriteLine("Direkte Sortieraktion gefunden.");
            return direct;
        }

        Console.WriteLine();
        Console.WriteLine("KEINE DIREKTE SORTIERAKTION MÖGLICH");
        Console.WriteLine("----------------------------------");
        Console.WriteLine("Deadlock bzw. gegenseitige Blockierung erkannt.");
        Console.WriteLine();
        Console.WriteLine("Diagnose der direkten Versuche:");

        foreach (string failure in directFailures.Take(20))
            Console.WriteLine("  " + failure);

        if (directFailures.Count > 20)
        {
            Console.WriteLine(
                $"  ... {directFailures.Count - 20} weitere Fehlversuche im Planner.");
        }

        Console.WriteLine();
        Console.WriteLine("Suche temporäre Befreiungsbewegung ...");

        AgentActionCandidate? relocation =
            FindBestRelocationCandidate(world);

        if (relocation != null)
        {
            Console.WriteLine("Temporäre Befreiungsbewegung gefunden.");
            return relocation;
        }


        Console.WriteLine();
        Console.WriteLine(
            "Klassische Deadlock-Suche ausgeschöpft. " +
            "Eskalation an lokales Ollama ...");

        AgentActionCandidate? aiFreePush =
            OllamaDeadlockSolver.TryFindFreePush(
                world,
                directFailures);

        if (aiFreePush != null)
        {
            Console.WriteLine();
            Console.WriteLine(
                "Sicherheitsgeprüfter KI-Freischub gefunden.");
            return aiFreePush;
        }


        throw new PlanningUnavailableException(
            "Keine direkte Sortieraktion, keine klassische Befreiungsbewegung " +
            "und kein sicherheitsgeprüfter Ollama-Freischub gefunden.\n" +
            string.Join(Environment.NewLine, directFailures.Take(30)));
    }

    // ============================================================
    // DIREKTE SORTIERAKTION
    // ============================================================

    private static AgentActionCandidate? FindBestDirectSortCandidate(
        WorldModel world,
        List<string>? failures)
    {
        List<AgentActionCandidate> candidates =
            new();


        // ============================================================
        // SLOT-STRATEGIE
        //
        // Früher durfte für jede Farbe ausschließlich der absolut
        // nächste freie Slot benutzt werden.
        //
        // Das kann zu einem künstlichen Deadlock führen:
        // Ein bereits korrekt abgelegter Karton blockiert geometrisch
        // genau den Schubkorridor des nächsten Slots, obwohl ein etwas
        // späterer Slot problemlos erreichbar wäre.
        //
        // Neue Regel:
        //
        // 1. Für jede Farbe die freien Slots in Prioritätsreihenfolge
        //    betrachten.
        // 2. Beim ersten Slot, den MINDESTENS EINE unsortierte Kiste
        //    dieser Farbe sicher erreichen kann, bleiben.
        // 3. Nur wenn kein Karton diesen Slot erreichen kann, wird der
        //    nächste freie Slot als Ausweichslot geprüft.
        //
        // Dadurch bleibt die gewünschte Füllreihenfolge so weit wie
        // physisch möglich erhalten, aber ein einzelner blockierter
        // Slot legt nicht mehr das gesamte System still.
        // ============================================================

        foreach (TargetZone zone
                 in world.Zones)
        {
            List<BoxState> unsortedBoxes =
                world.Boxes
                    .Where(
                        b =>
                            !b.Sorted &&
                            b.Color == zone.Color)
                    .ToList();

            if (unsortedBoxes.Count == 0)
            {
                continue;
            }


            List<TargetSlot> freeSlots =
                zone.Slots
                    .Where(
                        s => s.IsFree)
                    .OrderBy(
                        s => s.Priority)
                    .ThenBy(
                        s => s.Id)
                    .ToList();


            if (freeSlots.Count == 0)
            {
                foreach (BoxState box
                         in unsortedBoxes)
                {
                    failures?.Add(
                        $"Kiste #{box.Id}: " +
                        $"kein freier {box.Color}-Slot mehr.");
                }

                continue;
            }


            int skippedFreeSlots =
                0;


            foreach (TargetSlot slot
                     in freeSlots)
            {
                List<AgentActionCandidate> candidatesForThisSlot =
                    new();


                foreach (BoxState box
                         in unsortedBoxes)
                {
                    try
                    {
                        PushPlan plan =
                            PushPlanner.Create(
                                rover:
                                    world.Rover,

                                activeBox:
                                    box,

                                target:
                                    slot.Position,

                                allBoxes:
                                    world.Boxes);


                        string reason =
                            skippedFreeSlots == 0
                                ? "Direkte Sortieraktion"
                                : $"AUSWEICHSLOT: {skippedFreeSlots} frühere freie " +
                                  $"{PlannerColorName(zone.Color)}-Slot(s) sind derzeit " +
                                  $"geometrisch nicht erreichbar.";


                        candidatesForThisSlot.Add(
                            new AgentActionCandidate(
                                AgentActionKind.Sort,
                                box,
                                slot,
                                slot.Position,
                                plan,
                                plan.TotalTravelCm,
                                reason));
                    }
                    catch (Exception ex)
                    {
                        failures?.Add(
                            $"Kiste #{box.Id} -> " +
                            $"Slot #{slot.Id}: " +
                            ex.Message);
                    }
                }


                if (candidatesForThisSlot.Count > 0)
                {
                    if (skippedFreeSlots > 0)
                    {
                        Console.WriteLine(
                            $"AUSWEICHSLOT für {PlannerColorName(zone.Color)}: " +
                            $"Slot #{slot.Id} wird verwendet, weil " +
                            $"{skippedFreeSlots} frühere freie Slot(s) " +
                            $"aktuell nicht sicher erreichbar sind.");
                    }

                    candidates.AddRange(
                        candidatesForThisSlot);

                    // Für diese Farbe keine späteren Slots betrachten.
                    break;
                }


                skippedFreeSlots++;
            }
        }


        if (candidates.Count ==
            0)
        {
            return null;
        }


        return candidates
            .OrderBy(
                c => c.Score)
            .First();
    }

    // ============================================================
    // DEADLOCK-AUFLÖSUNG MIT EINEM ZWISCHENZIEL
    // ============================================================

    private static AgentActionCandidate? FindBestRelocationCandidate(
        WorldModel world)
    {
        List<AgentActionCandidate> candidates = new();
        int tested = 0;
        int physicallyPossible = 0;
        int unlocking = 0;

        foreach (BoxState box in world.Boxes.Where(b => !b.Sorted))
        {
            foreach (Point2D temporary in GenerateTemporaryPoints())
            {
                tested++;

                if (!TemporaryPointAllowed(world, box, temporary))
                    continue;

                PushPlan relocationPlan;

                try
                {
                    relocationPlan = PushPlanner.Create(
                        rover: world.Rover,
                        activeBox: box,
                        target: temporary,
                        allBoxes: world.Boxes);
                }
                catch
                {
                    continue;
                }

                physicallyPossible++;

                // Welt nach der temporären Verschiebung simulieren
                WorldModel simulated = world.DeepClone();

                BoxState simulatedBox =
                    simulated.Boxes.Single(b => b.Id == box.Id);

                simulatedBox.Position = temporary;
                simulated.Rover = relocationPlan.EndPose;

                AgentActionCandidate? unlockedSort =
                    FindBestDirectSortCandidate(
                        simulated,
                        failures: null);

                if (unlockedSort == null)
                    continue;

                unlocking++;

                double score =
                    relocationPlan.TotalTravelCm +
                    unlockedSort.Plan.TotalTravelCm +
                    RelocationPenalty;

                string reason =
                    $"Danach wird Kiste #{unlockedSort.Box.Id} direkt sortierbar " +
                    $"(Slot #{unlockedSort.Slot!.Id}).";

                candidates.Add(
                    new AgentActionCandidate(
                        AgentActionKind.Relocate,
                        box,
                        null,
                        temporary,
                        relocationPlan,
                        score,
                        reason));
            }
        }

        Console.WriteLine(
            $"Temporärpunkte geprüft: {tested}, " +
            $"physisch ausführbar: {physicallyPossible}, " +
            $"Deadlock lösend: {unlocking}");

        if (candidates.Count == 0)
            return null;

        AgentActionCandidate best = candidates
            .OrderBy(c => c.Score)
            .First();

        Console.WriteLine(
            $"Beste Befreiungsaktion: Kiste #{best.Box.Id} -> " +
            $"({best.Destination.X:F1}, {best.Destination.Y:F1}), " +
            $"Score {best.Score:F1}");

        return best;
    }

    private static IEnumerable<Point2D> GenerateTemporaryPoints()
    {
        for (double y = TemporaryMinY;
             y <= TemporaryMaxY + 0.001;
             y += TemporaryGridStepCm)
        {
            for (double x = TemporaryMinX;
                 x <= TemporaryMaxX + 0.001;
                 x += TemporaryGridStepCm)
            {
                yield return new Point2D(x, y);
            }
        }
    }

    private static bool TemporaryPointAllowed(
        WorldModel world,
        BoxState movingBox,
        Point2D temporary)
    {
        // Muss die Kiste tatsächlich merklich verschieben.
        if (Geometry2D.Distance(movingBox.Position, temporary) <
            TemporaryMinMoveCm)
        {
            return false;
        }

        // Zielgebiete nicht als Zwischenparkplatz missbrauchen.
        foreach (TargetZone zone in world.Zones)
        {
            if (zone.Contains(temporary, TemporaryZoneMarginCm))
                return false;
        }

        // Genügend Abstand zu allen anderen Kisten.
        foreach (BoxState other in world.Boxes)
        {
            if (other.Id == movingBox.Id)
                continue;

            if (Geometry2D.Distance(other.Position, temporary) <
                TemporaryMinBoxDistanceCm)
            {
                return false;
            }
        }

        return true;
    }


    // Eigene Hilfsfunktion innerhalb von TaskPlanner.
    // Eine Top-Level-Lokalfunktion aus Program.cs darf aus dieser
    // statischen Klasse nicht aufgerufen werden (CS8801).
    private static string PlannerColorName(BoxColor color) =>
        color == BoxColor.Red
            ? "ROT"
            : "BLAU";
}


// ================================================================
// EV3 ROVER
// ================================================================

public sealed class Ev3Rover : IDisposable
{
    private const double WheelDiameterCm = 4.29;
    private const double TrackWidthCm = 11.1;
    private const double TurnCalibration = 90.0 / 89.0;

    private const byte MotorsBC = 0x06;

    private readonly SerialPort port;
    private ushort messageCounter = 1;

    public Ev3Rover(string portName)
    {
        port = new SerialPort(
            portName,
            115200,
            Parity.None,
            8,
            StopBits.One);

        port.Handshake = Handshake.None;
        port.ReadTimeout = 15000;
        port.WriteTimeout = 3000;
    }

    public void Connect()
    {
        if (port.IsOpen)
            return;

        Console.WriteLine();
        Console.WriteLine($"Öffne {port.PortName} ...");
        port.Open();
        Console.WriteLine("Bluetooth-Verbindung geöffnet.");
    }

    public void ForwardCm(double distanceCm, int speed = 30)
    {
        if (distanceCm < 0)
            throw new ArgumentOutOfRangeException(nameof(distanceCm));

        int motorDegrees = DistanceToMotorDegrees(distanceCm);

        Console.WriteLine(
            $"Vorwärts {distanceCm:F2} cm ({motorDegrees} Motorgrad)");

        MoveSync(speed, turnRatio: 0, motorDegrees);
    }

    public void BackwardCm(double distanceCm, int speed = 30)
    {
        if (distanceCm < 0)
            throw new ArgumentOutOfRangeException(nameof(distanceCm));

        int motorDegrees = DistanceToMotorDegrees(distanceCm);

        Console.WriteLine(
            $"Rückwärts {distanceCm:F2} cm ({motorDegrees} Motorgrad)");

        MoveSync(-Math.Abs(speed), turnRatio: 0, motorDegrees);
    }

    public void TurnRight(double degrees, int speed = 25)
    {
        TurnDegrees(degrees, right: true, speed);
    }

    public void TurnLeft(double degrees, int speed = 25)
    {
        TurnDegrees(degrees, right: false, speed);
    }

    public void Stop()
    {
        byte[] command =
        {
            0xA3,
            0x00,
            MotorsBC,
            0x01
        };

        SendDirectCommand(command);
    }

    private static int DistanceToMotorDegrees(double distanceCm)
    {
        double circumference = Math.PI * WheelDiameterCm;

        return (int)Math.Round(
            distanceCm /
            circumference *
            360.0);
    }

    private void TurnDegrees(
        double robotDegrees,
        bool right,
        int speed)
    {
        if (robotDegrees < 0)
            throw new ArgumentOutOfRangeException(nameof(robotDegrees));

        double correctedDegrees = robotDegrees * TurnCalibration;

        double wheelPath =
            Math.PI *
            TrackWidthCm *
            correctedDegrees /
            360.0;

        int motorDegrees = DistanceToMotorDegrees(wheelPath);
        int turnRatio = right ? 200 : -200;

        Console.WriteLine(
            $"{(right ? "Rechts" : "Links")} {robotDegrees:F2}° " +
            $"({motorDegrees} Motorgrad)");

        MoveSync(Math.Abs(speed), turnRatio, motorDegrees);
    }

    private void MoveSync(
        int speed,
        int turnRatio,
        int motorDegrees)
    {
        List<byte> command = new();

        command.Add(0xB0);       // opOutputStepSync
        command.AddRange(LCX(0));
        command.AddRange(LCX(MotorsBC));
        command.AddRange(LCX(speed));
        command.AddRange(LCX(turnRatio));
        command.AddRange(LCX(motorDegrees));
        command.AddRange(LCX(1));

        command.Add(0xAA);       // opOutputReady
        command.AddRange(LCX(0));
        command.AddRange(LCX(MotorsBC));

        SendDirectCommand(command.ToArray());
    }

    private static byte[] LCX(int value)
    {
        if (value >= -32 && value <= 31)
        {
            return new[]
            {
                (byte)(value & 0x3F)
            };
        }

        if (value >= -128 && value <= 127)
        {
            return new[]
            {
                (byte)0x81,
                unchecked((byte)value)
            };
        }

        if (value >= short.MinValue && value <= short.MaxValue)
        {
            short v = (short)value;

            return new[]
            {
                (byte)0x82,
                (byte)(v & 0xFF),
                (byte)((v >> 8) & 0xFF)
            };
        }

        return new[]
        {
            (byte)0x83,
            (byte)(value & 0xFF),
            (byte)((value >> 8) & 0xFF),
            (byte)((value >> 16) & 0xFF),
            (byte)((value >> 24) & 0xFF)
        };
    }

    private void SendDirectCommand(byte[] opcodes)
    {
        if (!port.IsOpen)
            throw new InvalidOperationException("EV3 ist nicht verbunden.");

        ushort counter = messageCounter++;
        int bodyLength = 5 + opcodes.Length;
        byte[] packet = new byte[bodyLength + 2];

        packet[0] = (byte)(bodyLength & 0xFF);
        packet[1] = (byte)(bodyLength >> 8);
        packet[2] = (byte)(counter & 0xFF);
        packet[3] = (byte)(counter >> 8);
        packet[4] = 0x00;
        packet[5] = 0x00;
        packet[6] = 0x00;

        Array.Copy(opcodes, 0, packet, 7, opcodes.Length);

        port.Write(packet, 0, packet.Length);

        byte[] lengthBytes = ReadExactly(2);

        int replyLength =
            lengthBytes[0] |
            (lengthBytes[1] << 8);

        byte[] reply = ReadExactly(replyLength);

        if (reply.Length < 3)
            throw new IOException("EV3-Antwort zu kurz.");

        ushort replyCounter =
            (ushort)(reply[0] | (reply[1] << 8));

        if (replyCounter != counter)
        {
            throw new IOException(
                $"Falscher Message Counter: {replyCounter} statt {counter}");
        }

        if (reply[2] != 0x02)
        {
            throw new IOException(
                $"EV3 Direct Command Fehler: 0x{reply[2]:X2}");
        }
    }

    private byte[] ReadExactly(int count)
    {
        byte[] buffer = new byte[count];
        int offset = 0;

        while (offset < count)
        {
            int n = port.Read(
                buffer,
                offset,
                count - offset);

            if (n <= 0)
                throw new IOException("Bluetooth-Verbindung unterbrochen.");

            offset += n;
        }

        return buffer;
    }

    public void Dispose()
    {
        if (port.IsOpen)
        {
            try
            {
                Stop();
            }
            catch
            {
            }

            port.Close();
        }

        port.Dispose();
    }
}


// ================================================================
// TEE-WRITER: Konsole + Logdatei
// ================================================================

public sealed class TeeTextWriter : TextWriter
{
    private readonly TextWriter first;
    private readonly TextWriter second;

    public override Encoding Encoding => first.Encoding;

    public TeeTextWriter(TextWriter first, TextWriter second)
    {
        this.first = first;
        this.second = second;
    }

    public override void Write(char value)
    {
        first.Write(value);
        second.Write(value);
    }

    public override void Write(string? value)
    {
        first.Write(value);
        second.Write(value);
    }

    public override void WriteLine(string? value)
    {
        first.WriteLine(value);
        second.WriteLine(value);
    }

    public override void WriteLine()
    {
        first.WriteLine();
        second.WriteLine();
    }

    public override void Flush()
    {
        first.Flush();
        second.Flush();
    }
}


// ================================================================
// KAMERASYSTEM
//
// Aufgaben:
// - vier schwarze Feldmarken robust erkennen
// - C: Feldkalibrierung fixieren
// - ArUco ID 0 erkennen
// - H: aktuelle Roverrichtung als 90° definieren
// - 11-cm-Höhenkorrektur für den ArUco-Marker
// - 2 rote + 2 blaue Kisten automatisch erkennen
// - Weltkoordinaten liefern
//
// Die Homographien werden rein in C# gespeichert.
// Es werden keine OpenCV-Mat-Homographien über Frames gehalten.
// ================================================================

public sealed class VisionSystem : IDisposable
{
    private const int RequestedWidth = 1280;
    private const int RequestedHeight = 720;

    private const int RequiredStableFieldFrames = 10;
    private const double StableCornerDistancePx = 3.0;

    private const int MeasurementStableFrames = 3;
    private const int MeasurementTimeoutMs = 15000;

    private readonly int expectedRedBoxes;
    private readonly int expectedBlueBoxes;

    private int blackThreshold = 75;

    private readonly VideoCapture capture;

    private readonly OpenCvSharp.Aruco.Dictionary dictionary;
    private readonly DetectorParameters detectorParameters;
    private readonly ArucoDetector arucoDetector;

    private readonly Mat frame = new();

    private Homography2D? worldTransform;
    private Homography2D? roverHeightTransform;

    private FieldCornersVision? lastDetectedField;
    private int fieldStableFrames;

    private double headingOffsetDeg;
    private double? lastRawHeading;

    private readonly object sync = new();

    private readonly Thread visionThread;

    private volatile bool stopRequested;
    private volatile bool calibrationComplete;

    private Exception? workerException;

    private VisionSnapshot? latestCompleteSnapshot;

    // Vollständige Weltmessung
    private long completeFrameSequence;
    private int consecutiveCompleteFrames;

    // Rover-only Messung.
    // Diese läuft auch dann weiter, wenn eine Kiste am P teilweise
    // vom Rover verdeckt wird.
    private RoverPose? latestRoverPose;
    private long roverFrameSequence;
    private int consecutiveRoverFrames;


    public VisionSystem(
        int cameraIndex,
        int expectedRedBoxes,
        int expectedBlueBoxes)
    {
        this.expectedRedBoxes =
            expectedRedBoxes;

        this.expectedBlueBoxes =
            expectedBlueBoxes;

        capture =
            new VideoCapture(
                cameraIndex);

        if (!capture.IsOpened())
        {
            throw new InvalidOperationException(
                $"Webcam {cameraIndex} konnte nicht geöffnet werden.");
        }

        capture.FrameWidth =
            RequestedWidth;

        capture.FrameHeight =
            RequestedHeight;

        Console.WriteLine(
            $"Kamera geöffnet: " +
            $"{capture.FrameWidth:F0} x {capture.FrameHeight:F0}");

        dictionary =
            CvAruco.GetPredefinedDictionary(
                PredefinedDictionaryType.Dict4X4_50);

        detectorParameters =
            new DetectorParameters
            {
                CornerRefinementMethod =
                    CornerRefineMethod.Subpix
            };

        arucoDetector =
            new ArucoDetector(
                dictionary,
                detectorParameters,
                new RefineParameters());

        // ------------------------------------------------------------
        // WICHTIG:
        // Ab hier besitzt dieser Hintergrundthread die Webcam UND
        // das OpenCV-Fenster. Dadurch läuft das Kamerabild permanent
        // weiter, auch während der Hauptthread den EV3 fahren lässt.
        // ------------------------------------------------------------

        visionThread =
            new Thread(
                VisionLoop)
            {
                IsBackground = true,
                Name = "EV3-Vision-Live"
            };

        visionThread.Start();
    }


    // ============================================================
    // KALIBRIERUNG
    //
    // Die eigentliche Kamera-/Fensterarbeit läuft bereits im
    // Vision-Thread. Diese Methode wartet nur, bis C und H dort
    // erfolgreich verarbeitet wurden.
    // ============================================================

    public void CalibrateInteractively()
    {
        Console.WriteLine();
        Console.WriteLine("KAMERA-KALIBRIERUNG");
        Console.WriteLine("===================");
        Console.WriteLine();
        Console.WriteLine(
            "C = Feldkalibrierung übernehmen, sobald 10/10 stabil.");
        Console.WriteLine(
            "H = aktuelle Roverrichtung als 90° definieren.");
        Console.WriteLine(
            "+/- = Schwarzschwelle ändern.");
        Console.WriteLine(
            "Q oder ESC = Abbruch.");
        Console.WriteLine();

        lock (sync)
        {
            while (!calibrationComplete &&
                   workerException == null &&
                   !stopRequested)
            {
                Monitor.Wait(
                    sync,
                    100);
            }

            ThrowWorkerExceptionIfAny();

            if (!calibrationComplete)
            {
                throw new OperationCanceledException(
                    "Kamerakalibrierung abgebrochen.");
            }
        }

        Console.WriteLine();
        Console.WriteLine(
            "Kamera-Livebetrieb läuft jetzt permanent im Hintergrund.");
    }


    // ============================================================
    // AKTUELLE SZENE VERMESSEN
    //
    // Die Kamera läuft die ganze Zeit weiter.
    // Diese Methode verlangt drei NEUE vollständige Frames nach
    // ihrem Aufruf. Dadurch bekommt der Planner keinen alten Frame.
    // ============================================================

    public VisionSnapshot MeasureScene(bool printSnapshot = true)
    {
        Stopwatch stopwatch =
            Stopwatch.StartNew();

        lock (sync)
        {
            ThrowWorkerExceptionIfAny();

            long targetSequence =
                completeFrameSequence +
                MeasurementStableFrames;

            while (true)
            {
                ThrowWorkerExceptionIfAny();

                if (stopRequested)
                {
                    throw new OperationCanceledException(
                        "Kamerabetrieb wurde beendet.");
                }

                if (completeFrameSequence >=
                        targetSequence &&
                    consecutiveCompleteFrames >=
                        MeasurementStableFrames &&
                    latestCompleteSnapshot != null)
                {
                    VisionSnapshot snapshot =
                        CloneSnapshot(
                            latestCompleteSnapshot);

                    if (printSnapshot)
                    {
                        PrintSnapshot(
                            snapshot);
                    }

                    return snapshot;
                }

                if (stopwatch.ElapsedMilliseconds >
                    MeasurementTimeoutMs)
                {
                    throw new InvalidOperationException(
                        "Keine stabile vollständige Kamerawelt innerhalb " +
                        $"{MeasurementTimeoutMs / 1000} Sekunden erhalten. " +
                        $"Erwartet: Rover + {expectedRedBoxes} rote + " +
                        $"{expectedBlueBoxes} blaue Kisten.");
                }

                Monitor.Wait(
                    sync,
                    50);
            }
        }
    }


    // ============================================================
    // NUR ROVER MESSEN
    //
    // Für Navigation, Winkelkorrekturen und den Endanflug benötigen
    // wir nicht jedes Mal alle Kisten. Gerade am P kann der Rover
    // eine Kistenmarkierung verdecken. Deshalb gibt es hierfür einen
    // eigenen robusten Rover-Messpfad.
    // ============================================================

    public RoverPose MeasureRoverPose()
    {
        Stopwatch stopwatch =
            Stopwatch.StartNew();

        lock (sync)
        {
            ThrowWorkerExceptionIfAny();

            long targetSequence =
                roverFrameSequence +
                MeasurementStableFrames;

            while (true)
            {
                ThrowWorkerExceptionIfAny();

                if (stopRequested)
                {
                    throw new OperationCanceledException(
                        "Kamerabetrieb wurde beendet.");
                }

                if (roverFrameSequence >=
                        targetSequence &&
                    consecutiveRoverFrames >=
                        MeasurementStableFrames &&
                    latestRoverPose != null)
                {
                    return new RoverPose(
                        latestRoverPose.X,
                        latestRoverPose.Y,
                        latestRoverPose.HeadingDeg);
                }

                if (stopwatch.ElapsedMilliseconds >
                    MeasurementTimeoutMs)
                {
                    throw new InvalidOperationException(
                        "Keine stabile Rover-Kameramessung innerhalb " +
                        $"{MeasurementTimeoutMs / 1000} Sekunden erhalten.");
                }

                Monitor.Wait(
                    sync,
                    50);
            }
        }
    }


    // ============================================================
    // PERMANENTER KAMERA-THREAD
    // ============================================================

    private void VisionLoop()
    {
        try
        {
            while (!stopRequested)
            {
                if (!capture.Read(frame) ||
                    frame.Empty())
                {
                    Thread.Sleep(2);
                    continue;
                }

                using Mat display =
                    frame.Clone();


                // ----------------------------------------------------
                // FELDMARKEN NUR BIS ZUR C-KALIBRIERUNG SUCHEN.
                // Danach sind sie komplett eingefroren.
                // ----------------------------------------------------

                FieldCornersVision? detectedField;
                List<SquareCandidateVision> squareCandidates;

                if (worldTransform == null)
                {
                    detectedField =
                        DetectFieldCorners(
                            frame,
                            blackThreshold,
                            out squareCandidates);

                    if (detectedField != null)
                    {
                        if (lastDetectedField != null &&
                            FieldCornersAreClose(
                                detectedField,
                                lastDetectedField,
                                StableCornerDistancePx))
                        {
                            fieldStableFrames =
                                Math.Min(
                                    RequiredStableFieldFrames,
                                    fieldStableFrames + 1);
                        }
                        else
                        {
                            fieldStableFrames = 1;
                        }

                        lastDetectedField =
                            detectedField;
                    }
                    else
                    {
                        fieldStableFrames = 0;
                    }
                }
                else
                {
                    detectedField =
                        lastDetectedField;

                    squareCandidates =
                        new List<SquareCandidateVision>();
                }


                foreach (SquareCandidateVision candidate
                         in squareCandidates)
                {
                    Cv2.Circle(
                        display,
                        ToCvPoint(candidate.Center),
                        4,
                        new Scalar(255, 100, 0),
                        1);
                }


                if (detectedField != null)
                {
                    DrawFieldCorners(
                        display,
                        detectedField);
                }


                VisionFrameResult result =
                    ProcessRoverAndBoxes(
                        frame,
                        display);


                // ----------------------------------------------------
                // VOLLSTÄNDIGEN WELTZUSTAND FÜR HAUPTTHREAD ABLEGEN
                // ----------------------------------------------------

                bool complete =
                    calibrationComplete &&
                    result.Rover != null &&
                    result.RedBoxes.Count ==
                        expectedRedBoxes &&
                    result.BlueBoxes.Count ==
                        expectedBlueBoxes;

                lock (sync)
                {
                    if (result.Rover != null)
                    {
                        consecutiveRoverFrames++;
                        roverFrameSequence++;

                        latestRoverPose =
                            new RoverPose(
                                result.Rover.X,
                                result.Rover.Y,
                                result.Rover.HeadingDeg);
                    }
                    else
                    {
                        consecutiveRoverFrames = 0;
                    }

                    if (complete)
                    {
                        consecutiveCompleteFrames++;

                        completeFrameSequence++;

                        latestCompleteSnapshot =
                            new VisionSnapshot(
                                result.Rover!,
                                result.RedBoxes
                                    .Select(CloneDetectedBox)
                                    .ToArray(),
                                result.BlueBoxes
                                    .Select(CloneDetectedBox)
                                    .ToArray());
                    }
                    else
                    {
                        consecutiveCompleteFrames = 0;
                    }

                    Monitor.PulseAll(
                        sync);
                }


                DrawLiveStatus(
                    display,
                    detectedField,
                    result,
                    complete);


                Cv2.ImShow(
                    "EV3 Vision Live",
                    display);


                int key =
                    Cv2.WaitKey(1);

                if (key >= 0)
                {
                    HandleVisionKey(
                        key & 0xFF);
                }

                // Während der Kalibrierung dürfen C/H/+/- auch im
                // Konsolenfenster gedrückt werden. Nach H wird die
                // Konsole NICHT mehr vom Kamera-Thread gelesen,
                // damit er später kein ENTER des Planners "stiehlt".
                if (!calibrationComplete &&
                    Console.KeyAvailable)
                {
                    ConsoleKeyInfo info =
                        Console.ReadKey(
                            intercept: true);

                    int consoleKey =
                        info.Key == ConsoleKey.Escape
                            ? 27
                            : info.KeyChar;

                    HandleVisionKey(
                        consoleKey);
                }
            }
        }
        catch (Exception ex)
        {
            lock (sync)
            {
                workerException = ex;
                Monitor.PulseAll(sync);
            }
        }
        finally
        {
            try
            {
                Cv2.DestroyWindow(
                    "EV3 Vision Live");
            }
            catch
            {
            }
        }
    }


    // ============================================================
    // STATUS IM LIVE-BILD
    // ============================================================

    private void DrawLiveStatus(
        Mat display,
        FieldCornersVision? detectedField,
        VisionFrameResult result,
        bool complete)
    {
        string fieldText =
            worldTransform != null
                ? "Feldmarken: FIXIERT"
                : detectedField != null
                    ? $"Feldmarken: stabil {fieldStableFrames}/{RequiredStableFieldFrames}"
                    : "Feldmarken: NICHT 4/4";


        Cv2.PutText(
            display,
            fieldText,
            new Point(15, 30),
            HersheyFonts.HersheySimplex,
            0.65,
            detectedField != null
                ? new Scalar(0, 255, 0)
                : new Scalar(0, 0, 255),
            2);


        int textY = 60;


        Cv2.PutText(
            display,
            calibrationComplete
                ? "KAMERA: LIVE"
                : $"Schwarzschwelle: {blackThreshold}",
            new Point(15, textY),
            HersheyFonts.HersheySimplex,
            0.55,
            calibrationComplete
                ? new Scalar(0, 255, 0)
                : new Scalar(255, 255, 255),
            2);

        textY += 25;


        Cv2.PutText(
            display,
            worldTransform != null
                ? "Feldkalibrierung: AKTIV"
                : "Feldkalibrierung: C druecken",
            new Point(15, textY),
            HersheyFonts.HersheySimplex,
            0.55,
            worldTransform != null
                ? new Scalar(0, 255, 0)
                : new Scalar(0, 0, 255),
            2);

        textY += 25;


        Cv2.PutText(
            display,
            result.RoverDetected
                ? "ArUco ID 0: erkannt"
                : "ArUco ID 0: NICHT erkannt",
            new Point(15, textY),
            HersheyFonts.HersheySimplex,
            0.55,
            result.RoverDetected
                ? new Scalar(0, 255, 0)
                : new Scalar(0, 0, 255),
            2);

        textY += 25;


        if (worldTransform != null)
        {
            bool boxesComplete =
                result.RedBoxes.Count ==
                    expectedRedBoxes &&
                result.BlueBoxes.Count ==
                    expectedBlueBoxes;

            Cv2.PutText(
                display,
                $"Kisten: Rot {result.RedBoxes.Count}/{expectedRedBoxes}  " +
                $"Blau {result.BlueBoxes.Count}/{expectedBlueBoxes}",
                new Point(15, textY),
                HersheyFonts.HersheySimplex,
                0.55,
                boxesComplete
                    ? new Scalar(0, 255, 0)
                    : new Scalar(0, 165, 255),
                2);

            textY += 25;
        }


        if (result.Rover != null)
        {
            Cv2.PutText(
                display,
                $"X={result.Rover.X:F1}  " +
                $"Y={result.Rover.Y:F1}  " +
                $"Richtung={result.Rover.HeadingDeg:F1}",
                new Point(15, textY),
                HersheyFonts.HersheySimplex,
                0.55,
                new Scalar(255, 255, 255),
                2);

            textY += 25;
        }


        if (calibrationComplete)
        {
            Cv2.PutText(
                display,
                complete
                    ? "Weltmessung: vollstaendig"
                    : "Weltmessung: unvollstaendig",
                new Point(15, textY),
                HersheyFonts.HersheySimplex,
                0.55,
                complete
                    ? new Scalar(0, 255, 0)
                    : new Scalar(0, 165, 255),
                2);
        }
        else
        {
            Cv2.PutText(
                display,
                "C = Feld fixieren | H = Richtung 90 Grad",
                new Point(15, textY),
                HersheyFonts.HersheySimplex,
                0.5,
                new Scalar(255, 255, 255),
                1);
        }
    }


    // ============================================================
    // TASTEN AUS DEM OPENCV-FENSTER
    // ============================================================

    private void HandleVisionKey(
        int key)
    {
        if (key == 27 ||
            key == 'q' ||
            key == 'Q')
        {
            stopRequested = true;

            lock (sync)
            {
                Monitor.PulseAll(
                    sync);
            }

            return;
        }


        if (calibrationComplete)
        {
            // Nach der Kalibrierung keine versehentlichen Änderungen
            // mehr an Feld oder Winkel zulassen.
            return;
        }


        if (key == '+' ||
            key == '=')
        {
            blackThreshold =
                Math.Min(
                    200,
                    blackThreshold + 5);

            Console.WriteLine(
                $"Schwarzschwelle = {blackThreshold}");

            return;
        }


        if (key == '-' ||
            key == '_')
        {
            blackThreshold =
                Math.Max(
                    10,
                    blackThreshold - 5);

            Console.WriteLine(
                $"Schwarzschwelle = {blackThreshold}");

            return;
        }


        if (key == 'c' ||
            key == 'C')
        {
            if (worldTransform != null)
            {
                Console.WriteLine(
                    "Feldkalibrierung ist bereits fixiert.");

                return;
            }

            if (lastDetectedField == null ||
                fieldStableFrames <
                    RequiredStableFieldFrames)
            {
                Console.WriteLine(
                    $"Feldkalibrierung noch nicht bereit: " +
                    $"{fieldStableFrames}/{RequiredStableFieldFrames} stabile Frames.");

                return;
            }

            AcceptFieldCalibration(
                lastDetectedField);

            return;
        }


        if (key == 'h' ||
            key == 'H')
        {
            if (worldTransform == null)
            {
                Console.WriteLine(
                    "Zuerst C für die Feldkalibrierung drücken.");

                return;
            }

            if (!lastRawHeading.HasValue)
            {
                Console.WriteLine(
                    "Noch keine gültige Rover-Winkelmessung.");

                return;
            }

            headingOffsetDeg =
                NormalizeSignedAngleVision(
                    90.0 -
                    lastRawHeading.Value);

            calibrationComplete =
                true;

            Console.WriteLine();
            Console.WriteLine(
                $"Winkeloffset gesetzt: {headingOffsetDeg:F2}°");
            Console.WriteLine(
                "Aktuelle Roverrichtung gilt jetzt als 90°.");
            Console.WriteLine(
                "LIVE-KAMERA bleibt ab jetzt permanent aktiv.");

            lock (sync)
            {
                Monitor.PulseAll(
                    sync);
            }
        }
    }


    // ============================================================
    // FELDKALIBRIERUNG ÜBERNEHMEN
    // ============================================================

    private void AcceptFieldCalibration(
        FieldCornersVision corners)
    {
        Point2f[] imagePoints =
        {
            corners.TopLeft,
            corners.TopRight,
            corners.BottomRight,
            corners.BottomLeft
        };

        Point2f[] worldPoints =
        {
            new Point2f(0, 0),
            new Point2f(0, 100),
            new Point2f(100, 100),
            new Point2f(100, 0)
        };

        worldTransform =
            Homography2D.Create(
                imagePoints,
                worldPoints);


        // Experimentell ermittelte Korrektur für den ArUco-Marker,
        // dessen Ebene 11 cm über dem Boden liegt.
        Point2f[] roverMeasuredPoints =
        {
            new Point2f(13.78f, 17.42f), // real 20,20
            new Point2f(12.71f, 83.19f), // real 20,80
            new Point2f(76.61f, 82.67f), // real 80,80
            new Point2f(78.10f, 17.85f)  // real 80,20
        };

        Point2f[] roverRealPoints =
        {
            new Point2f(20, 20),
            new Point2f(20, 80),
            new Point2f(80, 80),
            new Point2f(80, 20)
        };

        roverHeightTransform =
            Homography2D.Create(
                roverMeasuredPoints,
                roverRealPoints);

        headingOffsetDeg = 0.0;
        lastRawHeading = null;


        Console.WriteLine();
        Console.WriteLine("========================================");
        Console.WriteLine("FELDKALIBRIERUNG ÜBERNOMMEN");
        Console.WriteLine("========================================");

        Console.WriteLine(
            $"Bild TL = ({corners.TopLeft.X:F1}, {corners.TopLeft.Y:F1}) -> (0,0)");

        Console.WriteLine(
            $"Bild TR = ({corners.TopRight.X:F1}, {corners.TopRight.Y:F1}) -> (0,100)");

        Console.WriteLine(
            $"Bild BR = ({corners.BottomRight.X:F1}, {corners.BottomRight.Y:F1}) -> (100,100)");

        Console.WriteLine(
            $"Bild BL = ({corners.BottomLeft.X:F1}, {corners.BottomLeft.Y:F1}) -> (100,0)");

        Console.WriteLine();
        Console.WriteLine(
            "Rover-Höhenkorrektur (11 cm) aktiviert.");
        Console.WriteLine(
            "Rover jetzt in 90°-Richtung stellen und H drücken.");
    }


    // ============================================================
    // ROVER + KISTEN EINES FRAMES
    // ============================================================

    private VisionFrameResult ProcessRoverAndBoxes(
        Mat source,
        Mat display)
    {
        arucoDetector.DetectMarkers(
            source,
            out Point2f[][] arucoCorners,
            out int[] arucoIds,
            out _);

        if (arucoIds.Length > 0)
        {
            CvAruco.DrawDetectedMarkers(
                display,
                arucoCorners,
                arucoIds,
                new Scalar(0, 255, 255));
        }

        int markerIndex =
            Array.IndexOf(
                arucoIds,
                0);

        bool roverDetected =
            markerIndex >= 0;

        RoverPose? rover =
            null;


        if (roverDetected)
        {
            Point2f[] corners =
                arucoCorners[markerIndex];

            Point2f roverImageCenter =
                new(
                    corners.Average(p => p.X),
                    corners.Average(p => p.Y));

            Point2f roverImageFront =
                new(
                    (corners[0].X + corners[1].X) / 2.0f,
                    (corners[0].Y + corners[1].Y) / 2.0f);

            Cv2.Circle(
                display,
                ToCvPoint(roverImageCenter),
                5,
                new Scalar(255, 0, 255),
                -1);

            Cv2.Line(
                display,
                ToCvPoint(roverImageCenter),
                ToCvPoint(roverImageFront),
                new Scalar(0, 0, 255),
                3);


            if (worldTransform != null)
            {
                Point2f rawCenter =
                    worldTransform.Transform(
                        roverImageCenter);

                Point2f rawFront =
                    worldTransform.Transform(
                        roverImageFront);

                Point2f correctedCenter =
                    rawCenter;

                Point2f correctedFront =
                    rawFront;

                if (roverHeightTransform != null)
                {
                    correctedCenter =
                        roverHeightTransform.Transform(
                            rawCenter);

                    correctedFront =
                        roverHeightTransform.Transform(
                            rawFront);
                }


                double dx =
                    correctedFront.X -
                    correctedCenter.X;

                double dy =
                    correctedFront.Y -
                    correctedCenter.Y;

                double rawHeading =
                    Math.Atan2(
                        dy,
                        dx) *
                    180.0 /
                    Math.PI;

                rawHeading =
                    NormalizeAngleVision(
                        rawHeading);

                lastRawHeading =
                    rawHeading;

                double heading =
                    NormalizeAngleVision(
                        rawHeading +
                        headingOffsetDeg);


                rover =
                    new RoverPose(
                        correctedCenter.X,
                        correctedCenter.Y,
                        heading);


                Cv2.PutText(
                    display,
                    $"{heading:F1} deg",
                    new Point(
                        ToCvPoint(roverImageCenter).X + 15,
                        ToCvPoint(roverImageCenter).Y - 15),
                    HersheyFonts.HersheySimplex,
                    0.65,
                    new Scalar(0, 0, 255),
                    2);
            }
        }


        List<VisionDetectedBox> redBoxes =
            new();

        List<VisionDetectedBox> blueBoxes =
            new();


        if (worldTransform != null)
        {
            redBoxes =
                DetectColoredBoxes(
                    source,
                    worldTransform,
                    BoxColor.Red,
                    expectedRedBoxes,
                    rover);

            blueBoxes =
                DetectColoredBoxes(
                    source,
                    worldTransform,
                    BoxColor.Blue,
                    expectedBlueBoxes,
                    rover);

            DrawDetectedBoxes(
                display,
                redBoxes,
                BoxColor.Red);

            DrawDetectedBoxes(
                display,
                blueBoxes,
                BoxColor.Blue);
        }


        return new VisionFrameResult(
            roverDetected,
            rover,
            redBoxes,
            blueBoxes);
    }


    // ============================================================
    // SCHWARZE FELDMARKEN
    // ============================================================

    private static FieldCornersVision? DetectFieldCorners(
        Mat source,
        int blackThreshold,
        out List<SquareCandidateVision> candidates)
    {
        candidates =
            new List<SquareCandidateVision>();

        using Mat hsv =
            new();

        using Mat value =
            new();

        using Mat binary =
            new();


        Cv2.CvtColor(
            source,
            hsv,
            ColorConversionCodes.BGR2HSV);

        Cv2.ExtractChannel(
            hsv,
            value,
            2);

        Cv2.Threshold(
            value,
            binary,
            blackThreshold,
            255,
            ThresholdTypes.BinaryInv);

        Cv2.FindContours(
            binary,
            out Point[][] contours,
            out _,
            RetrievalModes.External,
            ContourApproximationModes.ApproxSimple);


        double imageArea =
            source.Width *
            source.Height;

        double minimumArea =
            Math.Max(
                15.0,
                imageArea * 0.000015);

        double maximumArea =
            imageArea * 0.0015;


        foreach (Point[] contour
                 in contours)
        {
            double area =
                Math.Abs(
                    Cv2.ContourArea(
                        contour));

            if (area < minimumArea ||
                area > maximumArea)
            {
                continue;
            }

            Rect rect =
                Cv2.BoundingRect(
                    contour);

            if (rect.Width < 4 ||
                rect.Height < 4)
            {
                continue;
            }

            if (rect.Width >
                    source.Width * 0.08 ||
                rect.Height >
                    source.Height * 0.12)
            {
                continue;
            }

            double aspect =
                rect.Width /
                (double)rect.Height;

            if (aspect < 0.55 ||
                aspect > 1.80)
            {
                continue;
            }

            double perimeter =
                Cv2.ArcLength(
                    contour,
                    true);

            Point[] approximation =
                Cv2.ApproxPolyDP(
                    contour,
                    0.08 * perimeter,
                    true);

            if (approximation.Length != 4)
                continue;

            if (!Cv2.IsContourConvex(
                    approximation))
            {
                continue;
            }

            double extent =
                area /
                (rect.Width *
                 (double)rect.Height);

            if (extent < 0.50)
                continue;

            Point2f center =
                new(
                    rect.X +
                        rect.Width / 2.0f,
                    rect.Y +
                        rect.Height / 2.0f);

            double squareQuality =
                1.0 /
                (1.0 +
                 Math.Abs(
                     Math.Log(
                         aspect)));

            candidates.Add(
                new SquareCandidateVision(
                    center,
                    area,
                    squareQuality));
        }


        if (candidates.Count < 4)
            return null;


        // Feste Suchfenster passend zum aktuellen Stativ-/Kameraaufbau.
        Point2f expectedTopLeft =
            new(405.0f, 88.0f);

        Point2f expectedTopRight =
            new(951.0f, 86.0f);

        Point2f expectedBottomRight =
            new(1034.5f, 635.0f);

        Point2f expectedBottomLeft =
            new(349.5f, 644.0f);

        const double maxCornerDistance =
            50.0;


        SquareCandidateVision? topLeft =
            PickNearestCandidate(
                candidates,
                expectedTopLeft,
                maxCornerDistance);

        SquareCandidateVision? topRight =
            PickNearestCandidate(
                candidates,
                expectedTopRight,
                maxCornerDistance);

        SquareCandidateVision? bottomRight =
            PickNearestCandidate(
                candidates,
                expectedBottomRight,
                maxCornerDistance);

        SquareCandidateVision? bottomLeft =
            PickNearestCandidate(
                candidates,
                expectedBottomLeft,
                maxCornerDistance);


        if (topLeft == null ||
            topRight == null ||
            bottomRight == null ||
            bottomLeft == null)
        {
            return null;
        }


        return new FieldCornersVision(
            topLeft.Center,
            topRight.Center,
            bottomRight.Center,
            bottomLeft.Center);
    }


    private static SquareCandidateVision? PickNearestCandidate(
        IEnumerable<SquareCandidateVision> candidates,
        Point2f expected,
        double maximumDistance)
    {
        SquareCandidateVision? best =
            null;

        double bestDistance =
            double.MaxValue;

        foreach (SquareCandidateVision candidate
                 in candidates)
        {
            double distance =
                PointDistanceVision(
                    candidate.Center,
                    expected);

            if (distance <= maximumDistance &&
                distance < bestDistance)
            {
                bestDistance = distance;
                best = candidate;
            }
        }

        return best;
    }


    private static bool FieldCornersAreClose(
        FieldCornersVision a,
        FieldCornersVision b,
        double maximumDistance)
    {
        return
            PointDistanceVision(
                a.TopLeft,
                b.TopLeft) <= maximumDistance &&

            PointDistanceVision(
                a.TopRight,
                b.TopRight) <= maximumDistance &&

            PointDistanceVision(
                a.BottomRight,
                b.BottomRight) <= maximumDistance &&

            PointDistanceVision(
                a.BottomLeft,
                b.BottomLeft) <= maximumDistance;
    }


    private static double PointDistanceVision(
        Point2f a,
        Point2f b)
    {
        double dx =
            a.X - b.X;

        double dy =
            a.Y - b.Y;

        return
            Math.Sqrt(
                dx * dx +
                dy * dy);
    }


    // ============================================================
    // FARBERKENNUNG
    // ============================================================

    private static List<VisionDetectedBox> DetectColoredBoxes(
        Mat source,
        Homography2D worldTransform,
        BoxColor color,
        int expectedCount,
        RoverPose? rover)
    {
        using Mat hsv =
            new();

        using Mat mask =
            new();


        Cv2.CvtColor(
            source,
            hsv,
            ColorConversionCodes.BGR2HSV);


        if (color ==
            BoxColor.Red)
        {
            using Mat maskRed1 =
                new();

            using Mat maskRed2 =
                new();


            Cv2.InRange(
                hsv,
                new Scalar(0, 60, 55),
                new Scalar(14, 255, 255),
                maskRed1);

            Cv2.InRange(
                hsv,
                new Scalar(165, 60, 55),
                new Scalar(179, 255, 255),
                maskRed2);

            Cv2.BitwiseOr(
                maskRed1,
                maskRed2,
                mask);
        }
        else
        {
            Cv2.InRange(
                hsv,
                new Scalar(88, 55, 45),
                new Scalar(138, 255, 255),
                mask);
        }


        using Mat kernelOpen =
            Cv2.GetStructuringElement(
                MorphShapes.Rect,
                new Size(3, 3));

        using Mat kernelClose =
            Cv2.GetStructuringElement(
                MorphShapes.Rect,
                new Size(5, 5));


        Cv2.MorphologyEx(
            mask,
            mask,
            MorphTypes.Open,
            kernelOpen);

        Cv2.MorphologyEx(
            mask,
            mask,
            MorphTypes.Close,
            kernelClose);


        Cv2.FindContours(
            mask,
            out Point[][] contours,
            out _,
            RetrievalModes.External,
            ContourApproximationModes.ApproxSimple);


        List<VisionDetectedBox> candidates =
            new();


        foreach (Point[] contour
                 in contours)
        {
            double area =
                Math.Abs(
                    Cv2.ContourArea(
                        contour));

            if (area < 35.0 ||
                area > 2500.0)
            {
                continue;
            }

            Rect bounds =
                Cv2.BoundingRect(
                    contour);

            if (bounds.Width < 3 ||
                bounds.Height < 3)
            {
                continue;
            }

            Moments moments =
                Cv2.Moments(
                    contour);

            if (Math.Abs(
                    moments.M00) <
                1e-9)
            {
                continue;
            }

            Point2f imageCenter =
                new(
                    (float)(
                        moments.M10 /
                        moments.M00),
                    (float)(
                        moments.M01 /
                        moments.M00));

            Point2f worldCenter =
                worldTransform.Transform(
                    imageCenter);


            if (worldCenter.X < 0.0f ||
                worldCenter.X > 100.0f ||
                worldCenter.Y < 0.0f ||
                worldCenter.Y > 100.0f)
            {
                continue;
            }


            // LEGO-Teile des Rovers können im HSV-Bereich einer
            // Kistenfarbe liegen. Genau das trat im Test bei BLAU
            // unmittelbar vor dem Kontakt auf.
            //
            // Eine echte Kiste liegt am Punkt P etwa 15 cm vom
            // Roverachsen-Mittelpunkt entfernt. Farbflächen innerhalb
            // von 9 cm um den Rover können daher sicher als Roverteil
            // verworfen werden.
            if (rover != null)
            {
                double dxRover =
                    worldCenter.X -
                    rover.X;

                double dyRover =
                    worldCenter.Y -
                    rover.Y;

                double roverDistance =
                    Math.Sqrt(
                        dxRover * dxRover +
                        dyRover * dyRover);

                if (roverDistance <
                    9.0)
                {
                    continue;
                }
            }


            candidates.Add(
                new VisionDetectedBox(
                    new Point2D(
                        worldCenter.X,
                        worldCenter.Y),
                    imageCenter,
                    area,
                    bounds));
        }


        return candidates
            .OrderByDescending(
                b => b.Area)
            .Take(
                Math.Max(
                    0,
                    expectedCount))
            .OrderBy(
                b => b.Position.X)
            .ThenBy(
                b => b.Position.Y)
            .ToList();
    }


    private static void DrawDetectedBoxes(
        Mat display,
        IReadOnlyList<VisionDetectedBox> boxes,
        BoxColor color)
    {
        Scalar drawColor =
            color == BoxColor.Red
                ? new Scalar(0, 0, 255)
                : new Scalar(255, 0, 0);

        string prefix =
            color == BoxColor.Red
                ? "R"
                : "B";


        for (int i = 0;
             i < boxes.Count;
             i++)
        {
            VisionDetectedBox box =
                boxes[i];

            Cv2.Rectangle(
                display,
                box.Bounds,
                drawColor,
                2);

            Cv2.Circle(
                display,
                ToCvPoint(
                    box.ImageCenter),
                5,
                drawColor,
                -1);

            Cv2.PutText(
                display,
                $"{prefix}{i + 1} " +
                $"({box.Position.X:F1},{box.Position.Y:F1})",
                new Point(
                    box.Bounds.X +
                    box.Bounds.Width +
                    6,
                    Math.Max(
                        18,
                        box.Bounds.Y)),
                HersheyFonts.HersheySimplex,
                0.5,
                drawColor,
                2);
        }
    }


    // ============================================================
    // FELD INS BILD ZEICHNEN
    // ============================================================

    private static void DrawFieldCorners(
        Mat image,
        FieldCornersVision corners)
    {
        DrawFieldCorner(
            image,
            corners.TopLeft,
            "0,0");

        DrawFieldCorner(
            image,
            corners.TopRight,
            "0,100");

        DrawFieldCorner(
            image,
            corners.BottomRight,
            "100,100");

        DrawFieldCorner(
            image,
            corners.BottomLeft,
            "100,0");

        Cv2.Line(
            image,
            ToCvPoint(corners.TopLeft),
            ToCvPoint(corners.TopRight),
            new Scalar(0, 255, 0),
            2);

        Cv2.Line(
            image,
            ToCvPoint(corners.TopRight),
            ToCvPoint(corners.BottomRight),
            new Scalar(0, 255, 0),
            2);

        Cv2.Line(
            image,
            ToCvPoint(corners.BottomRight),
            ToCvPoint(corners.BottomLeft),
            new Scalar(0, 255, 0),
            2);

        Cv2.Line(
            image,
            ToCvPoint(corners.BottomLeft),
            ToCvPoint(corners.TopLeft),
            new Scalar(0, 255, 0),
            2);
    }


    private static void DrawFieldCorner(
        Mat image,
        Point2f point,
        string text)
    {
        Point p =
            ToCvPoint(
                point);

        Cv2.Circle(
            image,
            p,
            8,
            new Scalar(0, 255, 0),
            2);

        Cv2.PutText(
            image,
            text,
            new Point(
                p.X + 8,
                p.Y - 8),
            HersheyFonts.HersheySimplex,
            0.5,
            new Scalar(0, 255, 0),
            2);
    }


    // ============================================================
    // SNAPSHOT-KOPIEN
    // ============================================================

    private static VisionDetectedBox CloneDetectedBox(
        VisionDetectedBox box)
    {
        return new VisionDetectedBox(
            new Point2D(
                box.Position.X,
                box.Position.Y),
            new Point2f(
                box.ImageCenter.X,
                box.ImageCenter.Y),
            box.Area,
            new Rect(
                box.Bounds.X,
                box.Bounds.Y,
                box.Bounds.Width,
                box.Bounds.Height));
    }


    private static VisionSnapshot CloneSnapshot(
        VisionSnapshot snapshot)
    {
        return new VisionSnapshot(
            new RoverPose(
                snapshot.Rover.X,
                snapshot.Rover.Y,
                snapshot.Rover.HeadingDeg),
            snapshot.RedBoxes
                .Select(CloneDetectedBox)
                .ToArray(),
            snapshot.BlueBoxes
                .Select(CloneDetectedBox)
                .ToArray());
    }


    // ============================================================
    // AUSGABE
    // ============================================================

    private static void PrintSnapshot(
        VisionSnapshot snapshot)
    {
        Console.WriteLine();
        Console.WriteLine("KAMERA-WELTZUSTAND");
        Console.WriteLine("------------------");

        Console.WriteLine(
            $"Rover: " +
            $"({snapshot.Rover.X:F2}, {snapshot.Rover.Y:F2}), " +
            $"{snapshot.Rover.HeadingDeg:F2}°");

        int i = 1;

        foreach (VisionDetectedBox box
                 in snapshot.RedBoxes)
        {
            Console.WriteLine(
                $"Rot {i++}: " +
                $"({box.Position.X:F2}, {box.Position.Y:F2})");
        }

        i = 1;

        foreach (VisionDetectedBox box
                 in snapshot.BlueBoxes)
        {
            Console.WriteLine(
                $"Blau {i++}: " +
                $"({box.Position.X:F2}, {box.Position.Y:F2})");
        }
    }


    private void ThrowWorkerExceptionIfAny()
    {
        if (workerException != null)
        {
            throw new InvalidOperationException(
                "Fehler im permanenten Kamera-Thread.",
                workerException);
        }
    }


    private static Point ToCvPoint(
        Point2f p)
    {
        return new Point(
            (int)Math.Round(p.X),
            (int)Math.Round(p.Y));
    }


    private static double NormalizeAngleVision(
        double angle)
    {
        angle %=
            360.0;

        if (angle < 0)
        {
            angle +=
                360.0;
        }

        return angle;
    }


    private static double NormalizeSignedAngleVision(
        double angle)
    {
        angle =
            (angle + 180.0) %
            360.0;

        if (angle < 0)
        {
            angle +=
                360.0;
        }

        return
            angle - 180.0;
    }


    public void Dispose()
    {
        stopRequested = true;

        lock (sync)
        {
            Monitor.PulseAll(
                sync);
        }

        if (visionThread.IsAlive)
        {
            visionThread.Join(
                2000);
        }

        arucoDetector.Dispose();
        dictionary.Dispose();
        frame.Dispose();
        capture.Dispose();
    }
}

// ================================================================
// VISION-DATENTYPEN
// ================================================================

public sealed record VisionSnapshot(
    RoverPose Rover,
    IReadOnlyList<VisionDetectedBox> RedBoxes,
    IReadOnlyList<VisionDetectedBox> BlueBoxes);


public sealed record VisionDetectedBox(
    Point2D Position,
    Point2f ImageCenter,
    double Area,
    Rect Bounds);


public sealed record VisionFrameResult(
    bool RoverDetected,
    RoverPose? Rover,
    List<VisionDetectedBox> RedBoxes,
    List<VisionDetectedBox> BlueBoxes);


public sealed record SquareCandidateVision(
    Point2f Center,
    double Area,
    double SquareQuality);


public sealed record FieldCornersVision(
    Point2f TopLeft,
    Point2f TopRight,
    Point2f BottomRight,
    Point2f BottomLeft);


// ================================================================
// REINE C#-HOMOGRAPHIE
// ================================================================

public sealed class Homography2D
{
    private readonly double h00;
    private readonly double h01;
    private readonly double h02;

    private readonly double h10;
    private readonly double h11;
    private readonly double h12;

    private readonly double h20;
    private readonly double h21;


    private Homography2D(
        double h00,
        double h01,
        double h02,
        double h10,
        double h11,
        double h12,
        double h20,
        double h21)
    {
        this.h00 = h00;
        this.h01 = h01;
        this.h02 = h02;

        this.h10 = h10;
        this.h11 = h11;
        this.h12 = h12;

        this.h20 = h20;
        this.h21 = h21;
    }


    public static Homography2D Create(
        Point2f[] source,
        Point2f[] destination)
    {
        if (source == null ||
            destination == null)
        {
            throw new ArgumentNullException();
        }

        if (source.Length != 4 ||
            destination.Length != 4)
        {
            throw new ArgumentException(
                "Für die Homographie werden genau vier Punktpaare benötigt.");
        }


        double[,] a =
            new double[8, 8];

        double[] b =
            new double[8];


        for (int i = 0;
             i < 4;
             i++)
        {
            double x =
                source[i].X;

            double y =
                source[i].Y;

            double u =
                destination[i].X;

            double v =
                destination[i].Y;

            int r =
                2 * i;


            a[r, 0] = x;
            a[r, 1] = y;
            a[r, 2] = 1.0;
            a[r, 3] = 0.0;
            a[r, 4] = 0.0;
            a[r, 5] = 0.0;
            a[r, 6] = -u * x;
            a[r, 7] = -u * y;
            b[r] = u;


            a[r + 1, 0] = 0.0;
            a[r + 1, 1] = 0.0;
            a[r + 1, 2] = 0.0;
            a[r + 1, 3] = x;
            a[r + 1, 4] = y;
            a[r + 1, 5] = 1.0;
            a[r + 1, 6] = -v * x;
            a[r + 1, 7] = -v * y;
            b[r + 1] = v;
        }


        double[] h =
            SolveLinearSystem(
                a,
                b);


        return new Homography2D(
            h[0],
            h[1],
            h[2],
            h[3],
            h[4],
            h[5],
            h[6],
            h[7]);
    }


    public Point2f Transform(
        Point2f point)
    {
        double x =
            point.X;

        double y =
            point.Y;

        double denominator =
            h20 * x +
            h21 * y +
            1.0;

        if (Math.Abs(
                denominator) <
            1e-12)
        {
            throw new InvalidOperationException(
                "Ungültige Homographie: Nenner ist 0.");
        }

        double resultX =
            (
                h00 * x +
                h01 * y +
                h02
            ) /
            denominator;

        double resultY =
            (
                h10 * x +
                h11 * y +
                h12
            ) /
            denominator;

        return new Point2f(
            (float)resultX,
            (float)resultY);
    }


    private static double[] SolveLinearSystem(
        double[,] matrix,
        double[] rightSide)
    {
        const int n = 8;

        double[,] a =
            (double[,])matrix.Clone();

        double[] b =
            (double[])rightSide.Clone();


        for (int column = 0;
             column < n;
             column++)
        {
            int pivotRow =
                column;

            double pivotValue =
                Math.Abs(
                    a[column, column]);


            for (int row =
                     column + 1;
                 row < n;
                 row++)
            {
                double value =
                    Math.Abs(
                        a[row, column]);

                if (value >
                    pivotValue)
                {
                    pivotValue =
                        value;

                    pivotRow =
                        row;
                }
            }


            if (pivotValue <
                1e-12)
            {
                throw new InvalidOperationException(
                    "Die vier Kalibrierpunkte ergeben keine gültige Homographie.");
            }


            if (pivotRow !=
                column)
            {
                for (int c = 0;
                     c < n;
                     c++)
                {
                    double temp =
                        a[column, c];

                    a[column, c] =
                        a[pivotRow, c];

                    a[pivotRow, c] =
                        temp;
                }

                double tempB =
                    b[column];

                b[column] =
                    b[pivotRow];

                b[pivotRow] =
                    tempB;
            }


            double pivot =
                a[column, column];

            for (int c = column;
                 c < n;
                 c++)
            {
                a[column, c] /=
                    pivot;
            }

            b[column] /=
                pivot;


            for (int row = 0;
                 row < n;
                 row++)
            {
                if (row ==
                    column)
                {
                    continue;
                }

                double factor =
                    a[row, column];

                if (Math.Abs(
                        factor) <
                    1e-15)
                {
                    continue;
                }

                for (int c = column;
                     c < n;
                     c++)
                {
                    a[row, c] -=
                        factor *
                        a[column, c];
                }

                b[row] -=
                    factor *
                    b[column];
            }
        }


        return b;
    }
}


// ================================================================
// V4: Kontaktprognose und Offline-Vergleich mit realen Logmessungen
// Autor: Dr. Erhard Henkes
// ================================================================
public static class ContactMotionModel
{
    // Arbeitshypothese: kreisfoermige Kisten, rein translatorischer Kontakt.
    // Dieser Radius ist KEIN Sicherheitsabstand. Sicherheitspruefung bleibt separat.
    public const double NominalRadiusCm = 3.0;

    public static Dictionary<int, Point2D> Predict(
        IReadOnlyList<BoxState> boxes, Point2D pusherStart,
        double headingDeg, double travelCm)
    {
        if (!double.IsFinite(headingDeg) || !double.IsFinite(travelCm) || travelCm < 0)
            throw new InvalidOperationException("Ungueltiger Weg fuer Kontaktprognose.");
        double rad = headingDeg * Math.PI / 180.0;
        double ux = Math.Cos(rad), uy = Math.Sin(rad);
        double radius = NominalRadiusCm;
        var contactAt = boxes.ToDictionary(b => b.Id, _ => double.PositiveInfinity);
        foreach (BoxState box in boxes)
        {
            double dx = box.Position.X - pusherStart.X;
            double dy = box.Position.Y - pusherStart.Y;
            double along = dx * ux + dy * uy;
            double lateral = Math.Abs(-dx * uy + dy * ux);
            double edgeDistance = Math.Max(0.0, lateral - 7.5);
            if (edgeDistance > radius) continue;
            double halfChord = Math.Sqrt(radius * radius - edgeDistance * edgeDistance);
            if (along + halfChord < 0.0) continue;
            contactAt[box.Id] = Math.Max(0.0, along - halfChord);
        }
        // Frueheste Kontaktwege durch Ketten weitergeben. Alle aktivierten
        // Kisten haben hier dieselbe Geschwindigkeit; Drehung/Reibung fehlen.
        for (int pass = 0; pass < boxes.Count; pass++)
        {
            bool changed = false;
            foreach (BoxState moving in boxes)
            {
                if (contactAt[moving.Id] > travelCm) continue;
                foreach (BoxState other in boxes)
                {
                    if (moving.Id == other.Id) continue;
                    double dx = other.Position.X - moving.Position.X;
                    double dy = other.Position.Y - moving.Position.Y;
                    double along = dx * ux + dy * uy;
                    double lateral = Math.Abs(-dx * uy + dy * ux);
                    if (along <= 0 || lateral > 2.0 * radius) continue;
                    double gap = Math.Max(0.0, along - Math.Sqrt(
                        4.0 * radius * radius - lateral * lateral));
                    double contact = contactAt[moving.Id] + gap;
                    if (contact < contactAt[other.Id])
                    {
                        contactAt[other.Id] = contact;
                        changed = true;
                    }
                }
            }
            if (!changed) break;
        }
        return boxes.ToDictionary(b => b.Id, b =>
        {
            double move = Math.Max(0.0, travelCm - contactAt[b.Id]);
            return new Point2D(b.Position.X + move * ux, b.Position.Y + move * uy);
        });
    }
}

public sealed class ReplayBox
{
    public int Id { get; set; }
    public string Color { get; set; } = "";
    public double X { get; set; }
    public double Y { get; set; }
    public double AfterX { get; set; }
    public double AfterY { get; set; }
}

public sealed class ReplayCase
{
    public double AfterRoverX { get; set; }
    public double AfterRoverY { get; set; }
    public double AfterRoverHeading { get; set; }
    public string Source { get; set; } = "";
    public int Action { get; set; }
    public double Heading { get; set; }
    public double CenterX { get; set; }
    public double CenterY { get; set; }
    public double ContactTravel { get; set; }
    public double Push { get; set; }
    public double RoverX { get; set; }
    public double RoverY { get; set; }
    public double RoverHeading { get; set; }
    public List<int> Affected { get; set; } = new();
    public List<ReplayBox> Boxes { get; set; } = new();
}

public static class OfflineReplay
{
    private const string Number = @"[-+]?\d+(?:[,.]\d+)?";
    private static double ReadNumber(string s) =>
        double.Parse(s.Replace(',', '.'), CultureInfo.InvariantCulture);
    private static System.Text.RegularExpressions.Match Last(string text, string pattern) =>
        System.Text.RegularExpressions.Regex.Matches(text, pattern).Cast<System.Text.RegularExpressions.Match>()
            .LastOrDefault() ?? System.Text.RegularExpressions.Match.Empty;

    public static List<ReplayCase> ParseLog(string text, string source)
    {
        var result = new List<ReplayCase>();
        var actions = System.Text.RegularExpressions.Regex.Matches(text, @"PLANUNG FÜR AKTION (\d+)");
        for (int i = 0; i < actions.Count; i++)
        {
            int start = actions[i].Index;
            int end = i + 1 < actions.Count ? actions[i + 1].Index : text.Length;
            string part = text.Substring(start, end - start);
            int marker = part.IndexOf("KAMERA-AUSWERTUNG DES KI-FREISCHUBS", StringComparison.Ordinal);
            if (marker < 0) continue; // nicht ausgefuehrte Vorschlaege nicht als Versuch zaehlen
            string before = part.Substring(0, marker), after = part.Substring(marker);
            var center = Last(before, @"Kontaktzentrum Schieber: \((" + Number + @"), (" + Number + @")\)");
            var head = Last(before, @"Schubrichtung: (" + Number + ")");
            var contact = Last(before, @"Restweg bis Kontakt: (" + Number + ")");
            var push = Last(before, @"KI-Freischub: (" + Number + ")");
            var rover = Last(before, @"Rover real: \((" + Number + @"), (" + Number + @")\), (" + Number + ")");
            var affected = Last(before, @"Möglicherweise berührte Kisten: ([^\r\n]+)");
            if (!center.Success || !head.Success || !contact.Success || !push.Success || !rover.Success || !affected.Success)
            {
                Console.WriteLine($"Nicht auswertbar: {source}, Aktion {actions[i].Groups[1].Value}: Geometrie fehlt.");
                continue;
            }
            var item = new ReplayCase
            {
                Source = source,
                Action = int.Parse(actions[i].Groups[1].Value),
                Heading = ReadNumber(head.Groups[1].Value),
                CenterX = ReadNumber(center.Groups[1].Value),
                CenterY = ReadNumber(center.Groups[2].Value),
                ContactTravel = ReadNumber(contact.Groups[1].Value),
                Push = ReadNumber(push.Groups[1].Value),
                RoverX = ReadNumber(rover.Groups[1].Value),
                RoverY = ReadNumber(rover.Groups[2].Value),
                RoverHeading = ReadNumber(rover.Groups[3].Value),
                Affected = System.Text.RegularExpressions.Regex.Matches(affected.Groups[1].Value, @"#(\d+)")
                    .Cast<System.Text.RegularExpressions.Match>().Select(m => int.Parse(m.Groups[1].Value)).ToList()
            };
            string boxPattern = @"Kiste #(\d+) / (ROT|BLAU): \((" + Number + @"), (" + Number +
                @")\) -> \((" + Number + @"), (" + Number + @")\)";
            foreach (System.Text.RegularExpressions.Match m in System.Text.RegularExpressions.Regex.Matches(after, boxPattern))
                item.Boxes.Add(new ReplayBox
                {
                    Id = int.Parse(m.Groups[1].Value),
                    Color = m.Groups[2].Value,
                    X = ReadNumber(m.Groups[3].Value),
                    Y = ReadNumber(m.Groups[4].Value),
                    AfterX = ReadNumber(m.Groups[5].Value),
                    AfterY = ReadNumber(m.Groups[6].Value)
                });
            if (item.Boxes.Count == 4 && item.Boxes.Select(b => b.Id).Distinct().Count() == 4)
                result.Add(item);
            else Console.WriteLine($"Nicht auswertbar: {source}, Aktion {item.Action}: keine vier eindeutigen Kistenmessungen.");
        }
        return result;
    }

    public static void Run(string[] paths)
    {
        Console.WriteLine("EV3 V7 - OFFLINE-TEST: keine Kamera, kein COM-Port, keine Ollama-Anfrage.");
        Console.WriteLine("Vergleich: alte Gleichverschiebung / Kontaktmodell / reale Kamera-Endposition.");
        Console.WriteLine("Kontaktmodell ist eine Hypothese ohne Rotation und Reibung; kein Physikbeweis.");
        List<ReplayCase> cases;
        if (paths.Length == 0)
        {
            cases = JsonSerializer.Deserialize<List<ReplayCase>>(EmbeddedCases) ?? new();
            Console.WriteLine("Eingebaute 13 Freischuebe aus den vorliegenden Logs.");
        }
        else
        {
            cases = new();
            var seen = new HashSet<string>();
            foreach (string path in paths)
            {
                IEnumerable<string> files = Directory.Exists(path)
                    ? Directory.EnumerateFiles(path, "EV3_Log_*.txt").OrderBy(x => x)
                    : new[] { path };
                foreach (string file in files)
                {
                    string text = File.ReadAllText(file);
                    string hash = Convert.ToHexString(System.Security.Cryptography.SHA256.HashData(Encoding.UTF8.GetBytes(text)));
                    if (!seen.Add(hash)) continue;
                    var parsed = ParseLog(text, Path.GetFileName(file));
                    Console.WriteLine($"{Path.GetFileName(file)}: {parsed.Count} auswertbare KI-Freischuebe.");
                    cases.AddRange(parsed);
                }
            }
        }
        if (cases.Count == 0) { Console.WriteLine("Keine vollstaendigen KI-Freischuebe gefunden."); return; }
        double oldTotal = 0, newTotal = 0, maximum = 0;
        int observations = 0, oldMismatch = 0, newMismatch = 0;
        foreach (ReplayCase c in cases)
        {
            var boxes = c.Boxes.Select(b => new BoxState(b.Id,
                b.Color == "ROT" ? BoxColor.Red : BoxColor.Blue, new Point2D(b.X, b.Y))).ToList();
            double rad = c.RoverHeading * Math.PI / 180.0;
            Point2D start = new(c.RoverX + 6.0 * Math.Cos(rad), c.RoverY + 6.0 * Math.Sin(rad));
            var prediction = ContactMotionModel.Predict(boxes, start, c.RoverHeading, c.ContactTravel + c.Push);
            Console.WriteLine($"\n{c.Source}, Aktion {c.Action}");
            Console.WriteLine("Kiste | Weg real / neu | Endfehler alt / neu (cm)");
            foreach (ReplayBox b in c.Boxes)
            {
                Point2D initial = new(b.X, b.Y), measured = new(b.AfterX, b.AfterY);
                double oldMove = c.Affected.Contains(b.Id) ? c.Push : 0.0;
                double oldRad = c.Heading * Math.PI / 180.0;
                Point2D oldEnd = new(b.X + oldMove * Math.Cos(oldRad), b.Y + oldMove * Math.Sin(oldRad));
                double realMove = Geometry2D.Distance(initial, measured);
                double newMove = Geometry2D.Distance(initial, prediction[b.Id]);
                double oldError = Geometry2D.Distance(oldEnd, measured);
                double newError = Geometry2D.Distance(prediction[b.Id], measured);
                oldTotal += oldError; newTotal += newError; maximum = Math.Max(maximum, newError); observations++;
                if ((oldMove > 1.0) != (realMove > 1.0)) oldMismatch++;
                if ((newMove > 1.0) != (realMove > 1.0)) newMismatch++;
                Console.WriteLine($"#{b.Id}: {realMove:F2} / {newMove:F2} | {oldError:F2} / {newError:F2}");
            }
            if (paths.Length == 0)
            {
                // Nur fuer die eingebauten Faelle ist bekannt: alle Kisten offen.
                // Gemessene Rueckzugsposition wird in BEIDEN Welten benutzt.
                var pose = new RoverPose(c.AfterRoverX, c.AfterRoverY, c.AfterRoverHeading);
                var forecast = new WorldModel(pose, boxes.Select(b => new BoxState(
                    b.Id, b.Color, prediction[b.Id])).ToList(), TargetZoneFactory.CreateDefaultZones());
                var measuredWorld = new WorldModel(pose, c.Boxes.Select(b => new BoxState(b.Id,
                    b.Color == "ROT" ? BoxColor.Red : BoxColor.Blue,
                    new Point2D(b.AfterX, b.AfterY))).ToList(), TargetZoneFactory.CreateDefaultZones());
                Console.WriteLine($"Anschlusspruefung (gemessene Roverpose, freie Slots): " +
                    $"Prognose {TaskPlanner.CountDirectlySortableBoxes(forecast)}, " +
                    $"Kamerawelt {TaskPlanner.CountDirectlySortableBoxes(measuredWorld)} direkt sortierbare Kisten.");
                double nearest = measuredWorld.Boxes.Min(b => Geometry2D.Distance(
                    new Point2D(pose.X, pose.Y), b.Position));
                Console.WriteLine($"Kleinster Achsabstand zu einer Kiste nach Rueckzug: {nearest:F2} cm.");
            }
        }
        Console.WriteLine($"\n{cases.Count} Freischuebe, {observations} Kistenmessungen.");
        Console.WriteLine($"Mittlerer Endpositionsfehler alt / neu: {oldTotal / observations:F2} / {newTotal / observations:F2} cm.");
        Console.WriteLine($"Groesster Endpositionsfehler neu: {maximum:F2} cm.");
        Console.WriteLine($"Falsche bewegt/ruhend-Prognosen (>1 cm), alt / neu: {oldMismatch} / {newMismatch}.");
        Console.WriteLine("Rueckvergleich mit vorhandenen Versuchen; unabhaengiger physischer Test steht aus.");
    }

    private const string EmbeddedCases = """
[
  {
    "AfterRoverX": 61.3,
    "AfterRoverY": 22.17,
    "AfterRoverHeading": 86.93,
    "Source": "EV3_Log_20260909_163429(1).txt",
    "Action": 1,
    "Heading": 90.0,
    "CenterX": 61.71,
    "CenterY": 35.67,
    "ContactTravel": 6.83,
    "Push": 6.0,
    "RoverX": 61.47,
    "RoverY": 22.82,
    "RoverHeading": 90.12,
    "Affected": [
      1,
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 55.76,
        "Y": 43.11,
        "AfterX": 54.1,
        "AfterY": 49.6
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 55.92,
        "Y": 37.01,
        "AfterX": 55.49,
        "AfterY": 43.67
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 64.05,
        "Y": 43.5,
        "AfterX": 64.22,
        "AfterY": 49.5
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 64.69,
        "Y": 38.14,
        "AfterX": 64.88,
        "AfterY": 44.15
      }
    ]
  },
  {
    "AfterRoverX": 80.05,
    "AfterRoverY": 49.4,
    "AfterRoverHeading": 179.76,
    "Source": "EV3_Log_20260909_163429(1).txt",
    "Action": 2,
    "Heading": 180.0,
    "CenterX": 66.73,
    "CenterY": 49.5,
    "ContactTravel": 6.64,
    "Push": 6.0,
    "RoverX": 79.36,
    "RoverY": 49.43,
    "RoverHeading": 180.24,
    "Affected": [
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 54.1,
        "Y": 49.6,
        "AfterX": 50.95,
        "AfterY": 50.02
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 55.49,
        "Y": 43.67,
        "AfterX": 50.65,
        "AfterY": 43.27
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 64.22,
        "Y": 49.5,
        "AfterX": 57.0,
        "AfterY": 49.35
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 64.88,
        "Y": 44.15,
        "AfterX": 57.06,
        "AfterY": 43.57
      }
    ]
  },
  {
    "AfterRoverX": 72.73,
    "AfterRoverY": 52.28,
    "AfterRoverHeading": 182.87,
    "Source": "EV3_Log_20260909_163429(1).txt",
    "Action": 3,
    "Heading": 180.0,
    "CenterX": 59.5,
    "CenterY": 52.36,
    "ContactTravel": 6.69,
    "Push": 10.0,
    "RoverX": 72.2,
    "RoverY": 52.22,
    "RoverHeading": 180.4,
    "Affected": [
      1,
      2,
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 50.95,
        "Y": 50.02,
        "AfterX": 39.8,
        "AfterY": 49.95
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 50.65,
        "Y": 43.27,
        "AfterX": 44.93,
        "AfterY": 40.3
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 57.0,
        "Y": 49.35,
        "AfterX": 45.86,
        "AfterY": 49.27
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 57.06,
        "Y": 43.57,
        "AfterX": 51.95,
        "AfterY": 41.73
      }
    ]
  },
  {
    "AfterRoverX": 28.77,
    "AfterRoverY": 39.13,
    "AfterRoverHeading": 43.83,
    "Source": "EV3_Log_20260909_163429(1).txt",
    "Action": 4,
    "Heading": 45.0,
    "CenterX": 38.05,
    "CenterY": 48.17,
    "ContactTravel": 6.79,
    "Push": 8.0,
    "RoverX": 29.29,
    "RoverY": 38.88,
    "RoverHeading": 45.85,
    "Affected": [
      1,
      3
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 39.8,
        "Y": 49.95,
        "AfterX": 45.6,
        "AfterY": 58.22
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 44.93,
        "Y": 40.3,
        "AfterX": 47.06,
        "AfterY": 41.06
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 45.86,
        "Y": 49.27,
        "AfterX": 50.32,
        "AfterY": 52.89
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 51.95,
        "Y": 41.73,
        "AfterX": 53.79,
        "AfterY": 40.83
      }
    ]
  },
  {
    "AfterRoverX": 69.37,
    "AfterRoverY": 25.51,
    "AfterRoverHeading": 87.49,
    "Source": "EV3_Log_20260909_164902(1).txt",
    "Action": 1,
    "Heading": 90.0,
    "CenterX": 69.3,
    "CenterY": 38.75,
    "ContactTravel": 6.97,
    "Push": 10.0,
    "RoverX": 69.48,
    "RoverY": 25.79,
    "RoverHeading": 90.55,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 62.89,
        "Y": 47.15,
        "AfterX": 59.43,
        "AfterY": 55.49
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 64.34,
        "Y": 41.27,
        "AfterX": 62.94,
        "AfterY": 50.39
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 55.47,
        "Y": 44.3,
        "AfterX": 55.48,
        "AfterY": 44.29
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 57.09,
        "Y": 38.88,
        "AfterX": 57.06,
        "AfterY": 38.85
      }
    ]
  },
  {
    "AfterRoverX": 75.95,
    "AfterRoverY": 41.23,
    "AfterRoverHeading": 133.3,
    "Source": "EV3_Log_20260909_164902(1).txt",
    "Action": 2,
    "Heading": 135.0,
    "CenterX": 66.81,
    "CenterY": 50.79,
    "ContactTravel": 6.81,
    "Push": 12.0,
    "RoverX": 75.89,
    "RoverY": 41.75,
    "RoverHeading": 136.14,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 59.43,
        "Y": 55.49,
        "AfterX": 50.9,
        "AfterY": 64.93
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 62.94,
        "Y": 50.39,
        "AfterX": 54.66,
        "AfterY": 59.95
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 55.48,
        "Y": 44.29,
        "AfterX": 55.49,
        "AfterY": 44.3
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 57.06,
        "Y": 38.85,
        "AfterX": 57.08,
        "AfterY": 38.85
      }
    ]
  },
  {
    "AfterRoverX": 69.12,
    "AfterRoverY": 52.21,
    "AfterRoverHeading": 133.69,
    "Source": "EV3_Log_20260909_164902(1).txt",
    "Action": 3,
    "Heading": 135.0,
    "CenterX": 59.97,
    "CenterY": 61.73,
    "ContactTravel": 6.78,
    "Push": 10.0,
    "RoverX": 69.01,
    "RoverY": 52.69,
    "RoverHeading": 134.85,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 50.9,
        "Y": 64.93,
        "AfterX": 43.65,
        "AfterY": 73.13
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 54.66,
        "Y": 59.95,
        "AfterX": 47.33,
        "AfterY": 67.98
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 55.49,
        "Y": 44.3,
        "AfterX": 55.5,
        "AfterY": 44.29
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 57.08,
        "Y": 38.85,
        "AfterX": 57.02,
        "AfterY": 38.85
      }
    ]
  },
  {
    "AfterRoverX": 62.06,
    "AfterRoverY": 60.23,
    "AfterRoverHeading": 133.81,
    "Source": "EV3_Log_20260909_164902(1).txt",
    "Action": 4,
    "Heading": 135.0,
    "CenterX": 52.6,
    "CenterY": 69.72,
    "ContactTravel": 6.69,
    "Push": 8.0,
    "RoverX": 61.67,
    "RoverY": 60.82,
    "RoverHeading": 133.93,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 43.65,
        "Y": 73.13,
        "AfterX": 37.47,
        "AfterY": 79.74
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 47.33,
        "Y": 67.98,
        "AfterX": 41.37,
        "AfterY": 74.76
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 55.5,
        "Y": 44.29,
        "AfterX": 55.42,
        "AfterY": 44.29
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 57.02,
        "Y": 38.85,
        "AfterX": 57.02,
        "AfterY": 38.84
      }
    ]
  },
  {
    "AfterRoverX": 72.55,
    "AfterRoverY": 38.6,
    "AfterRoverHeading": 89.94,
    "Source": "EV3_Log_20260909_174218.txt",
    "Action": 1,
    "Heading": 90.0,
    "CenterX": 72.59,
    "CenterY": 52.0,
    "ContactTravel": 6.82,
    "Push": 10.0,
    "RoverX": 72.67,
    "RoverY": 39.18,
    "RoverHeading": 91.03,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 65.42,
        "Y": 60.89,
        "AfterX": 63.09,
        "AfterY": 69.72
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 67.62,
        "Y": 54.51,
        "AfterX": 66.51,
        "AfterY": 64.58
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 58.59,
        "Y": 57.85,
        "AfterX": 58.57,
        "AfterY": 57.84
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 60.47,
        "Y": 52.07,
        "AfterX": 60.47,
        "AfterY": 52.09
      }
    ]
  },
  {
    "AfterRoverX": 44.64,
    "AfterRoverY": 45.67,
    "AfterRoverHeading": 3.71,
    "Source": "EV3_Log_20260909_174218.txt",
    "Action": 2,
    "Heading": 0.0,
    "CenterX": 57.97,
    "CenterY": 46.1,
    "ContactTravel": 6.55,
    "Push": 10.0,
    "RoverX": 45.45,
    "RoverY": 46.05,
    "RoverHeading": 0.43,
    "Affected": [
      2,
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 63.09,
        "Y": 69.72,
        "AfterX": 63.07,
        "AfterY": 69.72
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 66.51,
        "Y": 64.58,
        "AfterX": 66.59,
        "AfterY": 64.64
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 58.57,
        "Y": 57.84,
        "AfterX": 58.64,
        "AfterY": 57.9
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 60.47,
        "Y": 52.09,
        "AfterX": 71.96,
        "AfterY": 52.75
      }
    ]
  },
  {
    "AfterRoverX": 78.29,
    "AfterRoverY": 39.41,
    "AfterRoverHeading": 90.57,
    "Source": "EV3_Log_20260909_175440(1).txt",
    "Action": 1,
    "Heading": 90.0,
    "CenterX": 78.23,
    "CenterY": 52.85,
    "ContactTravel": 6.87,
    "Push": 10.0,
    "RoverX": 78.11,
    "RoverY": 39.98,
    "RoverHeading": 90.34,
    "Affected": [
      1,
      2
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 71.08,
        "Y": 62.55,
        "AfterX": 70.33,
        "AfterY": 71.09
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 73.21,
        "Y": 55.31,
        "AfterX": 72.79,
        "AfterY": 65.34
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 62.87,
        "Y": 60.15,
        "AfterX": 62.93,
        "AfterY": 60.17
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 64.7,
        "Y": 53.2,
        "AfterX": 64.68,
        "AfterY": 53.21
      }
    ]
  },
  {
    "AfterRoverX": 76.25,
    "AfterRoverY": 42.04,
    "AfterRoverHeading": 139.23,
    "Source": "EV3_Log_20260909_175440(1).txt",
    "Action": 2,
    "Heading": 135.0,
    "CenterX": 66.48,
    "CenterY": 51.46,
    "ContactTravel": 6.76,
    "Push": 10.0,
    "RoverX": 75.5,
    "RoverY": 42.4,
    "RoverHeading": 134.21,
    "Affected": [
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 70.33,
        "Y": 71.09,
        "AfterX": 70.4,
        "AfterY": 71.12
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 72.79,
        "Y": 65.34,
        "AfterX": 72.86,
        "AfterY": 65.39
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 62.93,
        "Y": 60.17,
        "AfterX": 60.64,
        "AfterY": 64.1
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 64.68,
        "Y": 53.21,
        "AfterX": 55.94,
        "AfterY": 58.47
      }
    ]
  },
  {
    "AfterRoverX": 49.67,
    "AfterRoverY": 73.99,
    "AfterRoverHeading": 270.39,
    "Source": "EV3_Log_20260909_175440(1).txt",
    "Action": 3,
    "Heading": 270.0,
    "CenterX": 49.97,
    "CenterY": 60.97,
    "ContactTravel": 6.73,
    "Push": 10.0,
    "RoverX": 49.73,
    "RoverY": 73.65,
    "RoverHeading": 269.63,
    "Affected": [
      3,
      4
    ],
    "Boxes": [
      {
        "Id": 1,
        "Color": "ROT",
        "X": 70.4,
        "Y": 71.12,
        "AfterX": 70.44,
        "AfterY": 71.14
      },
      {
        "Id": 2,
        "Color": "ROT",
        "X": 72.86,
        "Y": 65.39,
        "AfterX": 72.77,
        "AfterY": 65.36
      },
      {
        "Id": 3,
        "Color": "BLAU",
        "X": 60.64,
        "Y": 64.1,
        "AfterX": 61.07,
        "AfterY": 64.13
      },
      {
        "Id": 4,
        "Color": "BLAU",
        "X": 55.94,
        "Y": 58.47,
        "AfterX": 56.09,
        "AfterY": 46.68
      }
    ]
  }
]
""";
}


// Strategiezustand stammt ausschliesslich aus ausgefuehrten Schueben und Kamera.
public static class FreePushStrategy
{
    private static readonly HashSet<int> PendingGroup = new();
    private static double GroupHeading;
    public static bool HasPendingGroup => PendingGroup.Count > 1;

    public static void Reset() => PendingGroup.Clear();

    public static void Observe(WorldModel world, AgentActionCandidate action,
        IReadOnlyDictionary<int, Point2D> before)
    {
        var moved = world.Boxes.Where(b => !b.Sorted &&
            Geometry2D.Distance(before[b.Id], b.Position) > 1.0).Select(b => b.Id).ToList();
        if (moved.Count > 1)
        {
            PendingGroup.Clear();
            PendingGroup.UnionWith(moved);
            GroupHeading = action.FreePush!.PushHeadingDeg;
            Console.WriteLine("STRATEGIE: Gruppe bewegt; als Naechstes quer dazu eine Einzelkiste abtrennen.");
        }
        else if (moved.Count == 1 && PendingGroup.Contains(moved[0]))
        {
            int id = moved[0];
            var peers = world.Boxes.Where(b => b.Id != id && PendingGroup.Contains(b.Id) && !b.Sorted).ToList();
            if (peers.Count == 0) { Reset(); return; }
            double oldGap = peers.Min(b => Geometry2D.Distance(before[id], before[b.Id]));
            double newGap = peers.Min(b => Geometry2D.Distance(
                world.Boxes.Single(x => x.Id == id).Position, b.Position));
            if (newGap >= 14.0 && newGap > oldGap + 1.0)
            {
                PendingGroup.Remove(id);
                if (PendingGroup.Count < 2) Reset();
                Console.WriteLine("STRATEGIE: Kamera bestaetigt vergroesserten Abstand der Einzelkiste.");
            }
            // Bei kleinerem Fortschritt bleibt das Trennziel aktiv.
        }
    }

    public static string Context()
    {
        if (!HasPendingGroup) return "STRATEGIE: Gruppe freistellen; anschliessend quer dazu eine Einzelkiste abtrennen.";
        return $"AKTIVES TRENNZIEL: Kisten {string.Join(",", PendingGroup.OrderBy(x => x).Select(x => "#" + x))} " +
            $"wurden gemeinsam in Richtung {GroupHeading:F1} Grad bewegt. " +
            $"Pruefe jetzt {Normalize(GroupHeading + 90):F1} oder {Normalize(GroupHeading - 90):F1} Grad, " +
            "also quer dazu. Genau eine dieser Kisten soll bewegt werden, ihre Nachbarn sollen stehenbleiben. " +
            "Nicht das Paar erneut gemeinsam verschieben. Versatz und Anfahrt muessen geometrisch zulaessig sein.";
    }

    private static double Normalize(double angle) => (angle % 360.0 + 360.0) % 360.0;

    // Beide Laengenszenarien muessen einen einzelnen, nach aussen gerichteten
    // Kontakt vorhersagen. Die konservative Sicherheitspruefung folgt separat.
    public static double TargetPairGain(WorldModel world, FreePushProposal proposal, bool requireCross = false)
    {
        if (!HasPendingGroup || !PendingGroup.Contains(proposal.PrimaryBoxId)) return double.NegativeInfinity;
        double delta = Math.Abs(Geometry2D.ShortestTurn(GroupHeading, proposal.PushHeadingDeg));
        if (requireCross && Math.Abs(delta - 90.0) > 15.0) return double.NegativeInfinity;
        BoxState? primary = world.Boxes.FirstOrDefault(b => b.Id == proposal.PrimaryBoxId && !b.Sorted);
        if (primary == null) return double.NegativeInfinity;
        var peers = world.Boxes.Where(b => !b.Sorted && b.Id != primary.Id && PendingGroup.Contains(b.Id)).ToList();
        if (peers.Count == 0) return double.NegativeInfinity;
        double oldGap = peers.Min(b => Geometry2D.Distance(primary.Position, b.Position));
        FreePushGeometry geometry = FreePushPlanner.CalculateGeometry(primary.Position,
            proposal.PushHeadingDeg, proposal.LateralOffsetCm, proposal.PushDistanceCm);
        double rad = geometry.PushHeadingDeg * Math.PI / 180.0;
        Point2D start = new(geometry.ApproachPoint.X + 6.0 * Math.Cos(rad),
            geometry.ApproachPoint.Y + 6.0 * Math.Sin(rad));
        double minimumGain = double.PositiveInfinity;
        foreach (double fraction in new[] { 0.7, 1.0 })
        {
            var positions = ContactMotionModel.Predict(world.Boxes, start,
                proposal.PushHeadingDeg, 6.5 + fraction * proposal.PushDistanceCm);
            var moved = world.Boxes.Where(b => Geometry2D.Distance(b.Position, positions[b.Id]) > 1.0).ToList();
            if (moved.Count != 1 || moved[0].Id != primary.Id) return double.NegativeInfinity;
            double gap = peers.Min(b => Geometry2D.Distance(positions[primary.Id], positions[b.Id]));
            minimumGain = Math.Min(minimumGain, gap - oldGap);
        }
        return minimumGain;
    }

    public static bool PredictsCrossSeparation(WorldModel world, FreePushProposal proposal) =>
        TargetPairGain(world, proposal, requireCross: true) >= 1.0;

    public static bool IsUseful(WorldModel world, FreePushProposal proposal,
        (int Sortable, int ReleasedPairs, double LocalGain) future, out string reason)
    {
        if (future.Sortable > 0) { reason = "Folgeschritt Sortieren wird moeglich."; return true; }
        if (HasPendingGroup)
        {
            double targetGain = TargetPairGain(world, proposal);
            if (targetGain >= 1.0)
            {
                reason = $"Aktives Paar wird um mindestens {targetGain:F1} cm getrennt; " +
                    "nur eine Kiste bewegt sich im Kontaktmodell. Globale Nahpaare sind hier kein Veto.";
                return true;
            }
            reason = "Aktives Paar wird nicht ausreichend einzeln getrennt; andere Seite oder Versatz suchen.";
            return false;
        }
        if (future.ReleasedPairs < 0 || future.LocalGain < 0.5)
        {
            reason = $"Kein ausreichender Fortschritt: 0 sortierbar, Nahpaare {future.ReleasedPairs}, " +
                $"Abstandsgewinn {future.LocalGain:F1} cm. Suche eine andere Richtung/Kiste.";
            return false;
        }
        reason = "Trennfortschritt prognostiziert.";
        return true;
    }

    public static AgentActionCandidate? TryCrossPush(WorldModel world)
    {
        if (!HasPendingGroup) return null;
        Console.WriteLine("\nDETERMINISTISCHE QUERSCHUB-SUCHE: " + Context());
        Stopwatch clock = Stopwatch.StartNew();
        int checkedCount = 0, physicalCount = 0;
        var choices = new List<(AgentActionCandidate Candidate, int Sortable, double TargetGain, int Pairs, double Gain)>();
        foreach (int id in PendingGroup.OrderBy(x => x))
            foreach (double heading in new[] { Normalize(GroupHeading + 90), Normalize(GroupHeading - 90) })
                foreach (double offset in new[] { -6.0, -3.0, 0.0, 3.0, 6.0 })
                    foreach (double distance in new[] { 6.0, 10.0, 14.0 })
                    {
                        checkedCount++;
                        var proposal = new FreePushProposal(id, heading, offset, distance,
                            "STRATEGIE: Nach Gruppenfreischub quer dazu eine Einzelkiste abtrennen.");
                        try
                        {
                            if (!PredictsCrossSeparation(world, proposal)) continue;
                            if (!FreePushPlanner.TryCreate(world, proposal, out AgentActionCandidate? candidate, out _)) continue;
                            physicalCount++;
                            var future = FreePushPlanner.EvaluateFuture(world, candidate!);
                            double targetGain = TargetPairGain(world, proposal);
                            bool useful = IsUseful(world, proposal, future, out string reason);
                            Console.WriteLine($"  QUER #{id}, {heading:F0} Grad, Versatz {offset:F1}, Schub {distance:F1}: " +
                                $"Paargewinn {targetGain:F2} cm; sortierbar {future.Sortable}; " +
                                $"globale Nahpaare {future.ReleasedPairs}; globaler Gewinn {future.LocalGain:F1}. " +
                                $"{(useful ? "AKZEPTIERT" : "VERWORFEN")}: {reason}");
                            if (!useful) continue;
                            candidate = candidate! with
                            {
                                Reason = candidate!.Reason +
                                $" QUERSCHUB: Paargewinn mindestens {targetGain:F1} cm; {future.Sortable} sortierbar. " + reason
                            };
                            choices.Add((candidate, future.Sortable, targetGain, future.ReleasedPairs, future.LocalGain));
                        }
                        catch (InvalidOperationException) { }
                    }
        clock.Stop();
        Console.WriteLine($"Querschub-Suche: {checkedCount} Varianten, {physicalCount} geometrisch zulaessig, " +
            $"{choices.Count} mit Fortschritt; {clock.Elapsed.TotalSeconds:F1} s.");
        return choices.OrderByDescending(c => c.Sortable).ThenByDescending(c => c.TargetGain).ThenByDescending(c => c.Pairs)
            .ThenByDescending(c => Math.Round(c.Gain, 1)).ThenBy(c => c.Candidate.Score)
            .Select(c => c.Candidate).FirstOrDefault();
    }
}

public sealed class PlanningUnavailableException : Exception
{
    public PlanningUnavailableException(string message) : base(message) { }
}

public static class StrategyRegression
{
    public static void Run()
    {
        Console.WriteLine("V7 REGRESSION: Stellung aus EV3_Log_20260909_221651, nach Aktion 1.");
        var world = new WorldModel(new RoverPose(73.46, 6.21, 88.40), new List<BoxState>
        {
            new(1, BoxColor.Red, new Point2D(67.06, 36.70)),
            new(2, BoxColor.Red, new Point2D(69.75, 31.87)),
            new(3, BoxColor.Blue, new Point2D(59.84, 23.70)),
            new(4, BoxColor.Blue, new Point2D(61.75, 18.20))
        }, TargetZoneFactory.CreateDefaultZones());
        var before = new Dictionary<int, Point2D>
        {
            [1] = new(67.61, 26.61),
            [2] = new(70.48, 21.89),
            [3] = new(59.82, 23.64),
            [4] = new(61.19, 18.15)
        };
        string original = JsonSerializer.Serialize(world);
        var executed = new AgentActionCandidate(AgentActionKind.FreePush, world.Boxes[1], null,
            world.Boxes[1].Position, null!, 0, "Nur Log-Replay, keine Ausfuehrung",
            new FreePushData(before[2], 90, -3, 10, new[] { 1, 2 }, "Replay"));
        FreePushStrategy.Reset();
        try
        {
            FreePushStrategy.Observe(world, executed, before);
            var pairTranslation = new FreePushProposal(2, 90, -3, 10, "Kein Einzelkontakt");
            if (FreePushStrategy.IsUseful(world, pairTranslation, (0, 2, 10.0), out _))
                throw new InvalidOperationException("REGRESSION FEHLER: erneute Paarverschiebung akzeptiert.");
            AgentActionCandidate selected = FreePushStrategy.TryCrossPush(world)
                ?? throw new InvalidOperationException("REGRESSION FEHLER: kein Querschub gefunden.");
            FreePushData data = selected.FreePush!;
            var proposal = new FreePushProposal(selected.Box.Id, data.PushHeadingDeg,
                data.LateralOffsetCm, data.PushDistanceCm, "Regression");
            var future = FreePushPlanner.EvaluateFuture(world, selected);
            if (!FreePushStrategy.PredictsCrossSeparation(world, proposal))
                throw new InvalidOperationException("REGRESSION FEHLER: kein einzelner Querschub.");
            if (!FreePushStrategy.IsUseful(world, proposal, future, out _))
                throw new InvalidOperationException("REGRESSION FEHLER: Zielpruefung widerspricht Auswahl.");
            bool oldGateRejected = future.Sortable == 0 && (future.ReleasedPairs < 0 || future.LocalGain < 0.5);
            if (!oldGateRejected)
                throw new InvalidOperationException("REGRESSION FEHLER: urspruenglicher Bewertungsfehler nicht reproduziert.");
            if (JsonSerializer.Serialize(world) != original)
                throw new InvalidOperationException("REGRESSION FEHLER: Planung hat reale Welt veraendert.");
            Console.WriteLine($"REGRESSION BESTANDEN: Kiste #{selected.Box.Id}, Richtung {data.PushHeadingDeg:F0}, " +
                $"Versatz {data.LateralOffsetCm:F1}, Schub {data.PushDistanceCm:F1} cm.");
            Console.WriteLine("V6 haette verworfen; V7 akzeptiert nach Geometrie-, Einzelkontakt- und Paarabstandspruefung.");
        }
        finally { FreePushStrategy.Reset(); }
    }
}