using AMRScene1;
using Simple3.Rendering;
using Simple3.RCS;
using Simple3.RCS.CarTypes;
using Simple3.CADTools;
using Simple3.Props;
using Simple3.UI;
using SimpleCore;
using SimpleCore.BasicProps;
using SimpleCore.Compiler;
using SimpleCore.Extras;
using SimpleCore.Library;
using SimpleCore.PropType;
using SimpleCore.Traffic;
using StandardScene.CarTypes;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Numerics;
using System.Runtime.InteropServices.ComTypes;
using System.Threading;
using System.Threading.Tasks;
using StandardScene.Utils;
using Simple3;

namespace AMRScene1
{
    class DummyCarTrackField
    {
        public float Speed = -1;
        public bool Reverse = false;
        public int ReverseDst = -1;
    }
    class DummyCarSiteField
    {
        public bool Shelf = false;
    }
    class DummyCarPlanField
    {
        public string action = "/";
    }

    [TemplateTrackCoderSettings(
        priority = 0,
        templateString = "agv.Go(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," +
						 "${track.Speed},${track.Reverse || track.ReverseDst == dst.id},${track.typeInfo});",
        trackFields = typeof(DummyCarTrackField),
        siteFields = typeof(DummyCarSiteField))]


    [TemplateTrackCoderSettings(
        priority = 5,
        useVerb = "plan.action=='put'&& dst.Shelf ",
        templateString = "agv.Put(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," +
                         "${track.Speed},${track.Reverse || track.ReverseDst == dst.id},${track.typeInfo});",
        blockVerb = "true",
          trackFields = typeof(DummyCarTrackField),
        siteFields = typeof(DummyCarSiteField),
        planFields = typeof(DummyCarPlanField))]
    [TemplateTrackCoderSettings(
        priority = 5,
        useVerb = "plan.action=='fetch'&& dst.Shelf ",
        templateString = "agv.Fetch(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," +
                         "${track.Speed},${track.Reverse || track.ReverseDst == dst.id},${track.typeInfo});",
        blockVerb = "true",
         trackFields = typeof(DummyCarTrackField),
         siteFields = typeof(DummyCarSiteField),
        planFields = typeof(DummyCarPlanField))]

    [CarType(Name = "模拟车-包络")]
    [EnvelopConfig(centerX = 0, centerY = 0, lengthX = 1000, lengthY = 700)]
    public class DummyCar:Car
    {
        [FieldMember] public string conf = "realistic";
        public class RotateSiteEnvelope : SiteEnvelopeDefinition<DummyCar>
        {
            public override bool Use() => true;

            public override void Prompt()
            {
                if (curSeg == 0|| plan.codeArr==null)
                {
                    return;
                }
                var code = plan.codeArr[curSeg - 1];
                if (code.Contains("Fetch"))
                {
                    reshape(1550, 1150, 0, 0);
                }
                else if (code.Contains("Put"))
                {
                    reshape(1000,700, 0, 0);
                }
            }

            public override bool Block() => false;

            public override int priority => -1;
        }
        public virtual AGV getAGV(int id) => new AGV(id);
        public class DummyCarStatus : CarStatus
        {
            public string simStat = "/";
            public double simulatedDistance = 0;
        }
        public override CarStatus status { get; set; } = new DummyCarStatus();

        public static async Task<DummyCar> Create() // boilerplate
        {
            return new()
            {
                lstatus = "正常",
                name = $"模拟车",
                haveCoordination = true
            };
        }

        public class AGV: AGVInterface
        {
            public DummyCar car;
            
            public AGV(int id)
            {
                car = (DummyCar) SimpleLib.GetCar(id);
            }
            public void Sleep(int millis)
            {
                Thread.Sleep(millis);
            }

            protected class Segment
            {
                public int trackID, srcID, dstID;
                public double srcX, srcY, dstX, dstY;
            }

                
            protected List<Segment> routeCache = []; // use circular list.
            
            protected virtual Task following(double dstX, double dstY, double speed, bool reverse, params float[] typeInfo)
            {
                var promise = new TaskCompletionSource<int>();
                int i = 0;
                IEnumerable<bool> iterActions()
                {
	                if (typeInfo.Length == 0 || (int)typeInfo[0] == 0) // line path
	                {
                        // car always run to completion.
		                while ((Math.Abs(car.x - dstX) > 10 || Math.Abs(car.y - dstY) > 10))// && car.running)
		                {
			                if (car.fields.TryGetValue("speed", out var sv))
				                speed = float.Parse(sv);
			                var dx = dstX - car.x;
			                var dy = dstY - car.y;
			                var d = Math.Sqrt(dx * dx + dy * dy);
			                var ed = (DateTime.Now - car.lastRefresh).TotalSeconds * speed;
			                if (ed > d)
			                {
				                car.th = (float)(Math.Atan2(dstY - car.y, dstX - car.x) / Math.PI * 180 + (reverse ? 180 : 0));
				                car.x = (float)dstX;
				                car.y = (float)dstY;
				                break;
			                }

			                dx = (float)(dx / d * ed);
			                dy = (float)(dy / d * ed);
			                car.th = (float)(Math.Atan2(dstY - car.y, dstX - car.x) / Math.PI * 180 + (reverse ? 180 : 0));
			                car.x += (float)dx;
			                car.y += (float)dy;
			                ((DummyCarStatus)car.status).simStat = $"following-iter-{i++}";
			                //Console.WriteLine($"move one frame:{car.x},{car.y},{car.speed}");
			                yield return true;
		                }
					}
                    else if ((int)typeInfo[0] == 1) // circularArc path
	                {
		                var center = new Vector2(typeInfo[1], typeInfo[2]);
                        var radius = typeInfo[3];
                        var angleStart = typeInfo[4];
                        var angleEnd = typeInfo[5];
                        
                        var dstAngle = (float)(Math.Atan2(dstY - center.Y, dstX - center.X) / Math.PI * 180);
                        var dir = 1; // counter-clockwise
                        if (Math.Abs(LessMath.thDiff(angleStart, dstAngle)) <
                            Math.Abs(LessMath.thDiff(angleEnd, dstAngle))) dir = -1;

                        while (true)//(car.running)
		                {
			                var pTh = (float)(Math.Atan2(car.y - center.Y, car.x - center.X) / Math.PI * 180);
			                var ed = (DateTime.Now - car.lastRefresh).TotalSeconds * speed;

                            var dRadius = Math.Abs(Vector2.Distance(center, new Vector2(car.x, car.y)) - radius);
							if (dRadius > 10)
                            {
								// first go to arc
								var targetX = (float)(center.X + radius * Math.Cos(pTh / 180 * Math.PI));
								var targetY = (float)(center.Y + radius * Math.Sin(pTh / 180 * Math.PI));
								car.th = (float)(Math.Atan2(targetY - car.y, targetX - car.x) / Math.PI * 180 + (reverse ? 180 : 0));

								if (ed > dRadius)
                                {
	                                car.x = targetX;
                                    car.y = targetY;
                                }

                                car.x += (float)((targetX - car.x) / dRadius * ed);
                                car.y += (float)((targetY - car.y) / dRadius * ed);
                            }
							else
							{
								// then go along arc
								if (Math.Abs(LessMath.thDiff(dstAngle, pTh)) < 0.01) break;
                                var eth = (float)(ed / radius / Math.PI * 180);
                                var dth = LessMath.thDiff(dstAngle, pTh);
                                if (Math.Abs(eth) > Math.Abs(dth))
                                {
                                    car.th = dstAngle + 90 * dir + (reverse ? 180 : 0);
                                    car.x = (float)(center.X + radius * Math.Cos(dstAngle / 180 * Math.PI));
                                    car.y = (float)(center.Y + radius * Math.Sin(dstAngle / 180 * Math.PI));
                                    break;
                                }

                                var newTh = pTh + eth * dir;
                                car.th = newTh + 90 * dir + (reverse ? 180 : 0);
                                car.x = (float)(center.X + radius * Math.Cos(newTh / 180 * Math.PI));
                                car.y = (float)(center.Y + radius * Math.Sin(newTh / 180 * Math.PI));
							}

							((DummyCarStatus)car.status).simStat = $"following-iter-{i++}";
							yield return true;
		                }
	                }

	                ((DummyCarStatus)car.status).simStat = $"following-done";
                    car.moveAction = null;
                    // Console.WriteLine($"** following to {dstX},{dstY} done");
                    promise.SetResult(1);
                    // Task.Run(()=>promise.SetResult(1));
                }

                car.moveAction = iterActions().GetEnumerator();
                return promise.Task;
            }

            /// <summary>
            /// 模拟车有TryLock的agv函数，进入函数需先调用AddRoute()
            /// </summary>
            /// <param name="srcid"></param>
            /// <param name="dstid"></param>
            /// <param name="trackid"></param>
            public void AddRoute(int srcid, int dstid, int trackid)
            {
	            var route_id = routeCache.Count;
	            lock (routeCache)
		            routeCache.Add(new Segment() { trackID = trackid, srcID = srcid, dstID = dstid });
	            if (car.route.Length == 0)
		            car.route = [srcid];
	            car.route = car.route.Append(dstid).ToArray();
			}

            private static ConcurrentDictionary<AbstractCar, Task> _taskmap = new();

            public Task mvmtTsk
            {
                get
                {
                    if (_taskmap.TryGetValue(car, out var tsk)) return tsk;
                    return _taskmap[car] = Task.CompletedTask;
                }
                set
                {
                    _taskmap[car] = value;
                }
            }

            // trackType: 0 line, 1 circularArc
            public void Go(double srcX, double srcY, int srcid, double dstX, double dstY, int dstid, int trackid,
	            int speed = -1, bool reverse = false, params float[] trackTypeInfo)
            {
	            // car.AppendDebug($"Go() track typeInfo:{string.Join(",", trackTypeInfo)}");

                AddRoute(srcid, dstid, trackid);
                // ReSharper disable once PossiblyMistakenUseOfParamsMethod
                var promise = new TaskCompletionSource<int>();
                Queue(async () =>
                {
                    while (!TryLock(dstid))
                        await Task.Delay(100);
                    // Console.WriteLine($"** ready to go to {dstid}({dstX},{dstY})");
                    mvmtTsk = mvmtTsk.ContinueWith(async _ =>
                    {
                        //Console.WriteLine($"** begin to go to {dstid}({dstX},{dstY})");
                        await following(dstX, dstY, speed > 0 ? speed : car.speed, reverse, trackTypeInfo);
                        ((DummyCarStatus)car.status).simulatedDistance += LessMath.dist(srcX, srcY, dstX, dstY);
                        promise.SetResult(1);
                        // Task.Run(() => promise.SetResult(1)); // following finished.
                    }).Unwrap();
                    //Console.WriteLine($"** issued to go to {dstid}({dstX},{dstY})");
                }, async () =>
                {
                    await promise.Task;
                    //Console.WriteLine($"** done go to {dstid}({dstX},{dstY})");
                    Leave(srcid);
                });
            }
            public void Fetch(double srcX, double srcY, int srcid, double dstX, double dstY, int dstid, int trackid,
               int speed = -1, bool reverse = false, params float[] trackTypeInfo)
            {
                // car.AppendDebug($"Go() track typeInfo:{string.Join(",", trackTypeInfo)}");

                AddRoute(srcid, dstid, trackid);
                // ReSharper disable once PossiblyMistakenUseOfParamsMethod
                var promise = new TaskCompletionSource<int>();
                Queue(async () =>
                {
                    while (!TryLock(dstid))
                        await Task.Delay(100);
                    // Console.WriteLine($"** ready to go to {dstid}({dstX},{dstY})");
                    mvmtTsk = mvmtTsk.ContinueWith(async _ =>
                    {
                        //Console.WriteLine($"** begin to go to {dstid}({dstX},{dstY})");
                        await following(dstX, dstY, speed > 0 ? speed : car.speed, reverse, trackTypeInfo);
                        ((DummyCarStatus)car.status).simulatedDistance += LessMath.dist(srcX, srcY, dstX, dstY);
                        promise.SetResult(1);
                        // Task.Run(() => promise.SetResult(1)); // following finished.
                    }).Unwrap();
                    //Console.WriteLine($"** issued to go to {dstid}({dstX},{dstY})");
                }, async () =>
                {
                    await promise.Task;
                    //Console.WriteLine($"** done go to {dstid}({dstX},{dstY})");
                    Leave(srcid);
                });
            }
            public void Put(double srcX, double srcY, int srcid, double dstX, double dstY, int dstid, int trackid,
               int speed = -1, bool reverse = false, params float[] trackTypeInfo)
            {
                // car.AppendDebug($"Go() track typeInfo:{string.Join(",", trackTypeInfo)}");

                AddRoute(srcid, dstid, trackid);
                // ReSharper disable once PossiblyMistakenUseOfParamsMethod
                var promise = new TaskCompletionSource<int>();
                Queue(async () =>
                {
                    while (!TryLock(dstid))
                        await Task.Delay(100);
                    // Console.WriteLine($"** ready to go to {dstid}({dstX},{dstY})");
                    mvmtTsk = mvmtTsk.ContinueWith(async _ =>
                    {
                        //Console.WriteLine($"** begin to go to {dstid}({dstX},{dstY})");
                        await following(dstX, dstY, speed > 0 ? speed : car.speed, reverse, trackTypeInfo);
                        ((DummyCarStatus)car.status).simulatedDistance += LessMath.dist(srcX, srcY, dstX, dstY);
                        promise.SetResult(1);
                        // Task.Run(() => promise.SetResult(1)); // following finished.
                    }).Unwrap();
                    //Console.WriteLine($"** issued to go to {dstid}({dstX},{dstY})");
                }, async () =>
                {
                    await promise.Task;
                    //Console.WriteLine($"** done go to {dstid}({dstX},{dstY})");
                    Leave(srcid);
                });
            }


            public void Nop(int srcid, int dstid, int trackid)
            {
                var route_id = routeCache.Count;
                lock (routeCache)
                    routeCache.Add(new Segment() { trackID = trackid, srcID = srcid, dstID = dstid });
                if (car.route.Length == 0)
                    car.route = [srcid];
                car.route = car.route.Append(dstid).ToArray();
                
                Queue(async () =>
                {
                    while (!TryLock(dstid))
                        await Task.Delay(5);
                }, async () =>
                {
                    Leave(srcid);
                });
            }
            
            public override bool TryLock(int siteId)
            {
                if (car.status.holdingLocks.Last() == siteId) return true;
                if (!car.status.usage.Get().scheduling) 
                    throw new Exception("abandoned");
                if (!car.status.usage.Get().scheduling)
                    throw new Exception("Stopped");
                lock (routeCache)
                    return TrafficControl.TryLock(car, siteId);
            }

            public override void Leave(int siteID)
            {
                TrafficControl.Leave(car, siteID);
            }
        }

        private bool running = false;


        public override async Task actualSendScript(string script)
        {
            if (running)
                throw new Exception($"dummy car {id} already running script");
            running = true;

            try
            {
                route = new int[0];
                AppendDebug($"dummy car {id} use jint for simulation");
                currentAgv = getAGV(id);
                var tcs = new TaskCompletionSource<int>();
                new Thread(() => {
                        try
                        {
                            SelfEvaluating(currentAgv, script);
                            tcs.SetResult(1);
                        }
                        catch (Exception ex)
                        {
                            tcs.SetException(ex);
                        }}
                ) { Name = $"eva_{name}({id}):{status.programs.now.name}" }.Start();

                await tcs.Task;
                await currentAgv.WaitAsync();

                Console.WriteLine($"{name}({id}) self evaluating script completed");
            }
            catch (Exception ex)
            {
                AppendDebug($"simulation error:{ExceptionFormatter.FormatEx(ex)}");
                Console.WriteLine($"* {id} evaluating script failed, ex:{ExceptionFormatter.FormatEx(ex)}");
                running = false;
                throw;
            }

            running = false;
        }
        

        

        [MethodMember(Name = "设定路线", Description = "从当前位置出发，不停地走路线")]
        public async void Path()
        {

            Site starting = null;
            float dist = float.MaxValue;
            foreach (var site in SimpleLib.GetAllSites())
            {
                var d = LessMath.dist(site.x, site.y, x, y);
                if (d < dist)
                {
                    dist = (float)d;
                    starting = site;
                }
            }

            var p = new Pen(Color.Red, 3);
            var lineCap =
                new AdjustableArrowCap(6, 6, true);
            p.CustomEndCap = lineCap;
            p.StartCap = LineCap.RoundAnchor;

            var segments = new List<Prop>();

            var painter = SimpleMonitor.getPainter("flat-goto");
            painter.clear();

            Dictionary<int, Action> triggers = new();
            CarProgram program = null;
            try
            {
                while (true)
                {
                    var pt = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                    var dstSite = (UISite)SimpleLib.GetSite(pt.site);

                    var plan = new SegmentPlan() { usingCar = this };
                    plan.fields["allow_destination_on_route"] = "true";
                    if (segments.Count > 0)
                        plan.fields["useMustCanGo"] = "false";
                    plan.FindRoute((UISite)starting, dstSite,false);

                    segments = segments.Concat(plan.segments.Skip(segments.Count > 0 ? 1 : 0)).ToList();
                    triggers[dstSite.id]= () => { Console.WriteLine($"VDASegment end {dstSite.id} reached"); };
                    Console.WriteLine($"goto site {pt.site}");
                    starting = dstSite;

                    painter.clear();
                    for (int i = 0; i + 2 < segments.Count; i += 2)
                    {
                        var st = (UISite)segments[i];
                        var ed = (UISite)segments[i + 2];
                        painter.drawLine(p, st.x, st.y, ed.x, ed.y);
                    }

                    if (program == null) program = plan.Compile("walk", false);
                    else
                        program.Append(plan, (go) =>
                        {
                            Task.Run(() =>
                            {
                                FairyUiHelper.Alert("提示", "go?");
                                go();
                            });
                        });
                }
            }
            catch (TaskCanceledException ex)
            {
                Console.WriteLine("end");
            }

            Console.WriteLine($"issue command");
            _ = Task.Factory.StartNew(() =>
            {
                Thread.Sleep(3000);
                painter.clear();
            });
            program.Forecast();
            // plan.segments = segments;
            // if (plan.segments.Count == 0)
            // {
            //     MessageBox.Show("未生成路径！");
            //     return;
            // }
            //
            // var program = plan.Compile("walk");
            foreach (var kvp in triggers)
            {
                program.TriggerOnSite(kvp.Key, kvp.Value);
                // program.AwaitOnSite(kvp.Key, go =>
                // {
                //     kvp.Value();
                //
                //     Task.Run(() =>
                //     {
                //         MessageBox.Show("go?");
                //         go();
                //     });
                // });
            }
            Console.WriteLine(program.script);
            G.pushStatus($"向AGV:{name}({id})下发行走任务");
            var tsk = program.Queue();
            _ = Task.Factory.StartNew(() =>
            {
                G.pushStatus($"AGV:{name}({id})开始执行任务");
                tsk.Wait();
                G.pushStatus($"AGV:{name}({id})执行任务完毕");
            });
            siteID = segments.Last().id;
        }

        [MethodMember(Name = "去某地",Description="从当前位置找一条路径去某站点")]
        public void Goto()
        {
            async void goto_fun(){
                if (GetLastSite() == -1)
                {
                    G.pushStatus("Car not initialized to any site!");
                    return;
                }
                try
                {
                    var pt = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                    Console.WriteLine($"goto site {pt.site}");

                    Site site1 = SimpleLib.GetSite(GetLastSite());

                    var plan = new SegmentPlan() { usingCar = this };
                    plan.FindRoute(site1, SimpleLib.GetSite(pt.site), findLoop:false);
                    // var code = $"{plan.Code()};agv.Wait();";
                    // Console.WriteLine(code);
                    await plan.Compile($"goto_{pt.site}").Queue();
                    Console.WriteLine("Done");
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ExceptionFormatter.FormatEx(ex));
                }
            }
            Task.Run(goto_fun);
        }
        [MethodMember(Name = "增加小车enums", Description = "双击添加标签")]
        public void AddEnums()
        {

            if (InputBox.ShowDialog("请输入小车enums\" enums:value") != Simple3.DialogResult.OK) return;
            string tag = InputBox.ResultValue;
            if (tag.Contains("："))
            {
                FairyUiHelper.Alert("提示", "需要切换英文输入法输入:");
                return;
            }
            if (!string.IsNullOrEmpty(tag) && tag.Contains(":"))
            {
                string[] tagValue = tag.Split(':');
                this.status.enums[tagValue[0]] = tagValue[1];

            }

        }

        [MethodMember(Name = "劫持", Description = "指定一个起点和终点，当小车到达起点后劫持小车至终点")]
        public void Hijack()
        {
            async void hijiack_fun()
            {
                G.pushStatus("Select starting point");
                var pt1 = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                G.pushStatus("Select ending point");
                var pt2 = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                if (GetLastSite() == -1)
                {
                    G.pushStatus("Car not initialized to any site!");
                    return;
                }
                try
                {
                    var plan = new SegmentPlan() { usingCar = this };
                    plan.FindRoute(SimpleLib.GetSite(pt1.site), SimpleLib.GetSite(pt2.site), findLoop: false);
                    await plan.Compile($"hijack", forecast:false).TryHijack().Queue();
                    Console.WriteLine("Done");
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ExceptionFormatter.FormatEx(ex));
                }
            }
            Task.Run(hijiack_fun);
        }

        [MethodMember(Name = "走到指定位置并设置Escape", Description = "点一个位置，再点一个位置")]
        public void GoEscaped()
        {
            async void goto_fun()
            {
                if (GetLastSite() == -1)
                {
                    G.pushStatus("Car not initialized to any site!");
                    return;
                }
                try
                {
                    var pt = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                    var ptesc = await Startup.UI.getPoint(new UIOps.getPointOptions() { site = true });
                    Console.WriteLine($"goto site {pt.site}, ptesc={ptesc.site}");

                    var planRoute = new SegmentPlan() { usingCar = this };
                    planRoute.fields["forbid_cross"] = "false";
                    planRoute.FindRoute(SimpleLib.GetSite(GetLastSite()), SimpleLib.GetSite(pt.site), findLoop: false);

                    var planEsc = new SegmentPlan() { usingCar = this };
                    planEsc.fields["forbid_cross"] = "false";
                    planEsc.FindRoute(SimpleLib.GetSite(pt.site), SimpleLib.GetSite(ptesc.site), findLoop: false);

                    await planRoute.Compile($"goto_{pt.site}", false).Forecast(planEsc).Queue();
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ExceptionFormatter.FormatEx(ex));
                }
            }
            Task.Run(goto_fun);
        }

        [MethodMember(Name = "设置角度", Description = "指定模拟车的朝向角度")]
        public void SetAngle()
        {
            if (InputBox.ShowDialog("输入角度", "模拟车设置角度", "0", InputBox.Buttons.OkCancel) == Simple3.DialogResult.Cancel) return;
            if (!float.TryParse(InputBox.ResultValue, out var angle)) return;
            th = angle;
        }

        protected override void draw(Graphics eGraphics)
        {
            eGraphics.FillRectangle(Brushes.Gray, -320, -240 , 640 , 480 );
            eGraphics.DrawRectangle(Pens.White, -320, -240 , 640 , 480 );
            eGraphics.DrawLine(Pens.White, 0, -240, 320, 0);
            eGraphics.DrawLine(Pens.White, 0, 240 , 320 , 0);
        }

        public DateTime lastRefresh =DateTime.MinValue;


        protected IEnumerator<bool> moveAction;
        private AGV currentAgv;
        
        
        public override void keepAlive()
        {
            if (lastRefresh == DateTime.MinValue)
            {
                lastRefresh = DateTime.Now;
                return;
            }

            if (moveAction != null)
                moveAction.MoveNext();

            haveCoordination = true;
            lastRefresh = DateTime.Now;
        }
    }
}
