using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace EMPlannerVerificationHost;

internal static class EmFixtureFactory
{
    public static PathSmoothingResult CreateGearPairReferencePath()
    {
        var points = new List<SmoothedPathPoint>
        {
            Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
            Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
            Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
        };
        var segments = new List<SmoothedPathSegment>
        {
            new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
            new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
        };
        var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
    }

    private static SmoothedPathPoint Point(
        double x,
        double arcLength,
        TravelDirection direction,
        bool isGearSwitchPoint,
        SmoothedPathPointSource source)
    {
        return new SmoothedPathPoint(x, 0d, 0d, 0d, arcLength, direction, 0d, 0d, 0d, 1d,
            isGearSwitchPoint, source);
    }
}