namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed partial class EmPlannerConfiguration
{
    public SchedulingConfiguration Scheduling { get; set; }
    public FrenetConfiguration Frenet { get; set; }
    public CorridorConfiguration Corridor { get; set; }
    public LateralConfiguration Lateral { get; set; }
    public LongitudinalConfiguration Longitudinal { get; set; }
    public SolverConfiguration Solver { get; set; }
    public ValidationConfiguration Validation { get; set; }

    public static EmPlannerConfiguration CreateDefault()
    {
        return new EmPlannerConfiguration
        {
            Scheduling = new SchedulingConfiguration
            {
                ReplanPeriodSeconds = 0.20d,
                TimeHorizonSeconds = 6d,                //窗口的规划时间长度。单位s
                DistanceHorizonMeters = 500d,           //最大探索s里程距离，单位m
                OutputTimeStepSeconds = 0.05d,
                SolverTimeoutSeconds = 0.10d,
                HandoffLookaheadSeconds = 0.30d,
                MaximumVehicleStateAgeSeconds = 0.20d,
                MaximumOptimizationTimeStepSeconds = 0.20d,
                MaximumOptimizationSpatialStepMeters = 0.10d,
                MaximumOptimizationKnotCount = 401,
                MaximumPublishedSampleCount = 5001,
            },
            Corridor = new CorridorConfiguration
            {
                LongitudinalSampleSpacingMeters = 0.10d,
                LateralSampleSpacingMeters = 0.025d,
                MaximumLateralOffsetMeters = 0.30d,
                AdditionalClearanceReserveMeters = 0.02d,
                MaximumCollisionCheckStepMeters = 0.025d,
            },
            Frenet = new FrenetConfiguration
            {
                MaximumProjectionDistanceMeters = 0.50d,
                MinimumFrenetDenominator = 0.20d,
                BoundaryAnchorToleranceMeters = 1e-8d,
            },
            Lateral = new LateralConfiguration
            {
                MaximumLateralStepPerIterationMeters = 0.05d,
                MaximumLateralSlope = 0.50d,
                MaximumLateralSecondDerivativePerMeter = 1d,
                MaximumLateralThirdDerivativePerSquareMeter = 2d,
                Weights = new LateralWeights
                {
                    ReferenceOffset = 10d,
                    HeadingDeviation = 1d,
                    SecondDerivative = 5d,
                    ThirdDerivative = 10d,
                    Curvature = 5d,
                    CurvatureVariation = 20d,
                    PreviousTrajectory = 5d,
                    RollingTerminal = 10d,
                },
            },
            Longitudinal = new LongitudinalConfiguration
            {
                MaximumForwardSpeedMetersPerSecond = 1d,
                MaximumReverseSpeedMetersPerSecond = 0.5d,
                DesiredForwardSpeedMetersPerSecond = 1d,
                DesiredReverseSpeedMetersPerSecond = 0.5d,
                MaximumAccelerationMetersPerSecondSquared = 0.20d,
                MaximumDecelerationMetersPerSecondSquared = 0.30d,
                MaximumJerkMetersPerSecondCubed = 0.50d,
                MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
                MaximumCurvatureRatePerMeterPerSecond = 0.50d,
                StopSpeedToleranceMetersPerSecond = 0.01d,
                ZeroSpeedHoldSeconds = 0.20d,
                Weights = new LongitudinalWeights
                {
                    ReferenceSpeed = 10d,
                    Acceleration = 1d,
                    Jerk = 10d,
                    PreviousTrajectory = 5d,
                    TerminalAcceleration = 1d,
                },
            },
            Solver = new SolverConfiguration
            {
                MaximumOuterIterations = 5,
                MaximumOsqpIterations = 4000,
                AbsoluteTolerance = 1e-5d,
                RelativeTolerance = 1e-5d,
                StrictResidualTolerance = 1e-5d,
                WarmStart = true,
                Polish = true,
                NativeVerbose = false,
            },
            Validation = new ValidationConfiguration
            {
                SpatialToleranceMeters = 1e-8d,
                KinematicTolerance = 1e-5d,
                TerminalPositionToleranceMeters = 0d,
                TerminalYawToleranceRadians = 0d,
            },
        };
    }

    internal EmPlannerConfiguration Copy()
    {
        return new EmPlannerConfiguration
        {
            Scheduling = Scheduling == null ? null : Scheduling.Copy(),
            Frenet = Frenet == null ? null : Frenet.Copy(),
            Corridor = Corridor == null ? null : Corridor.Copy(),
            Lateral = Lateral == null ? null : Lateral.Copy(),
            Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
            Solver = Solver == null ? null : Solver.Copy(),
            Validation = Validation == null ? null : Validation.Copy(),
        };
    }
}