using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MDCSToolBox.Clumsy.Tracks;
using MyParking.Shared;
using System;
using System.Numerics;
using System.Threading;

namespace MultiWheelC
{
    internal static class MovementTestPreparation
    {
        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
        public static bool AlignWheelsForward(
            ref DriveTask activeTask)
        {
            var preparation = new PrepareWheelsForward();
            var task = new DriveTask(preparation.Get());
            activeTask = task;

            try
            {
                task.Wait();
                return preparation.Completed;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"测试前舵轮回正失败：{ex.Message}");
                return false;
            }
            finally
            {
                task.Stop();
                if (ReferenceEquals(activeTask, task))
                    activeTask = null;
            }
        }

        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
        public static bool AreWheelsForward(
            float toleranceDegrees = 2f)
        {
            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
                return false;
            }

            try
            {
                var adapter = new MultiWheelChassisAdapter(
                    chassis,
                    PilotDefinition.Self.CarNum);
                var toleranceRadians =
                    toleranceDegrees * Math.PI / 180.0;

                if (adapter.AreParallelWheelsAligned(
                        0.0,
                        toleranceRadians))
                {
                    return true;
                }

                Console.WriteLine(
                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
                return false;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"检查舵轮方向失败：{ex.Message}");
                return false;
            }
        }
    }

    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
    public class AlignWheelsForwardTest : MovementTest
    {
        private DriveTask _task;

        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
        public override void Test()
        {
            MovementTestPreparation.AlignWheelsForward(
                ref _task);
        }

        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
        public override void TestStop()
        {
            _task?.Stop();
            _task = null;
        }
    }

    [MovementTest(name = "测试连续前进4m")]
    public class TestForward4m : MovementTest
    {
        public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
        public float CruiseSpeed = 0.3f;  // 巡航速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。
        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
        public override void Test()
        {
            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消连续前进4m测试。");
                return;
            }
            var source = new Vector2((float)location.x, (float)location.y);
            // Detour航向单位是度，三角函数需要弧度。
            var headingRadians = location.th * Math.PI / 180.0;
            var destination = new Vector2(
                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
            _recorder =
                        new TrackingExperimentRecorder(
                            controllerName: "Stanley",
                            trajectoryName: "Straight4m",
                            trialNumber: TrialNumber,
                            referenceStart: source,
                            referenceEnd: destination,
                            referenceSpeed: CruiseSpeed);
            _recorder.Start();
            try
            {
                _task = new DriveTask(
                new DstTracker
                {
                    Src = source,
                    Dst = destination,
                    CarDirectionBias = 0f,
                    MaxSpeed = CruiseSpeed
                }.Get());
                _task.Wait();
                // 保留少量停车后数据，便于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    [MovementTest(name = "测试原地自转90°")]
    public class TestRotate90 : MovementTest
    {
        public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
        public override void Test()
        {
            if (float.IsNaN(RelativeAngleDegrees) ||
                float.IsInfinity(RelativeAngleDegrees) ||
                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
                MaxAngularSpeedDegreesPerSecond <= 0f)
            {
                Console.WriteLine("原地旋转测试参数无效。");
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消原地旋转测试。");
                return;
            }

            var rotationCenter =
                new Vector2((float)location.x, (float)location.y);
            var targetWorldAngle =
                NormalizeDegrees(
                    (float)location.th + RelativeAngleDegrees);

            _recorder = new TrackingExperimentRecorder(
                controllerName: "InPlaceRotatePID",
                trajectoryName: "Rotate90",
                trialNumber: TrialNumber,
                referenceStart: rotationCenter,
                referenceEnd: rotationCenter,
                referenceSpeed:
                    MaxAngularSpeedDegreesPerSecond);
            _recorder.Start();

            try
            {
                _task = new DriveTask(
                    new MultiWheelRotateInPlace
                    {
                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
                        AngleTarget = targetWorldAngle,
                        PidparamsRead = () => new PIDParams
                        {
                            Kp =
                                PilotDefinition.Conf.InPlaceRotateKp,
                            Ki =
                                PilotDefinition.Conf.InPlaceRotateKi,
                            Kd =
                                PilotDefinition.Conf.InPlaceRotateKd,
                            DeadZone =
                                PilotDefinition.Conf
                                    .InPlaceRotateArriveDeg,
                            SpeedAccPerSec =
                                PilotDefinition.Conf.InPlaceRotateAcc,
                            OutputUpperThreshold =
                                MaxAngularSpeedDegreesPerSecond,
                            MaxI =
                                PilotDefinition.Conf.InPlaceRotateMaxI
                        },
                        CommandAngularSpeedObserver =
                            commandAngularSpeed =>
                                _recorder?.UpdateCommand(
                                    0f,
                                    commandAngularSpeed)
                    }.Get());

                _task.Wait();

                // 保留少量停止后的样本，用于观察角速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止原地旋转并保存当前已经采集的实验数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }

        // 将世界航向归一化到大约[-180°,180°]。
        private static float NormalizeDegrees(float angleDegrees)
        {
            return (float)(
                angleDegrees -
                Math.Round(angleDegrees / 360.0) * 360.0);
        }
    }

    [MovementTest(name = "测试左转90°圆弧")]
    public class TestArcMovement : MovementTest
    {
        public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
        public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
        public override void Test()
        {
            if (float.IsNaN(RadiusMillimeters) ||
                float.IsInfinity(RadiusMillimeters) ||
                RadiusMillimeters <= 0f ||
                float.IsNaN(CruiseSpeed) ||
                float.IsInfinity(CruiseSpeed) ||
                CruiseSpeed <= 0f)
            {
                Console.WriteLine("圆弧运动测试参数无效。");
                return;
            }

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

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消圆弧运动测试。");
                return;
            }

            var source =
                new Vector2((float)location.x, (float)location.y);
            var headingRadians =
                location.th * Math.PI / 180.0;

            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
            var center = new Vector2(
                source.X -
                    RadiusMillimeters *
                    (float)Math.Sin(headingRadians),
                source.Y +
                    RadiusMillimeters *
                    (float)Math.Cos(headingRadians));

            // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
            var startRadialAngleDegrees =
                (float)location.th - 90f;

            var controller = new ChassisController
            {
                BaseSpeed = CruiseSpeed
            }.Get();
            controller.FinishSpeed = 0f;

            var arc = new CircularArcTrack(
                center,
                RadiusMillimeters,
                startRadialAngleDegrees,
                startRadialAngleDegrees + 90f,
                direction: 1)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };

            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
            {
                Console.WriteLine(
                    "左转90°圆弧轨迹添加失败，取消测试。");
                return;
            }

            _recorder = new TrackingExperimentRecorder(
                controllerName: "GeometricController",
                trajectoryName:
                    $"LeftArc90_R{RadiusMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: source,
                referenceSpeed: CruiseSpeed);
            _recorder.Start();

            try
            {
                _task = new DriveTask(controller.Track());
                _task.Wait();

                // 保留少量停车后的样本，用于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止圆弧运动并保存当前已经采集的实验数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    public abstract class ClampMovementTestBase : MovementTest
    {
        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。

        private DriveTask _task;
        protected abstract bool Close { get; }

        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
        public override void Test()
        {
            var leftTarget = Close
                ? PilotDefinition.Self.LeftArmUpperPos
                : PilotDefinition.Self.LeftArmLowerPos;
            var rightTarget = Close
                ? PilotDefinition.Self.RightArmUpperPos
                : PilotDefinition.Self.RightArmLowerPos;

            if (float.IsNaN(leftTarget) ||
                float.IsInfinity(leftTarget) ||
                float.IsNaN(rightTarget) ||
                float.IsInfinity(rightTarget))
            {
                Console.WriteLine(
                    "夹臂目标位置无效，取消夹臂运动测试。");
                return;
            }

            // 防止重复点击时上一项夹臂任务仍在运行。
            TestStop();
            Console.WriteLine(
                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
                $"左目标={leftTarget}，右目标={rightTarget}");

            var task = new DriveTask(
                new ClampToTarget
                {
                    LeftClampTarget = leftTarget,
                    RightClampTarget = rightTarget,
                    TimeoutSeconds = TimeoutSeconds
                }.Get());
            _task = task;

            try
            {
                task.Wait();
            }
            finally
            {
                PilotDefinition.Self.SpeedLeftArm = 0f;
                PilotDefinition.Self.SpeedRightArm = 0f;
                if (ReferenceEquals(_task, task))
                    _task = null;
            }
        }

        // 停止夹臂任务并立即清零左右夹臂下发速度。
        public override void TestStop()
        {
            _task?.Stop();
            _task = null;
            PilotDefinition.Self.SpeedLeftArm = 0f;
            PilotDefinition.Self.SpeedRightArm = 0f;
        }
    }

    [MovementTest(name = "夹臂关闭测试")]
    public sealed class TestClampOpenMovement
        : ClampMovementTestBase
    {
        protected override bool Close => false;
    }

    [MovementTest(name = "夹臂启动测试")]
    public sealed class TestClampCloseMovement
        : ClampMovementTestBase
    {
        protected override bool Close => true;
    }

}






#region 旧版测试
// public abstract class DstTrackerTestBase : MovementTest
// {
//     public bool UseInteractivePick = true;
//     public float srcX;
//     public float srcY;
//     public float dstX;
//     public float dstY;
//     public float carDirectionBias;

//     private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
//     private DriveTask _dt;

//     protected DstTrackerTestBase(float defaultCarDirectionBias)
//     {
//         carDirectionBias = defaultCarDirectionBias;
//     }

//     public override void TestStop()
//     {
//         _dt?.Stop();
//         _painter?.Clear();
//     }

//     public override void Test()
//     {
//         Vector2 p1;
//         Vector2 p2;
//         if (UseInteractivePick)
//         {
//             p1 = UI.GetPoint("point1");
//             p2 = UI.GetPoint("point2");
//         }
//         else
//         {
//             p1 = new Vector2(srcX, srcY);
//             p2 = new Vector2(dstX, dstY);
//         }

//         _painter.Clear();
//         _dt = new DriveTask(new DstTracker
//         {
//             Src = p1,
//             Dst = p2,
//             CarDirectionBias = carDirectionBias,
//         }.Get());
//         _dt.Wait();
//     }
// }

// [MovementTest(name = "测试终点跟踪动作-前进")]
// public sealed class DstTrackerForward : DstTrackerTestBase
// {
//     public DstTrackerForward() : base(0f) { }
// }

// [MovementTest(name = "测试终点跟踪动作-后退")]
// public sealed class DstTrackerBackward : DstTrackerTestBase
// {
//     public DstTrackerBackward() : base(180f) { }
// }

// [MovementTest(name = "底盘旋转测试")]
// public class RotateToAngleTest : MovementTest
// {
//     private DriveTask _dt;

//     // 停止当前正在执行的底盘原地旋转任务。
//     public override void TestStop()
//     {
//         _dt?.Stop();
//     }

//     // 交互输入目标角度后执行底盘原地旋转测试。
//     public override void Test()
//     {
//         var input = UI.GetInput("输入旋转角度：");
//         if (!float.TryParse(input, out var angleTarget))
//         {
//             Console.WriteLine(
//                 $"旋转测试输入无效：{input}");
//             return;
//         }
//         // 防止重复启动测试时，上一项旋转任务仍在运行。
//         _dt?.Stop();
//         var task = new DriveTask(
//             new MultiWheelRotateInPlace
//             {
//                 AngleTarget = angleTarget,

//                 PidparamsRead = () => new PIDParams
//                 {
//                     Kp = PilotDefinition.Conf.InPlaceRotateKp,
//                     Ki = PilotDefinition.Conf.InPlaceRotateKi,
//                     Kd = PilotDefinition.Conf.InPlaceRotateKd,
//                     DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
//                     SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
//                     OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
//                     MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
//                 }
//             }.Get());
//         _dt = task;
//         try
//         {
//             task.Wait();
//         }
//         finally
//         {
//             // 防止旧任务结束时，错误清除后来启动的新任务。
//             if (ReferenceEquals(_dt, task))
//             {
//                 _dt = null;
//             }
//         }
//     }
// }

#endregion