using System;
using System.Drawing;
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>
    /// 从当前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 DetourVehicleStateProvider _stateProvider;

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

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

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

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

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

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

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

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

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

            var trajectory =
                TestTrajectoryFactory.CreateStraight4Meters(
                    initialState.PoseInWorld,
                    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: "ProfiledStraight4m",
                    trialNumber: TrialNumber,
                    referenceStart: referenceStart,
                    referenceEnd: referenceEnd,
                    referenceSpeed:
                        (float)CruiseSpeedMetersPerSecond,
                    sampleIntervalMs: 50,
                    referenceAccelerationMetersPerSecondSquared:
                        (float)AccelerationMetersPerSecondSquared,
                    referenceDecelerationMetersPerSecondSquared:
                        (float)DecelerationMetersPerSecondSquared);
            _recorder = recorder;

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

            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)
        {
            if (controller.LastVehicleState.HasValue)
            {
                recorder.UpdateProcessedState(
                    controller.LastVehicleState.Value);
            }

            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;
            var curvaturePerMeter = Math.Tan(
                command.FrontAngleRadians) /
                controlPointRadiusMeters;
            var angularSpeedRadiansPerSecond =
                command.SpeedMetersPerSecond *
                curvaturePerMeter;

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

        /// <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 DetourVehicleStateProvider _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.12;

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

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

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

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

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

            var trajectory =
                TestTrajectoryFactory
                    .CreateStraightLeftSemicircleStraight(
                        initialState.PoseInWorld,
                        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:
                        "ProfiledStraightSmoothLeftTurnStraight",
                    trialNumber: TrialNumber,
                    referenceStart: referenceStart,
                    referenceEnd: referenceEnd,
                    referenceSpeed:
                        (float)StraightMaximumSpeedMetersPerSecond,
                    sampleIntervalMs: 50,
                    referenceAccelerationMetersPerSecondSquared:
                        (float)AccelerationMetersPerSecondSquared,
                    referenceDecelerationMetersPerSecondSquared:
                        (float)DecelerationMetersPerSecondSquared);
            _recorder = recorder;

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

            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)
        {
            if (controller.LastVehicleState.HasValue)
            {
                recorder.UpdateProcessedState(
                    controller.LastVehicleState.Value);
            }

            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;
            var curvaturePerMeter = Math.Tan(
                command.FrontAngleRadians) /
                controlPointRadiusMeters;
            var angularSpeedRadiansPerSecond =
                command.SpeedMetersPerSecond *
                curvaturePerMeter;

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

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