using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons.Controllers;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Numerics;
using System.Threading;
using FundamentalLib;
using MyParking.Shared;

namespace MultiWheelC
{
    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
    public class PrepareWheelsForward : MovementDefinition
    {
        public float ToleranceDegrees = 2f;
        public float StableSeconds = 0.3f;
        public float TimeoutSeconds = 10f;
        public bool Completed { get; private set; }

        public override IEnumerable<bool> Get()
        {
            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
            }

            var adapter = new MultiWheelChassisAdapter(
                chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();
            var toleranceRadians =
                AngleMath.DegreesToRadians(ToleranceDegrees);
            var startTime = DateTime.UtcNow;
            DateTime? alignedSince = null;

            Completed = false;
            if (!adapter.PrepareParallelDirection(0.0))
            {
                throw new InvalidOperationException(
                    "无法将所有舵轮下发到车体前向0°。");
            }

            try
            {
                while (true)
                {
                    var aligned =
                        adapter.AreParallelWheelsAligned(
                            0.0,
                            toleranceRadians);

                    if (aligned)
                    {
                        if (!alignedSince.HasValue)
                            alignedSince = DateTime.UtcNow;

                        if ((DateTime.UtcNow -
                             alignedSince.Value).TotalSeconds >=
                            StableSeconds)
                        {
                            Completed = true;
                            yield break;
                        }
                    }
                    else
                    {
                        alignedSince = null;
                    }

                    if (TimeoutSeconds > 0f &&
                        (DateTime.UtcNow - startTime).TotalSeconds >
                        TimeoutSeconds)
                    {
                        throw new TimeoutException(
                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
                            "测试已经取消。");
                    }

                    yield return true;
                }
            }
            finally
            {
                // 只清零驱动速度，保留已经下发的0°舵角。
                adapter.StopImmediately();
            }
        }
    }

    #region 功能项
    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;
        }
    }
    #endregion

    #region 直线运动
    //在世界坐标系下，从路径起点追踪到终点并停车
    public class DstTracker : MovementDefinition
    {
        public Vector2 Src;
        public Vector2 Dst;
        // 本次轨迹的巡航速度上限，单位m/s。
        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
        public float CarDirectionBias = 0f;
        public Painter Painter = UI.GetPainter("DstTracker");
        public override IEnumerable<bool> Get()
        {
            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            DriveTask task = null;
            try
            {
                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);

                var tracker = new ChassisController
                {
                    BaseSpeed = MaxSpeed
                }.Get();
                // 要求路径末端速度下降到零。
                tracker.FinishSpeed = 0f;
                var linePath = new LineTrack(Src, Dst)
                {
                    CarDirectionBias = CarDirectionBias,
                    Speed = MaxSpeed
                };
                tracker.AddTrack(linePath);
                task = new DriveTask(tracker.Track());
                task.Wait();
                yield return false;
            }
            finally
            {
                task?.Stop();
                chassis.PredefinedDriveStop();
            }
        }

    }
    //直线行走基于轮里程
    // C层单车底盘：按照车轮里程行驶指定的相对距离。
    public class LineTracking : MovementDefinition
    {
        // 相对动作启动位置的行驶距离，单位mm。
        // 正数表示前进，负数表示后退。
        public float TargetDistance;
        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
        public float Kp = PilotDefinition.Conf.LineTrackKp;
        public float Ki = PilotDefinition.Conf.LineTrackKi;
        public float Kd = PilotDefinition.Conf.LineTrackKd;
        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
        public int SrcId = -1;
        public int DstId = -1;
        public Action<int> LeaveSrcFunction;
        // 接近目标后是否保留速度，交给下一个动作接管。
        public bool EnableHandover;
        // 进入动作衔接的剩余距离，单位mm。
        public float HandoverDistance = 80f;
        // HandoverSpeed小于0时，使用MaxSpeed的此比例。
        public float HandoverSpeedRatio = 0.5f;
        // 大于等于0时，直接作为衔接速度，单位m/s。
        public float HandoverSpeed = -1f;
        public float MinHandoverSpeed = 0.05f;
        private PIDController _pid;
        // 读取当前单车直线行驶里程，单位mm。
        private static float ReadPosition()
        {
            return
                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
        }

        // 根据动作启动位置和目标距离执行直线里程闭环。
        public override IEnumerable<bool> Get()
        {
            if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
            {
                throw new ArgumentOutOfRangeException(
                    nameof(TargetDistance),
                    "目标行驶距离必须是有限值。");
            }

            if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(MaxSpeed),
                    "最大速度必须是大于零的有限值。");
            }
            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            // 每次启动动作时重新读取起始编码器位置。
            var startPosition = ReadPosition();
            // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
            var targetPosition = startPosition + TargetDistance;
            _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
            {
                SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
            };
            var handoverRequested = false;
            var keepHandoverSpeed = false;
            DLog.Log(
                $"直线里程动作：" +
                $"起点={startPosition:F1}mm，" +
                $"距离={TargetDistance:F1}mm，" +
                $"目标={targetPosition:F1}mm",
                "straight_line");
            try
            {
                while (true)
                {
                    var currentPosition = ReadPosition();
                    var remainingDistance = targetPosition - currentPosition;
                    // 接近目标后，保留一定速度交给后续动作。
                    if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
                    {
                        var direction = Math.Sign(remainingDistance);
                        if (direction == 0)
                        {
                            direction = Math.Sign(TargetDistance);
                        }
                        var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
                        var maximumSpeed = Math.Abs(MaxSpeed);
                        var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
                        var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
                        var handoverSpeed = limitedSpeed * direction;
                        chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
                        handoverRequested = true;
                        // 保持一个调度周期，让速度命令实际生效。
                        yield return true;
                        break;
                    }
                    var speed = _pid.GetResponse(targetPosition);
                    chassis.SendXYThSpeed(speed, 0f, 0f);
                    if (_pid.IsArrived())
                    {
                        break;
                    }
                    yield return true;
                }
                if (SrcId != -1 &&
                    LeaveSrcFunction != null)
                {
                    LeaveSrcFunction(SrcId);
                    DLog.Log($"释放放车点{SrcId}", "straight_line");
                }
                // 只有正常完成动作衔接时才允许保留非零速度。
                keepHandoverSpeed = handoverRequested;
            }
            finally
            {
                // 普通完成、人工停止或异常退出时都必须停车。
                if (!keepHandoverSpeed)
                {
                    chassis.SendXYThSpeed(0f, 0f, 0f);
                }
            }
            yield return false;
        }
    }
    //直线行走基于detour
    public class LineTracking_based_detour : MovementDefinition
    {
        public float LineDistance = 1000f;
        public int SrcId = -1;
        public int DstId = -1;
        public Action<int> LeaveSrcFunction = null;
        public Painter painter = UI.GetPainter("Line", false);
        // C层单车轨迹：执行早期版本的两点直线跟踪动作。
        public override IEnumerable<bool> Get()
        {
            var curpose = DetourInterface.getCartLocation();
            Console.WriteLine($"curpose.th:{curpose.th}");
            var src = new Vector2((float)curpose.x, (float)curpose.y);
            var headingRadians =
                AngleMath.DegreesToRadians(curpose.th);
            var dst = new Vector2(
                (float)(curpose.x +
                    LineDistance * Math.Cos(headingRadians)),
                (float)(curpose.y +
                    LineDistance * Math.Sin(headingRadians)));
            // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
            //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
            Console.WriteLine($"src:{src.X} {src.Y}");
            Console.WriteLine($"dst:{dst.X} {dst.Y}");
            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);

            var tracker = new ChassisController().Get();
            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
            tracker.AddTrack(linePath);
            var _dt = new DriveTask(tracker.Track());
            _dt.Wait();
            if (SrcId != -1 && LeaveSrcFunction != null)
            {
                LeaveSrcFunction(SrcId);
                DLog.Log($"释放放车点{SrcId}", "straight_line");
            }
            yield return false;
        }
    }
    #endregion

    #region 旋转运动
    public class MultiWheelRotateInPlace : MovementDefinition
    {
        /// <summary>
        /// 旋转目标角度
        /// </summary>
        public float AngleTarget;

        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;

        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;

        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };

        public PIDController thPid;

        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
        public Action<float> CommandAngularSpeedObserver;

        // 自转前舵轮实际角度允许误差，单位deg。
        public float WheelAlignmentToleranceDegrees = 2f;

        // 自转舵轮连续保持到位的时间，单位s。
        public float WheelAlignmentStableSeconds = 0.3f;

        // 自转舵轮准备超时时间，单位s。
        public float WheelAlignmentTimeoutSeconds = 10f;

        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
        public override IEnumerable<bool> Get()
        {
            if (Chassis == null)
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");

            var adapter = new MultiWheelChassisAdapter(
                Chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();

            try
            {
                var alignmentStarted = DateTime.Now;
                DateTime? alignedSince = null;
                while (true)
                {
                    if (!adapter.PrepareSpin())
                        throw new InvalidOperationException(
                            "无法生成原地自转舵轮目标：" +
                            adapter.LastFailureReason);

                    if (adapter.AreSpinWheelsAligned)
                    {
                        if (alignedSince == null)
                            alignedSince = DateTime.Now;

                        if ((DateTime.Now - alignedSince.Value)
                                .TotalSeconds >=
                            WheelAlignmentStableSeconds)
                            break;
                    }
                    else
                    {
                        alignedSince = null;
                    }

                    if ((DateTime.Now - alignmentStarted)
                            .TotalSeconds >
                        WheelAlignmentTimeoutSeconds)
                        throw new TimeoutException(
                            "原地自转舵轮在限定时间内未稳定到位。");

                    yield return true;
                }

                var targetAngle =
                    (float)AngleMath.NormalizeDegrees(AngleTarget);
                var p = PidparamsRead();
                thPid = new PIDController(ThetaReader, p.Kp);
                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                    p.OutputUpperThreshold, p.SpeedAccPerSec);
                var lastCommandTime = DateTime.Now;

                while (true)
                {
                    var s = thPid.GetResponse(targetAngle, true);
                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                    CommandAngularSpeedObserver?.Invoke(s);
                    var now = DateTime.Now;
                    var interval = now - lastCommandTime;
                    lastCommandTime = now;

                    // PID输出s为deg/s，Shared命令统一使用rad/s。
                    // adapter.Send最终调用普通安全版SendXYThSpeed。
                    var omegaRadiansPerSecond =
                        (float)AngleMath.DegreesToRadians(s);
                    if (!adapter.Send(
                            new ChassisCommand(
                                PilotDefinition.Self.CarNum,
                                new Twist2D(
                                    0.0,
                                    0.0,
                                    omegaRadiansPerSecond)),
                            interval))
                    {
                        throw new InvalidOperationException(
                            "安全XYTh原地旋转底盘解算失败：" +
                            adapter.LastFailureReason);
                    }
                    if (thPid.IsArrived()) break;
                    yield return true;
                }

                Console.WriteLine($"final rotate to {targetAngle}");
            }
            finally
            {
                CommandAngularSpeedObserver?.Invoke(0f);
                adapter.StopImmediately();
            }
        }
    }
    #endregion

    #region 夹臂运动
    public class ClampToTarget : MovementDefinition
    {
        public float LeftClampTarget;
        public float RightClampTarget;
        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
        public float TimeoutSeconds = 30f;
        private PIDController leftpid, rightpid;

        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
        public override IEnumerable<bool> Get()
        {
            try
            {
                leftpid = new PIDController(
                    () => PilotDefinition.Self.ActualPosLeftArm,
                    ClampKp, ClampKi, ClampKd, ClampMaxI,
                    ClampDeadZone, MaxClampSpeed)
                {
                    SpeedAccPerSec = ClampSpeedAcc
                };

                rightpid = new PIDController(
                    () => PilotDefinition.Self.ActualPosRightArm,
                    ClampKp, ClampKi, ClampKd, ClampMaxI,
                    ClampDeadZone, MaxClampSpeed)
                {
                    SpeedAccPerSec = ClampSpeedAcc
                };

                var startTime = DateTime.UtcNow;
                while (true)
                {
                    if (TimeoutSeconds > 0f &&
                        (DateTime.UtcNow - startTime).TotalSeconds >
                        TimeoutSeconds)
                    {
                        Console.WriteLine(
                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
                            "停止左右夹臂。");
                        yield break;
                    }

                    var leftspeed =
                        leftpid.GetResponse(LeftClampTarget);
                    var rightspeed =
                        rightpid.GetResponse(RightClampTarget);
                    Console.WriteLine(
                        $"left arm speed:{leftspeed}  " +
                        $"right arm speed:{rightspeed}");

                    PilotDefinition.Self.SpeedLeftArm = leftspeed;
                    PilotDefinition.Self.SpeedRightArm = rightspeed;

                    var leftArrived = leftpid.IsArrived();
                    var rightArrived = rightpid.IsArrived();
                    if (leftArrived)
                        PilotDefinition.Self.SpeedLeftArm = 0f;
                    if (rightArrived)
                        PilotDefinition.Self.SpeedRightArm = 0f;

                    if (leftArrived && rightArrived)
                        break;

                    yield return true;
                }

                Console.WriteLine(
                    $"left clamp to target:{LeftClampTarget}  " +
                    $"right clamp to target:{RightClampTarget}");
            }
            finally
            {
                PilotDefinition.Self.SpeedLeftArm = 0f;
                PilotDefinition.Self.SpeedRightArm = 0f;
            }
        }
    }
    #endregion
}