using System;
using System.Collections.Generic;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;
using MultiWheelC.StateEstimation;

namespace MultiWheelC
{
    /// <summary>
    /// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。
    /// </summary>
    public enum InPlaceRotationFeedbackMode
    {
        DetourAbsoluteHeading,
        RelativeWheelOdometry
    }

    /// <summary>
    /// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。
    /// </summary>
    public class MultiWheelRotateInPlace : MovementDefinition
    {
        /// <summary>
        /// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。
        /// </summary>
        public float AngleTarget;

        /// <summary>
        /// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
        /// </summary>
        public InPlaceRotationFeedbackMode FeedbackMode =
            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;

        // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
        public Func<float> ThetaReader;

        public IVehicleStateProvider StateProvider;

        public MultiWheelChassis Chassis =
            PilotDefinition.Chassis as MultiWheelChassis;

        /// <summary>
        /// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
        /// </summary>
        public Func<PIDParams> PidparamsRead;

        public PIDController thPid;

        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
        public Action<float> CommandAngularSpeedObserver;

        // 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
        public float? WheelAlignmentToleranceDegrees;

        // 自转舵轮连续保持到位的时间，单位s。
        public float WheelAlignmentStableSeconds = 0.3f;

        // 自转舵轮准备超时时间，单位s。
        public float WheelAlignmentTimeoutSeconds = 10f;

        // 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
        public float? MinimumAngularSpeedDegreesPerSecond;

        // 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
        public float? RotationTimeoutSeconds;

        /// <summary>
        /// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
        /// </summary>
        public override IEnumerable<bool> Get()
        {
            if (Chassis == null)
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");

            var config = PilotDefinition.Conf;
            var pidParameters =
                PidparamsRead == null
                    ? new PIDParams
                    {
                        Kp = config.InPlaceRotateKp,
                        Ki = config.InPlaceRotateKi,
                        Kd = config.InPlaceRotateKd,
                        MaxI = config.InPlaceRotateMaxI,
                        DeadZone = config.InPlaceRotateArriveDeg,
                        SpeedAccPerSec = config.InPlaceRotateAcc,
                        OutputUpperThreshold =
                            config.InPlaceRotateMaxSpeed
                    }
                    : PidparamsRead();
            var wheelAlignmentToleranceDegrees =
                WheelAlignmentToleranceDegrees ??
                config.InPlaceRotateWheelAlignDeg;
            var minimumAngularSpeedDegreesPerSecond =
                MinimumAngularSpeedDegreesPerSecond ??
                config.InPlaceRotateMinimumSpeed;
            var rotationTimeoutSeconds =
                RotationTimeoutSeconds ??
                config.InPlaceRotateTimeoutSec;
            var stateProvider =
                StateProvider ??
                ParkingVehicleStateProviderFactory.Create(
                    Chassis,
                    config);

            ValidateParameters(
                pidParameters,
                wheelAlignmentToleranceDegrees,
                minimumAngularSpeedDegreesPerSecond,
                rotationTimeoutSeconds);

            var adapter = new MultiWheelChassisAdapter(
                Chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();

            try
            {
                var alignmentStarted = DateTime.Now;
                DateTime? alignedSince = null;
                while (true)
                {
                    if (!adapter.PrepareSpin(
                            alignmentToleranceDegrees:
                                wheelAlignmentToleranceDegrees))
                        throw new InvalidOperationException(
                            "无法生成原地自转舵轮目标：" +
                            adapter.LastFailureReason);

                    if (adapter.AreSpinWheelsAligned)
                    {
                        if (alignedSince == null)
                            alignedSince = DateTime.Now;

                        if ((DateTime.Now - alignedSince.Value)
                                .TotalSeconds >=
                            WheelAlignmentStableSeconds)
                            break;
                    }
                    else
                    {
                        alignedSince = null;
                    }

                    if ((DateTime.Now - alignmentStarted)
                            .TotalSeconds >
                        WheelAlignmentTimeoutSeconds)
                        throw new TimeoutException(
                            "原地自转舵轮在限定时间内未稳定到位。");

                    yield return true;
                }

                var alignmentToleranceRadians =
                    AngleMath.DegreesToRadians(
                        wheelAlignmentToleranceDegrees);
                if (!adapter.AdoptPreparedSpinForXYTh(
                        alignmentToleranceRadians))
                {
                    throw new InvalidOperationException(
                        "无法将已到位的自转舵角交接给XYTh：" +
                        adapter.LastFailureReason);
                }

                var useRelativeWheelOdometry =
                    FeedbackMode ==
                    InPlaceRotationFeedbackMode
                        .RelativeWheelOdometry;
                var wheelStateProvider =
                    useRelativeWheelOdometry
                        ? stateProvider as
                            WheelFeedbackVehicleStateProvider
                        : null;
                if (useRelativeWheelOdometry &&
                    wheelStateProvider == null)
                {
                    throw new InvalidOperationException(
                        "轮组相对角度自转需要" +
                        "WheelFeedbackVehicleStateProvider。");
                }

                var targetAngle =
                    useRelativeWheelOdometry
                        ? AngleTarget
                        : (float)AngleMath.NormalizeDegrees(
                            AngleTarget);
                var currentAngle =
                    useRelativeWheelOdometry
                        ? 0f
                        : ReadCurrentAngleDegrees(stateProvider);
                var cachedCurrentAngle = currentAngle;
                thPid = new PIDController(
                    () => cachedCurrentAngle,
                    pidParameters.Kp);
                thPid.ChangeParameters(
                    pidParameters.Kp,
                    pidParameters.Ki,
                    pidParameters.Kd,
                    pidParameters.MaxI,
                    pidParameters.DeadZone,
                    pidParameters.OutputUpperThreshold,
                    pidParameters.SpeedAccPerSec);
                var lastCommandTime = DateTime.Now;
                var rotationStarted = DateTime.Now;
                var accumulatedWheelAngleRadians = 0.0;
                var previousWheelOmegaRadiansPerSecond = 0.0;
                var previousWheelTimestampSeconds = 0.0;
                var hasPreviousWheelSample = false;

                while (true)
                {
                    if ((DateTime.Now - rotationStarted)
                            .TotalSeconds >
                        rotationTimeoutSeconds)
                    {
                        throw new TimeoutException(
                            $"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
                    }

                    if (useRelativeWheelOdometry)
                    {
                        if (!wheelStateProvider.TryGetWheelTwist(
                                out var wheelTwist,
                                out var wheelTimestampSeconds))
                        {
                            CommandAngularSpeedObserver?.Invoke(0f);
                            adapter
                                .StopXYThDrivePreserveSteeringState();

                            if (hasPreviousWheelSample)
                            {
                                throw new InvalidOperationException(
                                    "原地自转期间轮组角速度不可用：" +
                                    wheelStateProvider.LastFailureReason);
                            }

                            yield return true;
                            continue;
                        }

                        if (hasPreviousWheelSample)
                        {
                            var wheelDeltaTimeSeconds =
                                wheelTimestampSeconds -
                                previousWheelTimestampSeconds;
                            if (!NumericGuard.IsFinite(
                                    wheelDeltaTimeSeconds) ||
                                wheelDeltaTimeSeconds <= 0.0)
                            {
                                throw new InvalidOperationException(
                                    "轮组角速度采样时间没有单调递增。");
                            }

                            accumulatedWheelAngleRadians +=
                                0.5 *
                                (previousWheelOmegaRadiansPerSecond +
                                 wheelTwist
                                     .OmegaRadiansPerSecond) *
                                wheelDeltaTimeSeconds;
                        }

                        previousWheelOmegaRadiansPerSecond =
                            wheelTwist.OmegaRadiansPerSecond;
                        previousWheelTimestampSeconds =
                            wheelTimestampSeconds;
                        hasPreviousWheelSample = true;
                        currentAngle =
                            (float)AngleMath.RadiansToDegrees(
                                accumulatedWheelAngleRadians);
                    }
                    else
                    {
                        currentAngle =
                            ReadCurrentAngleDegrees(stateProvider);
                    }

                    cachedCurrentAngle = currentAngle;
                    var s = thPid.GetResponse(
                        targetAngle,
                        !useRelativeWheelOdometry);
                    var angleErrorDegrees =
                        useRelativeWheelOdometry
                            ? targetAngle - currentAngle
                            : (float)AngleMath
                                .ShortestDifferenceDegrees(
                                    targetAngle,
                                    currentAngle);

                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
                    if (Math.Abs(angleErrorDegrees) <=
                        pidParameters.DeadZone)
                    {
                        CommandAngularSpeedObserver?.Invoke(0f);
                        adapter
                            .StopXYThDrivePreserveSteeringState();

                        // 到位稳定只依赖已经单独校验的航向。
                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
                        if (thPid.IsArrived())
                            break;

                        yield return true;
                        continue;
                    }

                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
                    // 避免接近目标时反复出现微小命令但车辆实际不动。
                    if (Math.Abs(s) > 1e-6f &&
                        Math.Abs(s) <
                            minimumAngularSpeedDegreesPerSecond)
                    {
                        s = Math.Sign(angleErrorDegrees) *
                            minimumAngularSpeedDegreesPerSecond;
                    }

                    // PID加速限制在首周期可能暂时输出零；此时保留
                    // 已交接的自转状态，等待下一周期产生有效角速度。
                    if (Math.Abs(s) <= 1e-6f)
                    {
                        CommandAngularSpeedObserver?.Invoke(0f);
                        adapter
                            .StopXYThDrivePreserveSteeringState();
                        yield return true;
                        continue;
                    }

                    CommandAngularSpeedObserver?.Invoke(s);
                    var now = DateTime.Now;
                    var interval = now - lastCommandTime;
                    lastCommandTime = now;

                    // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
                    var omegaRadiansPerSecond =
                        (float)AngleMath.DegreesToRadians(s);
                    if (!adapter.SendBodyTwist(
                            new Twist2D(
                                0.0,
                                0.0,
                                omegaRadiansPerSecond),
                            interval))
                    {
                        throw new InvalidOperationException(
                            "安全XYTh原地旋转底盘解算失败：" +
                            adapter.LastFailureReason);
                    }
                    yield return true;
                }

                CommandAngularSpeedObserver?.Invoke(0f);
                adapter.StopXYThDrivePreserveSteeringState();

                if (!useRelativeWheelOdometry &&
                    IsLocalizationRecoveryPending(
                        stateProvider))
                {
                    BeginPostRotationPositionRecovery(
                        stateProvider);
                    var recoveryStarted = DateTime.Now;

                    while (true)
                    {
                        adapter
                            .StopXYThDrivePreserveSteeringState();

                        if (stateProvider.TryGetState(out _) &&
                            !IsLocalizationRecoveryPending(
                                stateProvider))
                        {
                            break;
                        }

                        if ((DateTime.Now - recoveryStarted)
                                .TotalSeconds >
                            config
                                .ParkingDetourJumpConfirmationTimeoutSeconds)
                        {
                            throw new InvalidOperationException(
                                "原地自转完成后Detour位置在限定时间内未恢复。" +
                                GetStateProviderFailureReason(
                                    stateProvider));
                        }

                        yield return true;
                    }
                }

                // 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
                var wheelPreparation =
                    new PrepareWheelsForward();
                foreach (var keepRunning in wheelPreparation.Get())
                {
                    if (!keepRunning)
                    {
                        break;
                    }

                    yield return true;
                }

                if (!wheelPreparation.Completed)
                {
                    throw new InvalidOperationException(
                        "原地自转完成后舵轮未能稳定回到车头方向。");
                }

                Console.WriteLine(
                    useRelativeWheelOdometry
                        ? "final relative wheel rotate to " +
                          $"{currentAngle:F2}deg, wheels forward"
                        : $"final rotate to {targetAngle}, wheels forward");
            }
            finally
            {
                CommandAngularSpeedObserver?.Invoke(0f);
                adapter.StopImmediately();
            }
        }

        /// <summary>
        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
        /// </summary>
        private void ValidateParameters(
            PIDParams pidParameters,
            float wheelAlignmentToleranceDegrees,
            float minimumAngularSpeedDegreesPerSecond,
            float rotationTimeoutSeconds)
        {
            EnsureFinitePositive(
                wheelAlignmentToleranceDegrees,
                nameof(WheelAlignmentToleranceDegrees),
                allowZero: true);
            EnsureFinitePositive(
                WheelAlignmentStableSeconds,
                nameof(WheelAlignmentStableSeconds),
                allowZero: true);
            EnsureFinitePositive(
                WheelAlignmentTimeoutSeconds,
                nameof(WheelAlignmentTimeoutSeconds));
            EnsureFinitePositive(
                minimumAngularSpeedDegreesPerSecond,
                nameof(MinimumAngularSpeedDegreesPerSecond));
            EnsureFinitePositive(
                rotationTimeoutSeconds,
                nameof(RotationTimeoutSeconds));

            if (float.IsNaN(AngleTarget) ||
                float.IsInfinity(AngleTarget))
            {
                throw new ArgumentOutOfRangeException(
                    nameof(AngleTarget),
                    "原地自转目标角度必须是有限值。");
            }

            if (!Enum.IsDefined(
                    typeof(InPlaceRotationFeedbackMode),
                    FeedbackMode))
            {
                throw new ArgumentOutOfRangeException(
                    nameof(FeedbackMode),
                    "原地自转反馈模式无效。");
            }

            if (FeedbackMode ==
                    InPlaceRotationFeedbackMode
                        .RelativeWheelOdometry &&
                Math.Abs(AngleTarget) >= 180f)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(AngleTarget),
                    "轮组相对自转角度必须满足-180° < angle < 180°。");
            }

            if (pidParameters == null)
            {
                throw new InvalidOperationException(
                    "原地自转PID参数读取结果为空。");
            }

            EnsureFinitePositive(
                pidParameters.DeadZone,
                "PidparamsRead.DeadZone");
            EnsureFinitePositive(
                pidParameters.OutputUpperThreshold,
                "PidparamsRead.OutputUpperThreshold");
            EnsureFinitePositive(
                pidParameters.SpeedAccPerSec,
                "PidparamsRead.SpeedAccPerSec");
            EnsureFinitePositive(
                pidParameters.Kp,
                "PidparamsRead.Kp");

            if (minimumAngularSpeedDegreesPerSecond >
                pidParameters.OutputUpperThreshold)
            {
                throw new InvalidOperationException(
                    "原地自转最小有效角速度不能大于最大角速度。");
            }
        }

        /// <summary>
        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
        /// </summary>
        private float ReadCurrentAngleDegrees(
            IVehicleStateProvider stateProvider)
        {
            if (ThetaReader != null)
            {
                var angleDegrees = ThetaReader();
                if (float.IsNaN(angleDegrees) ||
                    float.IsInfinity(angleDegrees))
                {
                    throw new InvalidOperationException(
                        "自定义航向读取结果不是有效角度。");
                }

                return (float)AngleMath.NormalizeDegrees(
                    angleDegrees);
            }

            if (stateProvider is
                WheelFeedbackVehicleStateProvider wheelProvider)
            {
                if (!wheelProvider.TryGetHeadingRadians(
                        out var wheelHeadingRadians))
                {
                    throw new InvalidOperationException(
                        "无法从Detour状态源读取有效车辆航向。" +
                        wheelProvider.LastHeadingFailureReason);
                }

                return (float)AngleMath.RadiansToDegrees(
                    wheelHeadingRadians);
            }

            if (stateProvider is
                DetourVehicleStateProvider detourProvider)
            {
                if (!detourProvider.TryGetHeadingRadians(
                        out var detourHeadingRadians))
                {
                    throw new InvalidOperationException(
                        "无法从Detour状态源读取有效车辆航向。" +
                        detourProvider.LastHeadingFailureReason);
                }

                return (float)AngleMath.RadiansToDegrees(
                    detourHeadingRadians);
            }

            if (stateProvider == null ||
                !stateProvider.TryGetState(out var state))
            {
                throw new InvalidOperationException(
                    "无法从Detour状态源读取有效车辆航向。" +
                    GetStateProviderFailureReason(
                        stateProvider));
            }

            return (float)AngleMath.RadiansToDegrees(
                state.PoseInWorld.YawRadians);
        }

        /// <summary>
        /// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
        /// </summary>
        private static void BeginPostRotationPositionRecovery(
            IVehicleStateProvider stateProvider)
        {
            if (stateProvider is
                WheelFeedbackVehicleStateProvider wheelProvider)
            {
                wheelProvider.BeginPostRotationPositionRecovery();
                return;
            }

            if (stateProvider is
                DetourVehicleStateProvider detourProvider)
            {
                detourProvider.BeginPostRotationPositionRecovery();
            }
        }

        /// <summary>
        /// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
        /// </summary>
        private static string GetStateProviderFailureReason(
            IVehicleStateProvider stateProvider)
        {
            if (stateProvider is
                WheelFeedbackVehicleStateProvider wheelProvider)
            {
                return wheelProvider.LastFailureReason;
            }

            if (stateProvider is
                DetourVehicleStateProvider detourProvider)
            {
                return detourProvider.LastFailureReason;
            }

            return string.Empty;
        }

        /// <summary>
        /// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
        /// </summary>
        private static bool IsLocalizationRecoveryPending(
            IVehicleStateProvider stateProvider)
        {
            if (stateProvider is
                    WheelFeedbackVehicleStateProvider wheelProvider)
            {
                return wheelProvider.TryGetLatestDetourDiagnostics(
                           out var jumpCandidateActive,
                           out _,
                           out _,
                           out _,
                           out _,
                           out _,
                           out _) &&
                       jumpCandidateActive;
            }

            return stateProvider is
                       DetourVehicleStateProvider detourProvider &&
                   detourProvider.IsJumpCandidateActive;
        }

        /// <summary>
        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
        /// </summary>
        private static void EnsureFinitePositive(
            float value,
            string parameterName,
            bool allowZero = false)
        {
            if (float.IsNaN(value) ||
                float.IsInfinity(value) ||
                (allowZero
                    ? value < 0f
                    : value <= 0f))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "原地自转参数必须是有效的正数。");
            }
        }
    }
}