using System;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;

namespace MultiWheelC
{
    public abstract class InPlaceRotateTestBase : MovementTest
    {
        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        private readonly string _trajectoryName;

        protected InPlaceRotateTestBase(
            float relativeAngleDegrees,
            string trajectoryName)
        {
            RelativeAngleDegrees =
                relativeAngleDegrees;
            _trajectoryName =
                trajectoryName;
        }

        // 从当前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 =
                (float)AngleMath.NormalizeDegrees(
                    location.th + RelativeAngleDegrees);

            _recorder = new TrackingExperimentRecorder(
                controllerName: "InPlaceRotatePID",
                trajectoryName: _trajectoryName,
                trialNumber: TrialNumber,
                referenceStart: rotationCenter,
                referenceEnd: rotationCenter,
                referenceSpeed: 0f,
                referenceAngularSpeed:
                    (float)AngleMath.DegreesToRadians(
                        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,
                                    (float)AngleMath.DegreesToRadians(
                                        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();
        }

    }

    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
    public sealed class TestRotate90 :
        InPlaceRotateTestBase
    {
        public TestRotate90()
            : base(90f, "Rotate90")
        {
        }
    }

    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
    public sealed class TestRotate180 :
        InPlaceRotateTestBase
    {
        public TestRotate180()
            : base(180f, "Rotate180")
        {
        }
    }
}