using System;
using CommonUsage.Chassis;
using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
    /// </summary>
    public static class ParkingVehicleStateProviderFactory
    {
        /// <summary>
        /// 使用当前车辆运行配置创建停车轨迹控制的默认状态源。
        /// </summary>
        public static WheelFeedbackVehicleStateProvider Create(
            MultiWheelChassis chassis)
        {
            return Create(
                chassis,
                PilotDefinition.Conf);
        }

        /// <summary>
        /// 使用指定配置快照创建停车轨迹控制的默认状态源。
        /// </summary>
        public static WheelFeedbackVehicleStateProvider Create(
            MultiWheelChassis chassis,
            PilotConfig config)
        {
            if (chassis == null)
            {
                throw new ArgumentNullException(nameof(chassis));
            }

            if (config == null)
            {
                throw new ArgumentNullException(nameof(config));
            }

            var velocityEstimator =
                new VelocityEstimator2D(
                    config.ParkingDetourLinearVelocityFilterSeconds,
                    config.ParkingDetourAngularVelocityFilterSeconds);
            var detourStateProvider =
                new DetourVehicleStateProvider(
                    velocityEstimator,
                    config.ParkingDetourMaximumLinearSpeed,
                    AngleMath.DegreesToRadians(
                        config
                            .ParkingDetourMaximumAngularSpeedDegrees),
                    config.ParkingDetourPositionJumpMargin,
                    AngleMath.DegreesToRadians(
                        config.ParkingDetourHeadingJumpMarginDegrees),
                    config.ParkingDetourVelocityPositionResidual,
                    AngleMath.DegreesToRadians(
                        config
                            .ParkingDetourVelocityHeadingResidualDegrees),
                    config.ParkingDetourStationaryConfirmationSeconds,
                    config.ParkingDetourHeadingOutlierConfirmationFrames,
                    config
                        .ParkingDetourHeadingOutlierPredictionTimeoutSeconds,
                    config.ParkingDetourJumpConfirmationFrames,
                    config.ParkingDetourJumpConfirmationTimeoutSeconds,
                    config.ParkingDetourMaximumAutomaticFrameShift,
                    AngleMath.DegreesToRadians(
                        config
                            .ParkingDetourMaximumAutomaticHeadingShiftDegrees),
                    config.ParkingDetourMaximumCachedFrameAgeSeconds,
                    config
                        .ParkingDetourLocalizationQualityConfirmationFrames);

            return new WheelFeedbackVehicleStateProvider(
                detourStateProvider,
                chassis,
                config.ParkingWheelVelocityFilterSeconds);
        }
    }
}