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

namespace MultiWheelC
{
    /// <summary>
    /// 为需要显式执行舵轮回正的测试管理准备动作及其DriveTask生命周期。
    /// </summary>
    internal static class MovementTestPreparation
    {
        /// <summary>
        /// 执行舵轮回正动作并返回四轮是否已经稳定朝向车体前方。
        /// </summary>
        public static bool AlignWheelsForward(
            ref DriveTask activeTask)
        {
            var preparation = new PrepareWheelsForward();
            var task = new DriveTask(preparation.Get());
            activeTask = task;

            try
            {
                task.Wait();
                return preparation.Completed;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"测试前舵轮回正失败：{ex.Message}");
                return false;
            }
            finally
            {
                task.Stop();
                if (ReferenceEquals(activeTask, task))
                    activeTask = null;
            }
        }

    }

    /// <summary>
    /// 提供可从测试界面单独触发的四舵轮回正动作。
    /// </summary>
    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
    public class AlignWheelsForwardTest : MovementTest
    {
        private DriveTask _task;

        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
        public override void Test()
        {
            MovementTestPreparation.AlignWheelsForward(
                ref _task);
        }

        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
        public override void TestStop()
        {
            _task?.Stop();
            _task = null;
        }
    }
}