using System;
using ClumsyCore;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace MultiWheelC
{
    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 TestClampCloseMovement
        : ClampMovementTestBase
    {
        protected override bool Close => false;
    }

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