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

namespace MultiWheelC
{
    // 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;
        }
    }
}