using System;
using System.Collections.Generic;
using System.Drawing;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Clumsy.Tracks;

namespace MultiWheelC
{
    //在世界坐标系下，从路径起点追踪到终点并停车
    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();
            }
        }

    }
}