using HalconDotNet;
using Rs.Camera;
using Rs.Controls;
using Rs.Framework;
using Rs.Motion;
using Rs.MotionPlat.Commom;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Rs.MotionPlat.Flow.Space
{
    public enum ELoadAndUnloadTestStep
    {
        到取料位上方,
        等待到取料位上方,
        到取料位下方,
        等待到取料位下方,
        开真空,
        取料真空检测,
        取料完成抬起,
        等待取料完成抬起,
        到下相机拍照起始位,
        等待到下相机拍照起始位,
        到下相机拍照结束位,
        等待到下相机拍照结束位,
        等待相机拍照完成,
        等待视觉处理结果,
        到放料位上方,
        等待到放料位上方,
        到放料位下方,
        等待到放料位下方,
        放料完成抬起,
        等待放料完成抬起,
        到等待命令位,
        等待到等待命令位,
        保压上料,
        等待保压上料完成,
        保压下料,
        等待保压下料完成
    }

    public class LoadAndUnloadTest:BaseFlow
    {
        private bool grabOK = false;
        private static LoadAndUnloadTest instance;

        public event Action<HObject[], List<MatchResult>> CameraOkEvent;
        public static LoadAndUnloadTest Instance
        {
            get
            {
                if (instance == null)
                {
                    instance = new LoadAndUnloadTest();
                }
                return instance;
            }
        }
        private LoadAndUnloadTest()
        {
            //HikCamera.Instance.GrabOkEvent += (camIndex,img) => {
            //    grabOK = true;
            //};
        }

        /// <summary>
        /// 当前取料穴位
        /// </summary>
        private ushort CurFetchSlotIndex = 1;
        /// <summary>
        /// 当前组装穴位
        /// </summary>
        private ushort CurAssembleSlotInex = 1;
        /// <summary>
        /// 吸嘴索引
        /// </summary>
        private ushort NozzleIndex = 1;

        bool[] stop = new bool[30];

        ELoadAndUnloadTestStep flowStep = ELoadAndUnloadTestStep.到取料位上方;
        HObject[] imgs = new HObject[2];
        List<MatchResult> mrs = new List<MatchResult>();
        int FetchNum = 0;//取料次数
        bool bFetchBack = false;
        double targetX = 0.0;
        double targetY = 0.0;
        TraySlot tray = new TraySlot();
        ErrorCode errCode = ErrorCode.Ok;

        public override void Run()
        {
            switch (flowStep)
            {
                case ELoadAndUnloadTestStep.到取料位上方:
                    SlotPoint sp1 = TrayPointManager.GetSlotPoint(GlobalVar.CurRecipe,"Input", CurFetchSlotIndex, EPointType.BASE);
                    SlotPoint dist = TrayPointManager.GetDistToNozzle1(NozzleIndex);
                    targetX = sp1.X + dist.X;
                    targetY = sp1.Y + dist.Y;
                    errCode = AxisControl.LoadX.MovePos(targetX, GlobalVar.WholeSpeed);
                    if (errCode != ErrorCode.Ok)
                    {
                        MessageQueue.Instance.Warn($"运动失败:{errCode}");  //运动失败
                    }
                    errCode = AxisControl.LoadY.MovePos(targetY, (int)(GlobalVar.WholeSpeed));
                    if (errCode != ErrorCode.Ok)
                    {
                        MessageQueue.Instance.Warn($"运动失败:{errCode}");  //运动失败
                    }
                    flowStep = ELoadAndUnloadTestStep.等待到取料位上方;
                    break;
                case ELoadAndUnloadTestStep.等待到取料位上方:
                    if (Ops.IsStop("LoadX", "LoadY"))
                    {
                        Thread.Sleep(100);
                        flowStep = ELoadAndUnloadTestStep.到取料位下方;

                    }
                    break;
                case ELoadAndUnloadTestStep.到取料位下方:
                    AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(-5, GlobalVar.WholeSpeed);
                    flowStep = ELoadAndUnloadTestStep.等待到取料位下方;
                    break;
                case ELoadAndUnloadTestStep.等待到取料位下方:
                    if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
                    {
                        flowStep = ELoadAndUnloadTestStep.开真空;
                    }
                    break;
                case ELoadAndUnloadTestStep.开真空:
                    IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空吸电磁阀", 1);
                    Thread.Sleep(60);
                    flowStep = ELoadAndUnloadTestStep.取料完成抬起;
                    break;

                case ELoadAndUnloadTestStep.取料完成抬起:
                    AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(0, GlobalVar.WholeSpeed);
                    flowStep = ELoadAndUnloadTestStep.等待取料完成抬起;
                    break;
                case ELoadAndUnloadTestStep.等待取料完成抬起:
                    if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
                    {
                        flowStep = ELoadAndUnloadTestStep.取料真空检测;
                        //Thread.Sleep(100);

                    }
                    break;
                case ELoadAndUnloadTestStep.取料真空检测:
                    if (IoManager.Instance.ReadIn($"{NozzleIndex}吸嘴真空吸检测") == 1 || GlobalVar.RunSpace)
                    {
                        FetchNum = 0;
                        if (NozzleIndex < 2)
                        {
                            NozzleIndex++;
                            flowStep = ELoadAndUnloadTestStep.到取料位上方;
                        }
                        else
                        {
                            NozzleIndex = 1;
                            flowStep = ELoadAndUnloadTestStep.到下相机拍照起始位;
                        }
                        CurFetchSlotIndex++;
                    }
                    else
                    {
                        FetchNum++;
                        if (FetchNum == 6)
                        {
                            Msg.ShowError($"吸嘴{NozzleIndex}取料{FetchNum}次失败报警,请处理后点击确定");
                            FetchNum = 0;
                        }
                        flowStep = ELoadAndUnloadTestStep.到取料位下方;
                    }
                    break;
                case ELoadAndUnloadTestStep.到下相机拍照起始位:
                    if (XYCanMove())
                    {
                        ImageProcess.ClearAutoTrigger();
                        HikCamera.Instance.SetExposure("locationCamera", 40);
                        HikCamera.Instance.SetGain("locationCamera", 20);
                        AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("Nozzle1CenterX")+30, GlobalVar.WholeSpeed);
                        AxisControl.LoadY.MovePos(SysConfigParam.GetValue<double>("Nozzle1CenterY"), GlobalVar.WholeSpeed);
                        flowStep = ELoadAndUnloadTestStep.等待到下相机拍照起始位;
                    }

                    break;
                case ELoadAndUnloadTestStep.等待到下相机拍照起始位:
                    if (Ops.IsStop("LoadX","LoadY"))
                    {
                        flowStep = ELoadAndUnloadTestStep.到下相机拍照结束位;
                        Thread.Sleep(100);
                        HikCamera.Instance.SetTrigger("locationCamera", ETriggerMode.Auto);
                        AxisControl.LoadX.SetPosCompare(1, new double[] { SysConfigParam.GetValue<double>("Nozzle1CenterX"), SysConfigParam.GetValue<double>("Nozzle2CenterX") });
                    }
                    break;

                case ELoadAndUnloadTestStep.到下相机拍照结束位:
                    if (XYCanMove())
                    {
                        IoManager.Instance.WriteOut("下左相机光源触发", 1);
                        Thread.Sleep(50);
                        AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("Nozzle9CenterX") - 10, GlobalVar.FlyCameraSpeed);
                        flowStep = ELoadAndUnloadTestStep.等待到下相机拍照结束位;
                    }

                    break;
                case ELoadAndUnloadTestStep.等待到下相机拍照结束位:
                    AxisControl.LoadX.IsStop(out stop[0]);
                    Stopwatch stopwatch = new Stopwatch();
                    if (Ops.IsStop("LoadX"))
                    {
                        AxisControl.LoadX.ClearCompare(1);
                        IoManager.Instance.WriteOut("下左相机光源触发", 0);
                        flowStep = ELoadAndUnloadTestStep.等待相机拍照完成;
                        stopwatch.Start();
                    }
                    break;
                case ELoadAndUnloadTestStep.等待相机拍照完成:
                    if (GlobalVar.RunSpace)
                    {
                        flowStep = ELoadAndUnloadTestStep.到放料位上方;
                        mrs = new List<MatchResult>();
                        mrs.Add(new MatchResult());
                        mrs.Add(new MatchResult());
                    }
                    else
                    {
                        imgs = ImageProcess.GetAutoImage();
                        if (imgs != null && imgs.Length == 2)
                        {
                            CameraOkEvent?.Invoke(imgs, null);
                            //拍照完成
                            flowStep = ELoadAndUnloadTestStep.等待视觉处理结果;

                        }
                    }
                    break;
                case ELoadAndUnloadTestStep.等待视觉处理结果:
                    mrs = VisionProcess.Instance.MatchDownCam(imgs);
                    if (mrs != null && mrs.Count == 2)
                    {
                        CameraOkEvent?.Invoke(imgs, mrs);
                        flowStep = ELoadAndUnloadTestStep.到放料位上方;
                        //Thread.Sleep(2000);
                    }
                    else
                    {
                        flowStep = ELoadAndUnloadTestStep.到下相机拍照起始位;
                    }
                    break;
                case ELoadAndUnloadTestStep.到放料位上方:
                    if (XYCanMove())
                    {
                        targetX = 0.0;
                        targetY = 0.0;
                        SlotPoint sp2 = TrayPointManager.GetSlotPoint(GlobalVar.CurRecipe, "Turnover", CurAssembleSlotInex, EPointType.RUN);
                        SlotPoint nozzleDist = TrayPointManager.GetDistToNozzle1(NozzleIndex);
                        AxisControl.LoadX.MovePos(sp2.X+nozzleDist.X, GlobalVar.WholeSpeed);
                        AxisControl.LoadY.MovePos(sp2.Y + nozzleDist.Y, GlobalVar.WholeSpeed);
                        flowStep = ELoadAndUnloadTestStep.等待到放料位上方;
                    }

                    break;
                case ELoadAndUnloadTestStep.等待到放料位上方:
                    AxisControl.LoadX.IsStop(out stop[0]);
                    AxisControl.LoadY.IsStop(out stop[1]);
                    if (Ops.IsStop("LoadX","LoadY"))
                    {
                        Thread.Sleep(100);
                        flowStep = ELoadAndUnloadTestStep.到放料位下方;

                    }
                    break;
                case ELoadAndUnloadTestStep.到放料位下方:
                    AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(-5, GlobalVar.WholeSpeed);
                    flowStep = ELoadAndUnloadTestStep.等待到放料位下方;
                    break;
                case ELoadAndUnloadTestStep.等待到放料位下方:
                    if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
                    {
                        IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空吸电磁阀", 0);
                        Thread.Sleep(50);
                        IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空破电磁阀", 1);
                        Thread.Sleep(100);
                        IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空破电磁阀", 0);
                        flowStep = ELoadAndUnloadTestStep.放料完成抬起;
                    }
                    break;
                case ELoadAndUnloadTestStep.放料完成抬起:
                    AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(0, GlobalVar.WholeSpeed);
                    flowStep = ELoadAndUnloadTestStep.等待放料完成抬起;
                    break;
                case ELoadAndUnloadTestStep.等待放料完成抬起:
                    if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
                    {
                        Thread.Sleep(100);
                        if (NozzleIndex < 2)
                        {
                            NozzleIndex++;
                            flowStep = ELoadAndUnloadTestStep.到放料位上方;
                        }
                        else
                        {
                            NozzleIndex = 1;
                            flowStep = ELoadAndUnloadTestStep.到等待命令位;
                        }
                        CurAssembleSlotInex++;
                        if (CurAssembleSlotInex == 91)
                        {
                            CurAssembleSlotInex = 1;
                            CurFetchSlotIndex = 1;
                        }
                    }
                    break;
                case ELoadAndUnloadTestStep.到等待命令位:
                    AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("LoadXStartPos"),GlobalVar.WholeSpeed);
                    AxisControl.LoadY.MovePos(SysConfigParam.GetValue<double>("LoadYStartPos"), GlobalVar.WholeSpeed);
                    flowStep = ELoadAndUnloadTestStep.等待到等待命令位;
                    break;
                case ELoadAndUnloadTestStep.等待到等待命令位:
                    if(Ops.IsStop("LoadX", "LoadY"))
                    {
                        flowStep = ELoadAndUnloadTestStep.保压上料;
                    }
                    break;
                case ELoadAndUnloadTestStep.保压上料:
                    //TurnoverFlow.Instance.PressLoad();
                    flowStep = ELoadAndUnloadTestStep.等待保压上料完成;
                    break;
                case ELoadAndUnloadTestStep.等待保压上料完成:
                    if(TurnoverFlow.Instance.LoadFinished())
                    {
                        flowStep = ELoadAndUnloadTestStep.等待保压下料完成;
                        //TurnoverFlow.Instance.PressUnload();
                    }
                    break;
                case ELoadAndUnloadTestStep.等待保压下料完成:
                    if (TurnoverFlow.Instance.UnloadFinished())
                    {
                        flowStep = ELoadAndUnloadTestStep.到取料位上方;
                    }
                    break;
                default:
                    break;
            }
        }

        private bool XYCanMove()
        {
            return true;
        }
    }
}