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

namespace EMPlannerVerificationHost;

internal static class EmFixtureFactory
{
    public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
    {
        if (firstSegmentLength <= 0d)
            throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
        double midpoint = 0.5d * firstSegmentLength;
        var points = new List<SmoothedPathPoint>
        {
            Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
            Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
            Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
            Point(0d, 2d * firstSegmentLength, 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, 2d * firstSegmentLength, 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);
    }
}