using System;
using System.Collections.Generic;
using System.Threading;
using ClumsyCore.Pilot;

namespace MultiWheelC
{
    public class Sleep : MovementDefinition
    {
        public float Second = 2f;

        public override IEnumerable<bool> Get()
        {
            if (Second <= 0)
            {
                yield return false;
                yield break;
            }

            var endTime = DateTime.UtcNow.AddSeconds(Second);
            while (DateTime.UtcNow < endTime)
            {
                Thread.Sleep(50);
                yield return true;
            }

            yield return false;
        }
    }

    public class DriverAble : MovementDefinition
    {
        public int WaitTimeoutMs = 2000;
        public int PollIntervalMs = 50;

        // C层单车硬件：请求全部驱动轮复位并恢复使能。
        public override IEnumerable<bool> Get()
        {
            PilotDefinition.Self.ResetFromC = true;

            try
            {
                var start = DateTime.Now;
                var timeoutMs = Math.Max(0, WaitTimeoutMs);
                var pollMs = Math.Max(1, PollIntervalMs);

                // 至少保留一个调度周期，确保M层能收到复位请求。
                yield return true;

                while (!PilotDefinition.Self.WheelAbleState &&
                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
                {
                    Thread.Sleep(pollMs);
                    yield return true;
                }
            }
            finally
            {
                PilotDefinition.Self.ResetFromC = false;
            }
        }
    }
    public class DriverDisable : MovementDefinition
    {
        public int WaitTimeoutMs = 3000;
        public int PollIntervalMs = 20;

        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
        public override IEnumerable<bool> Get()
        {
            var timeoutMs = Math.Max(0, WaitTimeoutMs);
            var pollMs = Math.Max(1, PollIntervalMs);
            var startTime = DateTime.UtcNow;
            var success = false;

            PilotDefinition.Self.DisableFromC = true;

            try
            {
                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
                yield return true;

                success = !PilotDefinition.Self.WheelAbleState;

                while (!success &&
                        (DateTime.UtcNow - startTime).TotalMilliseconds <
                            timeoutMs)
                {
                    Thread.Sleep(pollMs);

                    success =
                        !PilotDefinition.Self.WheelAbleState;

                    if (!success)
                    {
                        yield return true;
                    }
                }
            }
            finally
            {
                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
                PilotDefinition.Self.DisableFromC = false;
            }
            if (success)
            {
                Console.WriteLine(
                    $"驱动器下使能完成，" +
                    $"WheelAbleState=" +
                    $"{PilotDefinition.Self.WheelAbleState}");
            }
            else
            {
                Console.WriteLine(
                    $"驱动器下使能超时，" +
                    $"WheelAbleState=" +
                    $"{PilotDefinition.Self.WheelAbleState}，" +
                    $"等待{timeoutMs}ms");
            }
            yield return false;
        }
    }
}