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;

namespace MultiWheelC
{
    public class DstTracker : MovementDefinition
    {
        public Vector2 Src;
        public Vector2 Dst;
        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().Get();
                var linePath = new LineTrack(Src, Dst)
                {
                    CarDirectionBias = CarDirectionBias,
                    Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
                };
                tracker.AddTrack(linePath);
                task = new DriveTask(tracker.Track());
                task.Wait();
                yield return false;
            }
            finally
            {
                task?.Stop();
                chassis.SendXYThSpeed(0f, 0f, 0f);
            }
        }

    }

    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 MultiWheelRotateInPlace : MovementDefinition
    {
        /// <summary>
        /// 旋转目标角度
        /// </summary>
        public float AngleTarget;

        public float MaxSpeed;

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

        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;

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

        public PIDController thPid;

        // 将角度归一化到零到三百六十度范围内。
        private static float RangeAngle(float theta)
        {
            return (float)(theta - Math.Round(theta / 360.0f) * 360);
        }

        // 使用 PID 控制原地旋转到目标角度。
        public override IEnumerable<bool> Get()
        {
            try
            {
                var targetAngle = RangeAngle(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);

                while (true)
                {
                    var s = thPid.GetResponse(targetAngle, true);
                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                    Chassis.SendXYThSpeed(0, 0, s);
                    if (thPid.IsArrived()) break;
                    yield return true;
                }

                Console.WriteLine($"final rotate to {targetAngle}");
            }
            finally
            {
                Chassis.SendXYThSpeed(0, 0, 0);
            }
        }
    }

}