using System;
using System.Diagnostics;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MultiWheelC.StateEstimation;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 提供纵向开环辨识测试共用的舵轮准备、速度下发、采样和安全停车流程。
    /// </summary>
    public abstract class LongitudinalIdentificationTestBase
        : MovementTest
    {
        private const double MaximumTargetSpeedMetersPerSecond =
            0.70;
        private const double MinimumTargetSpeedMetersPerSecond =
            0.02;
        private const float MillimetersPerMeter = 1000f;

        private DriveTask _preparationTask;
        private MultiWheelChassis _chassis;
        private MultiWheelChassisAdapter _adapter;
        private WheelFeedbackVehicleStateProvider _stateProvider;
        private TrackingExperimentRecorder _recorder;
        private int _testRunning;
        private int _stopRequested;

        /// <summary>
        /// 获取或设置本次重复实验编号。
        /// </summary>
        public int TrialNumber = 1;

        /// <summary>
        /// 获取或设置速度阶跃前的静止记录时间，单位为s。
        /// </summary>
        public double BaselineSeconds = 1.0;

        /// <summary>
        /// 获取或设置目标速度保持时间，单位为s。
        /// </summary>
        public double CommandHoldSeconds = 5.0;

        /// <summary>
        /// 获取或设置停车后的继续记录时间，单位为s。
        /// </summary>
        public double PostStopSeconds = 2.0;

        /// <summary>
        /// 获取或设置速度命令循环周期，单位为ms。
        /// </summary>
        public int ControlIntervalMilliseconds = 50;

        /// <summary>
        /// 派生测试选择是否使用底盘DeAccPerSecond完成正常减速。
        /// </summary>
        protected abstract bool UseConfiguredDeceleration { get; }

        /// <summary>
        /// 获取用于CSV文件名区分停车方式的标识。
        /// </summary>
        protected abstract string StopModeName { get; }

        /// <summary>
        /// 读取目标速度，完成舵轮回正后执行纵向开环命令并保存CSV。
        /// </summary>
        public override void Test()
        {
            if (Interlocked.CompareExchange(
                    ref _testRunning,
                    1,
                    0) != 0)
            {
                Console.WriteLine(
                    "纵向辨识测试已经在运行，请先停止当前测试。");
                return;
            }

            Interlocked.Exchange(ref _stopRequested, 0);

            try
            {
                ValidateSettings();
                if (!TryReadTargetSpeed(
                        out var targetSpeedMetersPerSecond))
                {
                    return;
                }

                _chassis =
                    PilotDefinition.Chassis as MultiWheelChassis;
                if (_chassis == null)
                {
                    throw new InvalidOperationException(
                        "当前底盘不是MultiWheelChassis，无法执行纵向辨识。");
                }

                ValidateChassisAndTarget(
                    targetSpeedMetersPerSecond);

                Console.WriteLine(
                    "纵向辨识开始前请确认M层轮速诊断记录已经开启。");
                Hedingben.ToastText(
                    "请确认M层轮速诊断记录已开启。",
                    "LongitudinalIdentification");

                if (!MovementTestPreparation.AlignWheelsForward(
                        ref _preparationTask))
                {
                    throw new InvalidOperationException(
                        "四个舵轮未能稳定回正，纵向辨识已经取消。");
                }

                ThrowIfStopRequested();

                _adapter = new MultiWheelChassisAdapter(
                    _chassis,
                    PilotDefinition.Self.CarNum);
                _adapter.ResetToBodyFrame();
                _adapter.StopImmediately();

                _stateProvider =
                    ParkingVehicleStateProviderFactory.Create(
                        _chassis);
                if (!_stateProvider.TryGetState(
                        out var initialState))
                {
                    throw new InvalidOperationException(
                        "无法读取纵向辨识起点位姿：" +
                        _stateProvider.LastFailureReason);
                }

                _recorder = CreateRecorder(
                    targetSpeedMetersPerSecond,
                    initialState.PoseInWorld);
                _recorder.Start();

                Console.WriteLine(
                    $"纵向辨识参数：目标速度={targetSpeedMetersPerSecond:F3}m/s，" +
                    $"AccPerSecond={_chassis.AccPerSecond:F3}m/s²，" +
                    $"DeAccPerSecond={_chassis.DeAccPerSecond:F3}m/s²，" +
                    $"保持={CommandHoldSeconds:F1}s，停车方式={StopModeName}。");

                RunStationaryPhase(
                    BaselineSeconds,
                    "静止基线");
                RunTargetSpeedPhase(
                    targetSpeedMetersPerSecond);

                if (UseConfiguredDeceleration)
                {
                    RunConfiguredDecelerationPhase(
                        targetSpeedMetersPerSecond);
                }
                else
                {
                    _adapter.StopImmediately();
                    RunStationaryPhase(
                        PostStopSeconds,
                        "立即停车后记录");
                }

                Console.WriteLine(
                    "纵向辨识测试完成。请同时保存对应的M层轮速诊断CSV。");
                Hedingben.ToastText(
                    "纵向辨识完成，请停止并保存M层轮速诊断记录。",
                    "LongitudinalIdentification");
            }
            catch (OperationCanceledException)
            {
                Console.WriteLine("纵向辨识已由用户停止。");
            }
            catch (Exception exception)
            {
                Console.WriteLine(
                    "纵向辨识失败：" +
                    exception.Message);
                Hedingben.ToastText(
                    "纵向辨识失败：" +
                    exception.Message,
                    "LongitudinalIdentification");
            }
            finally
            {
                _adapter?.StopImmediately();
                _chassis?.PredefinedDriveStop();
                _recorder?.UpdateBodyCommand(0f, 0f, 0f);
                _recorder?.StopAndSave();

                _preparationTask?.Stop();
                _preparationTask = null;
                _recorder = null;
                _stateProvider = null;
                _adapter = null;
                _chassis = null;
                Interlocked.Exchange(ref _testRunning, 0);
            }
        }

        /// <summary>
        /// 请求停止当前辨识并立即清零底盘驱动速度。
        /// </summary>
        public override void TestStop()
        {
            Interlocked.Exchange(ref _stopRequested, 1);
            _preparationTask?.Stop();
            _adapter?.StopImmediately();
            _chassis?.PredefinedDriveStop();
        }

        /// <summary>
        /// 在固定周期内保持零命令并记录静止数据。
        /// </summary>
        private void RunStationaryPhase(
            double durationSeconds,
            string phaseName)
        {
            _recorder.UpdateBodyCommand(0f, 0f, 0f);
            RunPeriodicPhase(
                durationSeconds,
                phaseName,
                _ => UpdateWheelDiagnostics());
        }

        /// <summary>
        /// 通过标准车体速度适配器持续下发直线目标速度。
        /// </summary>
        private void RunTargetSpeedPhase(
            double targetSpeedMetersPerSecond)
        {
            _recorder.UpdateBodyCommand(
                (float)targetSpeedMetersPerSecond,
                0f,
                0f);

            RunPeriodicPhase(
                CommandHoldSeconds,
                "目标速度保持",
                interval =>
                {
                    var accepted = _adapter.SendBodyTwist(
                        new Twist2D(
                            targetSpeedMetersPerSecond,
                            0.0,
                            0.0),
                        interval);
                    if (!accepted)
                    {
                        throw new InvalidOperationException(
                            "底盘拒绝纵向速度命令：" +
                            _adapter.LastFailureReason);
                    }

                    UpdateWheelDiagnostics();
                });
        }

        /// <summary>
        /// 重复发送零速SendMotion，使底盘内部DeAccPerSecond斜坡真实参与减速。
        /// </summary>
        private void RunConfiguredDecelerationPhase(
            double targetSpeedMetersPerSecond)
        {
            _recorder.UpdateBodyCommand(0f, 0f, 0f);

            var rampDurationSeconds =
                Math.Abs(targetSpeedMetersPerSecond) /
                _chassis.DeAccPerSecond;
            var totalDurationSeconds =
                rampDurationSeconds +
                PostStopSeconds;

            RunPeriodicPhase(
                totalDurationSeconds,
                "配置化正常减速",
                interval =>
                {
                    // SendBodyTwist(Zero)会立即停车；辨识DeAccPerSecond时必须
                    // 直接保持SendMotion零目标，让底盘内部发送速度逐周期下降。
                    var accepted = _chassis.SendMotion(
                        0f,
                        0f,
                        0f,
                        interval);
                    if (!accepted)
                    {
                        throw new InvalidOperationException(
                            "底盘拒绝正常减速命令：" +
                            _chassis.LastMotionDecomposeFailureReason);
                    }

                    UpdateWheelDiagnostics();
                });
        }

        /// <summary>
        /// 按实际循环间隔运行一个阶段，并响应测试界面的停止请求。
        /// </summary>
        private void RunPeriodicPhase(
            double durationSeconds,
            string phaseName,
            Action<TimeSpan> cycleAction)
        {
            Console.WriteLine(
                $"纵向辨识阶段：{phaseName}，预计{durationSeconds:F2}s。");

            var periodSeconds =
                ControlIntervalMilliseconds / 1000.0;
            var phaseClock = Stopwatch.StartNew();
            var previousCycleSeconds = -periodSeconds;

            while (phaseClock.Elapsed.TotalSeconds <
                   durationSeconds)
            {
                ThrowIfStopRequested();

                var cycleStartSeconds =
                    phaseClock.Elapsed.TotalSeconds;
                var deltaTimeSeconds =
                    cycleStartSeconds -
                    previousCycleSeconds;
                previousCycleSeconds = cycleStartSeconds;

                cycleAction(
                    TimeSpan.FromSeconds(
                        deltaTimeSeconds));

                var elapsedMilliseconds =
                    (phaseClock.Elapsed.TotalSeconds -
                     cycleStartSeconds) * 1000.0;
                var remainingMilliseconds =
                    ControlIntervalMilliseconds -
                    elapsedMilliseconds;
                if (remainingMilliseconds > 1.0)
                {
                    Thread.Sleep(
                        (int)Math.Floor(
                            remainingMilliseconds));
                }
            }
        }

        /// <summary>
        /// 刷新轮组原始与滤波速度，供后台CSV记录器读取最新诊断值。
        /// </summary>
        private void UpdateWheelDiagnostics()
        {
            _stateProvider.TryGetWheelTwist(
                out _,
                out _);
        }

        /// <summary>
        /// 创建复用现有字段格式的纵向辨识CSV记录器。
        /// </summary>
        private TrackingExperimentRecorder CreateRecorder(
            double targetSpeedMetersPerSecond,
            Pose2D initialPoseInWorld)
        {
            var expectedTravelMeters =
                targetSpeedMetersPerSecond *
                CommandHoldSeconds;
            var referenceStart = new Vector2(
                (float)(
                    initialPoseInWorld.XMeters *
                    MillimetersPerMeter),
                (float)(
                    initialPoseInWorld.YMeters *
                    MillimetersPerMeter));
            var referenceEnd = new Vector2(
                referenceStart.X +
                (float)(
                    Math.Cos(initialPoseInWorld.YawRadians) *
                    expectedTravelMeters *
                    MillimetersPerMeter),
                referenceStart.Y +
                (float)(
                    Math.Sin(initialPoseInWorld.YawRadians) *
                    expectedTravelMeters *
                    MillimetersPerMeter));
            var speedMillimetersPerSecond =
                targetSpeedMetersPerSecond *
                MillimetersPerMeter;
            var trajectoryName =
                "LongitudinalStep_" +
                speedMillimetersPerSecond.ToString(
                    "+0;-0;0",
                    CultureInfo.InvariantCulture) +
                "mmps_" +
                StopModeName;

            return new TrackingExperimentRecorder(
                controllerName:
                    "OpenLoopLongitudinalIdentification",
                trajectoryName: trajectoryName,
                trialNumber: TrialNumber,
                referenceStart: referenceStart,
                referenceEnd: referenceEnd,
                referenceSpeed:
                    (float)targetSpeedMetersPerSecond,
                sampleIntervalMs:
                    ControlIntervalMilliseconds,
                referenceMotionFrameYawDegrees: 0f,
                referenceAccelerationMetersPerSecondSquared:
                    _chassis.AccPerSecond,
                referenceDecelerationMetersPerSecondSquared:
                    _chassis.DeAccPerSecond,
                diagnosticChassis: _chassis,
                diagnosticStateProvider: _stateProvider);
        }

        /// <summary>
        /// 从测试界面读取带方向的纵向目标速度，正值前进、负值倒车。
        /// </summary>
        private static bool TryReadTargetSpeed(
            out double targetSpeedMetersPerSecond)
        {
            targetSpeedMetersPerSecond = 0.0;
            var input = UI.GetInput(
                "输入纵向目标速度（m/s，正数前进、负数倒车，" +
                $"范围-{MaximumTargetSpeedMetersPerSecond:F1}～" +
                $"{MaximumTargetSpeedMetersPerSecond:F1}且不能为0）：");
            var parsed = double.TryParse(
                input,
                NumberStyles.Float,
                CultureInfo.CurrentCulture,
                out targetSpeedMetersPerSecond) ||
                double.TryParse(
                    input,
                    NumberStyles.Float,
                    CultureInfo.InvariantCulture,
                    out targetSpeedMetersPerSecond);

            if (!parsed ||
                double.IsNaN(targetSpeedMetersPerSecond) ||
                double.IsInfinity(targetSpeedMetersPerSecond) ||
                Math.Abs(targetSpeedMetersPerSecond) <
                    MinimumTargetSpeedMetersPerSecond ||
                Math.Abs(targetSpeedMetersPerSecond) >
                    MaximumTargetSpeedMetersPerSecond)
            {
                Console.WriteLine(
                    "目标速度必须是绝对值位于" +
                    $"{MinimumTargetSpeedMetersPerSecond:F2}～" +
                    $"{MaximumTargetSpeedMetersPerSecond:F2}m/s之间的有限数值。");
                return false;
            }

            return true;
        }

        /// <summary>
        /// 验证实验时间、循环周期和编号配置。
        /// </summary>
        private void ValidateSettings()
        {
            NumericGuard.EnsureFiniteNonNegative(
                BaselineSeconds,
                nameof(BaselineSeconds));
            NumericGuard.EnsureFinitePositive(
                CommandHoldSeconds,
                nameof(CommandHoldSeconds));
            NumericGuard.EnsureFiniteNonNegative(
                PostStopSeconds,
                nameof(PostStopSeconds));

            if (CommandHoldSeconds > 30.0 ||
                BaselineSeconds > 10.0 ||
                PostStopSeconds > 10.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(CommandHoldSeconds),
                    "辨识阶段时间超出测试允许范围。");
            }

            if (ControlIntervalMilliseconds < 20 ||
                ControlIntervalMilliseconds > 200)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(ControlIntervalMilliseconds),
                    "纵向辨识命令周期必须在20～200ms之间。");
            }

            if (TrialNumber <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(TrialNumber),
                    "测试编号必须大于零。");
            }
        }

        /// <summary>
        /// 验证底盘运行时加载的速度、加速度和减速度配置。
        /// </summary>
        private void ValidateChassisAndTarget(
            double targetSpeedMetersPerSecond)
        {
            NumericGuard.EnsureFinitePositive(
                _chassis.MaxSpeed,
                nameof(_chassis.MaxSpeed));
            NumericGuard.EnsureFinitePositive(
                _chassis.AccPerSecond,
                nameof(_chassis.AccPerSecond));
            NumericGuard.EnsureFinitePositive(
                _chassis.DeAccPerSecond,
                nameof(_chassis.DeAccPerSecond));

            if (Math.Abs(targetSpeedMetersPerSecond) >
                _chassis.MaxSpeed)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(targetSpeedMetersPerSecond),
                    "目标速度超过底盘当前MaxSpeed配置。");
            }
        }

        /// <summary>
        /// 在收到停止请求时中断当前阶段并转入finally安全停车。
        /// </summary>
        private void ThrowIfStopRequested()
        {
            if (Volatile.Read(ref _stopRequested) != 0)
            {
                throw new OperationCanceledException();
            }
        }
    }

    /// <summary>
    /// 测量速度阶跃、加速与稳态，并在保持结束后立即清零驱动速度。
    /// </summary>
    [MovementTest(name = "纵向辨识：速度阶跃与立即停车")]
    public sealed class LongitudinalStepImmediateStopTest
        : LongitudinalIdentificationTestBase
    {
        protected override bool UseConfiguredDeceleration =>
            false;

        protected override string StopModeName =>
            "ImmediateStop";
    }

    /// <summary>
    /// 测量速度阶跃、稳态以及由底盘DeAccPerSecond形成的正常减速过程。
    /// </summary>
    [MovementTest(name = "纵向辨识：速度阶跃与正常减速")]
    public sealed class LongitudinalStepConfiguredDecelerationTest
        : LongitudinalIdentificationTestBase
    {
        protected override bool UseConfiguredDeceleration =>
            true;

        protected override string StopModeName =>
            "ConfiguredDeceleration";
    }
}