using System.Drawing;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;

using Simple3.RCS;

using Simple3.RCS.CarTypes;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using StandardScene.Coders;

namespace StandardScene.CarTypes
{
    //钻车
    [TemplateTrackCoderSettings(
        priority = 18,
        useVerb = "(dst.name=='FetchSite1'||dst.name=='FetchSite2')&&(plan.action=='fetch')",
        templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 18,
        useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&(plan.action=='leave')",
        templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    //离开时钻车(后出的那台车)
    [TemplateTrackCoderSettings(
        priority = 18,
        useVerb = "dst.name=='ReadyFetchSite'&&plan.action=='leave'&&plan.Reverse==true",
        templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();" +
                            "agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait()",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    //离开时直线行走(先出的那台车)
    [TemplateTrackCoderSettings(
        priority = 18,
        useVerb = "(dst.name=='LeaveSite'||dst.name=='ReadyFetchSite')&&plan.action=='leave'&&plan.Reverse==false",
        templateString = "agv.Wait();agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

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


    //抱夹打开
    [TemplateSiteCoderSettings(
        priority = 18,
        useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&plan.action=='put'",
        templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]


    //临时夹抱动作
    [TemplateSiteCoderSettings(
        priority = 18,
        useVerb = "plan.action=='templateclamp'||plan.action=='templateput'",
        templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${plan.ClampClose});agv.Wait();agv.DriverAble();",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 2,
        useVerb = "plan.action=='fetch'",
        templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
                         "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
        blockVerb = "true",
        siteFields = typeof(MultiWheelLifterSiteFields),
        trackFields = typeof(MultiWheelLifterTrackFields),
        planFields = typeof(MultiWheelLifterPlanFields))]

    //[TemplateTrackCoderSettings(
    //    priority = 3,
    //    useVerb = "plan.action=='leave'&&(dst.name=='KeepDirectionSite1'||dst.name=='KeepDirectionSite2')",
    //    templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
    //                     "${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
    //                     "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
    //    blockVerb = "true",
    //    siteFields = typeof(MultiWheelLifterSiteFields),
    //    trackFields = typeof(MultiWheelLifterTrackFields),
    //    planFields = typeof(MultiWheelLifterPlanFields))]

    [TemplateTrackCoderSettings(
        priority = 3,
        useVerb = "plan.action=='leave'",
        templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
                         "${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
                         "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
        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 = 2000, lengthY = 2000, centerX = 0, centerY = 0)]

    [CarType(Name = "多车联动AGV")]
    public class MultiVehicleCar : GhostCar
    {
        [FieldMember] public float CarLength = 2000;
        [FieldMember] public float CarWidth = 1400;
        public static async Task<MultiVehicleCar> Create()
        {
            var car = new MultiVehicleCar()
            {
                lstatus = "连接中",
                address = "127.0.0.1",
                name = "联动AGV"
            };
            return car;
        }

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

        [MethodMember(Name = "模拟车联动初始化")]
        public void GhostMultiAgvSyncInit()
        {
            var car1 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
            while (true)
            {
                car1.ManualTeleport();
                car1.x = 1770;
                car1.y = 0;
                car1.th = 0;
                Thread.Sleep(100);
                if (LessMath.dist(car1.x, car1.y, 1770, 0) < 1 && car1.th == 0) break;
            }
            car1.NoSchedule();
            car1.siteID = -1;
            car1.tags.Clear();
            car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
            car1.fields["MultiVehicleSync"] = "True";

            var car2 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
            while (true)
            {
                //car2.ManualTeleport(-750, 0, 180);
                car2.ManualTeleport();
                car2.x = -750;
                car2.y = 0;
                car2.th = 180;
                Thread.Sleep(100);
                if (LessMath.dist(car2.x, car2.y, -750, 0) < 1 && car2.th == 180) break;
            }
            var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
            var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
            car2.TrafficReset(syncInitSite);
            car2.siteID = syncInitSite.id;
            car2.fields["MultiVehicleSync"] = "True";
        }

        [MethodMember(Name = "进入联动状态")]
        public void StartMultiAgvSync()
        {
            var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
            car1.NoSchedule();
            car1.siteID = -1;
            car1.tags.Clear();
            car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
            car1.fields["MultiVehicleSync"] = "True";

            var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
            var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
            var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
            car2.TrafficReset(syncInitSite);
            car2.siteID = syncInitSite.id;
            car2.fields["MultiVehicleSync"] = "True";
        }

        [MethodMember(Name = "退出联动状态")]
        public void ExitMultiAgvSync()
        {
            var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
            car1.fields["MultiVehicleSync"] = "False";
            car1.Reset();
            var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
            car2.fields["MultiVehicleSync"] = "False";
            car2.Reset();
        }

        //[MethodMember(Name = "返厂检修new", Description = "不再刷新小车状态，当然也不再被调度")]
        //[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 = "返厂检修";
        }
    }
}