StandardSence/StandardScene.QrLidar/CarTypes/MultiWheelLifterCar.cs

using CommonUsage.Mathematics;
using LessokajiWeaverUtilities.Utilities;
using Nancy.Helpers;
using Newtonsoft.Json;
using SimpleLite;
using SimpleLite.RCS;
using SimpleLite.RCS.CarTypes;
using SimpleLite.CADTools;
using SimpleLite.Props;
using SimpleLite.UI;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using StandardScene.Model;
using StandardScene.Coders;
using StandardScene.Utils;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Net.Http;
using System.Numerics;
using System.Runtime;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Track = SimpleCore.PropType.Track;

namespace StandardScene.CarTypes
{
    // 磁循迹器已与 Kiva 版去重合并为唯一 StandardScene.Magnetic.Coders.MagneticTrackCoder。
    // 原 MWL 版 NaiveMagGo 缺少 ${track.Speed}，合并后统一为含 Speed 的版本（用户确认）。
    // MultiWheelLifter 系字段袋下沉至基座 StandardScene.Core\CarTypes\MultiWheelLifterFields.cs
    // （MultiVehicleCar 等跨平台车型引用它们）。

    [TemplateTrackCoderSettings(
        priority = 30,
        useVerb = "track.SleepTime!=0",
        templateString = "agv.Wait();agv.Sleep(${track.SleepTime});agv.Wait();",
        blockVerb = "false",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 30,
        useVerb = "track.TrayTarget!=0",
        templateString = "agv.Wait();agv.TrayControl(${track.TrayTarget});agv.Wait();",
        blockVerb = "false",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    //调整方向+钻车
    [TemplateTrackCoderSettings(
        priority = 19,
        useVerb = "dst.name=='FetchSite'&&plan.action=='fetch'",
        templateString = "agv.Wait();agv.RotateToTarget(${plan.AngleTarget});agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${plan.FirstTire});agv.Wait();",
        blockVerb = "false",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]
    //[TemplateTrackCoderSettings(
    //    priority = 19,
    //    useVerb = "dst.FetchPlace && plan.action=='fetch'",
    //    templateString = "agv.Wait();agv.RotateToTarget(${plan.AngleTarget});agv.Wait();",
    //    blockVerb = "false",
    //    siteFields = typeof(MultiWheelLifterSiteFields),
    //    trackFields = typeof(MultiWheelLifterTrackFields),
    //    planFields = typeof(MultiWheelLifterPlanFields))]

    //下使能+夹抱+上使能
    [TemplateTrackCoderSettings(
        priority = 18,
        useVerb = "dst.name=='FetchSite'&&plan.action=='fetch'",
        templateString = "agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 19,
        useVerb = "dst.tag>0 && src.tag>0",
        templateString = "agv.QrGo(${src.x},${src.y},${src.id},${src.tag},${dst.x},${dst.y},${dst.id},${dst.tag},${track.id}," +
                         "${track.Speed},${track.Reverse || (track.ReverseDst == dst.id)}," +
                         "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection}," +
                         "${track.typeInfo});",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    // 通用避障/IO coder 已抽离为 StandardScene.Coders.*（见类前 [ProgramTrackCoderSettings] 引用）

    // 避障区尺寸(4参,含中心点)切换 coder 已抽离为 StandardScene.Coders.AvoidanceParamCoder（见类前 [ProgramTrackCoderSettings] 引用）

    // 纠偏阈值 coder 已抽离为 StandardScene.Coders.TrackingErrThreshCoder

    [ProgramTrackCoderSettings(priority = 27, program = typeof(LidarAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceDistanceCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceParamCoder))]

    [EnvelopConfig(lengthX = 3000, lengthY = 1300, centerX = 0, centerY = 0)]

    [CarType(Name = "多舵轮顶升车")]
    [I18N.DocumentTranslation(Name = "MultiWheel Lifter Car", locale = "en")]
    public class MultiWheelLifterCar : GhostCar
    {
        [FieldMember]public float CarLength = 2000;
        [FieldMember] public float CarWidth = 1400;
        public static async Task<MultiWheelLifterCar> Create()
        {
            var car = new MultiWheelLifterCar()
            {
                lstatus = "连接中",
                address = "127.0.0.1",
                name = "多舵轮顶升车"
            };
            return car;
        }

        [MethodMember(Name = "进入小车管理界面", Description = "在浏览器中打开小车 Web 页面")]
        [I18N.DocumentTranslation(Name = "Open vehicle management page", Description = "Open the car's management page in browser", locale = "en")]
        public void OpenVehicleManagementPage()
        {
            CarRemoteHelper.OpenVehicleWebPage(address);
        }

        public void newTrafficReset(int resetId = 0)
        {
            //if (!status.programs.task.IsCompleted &&
            //    MessageBox.Show($"小车{name}({id})当前有任务：{status.programs.printStatus()}，确认初始化？", "确认",
            //        MessageBoxButtons.YesNo) == DialogResult.No) return;
            try
            {
                if (GetLastSite() != -1 && Commons.GetVehicleStatus((Car)this) != VehicleStatus.NeedInit)
                {
                    Diagnosis.Post($"Car{id}初始化失败：已初始化");
                    return;
                }
                var debug = "ui invoke reset. ";

                // todo: 找点和线段的最近点。。。
                // lock
                UISite site1 = null;
                float dist = float.MaxValue;
                List<(float, Site, Site)> trackList = new List<(float, Site, Site)>();
                if (resetId == 0)
                {

                    (float bias, Vector2 hPnt, float fd) Project2DLine(
                        Vector2 pnt,
                        Vector2 segSt,
                        Vector2 segEnd
                    )
                    {
                        var dir = Vector2.Normalize(segEnd - segSt);
                        var fd = Vector2.Dot(pnt - segSt, dir);
                        var hPnt = segSt + fd * dir;
                        var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
                        return (bias, hPnt, fd);
                    }

                    foreach (var allTrack in SimpleLib.GetAllTracks())
                    {
                        var s = SimpleLib.GetSite(allTrack.siteA);
                        var e = SimpleLib.GetSite(allTrack.siteB);
                        var (b, h, fd) = Project2DLine(
                            new Vector2(x, y),
                            new Vector2(s.x, s.y),
                            new Vector2(e.x, e.y)
                        );
                        b = Math.Abs(b);
                        if (fd < 0 || fd > 1)
                            continue;
                        if (dist > b)
                        {
                            UISite ss;
                            if (fd < 0.5)
                                ss = (UISite)s;
                            else
                                ss = (UISite)e;

                            foreach (var ccar in SimpleLib.GetAllCars())
                            {
                                if (ccar == this)
                                    continue;
                                if (ccar.status.holdingLocks.Contains(s.id))
                                    goto next;
                            }

                            dist = b;
                            site1 = ss;
                        next:
                            ;
                        }
                    }
                    float radius = 5000;
                    //根据小车为圆心找到半径radius内的站点
                    var surroundSites = SimpleLib.GetAllSites().ToList().FindAll(p => LessMath.dist(x, y, p.x, p.y) <= radius);
                    foreach (var allTrack in SimpleLib.GetAllTracks())
                    {
                        var s = SimpleLib.GetSite(allTrack.siteA);
                        var e = SimpleLib.GetSite(allTrack.siteB);
                        if (!surroundSites.Contains(s) && !surroundSites.Contains(e) && surroundSites.Count != 0) continue;

                        var (b, h, fd) = Project2DLine(new Vector2(x, y), new Vector2(s.x, s.y), new Vector2(e.x, e.y));
                        b = Math.Abs(b);
                        double pathAngle = Double.NaN;
                        if (allTrack.direction == 1)
                            pathAngle = Math.Atan2((e.y - s.y), (e.x - s.x)) / Math.PI * 180;
                        else if (allTrack.direction == 2)
                            pathAngle = Math.Atan2((e.y - s.y), (e.x - s.x)) / Math.PI * 180 + 180;
                        else
                            trackList.Add((b, s, e));
                        if (Math.Abs(this.th - pathAngle) < 30)
                            trackList.Add((b, s, e));
                        foreach (var ccar in SimpleLib.GetAllCars())
                        {
                            if (ccar == this) continue;
                            if (ccar.status.holdingLocks.Contains(s.id)) goto next;
                        }
                    next:;

                    }
                    //var sites = trackList.OrderBy(p => p.Item1).ToArray();
                    List<(double, Site)> distList = new List<(double, Site)>();
                    foreach (var site in trackList)
                    {
                        if (site.Item2.fields.ContainsKey("no_reset") || site.Item3.fields.ContainsKey("no_reset"))
                            continue;
                        foreach (var ccar in SimpleLib.GetAllCars())
                        {
                            if (ccar == this) continue;
                            if (ccar.status.holdingLocks.Contains(site.Item2.id) || ccar.status.holdingLocks.Contains(site.Item3.id)) goto next;
                        }
                        distList.Add((LessMath.dist(site.Item2.x, site.Item2.y, x, y), site.Item2));
                        distList.Add((LessMath.dist(site.Item3.x, site.Item3.y, x, y), site.Item3));
                    next:;
                    }

                    site1 = (UISite)distList.OrderBy(p => p.Item1).First().Item2;

                    //foreach (var site in SimpleLib.GetAllSites())
                    //{
                    //    if (site.fields.ContainsKey("no_reset"))
                    //        continue;
                    //    foreach (var ccar in SimpleLib.GetAllCars())
                    //    {
                    //        if (ccar == this)
                    //            continue;
                    //        if (ccar.status.holdingLocks.Contains(site.id))
                    //            goto next;
                    //    }

                    //    var d = LessMath.dist(site.x, site.y, x, y);
                    //    if (d < dist)
                    //    {
                    //        dist = (float)d;
                    //        site1 = (UISite)site;
                    //    }
                    //    next:
                    //    ;
                    //}
                    TrafficReset(site1, true, strict: false);
                }
                else
                {
                    TrafficReset(SimpleLib.GetSite(resetId), true, strict: false);
                }


                siteID = site1.id;

                lstatus = "上线";
                debug += $"site={site1.id}";
                AppendDebug(debug);
                G.pushStatus($"Car {name}({id}) reset to Site {site1.id}");
            }
            catch (Exception e)
            {
                Diagnosis.Post($"{this.name}---初始化失败--{e.Message}", $"{this.name}", true);
            }
        }



        protected override void draw(Graphics eGraphics)
        {
            var HalfCarLength = CarLength / 2;
            var HalfCarWidth = CarWidth / 2;
            eGraphics.FillRectangle(Brushes.Gray, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
            eGraphics.DrawRectangle(Pens.White, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
            eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, -HalfCarWidth, HalfCarLength, 0);
            eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, HalfCarWidth, HalfCarLength, 0);
        }

        public override string SetDisplayInfo()
        {
            try
            {
                var str = $"{name}({id})\n";
                status.enums.TryGetValue("ChassisMode", out var manual);
                var ms = manual == "0" ? "自动" : "手动";
                status.enums.TryGetValue("Soc", out var soc);
                str = $"{str}{ms}|soc:{soc}";
                var alarmStr = Commons.GetCarStatus(this, "AlarmInfo");
                if (alarmStr != "")
                    str = $"{str}|{alarmStr}";
                if (status.enums.TryGetValue("ElectricCurrent", out var electricCurrent) &&
                    !string.IsNullOrEmpty(electricCurrent))
                {
                    var charging = float.Parse(electricCurrent) * 0.01f > 0 ? "充电中" : "未充电";
                    str = $"{str}|charge:{charging}";
                }

                var thresSpeed = Commons.CarValue(this, "ThresSpeed");
                str = $"{str}{((thresSpeed <= 0) ? "|避障触发" : "")}";
                return str;
            }
            catch (Exception e)
            {
                Console.WriteLine(e);
                return "bad car";
            }
        }

        private bool _running = false;

        [MethodMember(Name = "测试", Description = "循环")]
        [I18N.DocumentTranslation(Name = "Test", Description = "Looping", locale = "en")]
        public async void Test()
        {
            var siteA = SimpleLib.GetAllSites().First(s => s.name == "A");
            var siteB = SimpleLib.GetAllSites().First(s => s.name == "B");
            _running = true;
            while (_running)
            {
                var plan = new SegmentPlan() { usingCar = this };
                plan.FindRoute(SimpleLib.GetSite(this.GetLastSite()), siteA);
                await plan.Compile("GoA").Queue();
                var planB = new SegmentPlan() { usingCar = this };
                planB.FindRoute(SimpleLib.GetSite(this.GetLastSite()), siteB);
                await planB.Compile("GoB").Queue();
                await Task.Delay(1000);
            }
        }

        [MethodMember(Name = "停止测试")]
        [I18N.DocumentTranslation(Name = "Stop Test", Description = "Stop Looping", locale = "en")]
        public void Stop()
        {
            _running = false;
        }
        HttpClient hc = new HttpClient() { Timeout = TimeSpan.FromMilliseconds(300) };
        [MethodMember(Name = "小车暂停", Description = "小车暂停")]
        [I18N.DocumentTranslation(Name = "Emergency Stop",Description = "Emergency Stop the car", locale = "en")]
        public void EmergencyStop(string reason)
        {
            Diagnosis.Post($"car{name}:EmergencyStop");
            hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=1");
        }

        [MethodMember(Name = "小车暂停恢复", Description = "小车暂停恢复")]
        [I18N.DocumentTranslation(Name = "Emergency Release",Description = "Release Emergency Stop of the car", locale = "en")]
        public void EmergencyRelease()
        {
            Diagnosis.Post($"car{name}:EmergencyStop release");
            hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=0");
        }
        [MethodMember(Name = "小车重启C", Description = "小车重启Clumsy")]
        [I18N.DocumentTranslation(Name = "Reset Clumsy",Description = "Reset Clumsy of the car", locale = "en")]
        public void ResetClumsy()
        {
            Diagnosis.Post($"car{name}-{address}:ResetClumsy");
            hc.GetStringAsync($"http://{address}:8008/reset");
        }
    }
}