using System;
using System.Numerics;
using CommonUsage.Chassis;
using MultiWheelC.Fleet;
using MyParking.Shared;

namespace MultiWheelC.Tests
{
    internal static class FleetMemberAgentTests
    {
        private const long PlanId = 11;
        private const int VehicleId = 2;

        public static void Run()
        {
            VerifyActiveWatchdogExpires();
            VerifyValidMotionCommandRefreshesDeadline();
            VerifyRepeatedActivationDoesNotRefreshDeadline();
            VerifyLateActivationCannotResumeMotion();
            VerifyStopClearsWatchdog();

            Console.WriteLine(
                "FleetMemberAgent本地命令看门狗测试通过，共5个场景。");
        }

        private static void VerifyActiveWatchdogExpires()
        {
            var agent = CreateReadyAgent();
            AssertTrue(
                agent.Activate(
                    PlanId,
                    commandReceivedTimeSeconds: 10.0,
                    validForSeconds: 0.5),
                "成员车应当成功激活");

            AssertTrue(
                agent.UpdateCommandWatchdog(10.5),
                "截止时刻仍应视为有效");
            AssertFalse(
                agent.UpdateCommandWatchdog(10.501),
                "超过命令有效期后应停车");
            AssertState(
                agent,
                FleetMemberAgentState.Faulted,
                "命令超时");
            AssertTrue(
                !string.IsNullOrWhiteSpace(
                    agent.LastFailureReason),
                "命令超时应保留故障原因");
        }

        private static void VerifyValidMotionCommandRefreshesDeadline()
        {
            var agent = CreateReadyAgent();
            agent.Activate(
                PlanId,
                commandReceivedTimeSeconds: 20.0,
                validForSeconds: 0.5);

            var accepted = agent.Execute(
                PlanId,
                new FleetMemberCommand(
                    VehicleId,
                    Twist2D.Zero),
                commandReceivedTimeSeconds: 20.4,
                validForSeconds: 0.7);

            AssertTrue(
                accepted,
                "有效速度命令应被接受");
            AssertNear(
                agent.LastAcceptedCommandTimeSeconds,
                20.4,
                "最近命令接收时间");
            AssertNear(
                agent.CommandDeadlineSeconds,
                21.1,
                "速度命令刷新后的截止时间");
            AssertTrue(
                agent.UpdateCommandWatchdog(20.8),
                "刷新截止时间后车辆应保持Active");
        }

        private static void VerifyRepeatedActivationDoesNotRefreshDeadline()
        {
            var agent = CreateReadyAgent();
            agent.Activate(
                PlanId,
                commandReceivedTimeSeconds: 30.0,
                validForSeconds: 0.5);

            AssertTrue(
                agent.Activate(
                    PlanId,
                    commandReceivedTimeSeconds: 30.4,
                    validForSeconds: 0.5),
                "未超时的重复激活命令应允许幂等确认");
            AssertNear(
                agent.CommandDeadlineSeconds,
                30.5,
                "重复激活不能替代运动命令刷新截止时间");
            AssertFalse(
                agent.UpdateCommandWatchdog(30.501),
                "没有收到运动命令时仍应按最初激活期限停车");
        }

        private static void VerifyLateActivationCannotResumeMotion()
        {
            var agent = CreateReadyAgent();
            agent.Activate(
                PlanId,
                commandReceivedTimeSeconds: 40.0,
                validForSeconds: 0.5);

            AssertFalse(
                agent.Activate(
                    PlanId,
                    commandReceivedTimeSeconds: 40.6,
                    validForSeconds: 0.5),
                "迟到的激活命令不能恢复已经失联的车辆");
            AssertState(
                agent,
                FleetMemberAgentState.Faulted,
                "迟到激活命令");
        }

        private static void VerifyStopClearsWatchdog()
        {
            var agent = CreateReadyAgent();
            agent.Activate(
                PlanId,
                commandReceivedTimeSeconds: 50.0,
                validForSeconds: 0.5);

            agent.Stop();

            AssertState(
                agent,
                FleetMemberAgentState.Idle,
                "正常停止");
            AssertTrue(
                !agent.LastAcceptedCommandTimeSeconds.HasValue &&
                !agent.CommandDeadlineSeconds.HasValue,
                "正常停止后应清除命令看门狗");
        }

        private static FleetMemberAgent CreateReadyAgent()
        {
            var chassis = new MultiWheelChassis();
            chassis.AddWheel(CreateWheel(-500f, 300f));
            chassis.AddWheel(CreateWheel(-500f, -300f));
            chassis.AddWheel(CreateWheel(500f, 300f));
            chassis.AddWheel(CreateWheel(500f, -300f));
            chassis.Initialize();

            var agent = new FleetMemberAgent(
                new MultiWheelChassisAdapter(
                    chassis,
                    VehicleId),
                alignmentToleranceRadians:
                    AngleMath.DegreesToRadians(1.0),
                alignmentStableSeconds: 0.0);

            AssertTrue(
                agent.BeginRollingPreparation(
                    PlanId,
                    motionDirectionInBodyRadians: 0.0),
                "滚动运动系准备命令应被接受");
            AssertState(
                agent,
                FleetMemberAgentState.Preparing,
                "开始准备");
            AssertTrue(
                agent.UpdatePreparation(0.01) ==
                FleetMemberAgentState.Ready,
                "内存底盘舵轮应立即准备完成");

            return agent;
        }

        private static SteerWheel CreateWheel(
            float xMillimeters,
            float yMillimeters)
        {
            var speed = 0f;
            var angle = 0f;

            return new SteerWheel(
                new Vector2(
                    xMillimeters,
                    yMillimeters),
                angleLowerLimit: -120f,
                angleUpperLimit: 120f,
                speedWriter: value => speed = value,
                speedReader: () => speed,
                angleWriter: value => angle = value,
                angleReader: () => angle);
        }

        private static void AssertState(
            FleetMemberAgent agent,
            FleetMemberAgentState expected,
            string scenario)
        {
            AssertTrue(
                agent.State == expected,
                $"{scenario}后的状态应为{expected}，" +
                $"实际为{agent.State}。");
        }

        private static void AssertNear(
            double? actual,
            double expected,
            string name)
        {
            AssertTrue(
                actual.HasValue &&
                Math.Abs(actual.Value - expected) <= 1e-9,
                $"{name}不正确，期望{expected:F6}，" +
                $"实际{actual?.ToString("F6") ?? "null"}。");
        }

        private static void AssertTrue(
            bool condition,
            string message)
        {
            if (!condition)
            {
                throw new InvalidOperationException(message);
            }
        }

        private static void AssertFalse(
            bool condition,
            string message)
        {
            AssertTrue(!condition, message);
        }
    }
}