using CodeLibrary;
using eyemLib_Sharp;
using HuichuanLibrary;
using OnlineStore.Common;
using OnlineStore.LoadCSVLibrary;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Windows.Forms.VisualStyles;
using static eyemLib_Sharp.EyemLibDemo;

namespace OnlineStore.DeviceLibrary
{
    public partial class BoxEquip
    {
        //public Dictionary<string, List<DrawerInfo>> DrawerNums = new Dictionary<string, List<DrawerInfo>>();
        public AutoFindPos autoFindPos;
        public bool autoFindPosMode = false;
        private void InitDrawInfos()
        {
            autoFindPos = new AutoFindPos(Config);
            //DrawerNums.Add("A", new List<DrawerInfo>());
            //DrawerNums.Add("B", new List<DrawerInfo>());
            //List<DrawerPosition> drawerPositions = CSVPositionReader<DrawerPosition>.getPositionList();

            //foreach (DrawerPosition position in drawerPositions)
            //{
            //    if (position.StoreId.Equals(DeviceID))
            //    {

            //        DrawerNums[PosSide(position.PositionNum)].Add(new DrawerInfo(position.PositionNum,PositionNumList));
            //    }
            //}
        }
        /// <summary>
        /// 获取抽屉/仓位所在面
        /// </summary>
        /// <param name="pos">位置</param>
        /// <returns></returns>
        private string PosSide(string pos)
        {
            if (pos.Substring(2, 2).Equals("AA")) return "A";
            else if (pos.Substring(2, 2).Equals("BB")) return "B";
            return "";
        }
        /// <summary>
        /// 获取抽屉/仓位所在面的相机序号
        /// </summary>
        /// <param name="pos">位置</param>
        /// <returns></returns>
        private string CamPosSide(string pos)
        {
            if (pos.Substring(2, 2).Equals("AA")) return "box_A";
            else if (pos.Substring(2, 2).Equals("BB")) return "box_B";
            return "box_A";
        }
        /// <summary>
        /// 获取轴的负限位
        /// </summary>
        /// <param name="axisNo"></param>
        /// <returns></returns>

        public bool GetAxisNelSts(short axisNo)
        {
            AxisSts sts = HCBoardManager.GetAxisSts(axisNo);
            return sts.NEL == 1;
        }
        /// <summary>
        /// 获取轴的正限位
        /// </summary>
        /// <param name="axisNo"></param>
        /// <returns></returns>

        public bool GetAxisPelSts(short axisNo)
        {
            AxisSts sts = HCBoardManager.GetAxisSts(axisNo);
            return sts.PEL == 1;
        }
        private void ToDrawer_StartPoint()
        {
            //StringBuilder stringBuilder = new StringBuilder(MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4));
            //stringBuilder.Append(autoFindPos.CurRow.ToString().PadLeft(2, '0'));
            //stringBuilder.Append(autoFindPos.CurColumn.ToString().PadLeft(2, '0'));
            //stringBuilder.Append("0101");
            //if (MoveInfo.MoveParam.PosInfo.PosId.Equals(stringBuilder.ToString()))
            //{
            //    MoveAxisToP3();
            //    UpdownAxisTo_P6_P12();
            //    PullAxis_UpdownToLowP4();
            //}
            //else
            {
                autoFindPos.Startposition_MoveAxis = MoveAxis.GetAclPosition() + (autoFindPos.CurColumn - autoFindPos.PreColumn) * autoFindPos.Row_Spacing;
                autoFindPos.Startposition_PullUpdpwnAxis = PullAxis_Updown.GetAclPosition() + (autoFindPos.CurRow - autoFindPos.PreRow) * autoFindPos.Column_Spacing;
                autoFindPos.Startposition_UpDownAxis = UpdownAxis.GetAclPosition() + (autoFindPos.CurRow - autoFindPos.PreRow) * autoFindPos.Column_Spacing;
                int moveAxis, pullUpDwnAxis, UpdwnAxis;
                if (autoFindPos.Startposition_MoveAxis >= Config.MoveAxis_PosLimit)
                {
                    moveAxis = Config.MoveAxis_PosLimit;
                }
                else if (autoFindPos.Startposition_MoveAxis <= Config.MoveAxis_NegLimit)
                {
                    moveAxis = Config.MoveAxis_NegLimit;
                }
                else
                    moveAxis = autoFindPos.Startposition_MoveAxis;

                if (autoFindPos.Startposition_PullUpdpwnAxis >= Config.PullAxis_Updown_PosLimit)
                {
                    pullUpDwnAxis = Config.PullAxis_Updown_PosLimit;
                }
                else if (autoFindPos.Startposition_PullUpdpwnAxis <= Config.PullAxis_Updown_NegLimit)
                {
                    pullUpDwnAxis = Config.PullAxis_Updown_NegLimit;
                }
                else
                {
                    pullUpDwnAxis = autoFindPos.Startposition_PullUpdpwnAxis;
                }
                if (autoFindPos.Startposition_UpDownAxis >= Config.UpdownAxis_PosLimit)
                {
                    UpdwnAxis = Config.UpdownAxis_PosLimit;
                }
                else if (autoFindPos.Startposition_UpDownAxis <= Config.UpdownAxis_NegLimit)
                {
                    UpdwnAxis = Config.UpdownAxis_NegLimit;
                }
                else
                {
                    UpdwnAxis = autoFindPos.Startposition_UpDownAxis;
                }
                LogUtil.info($"视觉对位 运行到抽屉 行={autoFindPos.CurRow},列={autoFindPos.CurColumn}");
                MoveAxis.AbsMove(MoveInfo, moveAxis, Config.MoveAxis_FindPosSpeed);
                UpdownAxis.AbsMove(MoveInfo, UpdwnAxis, Config.UpdownAxis_FindPosSpeed);
                PullAxis_Updown.AbsMove(MoveInfo, pullUpDwnAxis, Config.PullAxis_Updown_FindPosSpeed);
            }

        }

        private float GetDrawerX()
        {
            if (CheckASide())
            {
                return Config.FindPos_Drawer_A_X;
            }
            else
            {
                return Config.FindPos_Drawer_B_X;
            }
        }
        private float GetDrawerY()
        {
            if (CheckASide())
            {
                return Config.FindPos_Drawer_A_Y;
            }
            else
            {
                return Config.FindPos_Drawer_B_Y;
            }
        }
        private float GetError_X()
        {
            return Config.FindPos_Drawer_X_Error;
        }
        private float GetError_Y()
        {
            return Config.FindPos_Drawer_Y_Error;
        }
        private bool CheckMarkXPoint(EyemOcsFXYR eyemOcsFXYR)
        {
            if (Math.Abs(GetDrawerX() - eyemOcsFXYR.fX) < GetError_X())
            {
                LogInfo($" {MoveInfo.SLog}：Mark点X轴坐标{eyemOcsFXYR.fX},模板Mark点X轴坐标{GetDrawerX()},X轴偏差{(GetDrawerX() - eyemOcsFXYR.fX)}，在误差范围内" +
                     $"[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                AutoFindPosSaveImage($"OK_Mark点X轴坐标{eyemOcsFXYR.fX},模板Mark点X轴坐标{GetDrawerX()},X轴偏差{(GetDrawerX() - eyemOcsFXYR.fX)}");
                return true;
            }
            AutoFindPosSaveImage($"NG_Mark点X轴坐标{eyemOcsFXYR.fX},模板Mark点X轴坐标{GetDrawerX()},X轴偏差{(GetDrawerX() - eyemOcsFXYR.fX)}");
            LogInfo($" {MoveInfo.SLog}：Mark点X轴坐标{eyemOcsFXYR.fX},模板Mark点X轴坐标{GetDrawerX()},X轴偏差{(GetDrawerX() - eyemOcsFXYR.fX)}，" +
                $"行走机构相对移动{(GetDrawerX() - eyemOcsFXYR.fX) * autoFindPos.XCoeffOfCoorToPulse}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
            MoveAxis.AbsMove(MoveInfo, Convert.ToInt32(MoveAxis.GetAclPosition() + (GetDrawerX() - eyemOcsFXYR.fX) * autoFindPos.XCoeffOfCoorToPulse), Config.MoveAxis_FindPosSpeed / 10);
            return false;
        }
        private bool CheckMarkYPoint(EyemOcsFXYR eyemOcsFXYR)
        {
            if (Math.Abs(GetDrawerY() - eyemOcsFXYR.fY) < GetError_Y())
            {
                LogInfo($"视觉对位 {MoveInfo.SLog}：Mark点Y轴坐标{eyemOcsFXYR.fY},模板Mark点Y轴坐标{GetDrawerY()},Y轴偏差{(GetDrawerY() - eyemOcsFXYR.fY)}，在误差范围内" +
                $"[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                autoFindPos.MarkYCoor = eyemOcsFXYR.fY;
                AutoFindPosSaveImage($"OK_Mark点Y轴坐标{eyemOcsFXYR.fY},模板Mark点Y轴坐标{GetDrawerY()},Y轴偏差{(GetDrawerY() - eyemOcsFXYR.fY)}");
                return true;
            }
            AutoFindPosSaveImage($"NG_Mark点Y轴坐标{eyemOcsFXYR.fY},模板Mark点Y轴坐标{GetDrawerY()},Y轴偏差{(GetDrawerY() - eyemOcsFXYR.fY)}");
            UpdownAxis.AbsMove(MoveInfo, Convert.ToInt32(UpdownAxis.GetAclPosition() + (GetDrawerY() - eyemOcsFXYR.fY) * autoFindPos.YCoeffOfCoorToPulse), Config.UpdownAxis_FindPosSpeed / 10);
            PullAxis_Updown.AbsMove(MoveInfo, Convert.ToInt32(PullAxis_Updown.GetAclPosition() + (GetDrawerY() - eyemOcsFXYR.fY) * autoFindPos.YCoeffOfCoorToPulse), Config.PullAxis_Updown_FindPosSpeed / 10);
            LogInfo($"视觉对位 {MoveInfo.SLog}：Mark点Y轴坐标{eyemOcsFXYR.fY},模板Mark点Y轴坐标{GetDrawerY()},Y轴偏差{(GetDrawerY() - eyemOcsFXYR.fY)}，" +
     $"移栽升降轴、料屉升降轴相对移动{(GetDrawerY() - eyemOcsFXYR.fY) * autoFindPos.YCoeffOfCoorToPulse}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
            return false;
        }
        private void AutoFindPosSaveImage(string imgInfo)
        {
            //SaveImage(CamPosSide(MoveInfo.MoveParam.PosInfo.PosId), $"{MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4)}", $"{DateTime.Now.ToString("yyyyMMddhhmmss")}_{autoFindPos.CurRow}_{autoFindPos.CurColumn}_{imgInfo}");
        }
        public void StartFindPos(InOutParam param)
        {
            if (isInSuddenDown || isNoAirpressure_Check ||
                (!runStatus.Equals(RunStatus.Runing))
                   || (!MoveInfo.MoveType.Equals(MoveType.None)))
            {
                LogUtil.error(Name + " 启动视觉对位出错，忙碌或报警中 ，storeStatus=" + runStatus + ",MoveType=" + MoveInfo.MoveType + ",isInSuddenDown=" + isInSuddenDown + ",isNoAirpressure_Check=" + isNoAirpressure_Check);
                return;
            }
            LogInfo(" 启动视觉对位【" + param.PosInfo.ToStr() + "】 ");
            param.MoveP = new LineMoveP(Config, param.PosInfo.PosId);
            MoveInfo.NewMove(MoveType.Working, param);
            SetBoxStatus(DeviceStatus.Debugging, RunStatus.Busy);
            MoveInfo.NextMoveStep(StepEnum.SA_01_Pos_Start_Find_Pos);
            PullAxis_Inout_To_P1();
        }
        private bool SaveDrawerPosition()
        {
            try
            {
                StringBuilder stringBuilder = new StringBuilder(MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4));
                stringBuilder.Append(autoFindPos.CurRow.ToString().PadLeft(2, '0'));
                stringBuilder.Append(autoFindPos.CurColumn.ToString().PadLeft(2, '0'));
                List<string> curDrawer = PositionNumList.FindAll(s => s.StartsWith(stringBuilder.ToString()));
                if (curDrawer != null && curDrawer.Count > 0)
                {
                    foreach (string item in curDrawer)
                    {
                        BoxPosition ktkPosition = CSVPositionReader<BoxPosition>.GetPositon(item);
                        if (ktkPosition != null)
                        {
                            ktkPosition.MoveAxis_P3 = MoveAxis.GetAclPosition();
                            ktkPosition.Updown_P6_P12 = UpdownAxis.GetAclPosition();
                            ktkPosition.Updown_P7_P13 = ktkPosition.Updown_P6_P12 + autoFindPos.UpdownAxis_Diff_Upper_Low;
                            ktkPosition.PullAxis_Updown_P4 = PullAxis_Updown.GetAclPosition();
                            ktkPosition.PullAxis_Updown_P2 = PullAxis_Updown.GetAclPosition() + autoFindPos.PullUpdown_Diff_mid;
                            ktkPosition.PullAxis_Updown_P3 = PullAxis_Updown.GetAclPosition() + autoFindPos.PullUpdown_Diff_high;
                        }
                        else
                        {
                            LogUtil.error($"不存在的库位：{item}");
                        }
                        //位置配置
                        string appPath = Application.StartupPath;
                        //如果总配置文件存在，保存到总的配置文件
                        string positionConfigFile = appPath + ConfigAppSettings.GetValue(Setting_Init.ConfigPath_BoxPosition);
                        if (!File.Exists(positionConfigFile))
                        {
                            string nameStr = DeviceID.ToString();
                            positionConfigFile = positionConfigFile.Replace(".csv", "_" + nameStr + ".csv");
                        }
                        bool result = CSVPositionReader<BoxPosition>.SavePostion(positionConfigFile, ktkPosition, false);
                        if (!result)
                        {
                            SetWarnMsg("库位【" + item + "】保存失败！");
                            return false;
                        }
                        else
                        {

                            // LogInfo($"存储机构-视觉对位 {MoveInfo.SLog}：库位[{item}]保存成功[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                        }
                        LogUtil.info($"库位{item}:MoveAxis_P3={ktkPosition.MoveAxis_P3}[{MoveAxis.GetAclPosition()}],Updown_P6_P12={ktkPosition.Updown_P6_P12}[{UpdownAxis.GetAclPosition()}]," +
                                     $"Updown_P7_P13={ktkPosition.Updown_P7_P13}[{UpdownAxis.GetAclPosition()}+{autoFindPos.UpdownAxis_Diff_Upper_Low}],PullAxis_Updown_P4={ktkPosition.PullAxis_Updown_P4}[{PullAxis_Updown.GetAclPosition()}],PullAxis_Updown_P2={ktkPosition.PullAxis_Updown_P2}[{PullAxis_Updown.GetAclPosition()}+{autoFindPos.PullUpdown_Diff_mid}]," +
                                     $"PullAxis_Updown_P3={ktkPosition.PullAxis_Updown_P3}[{PullAxis_Updown.GetAclPosition()}+{autoFindPos.PullUpdown_Diff_high}]");
                    }
                    LogInfo($"视觉对位 {MoveInfo.SLog}：抽屉[{autoFindPos.CurRow}，{autoFindPos.CurColumn}]位置信息保存成功[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    return true;
                }
            }

            catch (Exception e)
            {
                LogUtil.error("SaveDrawerPosition", e);
            }
            return false;
        }
        Bitmap bitmap = null;
        public void FindPosEnd()
        {
            MoveInfo.EndMove();
            SetBoxStatus(DeviceStatus.Debugging, RunStatus.Runing);
            if (MoveInfo.MoveParam != null)
                LogInfo($"视觉对位 {MoveInfo.SLog}：对位结束[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
            foreach (string item in autoFindPos.FailedDrawers)
            {
                LogUtil.info("视觉对位失败抽屉:" + item);
            }
            if (autoFindPos.FailedDrawers.Count > 0)
                autoFindPos.FailedDrawers.Clear();
        }
        /// <summary>
        /// 当前是否是第一或者最后一列
        /// </summary>
        /// <returns>true：是最后一列</returns>
        public bool IsFirstOrLastColum()
        {
            if (autoFindPos.CurColumn.Equals(1) || autoFindPos.CurColumn.Equals(Config.Drawer_Columns))
                return true;
            return false;
        }
        /// <summary>
        /// 回退一段距离
        /// </summary>
        private void BackDistance()
        {
            autoFindPos.Startposition_MoveAxis = MoveAxis.GetAclPosition() + (-autoFindPos.CurColumn + autoFindPos.PreColumn) * autoFindPos.Row_Spacing / 2;
            int moveAxis;
            if (autoFindPos.Startposition_MoveAxis >= Config.MoveAxis_PosLimit)
            {
                moveAxis = Config.MoveAxis_PosLimit;
            }
            else if (autoFindPos.Startposition_MoveAxis <= Config.MoveAxis_NegLimit)
            {
                moveAxis = Config.MoveAxis_NegLimit;
            }
            else
                moveAxis = autoFindPos.Startposition_MoveAxis;


            LogUtil.info($"料格水平检查 运行到抽屉 行走机构回退{autoFindPos.Startposition_MoveAxis}");
            MoveAxis.AbsMove(MoveInfo, moveAxis, Config.MoveAxis_FindPosSpeed);
        }
        EyemOcsFXYR ocsFXYR;
        public void AutoFindPosProcess()
        {
            if (MoveInfo.IsInWait)
            {
                CheckWait(MoveInfo);
            }
            if (MoveInfo.IsInWait)
            {
                return;
            }
            if (!autoFindPosMode)
                return;
            switch (MoveInfo.MoveStep)
            {
                case StepEnum.SA_01_Pos_Start_Find_Pos:
                    MoveInfo.NextMoveStep(StepEnum.SA_02_Pos_To_Drawer_StartPoint);
                    LogInfo($"视觉对位 {MoveInfo.SLog}：到抽屉起始点，" +
                        $"行走机构到抽屉起始点，移栽升降轴到抽屉起始点[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    ToDrawer_StartPoint();
                    break;
                case StepEnum.SA_02_Pos_To_Drawer_StartPoint:
                    //取图
                    //Bitmap bitmap = null;
                    MoveInfo.NextMoveStep(StepEnum.SA_03_Pos_AcqImg);
                    bitmap = AcqImage(CamPosSide(MoveInfo.MoveParam.PosInfo.PosId));
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                    LogInfo($"视觉对位 {MoveInfo.SLog}：取图,行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    break;
                case StepEnum.SA_03_Pos_AcqImg:
                    int res = AutoFindPos.GetMarkInfo(bitmap, $"{MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4)}", $"row{autoFindPos.CurRow}_col{autoFindPos.CurColumn}", out ocsFXYR);
                    if (res == 0)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_03_Pos_GetMark);
                        LogInfo($"视觉对位 {MoveInfo.SLog}：获取Mark成功[ret={res}],行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    }
                    else if (res == -4)
                    {
                        SetWarnMsg($"视觉对位异常[code={res}]，未找到Mark点。对位失败的行={autoFindPos.CurRow}[{MoveAxis.GetAclPosition()}]，列={autoFindPos.CurColumn}[{UpdownAxis.GetAclPosition()}]");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                        MoveInfo.NextMoveStep(StepEnum.SA_05_Pos_NextDrawer);
                    }
                    else
                    {
                        SetWarnMsg($"视觉对位异常[code={res}]，未找到Mark点。对位失败的行={autoFindPos.CurRow}[{MoveAxis.GetAclPosition()}]，列={autoFindPos.CurColumn}[{UpdownAxis.GetAclPosition()}]");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                        MoveInfo.NextMoveStep(StepEnum.SA_05_Pos_NextDrawer);
                    }
                    break;
                case StepEnum.SA_03_Pos_GetMark:
                    if (CheckMarkXPoint(ocsFXYR) && CheckMarkYPoint(ocsFXYR))
                    {
                        autoFindPos.IsPositiveDir = true;
                        MoveInfo.NextMoveStep(StepEnum.SA_05_Pos_NextDrawer);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(2000));
                        Task task = Task.Factory.StartNew(delegate {

                            //存储抽屉位置信息
                            if (SaveDrawerPosition())
                            {

                            }
                            else
                            {
                                autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                            }
                        });
                        task.Wait();
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_04_Pos_AdjPos);
                    }
                    break;
                case StepEnum.SA_04_Pos_AdjPos:
                    MoveInfo.NextMoveStep(StepEnum.SA_02_Pos_To_Drawer_StartPoint);
                    break;
                case StepEnum.SA_05_Pos_NextDrawer:
                    if (autoFindPos.CurColumn < autoFindPos.Columns)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_01_Pos_Start_Find_Pos);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                        autoFindPos.PreColumn = autoFindPos.CurColumn;
                        autoFindPos.PreRow = autoFindPos.CurRow;
                        autoFindPos.CurColumn++;
                    }
                    else
                    {
                        if (autoFindPos.CurRow < autoFindPos.Rows)
                        {
                            MoveInfo.NextMoveStep(StepEnum.SA_06_Neg_Start_Find_Pos);
                            MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                            autoFindPos.PreRow = autoFindPos.CurRow;
                            autoFindPos.CurRow++;
                            autoFindPos.PreColumn = autoFindPos.CurColumn;
                        }
                        else//结束
                        {
                            FindPosEnd();
                        }
                    }
                    break;
                case StepEnum.SA_06_Neg_Start_Find_Pos:
                    MoveInfo.NextMoveStep(StepEnum.SA_07_Neg_To_Drawer_StartPoint);
                    LogInfo($"视觉对位 {MoveInfo.SLog}：到抽屉起始点，" +
                        $"行走机构到抽屉起始点，移栽升降轴到抽屉起始点[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    ToDrawer_StartPoint();
                    break;

                case StepEnum.SA_07_Neg_To_Drawer_StartPoint:
                    //取图
                    //Bitmap bitmap1 = null;
                    MoveInfo.NextMoveStep(StepEnum.SA_08_Neg_AcqImg);
                    bitmap = AcqImage(CamPosSide(MoveInfo.MoveParam.PosInfo.PosId));
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                    LogInfo($"视觉对位 {MoveInfo.SLog}：取图,行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    break;
                case StepEnum.SA_08_Neg_AcqImg:
                    int res1 = AutoFindPos.GetMarkInfo(bitmap, $"{MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4)}", $"row{autoFindPos.CurRow}_col{autoFindPos.CurColumn}", out ocsFXYR);
                    if (res1 == 0)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_08_Neg_GetMark);
                        LogInfo($"视觉对位 {MoveInfo.SLog}：获取Mark成功[ret={res1}],行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    }
                    else if (res1 == -4)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_10_Neg_NextDrawer);
                        SetWarnMsg($"视觉对位异常[code={res1}]，未找到Mark点。对位失败的行={autoFindPos.CurRow}[{MoveAxis.GetAclPosition()}]，列={autoFindPos.CurColumn}[{UpdownAxis.GetAclPosition()}]");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_10_Neg_NextDrawer);
                        SetWarnMsg($"视觉对位异常[code={res1}]，未找到Mark点。对位失败的行={autoFindPos.CurRow}[{MoveAxis.GetAclPosition()}]，列={autoFindPos.CurColumn}[{UpdownAxis.GetAclPosition()}]");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                    }
                    break;
                case StepEnum.SA_08_Neg_GetMark:
                    if (CheckMarkXPoint(ocsFXYR) && CheckMarkYPoint(ocsFXYR))
                    {
                        autoFindPos.IsPositiveDir = false;
                        MoveInfo.NextMoveStep(StepEnum.SA_10_Neg_NextDrawer);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(2000));
                        Task task = Task.Factory.StartNew(delegate {
                            //存储抽屉位置信息
                            if (SaveDrawerPosition())
                            {

                            }
                            else
                            {
                                autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                            }
                        });
                        task.Wait();
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_09_Neg_AdjPos);
                    }
                    break;
                case StepEnum.SA_09_Neg_AdjPos:
                    MoveInfo.NextMoveStep(StepEnum.SA_07_Neg_To_Drawer_StartPoint);
                    break;
                case StepEnum.SA_10_Neg_NextDrawer:
                    if (autoFindPos.CurColumn > 1)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_06_Neg_Start_Find_Pos);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                        autoFindPos.PreRow = autoFindPos.CurRow;
                        autoFindPos.PreColumn = autoFindPos.CurColumn;
                        autoFindPos.CurColumn--;
                    }
                    else
                    {
                        if (autoFindPos.CurRow < autoFindPos.Rows)
                        {
                            MoveInfo.NextMoveStep(StepEnum.SA_01_Pos_Start_Find_Pos);
                            MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                            autoFindPos.PreRow = autoFindPos.CurRow;
                            autoFindPos.CurRow++;
                            autoFindPos.PreColumn = autoFindPos.CurColumn;
                        }
                        else//结束
                        {
                            FindPosEnd();
                        }
                    }
                    break;
                //料格水平检查
                case StepEnum.SCH_00_PreCheck:
                    if (IsFirstOrLastColum())
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_06_Finish);
                        LogInfo($"料格水平检查 {MoveInfo.SLog}：当前为第{autoFindPos.CurColumn}列，忽略");
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_01_BackDistance);
                        LogInfo($"料格水平检查 {MoveInfo.SLog}：当前为第{autoFindPos.CurRow}行,第{autoFindPos.CurColumn}列，开始回退");
                        BackDistance();
                    }
                    break;

                case StepEnum.SCH_01_BackDistance:
                    MoveInfo.NextMoveStep(StepEnum.SCH_02_AcqImage);
                    bitmap = AcqImage(CamPosSide(MoveInfo.MoveParam.PosInfo.PosId));
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
                    LogInfo($"料格水平检查 {MoveInfo.SLog}：取图,行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    break;

                case StepEnum.SCH_02_AcqImage:
                    int res2 = AutoFindPos.GetMarkInfo(bitmap, $"{MoveInfo.MoveParam.PosInfo.PosId.Substring(0, 4)}", $"row{autoFindPos.CurRow}_col{autoFindPos.CurColumn}", out ocsFXYR, SelectColor.Green);
                    if (res2 == 0)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_03_CheckXCoord);
                        LogInfo($"料格水平检查 {MoveInfo.SLog}：获取Mark成功[ret={res2}],行={autoFindPos.CurRow},列={autoFindPos.CurColumn}[{MoveInfo.MoveParam.PosInfo.GetPosSide()}面]");
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_06_Finish);
                        SetWarnMsg($"料格水平检查异常[code={res2}]，未找到Mark点。对位失败的行={autoFindPos.CurRow}[{MoveAxis.GetAclPosition()}]，列={autoFindPos.CurColumn}[{UpdownAxis.GetAclPosition()}]");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                    }
                    break;

                case StepEnum.SCH_03_CheckXCoord:
                    if (CheckMarkXPoint(ocsFXYR))
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_05_CheckYDiff);
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SCH_04_AdjXPos);
                    }
                    break;

                case StepEnum.SCH_04_AdjXPos:
                    MoveInfo.NextMoveStep(StepEnum.SCH_01_BackDistance);
                    LogInfo($"料格水平检查 {MoveInfo.SLog}：当前为第{autoFindPos.CurRow}行,第{autoFindPos.CurColumn}列，准备重新取图");
                    break;

                case StepEnum.SCH_05_CheckYDiff:
                    MoveInfo.NextMoveStep(StepEnum.SCH_06_Finish);
                    if (Math.Abs(GetDrawerY() - ocsFXYR.fY) < GetError_Y())
                    {
                        LogInfo($"料格水平检查 {MoveInfo.SLog}：第{autoFindPos.CurRow}行,第{autoFindPos.CurColumn}列OK");
                    }
                    else
                    {
                        SetWarnMsg($"料格水平检查 {MoveInfo.SLog}：第{autoFindPos.CurRow}行,第{autoFindPos.CurColumn}列NG:模板Mark点Y坐标={GetDrawerY()}，" +
                            $"检测到的Mark点坐标={ocsFXYR.fY},相差{Math.Abs(GetDrawerY() - ocsFXYR.fY)}像素");
                        autoFindPos.FailedDrawers.Add($"{autoFindPos.CurRow},{autoFindPos.CurColumn}");
                    }
                    break;

                case StepEnum.SCH_06_Finish:
                    if (autoFindPos.IsPositiveDir)
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_05_Pos_NextDrawer);
                    }
                    else
                    {
                        MoveInfo.NextMoveStep(StepEnum.SA_10_Neg_NextDrawer);
                    }
                    LogInfo($"料格水平检查 {MoveInfo.SLog}：检查结束，进行下一个抽屉");
                    break;
            }
        }
    }

    public class AutoFindPos
    {
        /// <summary>
        /// 运行方向是否是正向
        /// </summary>
        public bool IsPositiveDir { get; set; } = true;
        public int Startposition_MoveAxis { get; set; }
        public int Startposition_UpDownAxis { get; set; }
        public int Startposition_PullUpdpwnAxis { get; set; }
        /// <summary>
        /// 当前行
        /// </summary>
        public int CurRow { get; set; } = 1;
        public int PreRow { get; set; } = 1;
        /// <summary>
        /// x坐标到脉冲的转换系数
        /// </summary>
        public float XCoeffOfCoorToPulse { get; set; } = 1;
        /// <summary>
        /// 坐标到脉冲的转换系数
        /// </summary>
        public float YCoeffOfCoorToPulse { get; set; } = 1;
        /// <summary>
        /// 当前列
        /// </summary>
        public int CurColumn { get; set; } = 1;
        public int PreColumn { get; set; } = 1;
        /// <summary>
        /// 抽屉行数
        /// </summary>
        public int Rows { get; private set; }
        /// <summary>
        /// 抽屉列数
        /// </summary>

        public int Columns { get; private set; }

        /// <summary>
        /// 抽屉间行间距
        /// </summary>
        public int Row_Spacing { get; private set; }
        /// <summary>
        /// 抽屉间列间距
        /// </summary>
        public int Column_Spacing { get; private set; }
        /// <summary>
        /// 料屉中点与低点的差
        /// </summary>
        public int PullUpdown_Diff_mid { get; set; }
        /// <summary>
        /// 料屉高点与低点的差
        /// </summary>
        public int PullUpdown_Diff_high { get; set; }
        /// <summary>
        /// 移栽升降P13-P12的差
        /// </summary>
        public int UpdownAxis_Diff_Upper_Low { get; set; }
        public int RowsInDraw { get; set; }
        public int ColumnsInDraw { get; set; }
        /// <summary>
        /// 红色Mark点的Y坐标
        /// </summary>
        public float MarkYCoor { get; set; }
        public List<string> FailedDrawers { get; set; }
        BoxEquip_Config boxEquip_Config;
        public AutoFindPos(BoxEquip_Config boxEquip_Config)
        {
            this.boxEquip_Config = boxEquip_Config;
            Row_Spacing = boxEquip_Config.Row_Spacing;
            Column_Spacing = boxEquip_Config.Column_Spacing;
            Rows = boxEquip_Config.Drawer_Rows;
            Columns = boxEquip_Config.Drawer_Columns;
            FailedDrawers = new List<string>();
        }
        public void SetParam(int curRow, int curColumn, int startRow, int startCol, float xCooef, float yCooef)
        {
            CurRow = startRow;
            CurColumn = startCol;
            PreColumn = curColumn;
            PreRow = curRow;
            Row_Spacing = boxEquip_Config.Row_Spacing;
            Column_Spacing = boxEquip_Config.Column_Spacing;
            Rows = boxEquip_Config.Drawer_Rows;
            Columns = boxEquip_Config.Drawer_Columns;
            XCoeffOfCoorToPulse = xCooef;
            YCoeffOfCoorToPulse = yCooef;
        }
        /// <summary>
        /// Mark点坐标信息 
        /// </summary>
        /// <param name="bitmap">输入图片</param>
        /// <param name="markCoor">mark点坐标信息</param>
        /// <returns>0 正常 -4未定位到 </returns>
        public static int GetMarkInfo(Bitmap bitmap, string subpath, string filename, out EyemOcsFXYR markCoor, out Bitmap resBitmap, SelectColor color = SelectColor.Red)
        {
            int flag;
            markCoor = new EyemOcsFXYR();
            EyemImage tpDstImg;
            EyemImage image = eyemCvtToEyemImage(bitmap);
            EyemHSVModel tpHsvModel = new EyemHSVModel();
            switch (color)
            {
                case SelectColor.Red:
                    tpHsvModel.dpRangeL = new double[] { 0, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 10, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 156, 43, 46 }; tpHsvModel.dpRangeUExt = new double[] { 180, 255, 255 };
                    break;

                case SelectColor.Blue:
                    tpHsvModel.dpRangeL = new double[] { 100, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 124, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
                case SelectColor.Green:
                    tpHsvModel.dpRangeL = new double[] { 35, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 77, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
            }
            flag = eyemMarkerTracing(image, tpHsvModel, ref markCoor, out tpDstImg);
            string path = Application.StartupPath + ConfigAppSettings.GetValue(Setting_Init.ImagePath) + subpath + "\\" + DateTime.Now.ToString("yyyyMMdd");
            string path1 = path + "\\" + "Src\\";
            string path2 = path + "\\" + "ResOut\\";
            string time = DateTime.Now.ToString("HHmmss");
            if (!System.IO.Directory.Exists(path1))
                Directory.CreateDirectory(path1);
            if (!System.IO.Directory.Exists(path2))
                Directory.CreateDirectory(path2);

            bitmap.Save(path1 + filename + $"_{flag}_{time}" + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp);
            resBitmap = eyemCvtToBitmap(tpDstImg);
            if (resBitmap != null)
            {
                resBitmap.Save(string.Format("{0}{1}_{2}_{3}_{4}_{5}_{6}.bmp", path2, filename, flag, time, markCoor.fX, markCoor.fY, markCoor.fR));
            }
            Marshal.FreeHGlobal(image.vpImage);
            eyemImageFree(ref tpDstImg);
            return flag;
        }

        public static int GetMarkInfo(EyemImage image, string subpath, string filename, out EyemOcsFXYR markCoor, out Bitmap resBitmap, SelectColor color = SelectColor.Red)
        {
            int flag;
            markCoor = new EyemOcsFXYR();
            EyemImage tpDstImg;
            EyemHSVModel tpHsvModel = new EyemHSVModel();
            switch (color)
            {
                case SelectColor.Red:
                    tpHsvModel.dpRangeL = new double[] { 0, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 10, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 156, 43, 46 }; tpHsvModel.dpRangeUExt = new double[] { 180, 255, 255 };
                    break;

                case SelectColor.Blue:
                    tpHsvModel.dpRangeL = new double[] { 100, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 124, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
                case SelectColor.Green:
                    tpHsvModel.dpRangeL = new double[] { 35, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 77, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
            }
            flag = eyemMarkerTracing(image, tpHsvModel, ref markCoor, out tpDstImg);
            string path = Application.StartupPath + ConfigAppSettings.GetValue(Setting_Init.ImagePath) + subpath + "\\" + DateTime.Now.ToString("yyyyMMdd");
            string path1 = path + "\\" + "Src\\";
            string path2 = path + "\\" + "ResOut\\";
            string time = DateTime.Now.ToString("HHmmss");
            if (!System.IO.Directory.Exists(path1))
                Directory.CreateDirectory(path1);
            if (!System.IO.Directory.Exists(path2))
                Directory.CreateDirectory(path2);
            Bitmap bitmap = eyemCvtToBitmap(image);
            bitmap.Save(path1 + filename + $"_{flag}_{time}" + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp);
            resBitmap = eyemCvtToBitmap(tpDstImg);
            if (resBitmap != null)
            {
                resBitmap.Save(string.Format("{0}{1}_{2}_{3}_{4}_{5}_{6}.bmp", path2, filename, flag, time, markCoor.fX, markCoor.fY, markCoor.fR));
            }
            Marshal.FreeHGlobal(image.vpImage);
            eyemImageFree(ref tpDstImg);
            return flag;
        }


        public static int GetMarkInfo(Bitmap bitmap, string subpath, string filename, out EyemOcsFXYR markCoor, SelectColor color = SelectColor.Red)
        {
            int flag;
            markCoor = new EyemOcsFXYR();
            if (bitmap == null)
                return -1;
            EyemImage tpDstImg;
            EyemImage image = eyemCvtToEyemImage(bitmap);
            EyemHSVModel tpHsvModel = new EyemHSVModel();
            switch (color)
            {
                case SelectColor.Red:
                    tpHsvModel.dpRangeL = new double[] { 0, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 10, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 156, 43, 46 }; tpHsvModel.dpRangeUExt = new double[] { 180, 255, 255 };
                    break;

                case SelectColor.Blue:
                    tpHsvModel.dpRangeL = new double[] { 100, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 124, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
                case SelectColor.Green:
                    tpHsvModel.dpRangeL = new double[] { 35, 43, 46 }; tpHsvModel.dpRangeU = new double[] { 77, 255, 255 };
                    tpHsvModel.dpRangeLExt = new double[] { 0, 0, 0 }; tpHsvModel.dpRangeUExt = new double[] { 0, 0, 0 };
                    break;
            }
            flag = eyemMarkerTracing(image, tpHsvModel, ref markCoor, out tpDstImg);
            string path = Application.StartupPath + ConfigAppSettings.GetValue(Setting_Init.ImagePath) + subpath + "\\" + DateTime.Now.ToString("yyyyMMdd");
            string path1 = path + "\\" + "Src\\";
            string path2 = path + "\\" + "ResOut\\";
            string time = DateTime.Now.ToString("HHmmss");
            if (!System.IO.Directory.Exists(path1))
                Directory.CreateDirectory(path1);
            if (!System.IO.Directory.Exists(path2))
                Directory.CreateDirectory(path2);
            bitmap.Save(path1 + filename + $"_{flag}_{time}" + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp);
            Bitmap resBitmap = eyemCvtToBitmap(tpDstImg);
            if (resBitmap != null)
            {
                resBitmap.Save(string.Format("{0}{1}_{2}_{3}_{4}_{5}_{6}.bmp", path2, filename, flag, time, markCoor.fX, markCoor.fY, markCoor.fR));
            }
            Marshal.FreeHGlobal(image.vpImage);
            eyemImageFree(ref tpDstImg);
            return flag;
        }
    }
}