using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class FoundationChecks
{
    public static void Run()
    {
        var reverseState = new VehicleMotionState(
            new Pose2D(1.5d, -2d, 0.25d),
            -0.15d,
            -0.03d,
            new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
            17L);

        EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
            "reverse signed speed");

        var point = new EmTrajectoryPoint(
            1.25d,
            -0.75d,
            0.5d,
            -0.12d,
            0.4d,
            -0.2d,
            3,
            0.6d,
            0.8d,
            TravelDirection.Reverse,
            EmBoundaryType.GearSwitchApproach,
            -0.04d,
            0.03d);

        EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
        EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
        EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
        EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
    }
}