using System;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MyParking.Shared;
using MultiWheelC.StateEstimation;

namespace MultiWheelC
{
    public abstract class InPlaceRotateTestBase : MovementTest
    {
        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
        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()
        {
            var config = PilotDefinition.Conf;

            if (float.IsNaN(RelativeAngleDegrees) ||
                float.IsInfinity(RelativeAngleDegrees) ||
                float.IsNaN(config.InPlaceRotateMaxSpeed) ||
                float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
                config.InPlaceRotateMaxSpeed <= 0f ||
                float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
                float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
                config.InPlaceRotateMinimumSpeed <= 0f ||
                config.InPlaceRotateMinimumSpeed >
                    config.InPlaceRotateMaxSpeed)
            {
                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 chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
                return;
            }

            var stateProvider =
                ParkingVehicleStateProviderFactory.Create(
                    chassis);
            if (!stateProvider.TryGetState(out _))
            {
                Console.WriteLine(
                    "无法读取原地旋转起点状态：" +
                    stateProvider.LastFailureReason);
                return;
            }

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

            Console.WriteLine(
                "原地自转实际参数：" +
                $"Kp={config.InPlaceRotateKp:F3}，" +
                $"Ki={config.InPlaceRotateKi:F3}，" +
                $"Kd={config.InPlaceRotateKd:F3}，" +
                $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
                $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
                $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
                $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
                $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
                $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
                $"起点航向={location.th:F2}°，" +
                $"目标航向={targetWorldAngle:F2}°。");
            Console.WriteLine(
                "原地自转CSV保存目录：" +
                TrackingExperimentRecorder.DefaultOutputDirectory);

            _recorder = new TrackingExperimentRecorder(
                controllerName: "InPlaceRotateFilteredPID",
                trajectoryName: _trajectoryName,
                trialNumber: TrialNumber,
                referenceStart: rotationCenter,
                referenceEnd: rotationCenter,
                referenceSpeed: 0f,
                referenceAngularSpeed:
                    (float)AngleMath.DegreesToRadians(
                        config.InPlaceRotateMaxSpeed),
                diagnosticChassis: chassis,
                diagnosticStateProvider: stateProvider);
            _recorder.Start();

            try
            {
                _task = new DriveTask(
                    new MultiWheelRotateInPlace
                    {
                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
                        AngleTarget = targetWorldAngle,
                        Chassis = chassis,
                        StateProvider = stateProvider,
                        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：输入角度原地自转")]
    public sealed class TestRotateAngle :
        InPlaceRotateTestBase
    {
        public TestRotateAngle()
            : base(0f, "RotateCustomAngle")
        {
        }

        /// <summary>
        /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
        /// </summary>
        public override void Test()
        {
            var input = UI.GetInput(
                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");

            if ((!float.TryParse(
                     input,
                     NumberStyles.Float,
                     CultureInfo.CurrentCulture,
                     out var relativeAngleDegrees) &&
                 !float.TryParse(
                     input,
                     NumberStyles.Float,
                     CultureInfo.InvariantCulture,
                     out relativeAngleDegrees)) ||
                float.IsNaN(relativeAngleDegrees) ||
                float.IsInfinity(relativeAngleDegrees))
            {
                Console.WriteLine("旋转角度输入无效，测试已经取消。");
                return;
            }

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

            var stateProvider =
                ParkingVehicleStateProviderFactory.Create(
                    chassis);
            if (!stateProvider.TryGetState(out _))
            {
                Console.WriteLine(
                    "无法读取原地旋转起点状态：" +
                    stateProvider.LastFailureReason);
                return;
            }

            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
            {
                Console.WriteLine("旋转角度不能为0，测试已经取消。");
                return;
            }

            // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
            if (Math.Abs(relativeAngleDegrees) >= 180f)
            {
                Console.WriteLine(
                    "输入角度必须满足-180° < angle < 180°；" +
                    "当前最短角控制不支持指定精确±180°的旋转方向。");
                return;
            }

            RelativeAngleDegrees = relativeAngleDegrees;
            base.Test();
        }
    }

}