using LessokajiWeaverUtilities.Utilities;
using Newtonsoft.Json;
using Simple3;
using Simple3.RCS;
using Simple3.RCS.CarTypes;
using Simple3.CADTools;
using Simple3.Props;
using Simple3.UI;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Extras;
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.Reflection;
using System.Threading;
using System.Threading.Tasks;
using StandardScene.Utils;
using StandardScene.Model;
using StandardScene.Coders;
using Track = SimpleCore.PropType.Track;

namespace StandardScene.CarTypes
{
    // MagneticTrackCoder 已物理迁出至 StandardScene.Magnetic.Coders（scene.mag 平台）。
    // Kiva 系字段袋（KivaCarFields/KivaSiteFields/KivaTrackFields/KivaPlanFields）下沉至
    // 基座 StandardScene.Core\CarTypes\KivaFields.cs（ArmCar 等跨平台车型继承它们）。

    //标定轮里程计
    [TemplateTrackCoderSettings(
        priority = 10,
        useVerb = "track.CalibrateWheelEncoder && track.ReverseDst != dst.id",
        blockVerb = "true",
        templateString = "agv.CalibrateWheelEncoder(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id});",
        siteFields = typeof(KivaSiteFields),
        trackFields = typeof(KivaTrackFields))]

    // 通用避障/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 = 1,
    //    templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
    //                     "${track.id},${track.Speed},${track.Reverse||(track.ReverseDst==dst.id)}," +
    //                     "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.typeInfo});",
    //    siteFields = typeof(KivaSiteFields),
    //    trackFields = typeof(KivaTrackFields),
    //    planFields = typeof(KivaPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 30,
        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(KivaSiteFields),
        trackFields = typeof(KivaTrackFields),
        planFields = typeof(KivaPlanFields))]
    //[TemplateTrackCoderSettings(
    //    priority = 2,
    //    useVerb = "track.ReverseDst==dst.id",
    //    templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id},${track.speed},0,true);",
    //    blockVerb = "true",
    //    siteFields = typeof(KivaSiteFields),
    //    trackFields = typeof(KivaTrackFields))]

    [TemplateTrackCoderSettings(
        priority = 20,
        useVerb = "plan.action=='fetch' && dst.Shelf&&plan.curSeg==plan.segN-2",
        templateString = "agv.Wait();" +
                         "agv.Fetch(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${dst.FetchSpeed},${dst.FetchLidarArea},${dst.FetchIOArea},${dst.FetchReverse}," +
                         "${dst.FetchBlindMoveDist},${dst.FetchLiftDownTarget},${dst.FetchLiftUpTarget}," +
                         "${dst.FetchUseQr},${dst.FetchQrMode},${dst.FetchIsUpQr},${dst.FetchUseDetector},${dst.FetchDetector},${dst.FetchDetectWidth},${dst.FetchDetectDepth}," +
                         "${plan.CarLength},${plan.CarWidth},${dst.FetchLeaveSrcEarly},${dst.FetchShieldObstacleDist});" +
                         "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(KivaSiteFields),
        trackFields = typeof(KivaTrackFields),
        planFields = typeof(KivaPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 20,
        useVerb = "plan.action=='put' && dst.Shelf&&plan.curSeg==plan.segN-2",
        templateString = "agv.Wait();" +
                         "agv.Put(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${dst.PutSpeed},${dst.PutLidarArea},${dst.PutIOArea},${dst.PutReverse},${dst.PutSyncRotate}," +
                         "${dst.PutBlindMoveDist},${dst.PutLiftDownTarget},${dst.PutLiftUpTarget}," +
                         "${dst.PutUseQr},${dst.PutQrMode},${dst.PutIsUpQr},${dst.PutUseDetector},${dst.PutDetector}," +
                         "${dst.PutDetectWidth},${dst.PutDetectDepth},${dst.PutLeaveSrcEarly},${dst.PutShieldObstacleDist});" +
                         "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(KivaSiteFields),
        trackFields = typeof(KivaTrackFields),
        planFields = typeof(KivaPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 22,
        useVerb = "src.Shelf&&plan.curSeg==1",
        templateString = "agv.LeaveShelf(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${src.LeaveShelfSpeed},${src.LeaveShelfLidarArea},${src.LeaveShelfIOArea},${src.LeaveShelfReverse},${src.LeaveShelfSyncRotate}," +
                         "${src.LeaveShelfBlindMoveDist},${src.LeaveShelfLiftDownTarget},${src.LeaveShelfRecoveryObstacleDist},);",
        blockVerb = "true",
        siteFields = typeof(KivaSiteFields),
        trackFields = typeof(KivaTrackFields),
        planFields = typeof(KivaPlanFields))]

    [TemplateSiteCoderSettings(
        priority = 25,
        useVerb = "plan.action=='fetch'&& plan.segN == 1 &&dst.Shelf",
        templateString =
            "agv.FetchInPlace(${dst.FetchUseQr},${dst.FetchQrMode},${dst.FetchIsUpQr},${dst.FetchLiftUpTarget},${plan.CarLength},${plan.CarWidth});" +
            "agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(KivaSiteFields),
        trackFields = typeof(KivaTrackFields),
        planFields = typeof(KivaPlanFields))]
    //[TemplateSiteCoderSettings(
    //    priority = 100,
    //    useVerb = "plan.curSeg == 0",
    //    templateString = "agv.StartingMission();agv.Wait();",
    //    blockVerb = "false",
    //    siteFields = typeof(KivaSiteFields),
    //    trackFields = typeof(KivaTrackFields),
    //    planFields = typeof(KivaPlanFields))]



    [ProgramTrackCoderSettings(priority = 5, program = typeof(KivaCarTrackCoder))]
    [ProgramTrackCoderSettings(priority = 17, program = typeof(LidarAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceDistanceCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
    [ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
    [CarType(Name = "Kiva", editor = typeof(Kiva))]
    [I18N.DocumentTranslation(Name = "Kiva", locale = "en")]
    [EnvelopConfig(centerX = 0, centerY = 0, lengthX = 950, lengthY = 650)]
    public class Kiva : GhostCar, IScriptErrorRecoverable
    {
        /// <summary>脚本异常自恢复（AbstractLoopMission 经 IScriptErrorRecoverable 调用），转调 newReset。</summary>
        public void RecoverReset(int resetSiteId = 0) => newReset(resetSiteId);

        [FieldMember] public float CarLength = 2000;
        [FieldMember] public float CarWidth = 1400;
        public class RotateSiteEnvelope : SiteEnvelopeDefinition<Kiva>
        {
            public override bool Use() => true;

            public override void Prompt()
            {
                if (curSeg == 0)
                {
                    return;
                }

                if (plan.codeArr == null)
                {
                    return;
                }

                var carConfig = usingCar.GetType().GetCustomAttribute<EnvelopConfigAttribute>();
                var carLength = carConfig.lengthX;
                var carWidth = carConfig.lengthY;
                var code = plan.codeArr[curSeg - 1];
                if (code.Contains("Fetch"))
                {
                    var lengthX = plan.fields.TryGetValue("MaterialLength", out var materialLength)
                        ? (int.TryParse(materialLength, out var lengthResult) ? lengthResult > 0 ? lengthResult : carLength : carLength)
                        : carLength;
                    var lengthY = plan.fields.TryGetValue("MaterialWidth", out var materialWidth)
                        ? (int.TryParse(materialWidth, out var widthResult) ? widthResult > 0 ? widthResult : carWidth : carWidth)
                        : carWidth;
                    reshape(lengthX, lengthY, 0, 0);
                }
                else if (code.Contains("Put"))
                {
                    reshape(carLength, carWidth, 0, 0);
                }
            }

            public override bool Block() => false;

            public override int priority => 1;
        }

        public static async Task<Kiva> Create()
        {
            var kiva = new Kiva()
            {
                lstatus = "连接中",
                address = "127.0.0.1",
                name = $"Kiva",
                haveCoordination = true,
                speed = 1,
            };
            return kiva;
        }

        private const float DrawWidth = 950, DrawLength = 650;
        private readonly Pen orientPen = new Pen(Color.White, 3);
        private bool twinkle = false;

        protected override void draw(Graphics eGraphics)
        {
            try
            {
                var DrawWidth = CarLength;
                var DrawLength = CarWidth;
                var lineCap = new AdjustableArrowCap(5, 5, true);
                orientPen.CustomEndCap = lineCap;
                orientPen.StartCap = LineCap.RoundAnchor;
                var alarmLevel = Commons.GetCarStatus(this, "AlarmLevel");
                var soc = Commons.CarValue((Car)this, "Soc");
                var electricCurrent = Commons.CarValue((Car)this, "ElectricCurrent");
                var thresSpeed = Commons.CarValue(this, "ThresSpeed");
                LadderLogic.FlipFlop(ref twinkle, 500, true, false);
                Rectangle destRect = new Rectangle(
                    (int)((int)(-DrawWidth / 2)),
                    (int)((int)(-DrawLength / 2)),
                    (int)((int)DrawWidth),
                    (int)((int)DrawLength)
                );
                if (int.TryParse(alarmLevel, out var alarm) && alarm > 0)
                {
                    eGraphics.FillRectangle(twinkle ? Brushes.DarkRed : Brushes.Green, destRect);
                }
                else if (alarmLevel == "-1" || this.siteID == -1)
                    eGraphics.FillRectangle(Brushes.LightSeaGreen, destRect);
                else if (electricCurrent > 1)
                    eGraphics.FillRectangle(Brushes.YellowGreen, destRect);
                else if (thresSpeed <= 0)
                {
                    eGraphics.FillRectangle(twinkle ? Brushes.YellowGreen : Brushes.Green, destRect);
                }
                else if (soc < 25)
                {
                    eGraphics.FillRectangle(twinkle ? Brushes.DarkRed : Brushes.LightSeaGreen, destRect);
                }
                else
                    eGraphics.FillRectangle(Brushes.Green, destRect);

                eGraphics.DrawEllipse(Pens.Yellow, -160, -160, 320, 320);
                eGraphics.DrawLine(orientPen, 0, 0, 480, 0);

            }
            catch (Exception e)
            {
                Diagnosis.Post("绘制小车异常" + ExceptionFormatter.FormatEx(e), "绘制小车异常", true);
            }
        }


        [MethodMember(Name = "进入小车管理界面", Description = "在浏览器中打开小车 Web 页面")]
        public void OpenVehicleManagementPage()
        {
            CarRemoteHelper.OpenVehicleWebPage(address);
        }

        [MethodMember(Name = "显示车辆监控", Description = "打开车辆状态监控窗口")]
        public void ShowVehicleMonitor() => CarRemoteHelper.OpenVehicleWebPage(address);

        [MethodMember(Name = "模拟电量", Description = "/")]
        public void SimarSoc()
        {
            if (InputBox.ShowDialog("模拟电量", "模拟电量Soc", "0", InputBox.Buttons.OkCancel) == Simple3.DialogResult.Cancel) return;
            string value = InputBox.ResultValue;
            try
            {
                status.enums["Soc"] = (60 + int.Parse(value)).ToString();
                status.enums["Voltage"] = (50 + int.Parse(value)).ToString();
                status.enums["ElectricCurrent"] = (40 + int.Parse(value)).ToString();

            }
            catch (Exception)
            {

                
            }
     

        }



        [MethodMember(Name = "立即强制结束new", Description = "立刻终止小车运行，重启C，并让小车下线",
            RequiresPlatformConfirm = true,
            ConfirmMessage = "确定强制结束该车上的作业并下线？")]
        [I18N.DocumentTranslation(Name = "Force Stop new", Description = "Stop car & reset clumsy", locale = "en")]
        public override void ForceStop()
        {
            try
            {
                NoSchedule(true);
                siteID = -1;
                Commons.ClearTags(tags);
                new Thread(() =>
                {
                    try
                    {
                        AppendDebug("restarting clumsy");
                        Get("reset");
                        AppendDebug("wait for any pending task to flush.");
                        try
                        {
                            status.programs.task.Wait();
                        }
                        catch
                        {
                        }

                        AppendDebug("Clumsy Restarted");
                        NoSchedule(true);
                        siteID = -1;
                        Commons.DeleteTag(tags, "occupied");
                    }
                    catch (Exception ex)
                    {
                        FairyUiHelper.Alert("错误",
                            $@"未能立即强制结束小车{name}({id})，原因：{ExceptionFormatter.FormatEx(ex)}");
                    }
                })
                {
                    Name = $"ForceStop:{name}({id})"
                }.Start();
            }
            catch (Exception e)
            {
                Console.WriteLine(e);
                throw;
            }
        }

        [MethodMember(Name = "返厂检修new", Description = "不再刷新小车状态，当然也不再被调度",
            RequiresPlatformConfirm = true,
            ConfirmMessage = "小车确实已经离开路径了么？确认返厂检修？")]
        [I18N.DocumentTranslation(Name = "Blown new", Description = "Car no longer refresh status and scheduling", locale = "en")]
        public new void Blown()
        {
            AppendDebug("ui-blown");
            Diagnosis.Post($"Car {name}({id}) blown");
            NoSchedule();
            siteID = -1;
            tags.Clear();
            status.usage.AddUsage(
                "base",
                new CarUsage.CarUsageInfo { scheduling = false, refreshing = false }
            );
            lstatus = "返厂检修";
        }

        [MethodMember(Name = "现场检修new ", Description = "让小车不再被调度，但仍然刷新状态，并且当前点不再使用")]
        [I18N.DocumentTranslation(Name = "Repair new", Description = "Car no longer scheduling but still refresh status", locale = "en")]
        public new void Repair()
        {
            AppendDebug("ui-repair");
            Diagnosis.Post($"Repair Car {name}({id})");
            NoSchedule();
            siteID = -1;
            Commons.DeleteTag(tags, "occupied");

            lstatus = "现场检修";
        }

        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);
            }
        }

        HttpClient hc = new HttpClient() { Timeout = TimeSpan.FromMilliseconds(300) };
        // [MethodMember(name = "小车暂停", desc = "小车暂停")]
        public void EmergencyStop(string reason)
        {
            Diagnosis.Post($"car{name}:EmergencyStop");
            hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=1");
        }

        //[MethodMember(name = "小车暂停恢复", desc = "小车暂停恢复")]
        public void EmergencyRelease()
        {
            Diagnosis.Post($"car{name}:EmergencyStop release");
            hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=0");
        }
        //[MethodMember(name = "小车重启C", desc = "小车重启Clumsy")]
        public void ResetClumsy()
        {
            Diagnosis.Post($"car{name}-{address}:ResetClumsy");
            hc.GetStringAsync($"http://{address}:8008/reset");
        }
    }

    /// <summary>
    /// 专门用于控制turn的问题
    /// </summary>
    public class KivaCarTrackCoder : ITrackCoder
    {
        public bool toBlock()
        {
            return false;
        }
        /// <summary>
        /// 用于记录上一路径的角度
        /// </summary>
        public static double lastTh = -1;

        public bool Code(SegmentPlan plan, Track track, Site src, Site dst, int i)
        {
            var trackInfo = StringDictConvert<KivaTrackFields>.Convert(track.fields);
            var srcInfo = StringDictConvert<KivaSiteFields>.Convert(src.fields);
            Console.WriteLine($@"srcInfo.Turn:{srcInfo.Turn},i={1}");
            if (!srcInfo.Turn) //不包含旋转指令，直接跳过
            {
                return true;
            }
            if (i == 1)
            {
                return true;
            }
            var lastSite = plan.segments[i - 3] as Site;
            if (lastSite == null)
            {
                return true;
            }
            var lastPathDir = Math.Round(Math.Atan2(src.y - lastSite.y, src.x - lastSite.x) / Math.PI * 2) * 90;
            var curPathDir = Math.Round(Math.Atan2(dst.y - src.y, dst.x - src.x) / Math.PI * 2) * 90;
            if (Math.Abs(curPathDir - lastPathDir) < 45)
            {
                return true;
            }
            var angleTarget = trackInfo.ReverseDst == dst.id || trackInfo.Reverse ? curPathDir + 180 : curPathDir;

            plan.codeArr[i] += $"agv.Wait(); agv.SyncRotate({angleTarget}); agv.Wait(); ";

            return true;
        }
    }

}