using LessokajiWeaverUtilities.Utilities;
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 System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Net.Http;
using System.Numerics;
using System.Threading.Tasks;
using StandardScene.Model;
using StandardScene.Coders;
using StandardScene.Utils;

namespace StandardScene.CarTypes
{
    class ForkliftSiteFields : BasicSiteFields
    {
        public int AngleTarget = 0;
        public bool Turn = false;

        public float FetchSpeed = 0;
        public int FetchLidarArea = -2;
        public int FetchIOArea = -1;
        public float FetchBlindMoveDist = 0;
        public float FetchLiftDownTarget = -1;
        public float FetchLiftUpTarget = -1;
        public bool FetchUseDetector = false;
        public int FetchDetector = 0;
        public float FetchDetectWidth = -1;
        public float FetchDetectDepth = -1;
        public bool FetchLeaveSrcEarly = false;
        public float FetchShieldObstacleDist = -1;

        public float PutSpeed = 0;
        public int PutLidarArea = -1;
        public int PutIOArea = -1;
        public float PutBlindMoveDist = 0;
        public float PutLiftDownTarget = -1;
        public float PutLiftUpTarget = -1;
        public bool PutUseDetector = false;
        public int PutDetector = 0;
        public float PutDetectWidth = -1;
        public float PutDetectDepth = -1;
        public bool PutLeaveSrcEarly = false;
        public float PutShieldObstacleDist = -1;

        public float LeaveShelfSpeed = 0;
        public int LeaveShelfLidarArea = -1;
        public int LeaveShelfIOArea = -1;
        public float LeaveShelfBlindMoveDist = 0;
        public float LeaveShelfLiftDownTarget = -1;
        public float LeaveShelfRecoveryObstacleDist = -1;
    }

    class ForkliftTrackFields : BasicTrackFields
    {
        public int ManeuverDir = 0;
        public int ForwardDst = 0;
        public int ForwardObChooseDst = -2;
        public float BlindMoveDist = 0;
        public bool UseDetector = false;
        public int DetectorMode = -1;
        public float DetectWidth = -1;
        public float DetectDepth = -1;
        public bool LeaveSrcEarly = false;
        public float ShieldObstacleDist = -1;
    }

    class ForkliftPlanFields : BasicPlanFields
    {
        public bool reverse = false;
        public int level = 0;
    }


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

    // 避障区尺寸(2参)切换 coder 已抽离为 StandardScene.Coders.AvoidanceParamLWCoder（方案 B；见下方 [ProgramTrackCoderSettings] 引用）
    [ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceParamLWCoder))]

    //plan.SegN 是指 当前路径所有点线的数量   
    //plan.SegN-2 就是终点倒数第二个站点
    [TemplateTrackCoderSettings(
        priority = 20,
        useVerb = "plan.action=='fetch' && dst.Shelf",
        templateString = "agv.Wait();" +
                         "agv.Fetch(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${dst.FetchSpeed},${dst.FetchLidarArea},${dst.FetchIOArea}," +
                         "${dst.FetchBlindMoveDist},${dst.FetchLiftDownTarget},${dst.FetchLiftUpTarget}," +
                         "${dst.FetchUseDetector},${dst.FetchDetector},${dst.FetchDetectWidth},${dst.FetchDetectDepth}," +
                         "${plan.CarLength},${plan.CarWidth},${dst.FetchLeaveSrcEarly},${dst.FetchShieldObstacleDist});" +
                         "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(ForkliftSiteFields),
        trackFields = typeof(ForkliftTrackFields),
        planFields = typeof(ForkliftPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 20,
        useVerb = "plan.action=='put' && dst.Shelf",
        templateString = "agv.Wait();" +
                         "agv.Put(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${dst.PutSpeed},${dst.PutLidarArea},${dst.PutIOArea}," +
                         "${dst.PutBlindMoveDist},${dst.PutLiftDownTarget},${dst.PutLiftUpTarget}," +
                         "${dst.PutUseDetector},${dst.PutDetector}," +
                         "${dst.PutDetectWidth},${dst.PutDetectDepth},${dst.PutLeaveSrcEarly},${dst.PutShieldObstacleDist});" +
                         "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(ForkliftSiteFields),
        trackFields = typeof(ForkliftTrackFields),
        planFields = typeof(ForkliftPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 22,
        useVerb = "src.Shelf",
        templateString = "agv.LeaveShelf(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${src.LeaveShelfSpeed},${src.LeaveShelfLidarArea},${src.LeaveShelfIOArea}," +
                         "${src.LeaveShelfBlindMoveDist},${src.LeaveShelfLiftDownTarget},${src.LeaveShelfRecoveryObstacleDist},);" +
                         "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(ForkliftSiteFields),
        trackFields = typeof(ForkliftTrackFields),
        planFields = typeof(ForkliftPlanFields))]

    [TemplateSiteCoderSettings(
        priority = 25,
        useVerb = "plan.action=='fetch'&&plan.segN==1&&dst.Shelf",
        templateString =
            "agv.FetchInPlace(${dst.FetchLiftUpTarget},${plan.CarLength},${plan.CarWidth});" +
            "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(ForkliftSiteFields),
        trackFields = typeof(ForkliftTrackFields),
        planFields = typeof(ForkliftPlanFields))]
    // 纠偏阈值 coder 已抽离为 StandardScene.Coders.TrackingErrThreshCoder

    [ProgramTrackCoderSettings(priority = 20, program = typeof(LidarAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
    [CarType(Name = "叉车", editor = typeof(Forklift))]
    [I18N.DocumentTranslation(Name = "ForkLift", locale = "en")]
    [EnvelopConfig(centerX = -900, centerY = 0, lengthX = 3000, lengthY = 1500)]
	public class Forklift : GhostCar
    {
	    public static async Task<Forklift> Create() // boilerplate
	    {
		    var fl = new Forklift()
		    {
			    lstatus = "连接中",
			    address = "127.0.0.1",
			    name = $"叉车",
			    haveCoordination = true,
			    speed = 1,
		    };

		    return fl;
	    }

		private const float DrawWidth = 1100, DrawLength = 600, DrawForkLen = 1532;
	    private readonly Pen _orientPen = new Pen(Color.White, 3);

		protected override void draw(Graphics eGraphics)
	    {
		    var lineCap = new AdjustableArrowCap(6, 6, true);
		    _orientPen.CustomEndCap = lineCap;
		    _orientPen.StartCap = LineCap.RoundAnchor;

		    eGraphics.FillRectangle(Brushes.DimGray, -DrawLength / 2, -DrawWidth / 2, DrawForkLen, 200);
		    eGraphics.FillRectangle(Brushes.DimGray, -DrawLength / 2, DrawWidth / 2 - 200, DrawForkLen, 200);
		    eGraphics.FillRectangle(Brushes.Gray, DrawForkLen - DrawLength / 2, -DrawWidth / 2, DrawLength, DrawWidth);

		    eGraphics.DrawEllipse(Pens.White, -160, -160, 320, 320);
		    eGraphics.DrawLine(_orientPen, 0, 0, 480, 0);
		}
        [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 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";
            }
        }

        HttpClient hc = new HttpClient() { Timeout = TimeSpan.FromMilliseconds(300) };
        [MethodMember(Name = "小车暂停", Description = "小车暂停")]
        [I18N.DocumentTranslation(Name = "EmergencyStop", Description = "EmergencyStop", 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 = "EmergencyRelease", Description = "EmergencyRelease", 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 = "ResetClumsy", Description = "ResetClumsy", locale = "en")]
        public void ResetClumsy()
        {
            Diagnosis.Post($"car{name}-{address}:ResetClumsy");
            hc.GetStringAsync($"http://{address}:8008/reset");
        }

        [MethodMember(Name = "显示车辆监控", Description = "打开车辆状态监控窗口")]
        public void ShowVehicleMonitor() => CarRemoteHelper.OpenVehicleWebPage(address);
        public void newReset(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);
            }
        }

        [MethodMember(Name = "测试", Description = "循环")]
        [I18N.DocumentTranslation(Name = "Test", Description = "Loop", locale = "en")]
        public async void Test()
        {
            var siteA = SimpleLib.GetAllSites().First(s => s.name == "A");
            var siteB = SimpleLib.GetAllSites().First(s => s.name == "B");
            while (true)
            {
                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);
            }
        }
    }
}
