using System;
using System.Drawing;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
    /// </summary>
    internal static class TrajectoryExperimentInput
    {
        private const double MaximumOffsetCentimeters = 30.0;

        /// <summary>
        /// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
        /// </summary>
        public static bool TryReadLateralOffsetMeters(
            out double lateralOffsetMeters)
        {
            lateralOffsetMeters = 0.0;
            var input = UI.GetInput(
                "输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
            var parsed = double.TryParse(
                input,
                NumberStyles.Float,
                CultureInfo.CurrentCulture,
                out var offsetCentimeters) ||
                double.TryParse(
                    input,
                    NumberStyles.Float,
                    CultureInfo.InvariantCulture,
                    out offsetCentimeters);

            if (!parsed ||
                double.IsNaN(offsetCentimeters) ||
                double.IsInfinity(offsetCentimeters) ||
                Math.Abs(offsetCentimeters) >
                    MaximumOffsetCentimeters)
            {
                Console.WriteLine(
                    "轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
                return false;
            }

            lateralOffsetMeters =
                offsetCentimeters / 100.0;
            return true;
        }

        /// <summary>
        /// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
        /// </summary>
        public static Pose2D OffsetPoseLaterally(
            Pose2D poseInWorld,
            double lateralOffsetMeters)
        {
            var yawRadians = poseInWorld.YawRadians;
            return new Pose2D(
                poseInWorld.XMeters -
                    Math.Sin(yawRadians) *
                    lateralOffsetMeters,
                poseInWorld.YMeters +
                    Math.Cos(yawRadians) *
                    lateralOffsetMeters,
                yawRadians);
        }

        /// <summary>
        /// 生成带毫米偏移标识的实验轨迹名称。
        /// </summary>
        public static string BuildTrajectoryName(
            string baseName,
            double lateralOffsetMeters)
        {
            return baseName +
                   "_Offset" +
                   (lateralOffsetMeters * 1000.0)
                   .ToString("+0;-0;0", CultureInfo.InvariantCulture) +
                   "mm";
        }
    }

    /// <summary>
    /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
    /// </summary>
    [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
    public sealed class NewControllerStraight4mTest
        : MovementTest
    {
        private const float MillimetersPerMeter = 1000f;

        private readonly Painter _painter =
            UI.GetPainter("NewControllerStraight4m");

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        private IVehicleStateProvider _stateProvider;

        /// <summary>
        /// 获取或设置本次测试编号，用于区分重复实验CSV。
        /// </summary>
        public int TrialNumber = 1;

        /// <summary>
        /// 获取或设置4m直线的巡航参考速度，单位为m/s。
        /// </summary>
        public double CruiseSpeedMetersPerSecond = 0.40;

        /// <summary>
        /// 获取或设置参考速度加速度，单位为m/s²。
        /// </summary>
        public double AccelerationMetersPerSecondSquared = 0.20;

        /// <summary>
        /// 获取或设置参考速度减速度，单位为m/s²。
        /// </summary>
        public double DecelerationMetersPerSecondSquared = 0.08;

        /// <summary>
        /// 获取或设置离散轨迹点间距，单位为m。
        /// </summary>
        public double PointSpacingMeters = 0.02;

        /// <summary>
        /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
        /// </summary>
        public override void Test()
        {
            if (_task != null)
            {
                Console.WriteLine(
                    "新版4m直线轨迹测试已经在运行，请先停止当前测试。");
                return;
            }

            if (!TrajectoryExperimentInput
                    .TryReadLateralOffsetMeters(
                        out var lateralOffsetMeters))
            {
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
                return;
            }

            var detourStateProvider =
                new DetourVehicleStateProvider();
            if (!detourStateProvider.TryGetState(
                    out var initialState))
            {
                Console.WriteLine(
                    "无法读取有效Detour起点位姿：" +
                    detourStateProvider.LastFailureReason);
                _stateProvider = null;
                return;
            }

            _stateProvider =
                new WheelFeedbackVehicleStateProvider(
                    detourStateProvider,
                    chassis);

            var trajectoryStartPose =
                TrajectoryExperimentInput.OffsetPoseLaterally(
                    initialState.PoseInWorld,
                    lateralOffsetMeters);
            var trajectory =
                TestTrajectoryFactory.CreateStraight4Meters(
                    trajectoryStartPose,
                    CruiseSpeedMetersPerSecond,
                    AccelerationMetersPerSecondSquared,
                    DecelerationMetersPerSecondSquared,
                    PointSpacingMeters);

            DrawTrajectory(trajectory);

            var referenceStart = ToMillimeterVector(
                trajectory.StartPoint.PoseInWorld);
            var referenceEnd = ToMillimeterVector(
                trajectory.EndPoint.PoseInWorld);
            var recorder =
                new TrackingExperimentRecorder(
                    controllerName: "NewStanleyPid",
                    trajectoryName:
                        TrajectoryExperimentInput.BuildTrajectoryName(
                            "ProfiledStraight4m",
                            lateralOffsetMeters),
                    trialNumber: TrialNumber,
                    referenceStart: referenceStart,
                    referenceEnd: referenceEnd,
                    referenceSpeed:
                        (float)CruiseSpeedMetersPerSecond,
                    sampleIntervalMs: 50,
                    referenceAccelerationMetersPerSecondSquared:
                        (float)AccelerationMetersPerSecondSquared,
                    referenceDecelerationMetersPerSecondSquared:
                        (float)DecelerationMetersPerSecondSquared,
                    diagnosticChassis: chassis);
            _recorder = recorder;

            var controlPointRadiusMeters =
                chassis.ControlPointRadius /
                MillimetersPerMeter;
            var movement =
                new TrajectoryTrackingMovement
                {
                    Trajectory = trajectory,
                    StateProvider = _stateProvider,
                    StanleyUsesActualSpeed = true,
                    MaximumCommandSpeedMetersPerSecond = 0.50,
                    CycleObserver = controller =>
                        RecordControlCycle(
                            recorder,
                            controller,
                            controlPointRadiusMeters,
                            _stateProvider as
                                WheelFeedbackVehicleStateProvider)
                };

            recorder.Start();

            try
            {
                _task = new DriveTask(movement.Get());
                _task.Wait();

                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
                Thread.Sleep(400);
                recorder.UpdateCommand(0f, 0f);
                if (_stateProvider.TryGetState(
                        out var stoppedState))
                {
                    recorder.UpdateProcessedState(
                        stoppedState);
                }
            }
            finally
            {
                _task?.Stop();
                recorder.UpdateCommand(0f, 0f);
                recorder.StopAndSave();
                _painter.Clear();
                _task = null;
                _recorder = null;
                _stateProvider = null;
            }
        }

        /// <summary>
        /// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。
        /// </summary>
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
            _painter.Clear();
            _task = null;
            _recorder = null;
            _stateProvider = null;
        }

        /// <summary>
        /// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。
        /// </summary>
        private void DrawTrajectory(
            Trajectory.Trajectory2D trajectory)
        {
            _painter.Clear();

            for (var index = 0;
                 index < trajectory.Count;
                 index++)
            {
                var point = ToMillimeterVector(
                    trajectory[index].PoseInWorld);
                _painter.DrawDot(
                    Color.Cyan,
                    point.X,
                    point.Y,
                    3f);

                if (index == 0)
                {
                    continue;
                }

                var previousPoint = ToMillimeterVector(
                    trajectory[index - 1].PoseInWorld);
                _painter.DrawLine(
                    Color.DeepSkyBlue,
                    previousPoint.X,
                    previousPoint.Y,
                    point.X,
                    point.Y,
                    width: 2);
            }

            var start = ToMillimeterVector(
                trajectory.StartPoint.PoseInWorld);
            var end = ToMillimeterVector(
                trajectory.EndPoint.PoseInWorld);
            _painter.DrawDot(
                Color.LimeGreen,
                start.X,
                start.Y,
                8f);
            _painter.DrawDot(
                Color.OrangeRed,
                end.X,
                end.Y,
                8f);
        }

        /// <summary>
        /// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。
        /// </summary>
        private static void RecordControlCycle(
            TrackingExperimentRecorder recorder,
            ParkingGeometricController controller,
            double controlPointRadiusMeters,
            WheelFeedbackVehicleStateProvider stateProvider)
        {
            if (controller.LastVehicleState.HasValue)
            {
                recorder.UpdateProcessedState(
                    controller.LastVehicleState.Value);
            }

            UpdateVelocityDiagnostics(
                recorder,
                stateProvider);

            if (!controller.LastCommand.HasValue)
            {
                return;
            }

            if (controller.LastProjection.HasValue &&
                controller.LastReferenceSpeedMetersPerSecond.HasValue)
            {
                var projection =
                    controller.LastProjection.Value;
                recorder.UpdateControlReference(
                    projection.ArcLengthMeters,
                    controller.LastReferenceSpeedMetersPerSecond.Value,
                    projection.LateralErrorMeters,
                    projection.HeadingErrorRadians,
                    projection.DistanceToTrajectoryMeters,
                    projection.RemainingDistanceMeters);
            }

            var command = controller.LastCommand.Value;
            recorder.UpdateGcpCommand(
                command.FrontAngleRadians,
                command.RearAngleRadians);
            var curvaturePerMeter = Math.Tan(
                command.FrontAngleRadians) /
                controlPointRadiusMeters;
            var angularSpeedRadiansPerSecond =
                command.SpeedMetersPerSecond *
                curvaturePerMeter;

            recorder.UpdateCommand(
                (float)command.SpeedMetersPerSecond,
                (float)angularSpeedRadiansPerSecond);
        }

        /// <summary>
        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
        /// </summary>
        private static void UpdateVelocityDiagnostics(
            TrackingExperimentRecorder recorder,
            WheelFeedbackVehicleStateProvider stateProvider)
        {
            if (stateProvider == null ||
                !stateProvider.TryGetLatestVelocityDiagnostics(
                    out var detourBodyVx,
                    out var detourVelocityValid,
                    out var rawWheelBodyVx,
                    out var filteredWheelBodyVx,
                    out var wheelVelocityValid))
            {
                return;
            }

            recorder.UpdateVelocityDiagnostics(
                detourBodyVx,
                detourVelocityValid,
                rawWheelBodyVx,
                filteredWheelBodyVx,
                wheelVelocityValid);
        }

        /// <summary>
        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
        /// </summary>
        private static Vector2 ToMillimeterVector(
            Pose2D poseInWorld)
        {
            return new Vector2(
                (float)(
                    poseInWorld.XMeters *
                    MillimetersPerMeter),
                (float)(
                    poseInWorld.YMeters *
                    MillimetersPerMeter));
        }

    }

    /// <summary>
    /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
    /// </summary>
    [MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
    public sealed class NewControllerStraightSemicircleStraightTest
        : MovementTest
    {
        private const float MillimetersPerMeter = 1000f;

        private readonly Painter _painter =
            UI.GetPainter(
                "NewControllerStraightSemicircleStraight");

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        private IVehicleStateProvider _stateProvider;

        /// <summary>
        /// 获取或设置本次测试编号，用于区分重复实验CSV。
        /// </summary>
        public int TrialNumber = 1;

        /// <summary>
        /// 获取或设置半圆前后两段直线的长度，单位为m。
        /// </summary>
        public double StraightLengthMeters = 3.0;

        /// <summary>
        /// 获取或设置左转半圆的转弯半径，单位为m。
        /// </summary>
        public double TurnRadiusMeters = 2.0;

        /// <summary>
        /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
        /// </summary>
        public double CurvatureTransitionLengthMeters = 0.70;

        /// <summary>
        /// 获取或设置两段直线的最大参考速度，单位为m/s。
        /// </summary>
        public double StraightMaximumSpeedMetersPerSecond = 0.40;

        /// <summary>
        /// 获取或设置半圆段的最大参考速度，单位为m/s。
        /// </summary>
        public double SemicircleMaximumSpeedMetersPerSecond = 0.30;

        /// <summary>
        /// 获取或设置参考速度加速度，单位为m/s²。
        /// </summary>
        public double AccelerationMetersPerSecondSquared = 0.20;

        /// <summary>
        /// 获取或设置参考速度减速度，单位为m/s²。
        /// </summary>
        public double DecelerationMetersPerSecondSquared = 0.08;

        /// <summary>
        /// 获取或设置离散轨迹点间距，单位为m。
        /// </summary>
        public double PointSpacingMeters = 0.02;

        /// <summary>
        /// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
        /// </summary>
        public override void Test()
        {
            if (_task != null)
            {
                Console.WriteLine(
                    "新版直线-左半圆-直线测试已经在运行，请先停止当前测试。");
                return;
            }

            if (!TrajectoryExperimentInput
                    .TryReadLateralOffsetMeters(
                        out var lateralOffsetMeters))
            {
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
                return;
            }

            var detourStateProvider =
                new DetourVehicleStateProvider();
            if (!detourStateProvider.TryGetState(
                    out var initialState))
            {
                Console.WriteLine(
                    "无法读取有效Detour起点位姿：" +
                    detourStateProvider.LastFailureReason);
                _stateProvider = null;
                return;
            }

            _stateProvider =
                new WheelFeedbackVehicleStateProvider(
                    detourStateProvider,
                    chassis);

            var trajectoryStartPose =
                TrajectoryExperimentInput.OffsetPoseLaterally(
                    initialState.PoseInWorld,
                    lateralOffsetMeters);
            var trajectory =
                TestTrajectoryFactory
                    .CreateStraightLeftSemicircleStraight(
                        trajectoryStartPose,
                        StraightLengthMeters,
                        TurnRadiusMeters,
                        CurvatureTransitionLengthMeters,
                        StraightMaximumSpeedMetersPerSecond,
                        SemicircleMaximumSpeedMetersPerSecond,
                        AccelerationMetersPerSecondSquared,
                        DecelerationMetersPerSecondSquared,
                        PointSpacingMeters);

            DrawTrajectory(trajectory);

            var referenceStart = ToMillimeterVector(
                trajectory.StartPoint.PoseInWorld);
            var referenceEnd = ToMillimeterVector(
                trajectory.EndPoint.PoseInWorld);
            var recorder =
                new TrackingExperimentRecorder(
                    controllerName: "NewStanleyPid",
                    trajectoryName:
                        TrajectoryExperimentInput.BuildTrajectoryName(
                            "ProfiledStraightSmoothLeftTurnStraight",
                            lateralOffsetMeters),
                    trialNumber: TrialNumber,
                    referenceStart: referenceStart,
                    referenceEnd: referenceEnd,
                    referenceSpeed:
                        (float)StraightMaximumSpeedMetersPerSecond,
                    sampleIntervalMs: 50,
                    referenceAccelerationMetersPerSecondSquared:
                        (float)AccelerationMetersPerSecondSquared,
                    referenceDecelerationMetersPerSecondSquared:
                        (float)DecelerationMetersPerSecondSquared,
                    diagnosticChassis: chassis);
            _recorder = recorder;

            var controlPointRadiusMeters =
                chassis.ControlPointRadius /
                MillimetersPerMeter;
            var movement =
                new TrajectoryTrackingMovement
                {
                    Trajectory = trajectory,
                    StateProvider = _stateProvider,
                    StanleyUsesActualSpeed = true,
                    MaximumCommandSpeedMetersPerSecond =
                        StraightMaximumSpeedMetersPerSecond,
                    CycleObserver = controller =>
                        RecordControlCycle(
                            recorder,
                            controller,
                            controlPointRadiusMeters,
                            _stateProvider as
                                WheelFeedbackVehicleStateProvider)
                };

            recorder.Start();

            try
            {
                _task = new DriveTask(movement.Get());
                _task.Wait();

                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
                Thread.Sleep(400);
                recorder.UpdateCommand(0f, 0f);
                if (_stateProvider.TryGetState(
                        out var stoppedState))
                {
                    recorder.UpdateProcessedState(
                        stoppedState);
                }
            }
            finally
            {
                _task?.Stop();
                recorder.UpdateCommand(0f, 0f);
                recorder.StopAndSave();
                _painter.Clear();
                _task = null;
                _recorder = null;
                _stateProvider = null;
            }
        }

        /// <summary>
        /// 停止组合轨迹测试、保存已有数据并清除Clumsy轨迹可视化。
        /// </summary>
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
            _painter.Clear();
            _task = null;
            _recorder = null;
            _stateProvider = null;
        }

        /// <summary>
        /// 将组合轨迹的离散点和相邻线段转换为Clumsy毫米坐标后绘制。
        /// </summary>
        private void DrawTrajectory(
            Trajectory.Trajectory2D trajectory)
        {
            _painter.Clear();

            for (var index = 0;
                 index < trajectory.Count;
                 index++)
            {
                var point = ToMillimeterVector(
                    trajectory[index].PoseInWorld);
                _painter.DrawDot(
                    Color.Cyan,
                    point.X,
                    point.Y,
                    3f);

                if (index == 0)
                {
                    continue;
                }

                var previousPoint = ToMillimeterVector(
                    trajectory[index - 1].PoseInWorld);
                _painter.DrawLine(
                    Color.DeepSkyBlue,
                    previousPoint.X,
                    previousPoint.Y,
                    point.X,
                    point.Y,
                    width: 2);
            }

            var start = ToMillimeterVector(
                trajectory.StartPoint.PoseInWorld);
            var end = ToMillimeterVector(
                trajectory.EndPoint.PoseInWorld);
            _painter.DrawDot(
                Color.LimeGreen,
                start.X,
                start.Y,
                8f);
            _painter.DrawDot(
                Color.OrangeRed,
                end.X,
                end.Y,
                8f);
        }

        /// <summary>
        /// 将组合轨迹控制周期的状态、参考量和最终GCP命令同步给实验记录器。
        /// </summary>
        private static void RecordControlCycle(
            TrackingExperimentRecorder recorder,
            ParkingGeometricController controller,
            double controlPointRadiusMeters,
            WheelFeedbackVehicleStateProvider stateProvider)
        {
            if (controller.LastVehicleState.HasValue)
            {
                recorder.UpdateProcessedState(
                    controller.LastVehicleState.Value);
            }

            UpdateVelocityDiagnostics(
                recorder,
                stateProvider);

            if (!controller.LastCommand.HasValue)
            {
                return;
            }

            if (controller.LastProjection.HasValue &&
                controller.LastReferenceSpeedMetersPerSecond.HasValue)
            {
                var projection =
                    controller.LastProjection.Value;
                recorder.UpdateControlReference(
                    projection.ArcLengthMeters,
                    controller.LastReferenceSpeedMetersPerSecond.Value,
                    projection.LateralErrorMeters,
                    projection.HeadingErrorRadians,
                    projection.DistanceToTrajectoryMeters,
                    projection.RemainingDistanceMeters);
            }

            var command = controller.LastCommand.Value;
            recorder.UpdateGcpCommand(
                command.FrontAngleRadians,
                command.RearAngleRadians);
            var curvaturePerMeter = Math.Tan(
                command.FrontAngleRadians) /
                controlPointRadiusMeters;
            var angularSpeedRadiansPerSecond =
                command.SpeedMetersPerSecond *
                curvaturePerMeter;

            recorder.UpdateCommand(
                (float)command.SpeedMetersPerSecond,
                (float)angularSpeedRadiansPerSecond);
        }

        /// <summary>
        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
        /// </summary>
        private static void UpdateVelocityDiagnostics(
            TrackingExperimentRecorder recorder,
            WheelFeedbackVehicleStateProvider stateProvider)
        {
            if (stateProvider == null ||
                !stateProvider.TryGetLatestVelocityDiagnostics(
                    out var detourBodyVx,
                    out var detourVelocityValid,
                    out var rawWheelBodyVx,
                    out var filteredWheelBodyVx,
                    out var wheelVelocityValid))
            {
                return;
            }

            recorder.UpdateVelocityDiagnostics(
                detourBodyVx,
                detourVelocityValid,
                rawWheelBodyVx,
                filteredWheelBodyVx,
                wheelVelocityValid);
        }

        /// <summary>
        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
        /// </summary>
        private static Vector2 ToMillimeterVector(
            Pose2D poseInWorld)
        {
            return new Vector2(
                (float)(
                    poseInWorld.XMeters *
                    MillimetersPerMeter),
                (float)(
                    poseInWorld.YMeters *
                    MillimetersPerMeter));
        }
    }
}