using MultiWheelC.TrajectoryPlanning.EMPlanner;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class SegmentationChecks
{
    public static void Run()
    {
        var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath();
        var segments = ReferencePathSegmenter.Create(referencePath);

        EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count");
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
            segments[0].EndBoundary.BoundaryType, "forward end boundary");
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture,
            segments[1].StartBoundary.BoundaryType, "reverse start boundary");
        EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary),
            "gear-pair boundary identities differ");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length");
        EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start");

        ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d);
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop,
            rolling.TerminalBoundary.BoundaryType, "rolling terminal type");
        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS,
            "rolling terminal local s");
        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength,
            "rolling anchor path s");
        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X,
            "rolling anchor x");

        ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d);
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
            gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS,
            "gear terminal local s");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength,
            "gear anchor path s");
        for (int index = 0; index < gearSwitch.Points.Count; index++)
        {
            EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction,
                "horizon point direction");
        }
    }
}