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
    {
        // 底盘旋转测试不支持停止操作。
        public override void TestStop()
        {
            throw new NotImplementedException();
        }

        // 交互输入目标角度后执行底盘原地旋转测试。
        public override void Test()
        {
            var target = UI.GetInput("输入旋转角度：");
            new DriveTask(new MultiWheelRotateInPlace()
            {
                AngleTarget = float.Parse(target),
                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()).Wait();
        }
    }
}