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

namespace MultiWheelC
{
    /// <summary>
    /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
    /// </summary>
    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
    public sealed class CompositeStopTurnGoTest : MovementTest
    {
        private const float MillimetersPerMeter = 1000f;

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

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        public int TrialNumber = 1; // 重复实验编号。
        public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
        public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
        public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
        public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
        public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
        public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。

        /// <summary>
        /// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
        /// </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;
            }

            var stateProvider = new DetourVehicleStateProvider();
            if (!stateProvider.TryGetState(out var initialState))
            {
                Console.WriteLine(
                    "无法读取组合运动起点位姿：" +
                    stateProvider.LastFailureReason);
                return;
            }

            var firstTrajectory =
                TestTrajectoryFactory
                    .CreateStraightSmoothLeftTurnStraight(
                        initialState.PoseInWorld,
                        StraightLengthMeters,
                        TurnRadiusMeters,
                        AngleMath.DegreesToRadians(
                            TurnAngleDegrees),
                        CurvatureTransitionLengthMeters,
                        StraightMaximumSpeedMetersPerSecond,
                        CurveMaximumSpeedMetersPerSecond,
                        AccelerationMetersPerSecondSquared,
                        DecelerationMetersPerSecondSquared,
                        PointSpacingMeters);

            var firstStopPose =
                firstTrajectory.EndPoint.PoseInWorld;
            var finalStraightYawRadians =
                AngleMath.NormalizeRadians(
                    firstStopPose.YawRadians +
                    AngleMath.DegreesToRadians(
                        InPlaceLeftTurnDegrees));
            var finalStraightStartPose =
                new Pose2D(
                    firstStopPose.XMeters,
                    firstStopPose.YMeters,
                    finalStraightYawRadians);
            var finalTrajectory =
                TestTrajectoryFactory.CreateStraight(
                    finalStraightStartPose,
                    FinalStraightLengthMeters,
                    StraightMaximumSpeedMetersPerSecond,
                    AccelerationMetersPerSecondSquared,
                    DecelerationMetersPerSecondSquared,
                    PointSpacingMeters);

            DrawPlan(
                firstTrajectory,
                finalTrajectory,
                firstStopPose);

            var plan = new MotionPlanSegment[]
            {
                new TrackMotionPlanSegment(firstTrajectory)
                {
                    // 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
                    FinishDistanceMeters = 0.05,
                    FinishSpeedMetersPerSecond = 0.03,
                    FinishHeadingToleranceRadians =
                        AngleMath.DegreesToRadians(3.0)
                },
                new RotateInPlaceMotionPlanSegment(
                    finalStraightYawRadians),
                new TrackMotionPlanSegment(finalTrajectory)
            };

            _recorder = new TrackingExperimentRecorder(
                controllerName: "NewStanleyPidComposite",
                trajectoryName:
                    "SmoothTurnStopRotateStraight",
                trialNumber: TrialNumber,
                referenceStart: ToMillimeterVector(
                    firstTrajectory.StartPoint.PoseInWorld),
                referenceEnd: ToMillimeterVector(
                    finalTrajectory.EndPoint.PoseInWorld),
                referenceSpeed:
                    (float)StraightMaximumSpeedMetersPerSecond,
                sampleIntervalMs: 50,
                referenceAccelerationMetersPerSecondSquared:
                    (float)AccelerationMetersPerSecondSquared,
                referenceDecelerationMetersPerSecondSquared:
                    (float)DecelerationMetersPerSecondSquared);
            _recorder.Start();

            var controlPointRadiusMeters =
                chassis.ControlPointRadius /
                MillimetersPerMeter;
            var movement = new MotionPlanExecutor
            {
                Segments = plan,
                StateProvider = stateProvider,
                ConfigureTrackingMovement = tracking =>
                {
                    tracking.MaximumCommandSpeedMetersPerSecond =
                        StraightMaximumSpeedMetersPerSecond;
                    tracking
                        .LongitudinalSpeedErrorDeadbandMetersPerSecond =
                        0.025;
                },
                SegmentStarted = (index, segment) =>
                {
                    _recorder?.ClearControlReference();
                    _recorder?.UpdateCommand(0f, 0f);
                    Console.WriteLine(
                        $"组合运动开始第{index + 1}段：" +
                        segment.GetType().Name);
                },
                TrackingCycleObserver = (index, controller) =>
                    RecordTrackingCycle(
                        controller,
                        controlPointRadiusMeters),
                RotationCommandObserver = (index, omega) =>
                    _recorder?.UpdateCommand(
                        0f,
                        (float)omega)
            };

            try
            {
                _task = new DriveTask(movement.Get());
                _task.Wait();
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
                _painter?.Clear();
            }
        }

        /// <summary>
        /// 停止组合运动、保存已有实验数据并清除计划轨迹。
        /// </summary>
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
            _task = null;
            _recorder = null;
            _painter?.Clear();
        }

        /// <summary>
        /// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
        /// </summary>
        private void DrawPlan(
            Trajectory2D firstTrajectory,
            Trajectory2D finalTrajectory,
            Pose2D rotationPoseInWorld)
        {
            _painter.Clear();
            DrawTrajectory(
                firstTrajectory,
                Color.DeepSkyBlue);
            DrawTrajectory(
                finalTrajectory,
                Color.Gold);

            var rotationPoint =
                ToMillimeterVector(rotationPoseInWorld);
            _painter.DrawDot(
                Color.Magenta,
                rotationPoint.X,
                rotationPoint.Y,
                10f);
        }

        /// <summary>
        /// 绘制一段离散世界坐标系轨迹。
        /// </summary>
        private void DrawTrajectory(
            Trajectory2D trajectory,
            Color color)
        {
            for (var index = 0;
                 index < trajectory.Count;
                 index++)
            {
                var point = ToMillimeterVector(
                    trajectory[index].PoseInWorld);
                _painter.DrawDot(
                    color,
                    point.X,
                    point.Y,
                    3f);

                if (index == 0)
                {
                    continue;
                }

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

        /// <summary>
        /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
        /// </summary>
        private void RecordTrackingCycle(
            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>
        /// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
        /// </summary>
        private static Vector2 ToMillimeterVector(
            Pose2D poseInWorld)
        {
            return new Vector2(
                (float)(poseInWorld.XMeters *
                    MillimetersPerMeter),
                (float)(poseInWorld.YMeters *
                    MillimetersPerMeter));
        }
    }
}