using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;

namespace EMPlannerVerificationHost;

internal static class ExecutorChecks
{
    public static void Run()
    {
        VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse");
        VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward");
        VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals();
        VerifiesGenericControlCommandMapping();
        VerifiesVehicleStateProviderRemainsSnapshotOnly();
    }

    public static void RunRollingEndToEnd()
    {
        VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse,
            "end-to-end forward-to-reverse");
        VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward,
            "end-to-end reverse-to-forward");
        VerifiesGoalCommandStopsAtAndAfterTerminal();
    }

    private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
        string name)
    {
        var machine = new GearSwitchStateMachine(0.01d, 0.20d);
        DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(500d);

        GearSwitchStateUpdate approaching = machine.Update(start, 0.10d, desiredDirection, currentDirection, false, false,
            false);
        Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.State, name + " approaches switch");
        Verification.True(approaching.AllowsTrajectoryMotion, name + " approach permits trajectory motion");

        GearSwitchStateUpdate holding = machine.Update(start.AddMilliseconds(100), 0.005d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(holding, GearSwitchState.HoldingZero, name + " holds at boundary");

        GearSwitchStateUpdate movingAgain = machine.Update(start.AddMilliseconds(110), 0.01d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(movingAgain, GearSwitchState.HoldingZero, name + " holds while measured speed is at tolerance");

        GearSwitchStateUpdate dwellRestart = machine.Update(start.AddMilliseconds(120), 0d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(dwellRestart, GearSwitchState.HoldingZero, name + " starts zero-speed dwell");
        GearSwitchStateUpdate dwellShort = machine.Update(start.AddMilliseconds(319), 0d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(dwellShort, GearSwitchState.HoldingZero, name + " does not request before full dwell");

        GearSwitchStateUpdate requesting = machine.Update(start.AddMilliseconds(320), 0d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(requesting, GearSwitchState.RequestingDirectionChange, name + " requests after continuous dwell");
        Verification.True(requesting.RequestDirectionChange, name + " emits exactly one direction request");

        GearSwitchStateUpdate awaiting = machine.Update(start.AddMilliseconds(321), 0d, desiredDirection,
            currentDirection, false, true, false);
        AssertHeld(awaiting, GearSwitchState.AwaitingDirectionConfirmation, name + " awaits confirmation");
        Verification.True(!awaiting.RequestDirectionChange, name + " does not repeat direction request");

        GearSwitchStateUpdate following = machine.Update(start.AddMilliseconds(322), 0d, desiredDirection,
            desiredDirection, true, false, false);
        Verification.Equal(GearSwitchState.Following, following.State, name + " follows confirmed next segment");
        Verification.True(!following.HoldZero && following.AllowsTrajectoryMotion,
            name + " resumes only after confirmation");

        GearSwitchStateUpdate completed = machine.Update(start.AddMilliseconds(323), 0d, desiredDirection,
            desiredDirection, false, false, true);
        AssertHeld(completed, GearSwitchState.Completed, name + " completes at terminal while holding zero");
        Verification.True(completed.IsTrajectoryComplete, name + " marks terminal completion");
    }

    private static void VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(600d);
        var executor = new TrajectoryExecutor();
        EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
            EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);

        TrajectoryExecutionState approaching = executor.Update(effectiveAt.AddSeconds(0.10d),
            CreateMeasuredState(0.10d, effectiveAt.AddSeconds(0.10d), 60L), gearTrajectory, TravelDirection.Reverse,
            TravelDirection.Forward, false);
        Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.GearSwitchState,
            "executor enters approach before exact gear boundary");
        Verification.NearlyEqual(0.10d, approaching.SelectedPoint.SignedLongitudinalVelocity,
            "executor samples moving approach point");

        TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
            CreateMeasuredState(0.005d, effectiveAt.AddSeconds(0.30d), 61L), gearTrajectory, TravelDirection.Reverse,
            TravelDirection.Forward, false);
        AssertHeld(holding, GearSwitchState.HoldingZero, "executor holds at exact gear boundary");
        Verification.Equal(EmBoundaryType.GearSwitchApproach, holding.SelectedPoint.BoundaryType,
            "executor preserves exact gear boundary point");
        Verification.NearlyEqual(0d, holding.SelectedPoint.SignedLongitudinalVelocity,
            "executor does not release nonzero speed while holding");

        foreach (EmBoundaryType terminalBoundary in new[] { EmBoundaryType.Goal, EmBoundaryType.RollingSafetyStop })
        {
            var terminalExecutor = new TrajectoryExecutor();
            EmTrajectory terminal = CreateTrajectory(effectiveAt, TravelDirection.Forward, terminalBoundary,
                terminalBoundary == EmBoundaryType.Goal ? EmTerminalType.Goal : EmTerminalType.RollingSafetyStop);
            TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
                CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 62L), terminal, TravelDirection.Forward,
                TravelDirection.Forward, false);
            AssertHeld(completed, GearSwitchState.Completed, "executor completes " + terminalBoundary);
            Verification.True(completed.IsTrajectoryComplete, "executor marks " + terminalBoundary + " completion");
        }
    }

    private static void VerifiesGenericControlCommandMapping()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(700d);
        var adapter = new TrajectoryControlAdapter();

        VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Forward, 0.12d, 0.35d, 0.042d,
            "forward command");
        VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Reverse, -0.12d, 0.35d, -0.042d,
            "reverse command");

        var executor = new TrajectoryExecutor();
        EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
            EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
        executor.Update(effectiveAt, CreateMeasuredState(0.10d, effectiveAt, 80L), gearTrajectory,
            TravelDirection.Reverse, TravelDirection.Forward, false);
        TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 81L), gearTrajectory,
            TravelDirection.Reverse, TravelDirection.Forward, false);
        TrajectoryControlCommand holdCommand = adapter.CreateCommand(holding.SelectedPoint, holding);
        Verification.NearlyEqual(0d, holdCommand.SignedLongitudinalVelocity,
            "holding command overrides signed longitudinal velocity");
        Verification.NearlyEqual(0d, holdCommand.YawRate, "holding command overrides yaw rate");
        Verification.True(holdCommand.HoldBrake && !holdCommand.IsTrajectoryComplete,
            "gear holding command requests brake without completing trajectory");

        TrajectoryExecutionState requesting = executor.Update(effectiveAt.AddSeconds(0.50d),
            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.50d), 82L), gearTrajectory,
            TravelDirection.Reverse, TravelDirection.Forward, false);
        TrajectoryControlCommand requestCommand = adapter.CreateCommand(requesting.SelectedPoint, requesting);
        Verification.True(requestCommand.RequestDirectionChange && requestCommand.HoldBrake,
            "direction request remains a generic zero-speed brake command");

        EmTrajectory terminalTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward, EmBoundaryType.Goal,
            EmTerminalType.Goal);
        var terminalExecutor = new TrajectoryExecutor();
        TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 82L), terminalTrajectory,
            TravelDirection.Forward, TravelDirection.Forward, false);
        TrajectoryControlCommand completeCommand = adapter.CreateCommand(completed.SelectedPoint, completed);
        Verification.NearlyEqual(0d, completeCommand.SignedLongitudinalVelocity,
            "completed command keeps signed longitudinal velocity at zero");
        Verification.NearlyEqual(0d, completeCommand.YawRate, "completed command keeps yaw rate at zero");
        Verification.True(completeCommand.HoldBrake && completeCommand.IsTrajectoryComplete,
            "completed command holds brake and reports completion");

        Verification.True(typeof(TrajectoryControlCommand).GetProperty("BodyLateralVelocity") == null,
            "generic command exposes no body lateral velocity");
    }

    private static void VerifiesFollowingCommand(TrajectoryControlAdapter adapter, DateTimeOffset effectiveAt,
        TravelDirection direction, double signedVelocity, double curvature, double expectedYawRate, string name)
    {
        EmTrajectory trajectory = CreateMotionTrajectory(effectiveAt, direction, signedVelocity, curvature);
        var executor = new TrajectoryExecutor();
        VehicleMotionState measured = CreateMeasuredState(signedVelocity, effectiveAt, 83L);
        TrajectoryExecutionState following = executor.Update(effectiveAt, measured, trajectory, direction, direction, false);
        TrajectoryControlCommand command = adapter.CreateCommand(following.SelectedPoint, following);
        TrajectoryControlCommand executorCommand = executor.UpdateCommand(effectiveAt, measured, trajectory, direction,
            direction, false);

        Verification.NearlyEqual(following.SelectedPoint.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
            name + " copies signed longitudinal velocity exactly");
        Verification.NearlyEqual(following.SelectedPoint.YawRate, command.YawRate,
            name + " copies yaw rate exactly");
        Verification.NearlyEqual(expectedYawRate, command.YawRate, name + " preserves signed yaw rate");
        Verification.Equal(direction, command.Direction, name + " preserves direction");
        Verification.True(!command.HoldBrake && !command.RequestDirectionChange && !command.IsTrajectoryComplete,
            name + " remains a normal following command");
        Verification.NearlyEqual(command.SignedLongitudinalVelocity, executorCommand.SignedLongitudinalVelocity,
            name + " executor command preserves signed longitudinal velocity");
        Verification.NearlyEqual(command.YawRate, executorCommand.YawRate,
            name + " executor command preserves yaw rate");
        Verification.True(command.SignedLongitudinalVelocity != 0d || command.YawRate == 0d,
            name + " never creates in-place rotation");
        Verification.NearlyEqual(Math.Abs(signedVelocity), following.SelectedPoint.Speed,
            name + " leaves speed available in execution telemetry");
        Verification.NearlyEqual(signedVelocity * Math.Cos(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityX,
            name + " leaves world velocity X in execution telemetry");
        Verification.NearlyEqual(signedVelocity * Math.Sin(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityY,
            name + " leaves world velocity Y in execution telemetry");
        Verification.NearlyEqual(curvature, following.SelectedPoint.VehicleCurvature,
            name + " leaves curvature available in execution telemetry");
    }

    private static void VerifiesVehicleStateProviderRemainsSnapshotOnly()
    {
        DateTimeOffset capturedAt = DateTimeOffset.UnixEpoch.AddSeconds(710d);
        IVehicleStateProvider provider = new FixedVehicleStateProvider(CreateMeasuredState(0.02d, capturedAt, 84L));
        Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
    }

    private static void VerifiesGoalCommandStopsAtAndAfterTerminal()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(950d);
        EmTrajectory goalTrajectory = CreateMotionTrajectory(effectiveAt, TravelDirection.Forward, 0.10d, 0.20d);
        var executor = new TrajectoryExecutor();
        VehicleMotionState moving = CreateMeasuredState(0.10d, effectiveAt, 95L);
        TrajectoryControlCommand following = executor.UpdateCommand(effectiveAt, moving, goalTrajectory,
            TravelDirection.Forward, TravelDirection.Forward, false);
        Verification.NearlyEqual(goalTrajectory.Points[0].SignedLongitudinalVelocity, following.SignedLongitudinalVelocity,
            "goal approach command originates from trajectory point");
        Verification.NearlyEqual(goalTrajectory.Points[0].YawRate, following.YawRate,
            "goal approach command preserves trajectory yaw rate");

        TrajectoryControlCommand atGoal = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 96L), goalTrajectory, TravelDirection.Forward,
            TravelDirection.Forward, false);
        TrajectoryControlCommand afterGoal = executor.UpdateCommand(effectiveAt.AddSeconds(5d),
            CreateMeasuredState(0d, effectiveAt.AddSeconds(5d), 97L), goalTrajectory, TravelDirection.Forward,
            TravelDirection.Forward, false);
        AssertCompletedCommand(atGoal, "exact goal terminal command");
        AssertCompletedCommand(afterGoal, "after goal terminal command");
    }

    private static void AssertCompletedCommand(TrajectoryControlCommand command, string name)
    {
        Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity, name + " signed velocity");
        Verification.NearlyEqual(0d, command.YawRate, name + " yaw rate");
        Verification.True(command.HoldBrake && command.IsTrajectoryComplete, name + " holds completed trajectory");
    }

    private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
    {
        Verification.Equal(expectedState, update.State, name + " state");
        Verification.True(update.HoldZero && !update.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
    }

    private static void AssertHeld(TrajectoryExecutionState state, GearSwitchState expectedState, string name)
    {
        Verification.Equal(expectedState, state.GearSwitchState, name + " state");
        Verification.True(state.HoldZero && !state.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
    }

    private static VehicleMotionState CreateMeasuredState(double speed, DateTimeOffset capturedAt, long sequenceId)
    {
        return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, 0d, capturedAt, sequenceId);
    }

    private static EmTrajectory CreateTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
        EmBoundaryType terminalBoundary, EmTerminalType terminalType)
    {
        double signedSpeed = direction == TravelDirection.Forward ? 0.10d : -0.10d;
        var metadata = new EmTrajectoryMetadata("executor-" + terminalBoundary, effectiveAt, effectiveAt, 70L,
            "executor-reference", 60L, string.Empty, 4, direction, terminalType);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(signedSpeed * 3d, 0d, 0d, 0d, 0.30d, 0d, 4, 0.03d, 0.03d, direction,
                terminalBoundary, 0d, 0d),
        });
    }

    private static EmTrajectory CreateMotionTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
        double signedVelocity, double curvature)
    {
        var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
            "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,
                EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, 0d, 0.30d, curvature, 5, 0.04d, 0.04d, direction,
                EmBoundaryType.Goal, 0d, 0d),
        });
    }

    private sealed class FixedVehicleStateProvider : IVehicleStateProvider
    {
        private readonly VehicleMotionState state;

        public FixedVehicleStateProvider(VehicleMotionState state)
        {
            this.state = state;
        }

        public VehicleMotionState Capture()
        {
            return state;
        }
    }
}