using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using MDCSToolBox.Commons.Controllers;
using System;
using System.Numerics;

namespace MultiWheelC
{
    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;
                }
            }
        }
    }
}