using System;
using ClumsyCore;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MyParking.Shared;

namespace MultiWheelC
{
    internal static class MovementTestPreparation
    {
        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
        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;
            }
        }

        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
        public static bool AreWheelsForward(
            float toleranceDegrees = 2f)
        {
            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
                return false;
            }

            try
            {
                var adapter = new MultiWheelChassisAdapter(
                    chassis,
                    PilotDefinition.Self.CarNum);
                var toleranceRadians =
                    AngleMath.DegreesToRadians(toleranceDegrees);

                if (adapter.AreParallelWheelsAligned(
                        0.0,
                        toleranceRadians))
                {
                    return true;
                }

                Console.WriteLine(
                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
                return false;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"检查舵轮方向失败：{ex.Message}");
                return false;
            }
        }
    }

    [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;
        }
    }
}