LGES-OC5-PtoP-Welder/WeldingComponents/WeldingCommon/Sequences/Main/SeqScoutNormal.cs

using Aurora.Api;
using AuroraLaserScanner;
using Matrox.MatroxImagingLibrary;
using MilCommon;
using System;
using System.Collections.Generic;
using System.Configuration;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Threading;
using VisionCommon;
using WeldingCommon.Alarm;
using WeldingCommon.Data;
using WeldingCommon.Settings;
using WeldingCommon.Utils;

namespace WeldingCommon.Sequences
{
    public class SeqScoutNormal
    {
        static readonly Logger Log = new Logger("SeqScoutNormal");

        const string SEQ_ENTER = "[SeqScoutNormal] SEQ_ENTER";   // inputParam 으로 받자.
        const string SEQ_CHECK_INIT_CONDITION = "[SeqScoutNormal] Check Init Condition";  // Check Init Condition

        const string SEQ_CAPTURE_NEXT_POSITION = "[SeqScoutNormal] Capture Next Position";
        //const string SEQ_SCOUT_DECISION = "Decision"; // ==> 이름 바꿈, 아래
        const string SEQ_CHECK_SCOUT_COUNT = "[SeqScoutNormal] Check Scout Count";
        const string SEQ_SAVE_IMAGE_FILE = "[SeqScoutNormal] Save Image File";

        const string SEQ_SAVE_RESULT_DATA = "[SeqScoutNormal] Save Result Data";

        const string SEQ_WRITE_COMPLETION_PLC_BIT = "[SeqScoutNormal] Write Completion PLC Bit";
        const string SEQ_COMPLETE = "[SeqScoutNormal] SEQ_COMPLETE";         // 완료

        static class ContextKey
        {
            public const string KEY_SEQUENCE_RESULT = "KEY_SEQUENCE_RESULT_SeqScoutNormal";
            public const string KEY_SEQUENCE_START_TIME = "KEY_SEQUENCE_START_TIME_SeqScoutNormal";
        }

        private SeqScoutNormal_InitParam _InitParam;

        private Scanner _Scanner;
        private SystemParameter _SystemParameter;

        private FovRecipe _FovRecipe;
        private CameraAgentBase _CameraAgent;
        private ICameraDisplay _CameraDisplay;
        private CamConfig _CamConfig;
        private int _ScoutNo;

        private MIL_ID _GrabImage = MIL.M_NULL;
        private MIL_ID _ScannerFieldImage = MIL.M_NULL;

        private Stopwatch _StopWatchTotal;
        private object scannerFieldImage_obj;
        private static List<ChildBufRowCol> _ChildBufRowCols;
        private static List<int> _ScoutNoList = new List<int>();

        public SequenceBuilder Builder()
        {
            var sb = new SequenceBuilder(1000, true);
            ////var sb = new SequenceBuilder(1000, false);        // With log

            sb.AddAction(SEQ_ENTER, Sequence_Enter);
            sb.AddDecision(SEQ_CHECK_INIT_CONDITION, CheckInitCondition); //분기

            sb.AddAction(SEQ_CAPTURE_NEXT_POSITION, CaptureNextPosition);
            sb.AddAction(SEQ_SAVE_IMAGE_FILE, SaveImageFile);

            sb.AddAction(SEQ_SAVE_RESULT_DATA, SaveResultData);      // 해당 공정의 정보를 기록 함.
            sb.AddAction(SEQ_WRITE_COMPLETION_PLC_BIT, WriteCompletionPLCBit);      // 정상적으로 종료 되면, complete bit write
            sb.AddAction(SEQ_COMPLETE, Complete);


            sb.Flows(sb.Init > sb[SEQ_ENTER] > sb[SEQ_CHECK_INIT_CONDITION]
                    > sb[SEQ_CAPTURE_NEXT_POSITION] > sb[SEQ_SAVE_IMAGE_FILE]
                    > sb[SEQ_SAVE_RESULT_DATA] > sb[SEQ_WRITE_COMPLETION_PLC_BIT] > sb[SEQ_COMPLETE]);

            sb.Flows(sb[SEQ_CHECK_INIT_CONDITION] > sb[SEQ_COMPLETE] > sb.Final);


            sb.OnStopped(OnStopped);
            sb.OnForceError(OnForceError);


            //sb.Draw("SeqScoutNormal.pdf");

            return sb;
        }
        private List<int> GenerateSortedScoutNoList(List<ChildBufRowCol> childBufRowCols)
        {
            var scoutNoPosIdxList = childBufRowCols.Select((childBufRowCol, index) => new { ScoutNo = index, PosIdx = childBufRowCol.PosIdx }).ToList();

            var sortedScoutNoPosIdxList = scoutNoPosIdxList.OrderBy(item => item.PosIdx).ToList();

            var sortedScoutNoList = sortedScoutNoPosIdxList.Select(item => item.ScoutNo).ToList();

            return sortedScoutNoList;
        }
        private SequenceActionResult Sequence_Enter(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.I("SeqScoutNormal Start");

            _InitParam = param.Param as SeqScoutNormal_InitParam;
            _SystemParameter = context.GetSetting<SystemParameter>(CommonSettingIds.SystemParameter);

            InitDev(context);
            if (!CheckDeviceReady(context)) { //Check Device
                AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutNormal_DeviceNotReady);
                Log.E("The devices are not active.");
                return null;
            }

            if (InitSequenceVariable(context) == false) {
                AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutNormal_InitVariableError);
                return null;
            }

            // ian0513, 이게 여기 있어야 할까??, 무엇을 측정 하기 위한건지 명확히 알고 실제 공정이면 다른 밑의 Node 로 변경
            _StopWatchTotal = new Stopwatch();
            _StopWatchTotal.Start();

            return SequenceAction.Ok();
        }

        private bool InitSequenceVariable(SequenceContext context)
        {
            // VisionDevice 추가
            // InitializeCameraDevice
            _FovRecipe = VisionRecipeManager.GetFovRecipe(_InitParam.ScoutSeqParams.FovInfo);
            _CameraAgent = CameraAgentManager.GetCameraAgent(_InitParam.ScoutSeqParams.FovInfo);
            _CameraDisplay = CameraDisplayManager.GetDisplay(_InitParam.ScoutSeqParams.FovInfo, _InitParam.ScoutSeqParams.DisplayType);
            _CamConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(eCamId.CAM1.ToString()) as CamConfig;

            // Field Image Buffer 설정
            //MILEnvironment 사용 시 Camconfig에 Channel 설정이 되어 있어야함
            MilEnvironment.Instance.AllocParentImage(_CamConfig, ref scannerFieldImage_obj);
            _ScannerFieldImage = (MIL_ID)scannerFieldImage_obj;
            //MIL.MbufAllocColor(MilSystem, 3, FieldSizeX, FieldSizeY, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC, ref _ScannerFieldImage);
            //MbufAlloc2d가 GrayImage가 맞는지 확인
            //1bit or 8bit 확인 필요
            //MIL.MbufAlloc2d(MilSystem, FieldSizeX, FieldSizeY, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC, ref ScannerFieldImage);
            // Field Image Buffer 설정 end

            //CamConfig의 PosIndex 순서로 ScoutNoList를 파싱해서 사용하도록 함
            //_ChildBufRowCols = _CamConfig.ChildImage.ChildBufRowCols;
            var mainPage = CommonHelper.GetMainPage();
            _ChildBufRowCols = mainPage.LoadScoutField();
            _ScoutNoList = GenerateSortedScoutNoList(_ChildBufRowCols);
            _ScoutNo = 0;

            return true;
        }
        private void InitDev(SequenceContext context)
        {
            //Device Init
            _Scanner = context.GetLaserScanner();
        }

        private bool CheckDeviceReady(SequenceContext context)
        {
            List<string> deviceListToCheck = new List<string>() { CommonDeviceIds.Scanner };
            bool isAllDeviceActivated = true;
            foreach (string item in deviceListToCheck) {
                if (!context.Devices[item].IsActivated) {
                    isAllDeviceActivated = isAllDeviceActivated && false;
                }
            }

            return isAllDeviceActivated;
        }

        private bool CheckCondition()
        {
            if (CheckCondition_Interlock()) {
                if (InitializeDeviceStatus()) {
                    return true;
                }
            }
            return false;
        }

        private bool CheckCondition_Interlock()
        {
            // PLC, Data, IO 등 Interlock checking
            return true;
        }

        private bool InitializeDeviceStatus()
        {
            // Device 의 status 를 맞추자. ScoutNormal 는 Scanner 를 설정 한다. 초기 설정 해야 하는 값으로
            _Scanner.SetJumpSpeed(_SystemParameter.FieldScanJumpSpeed);

            //Vision Correction File 선택
            _Scanner.SelectCorrectionTable(1);

            _CameraAgent.StopLive();

            _Scanner.SetJumpSpeedCtrl(_SystemParameter.FieldScanJumpSpeed);
            _Scanner.SetScannerDelay(500, 0, 0);

            return true;
        }

        private string CheckInitCondition(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param)
        {
            if (!CheckCondition()) {
                AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutNormal_CheckInitCondition);
                Log.E("Condition is not in state to start sequence.");
                context.PutValue(ContextKey.KEY_SEQUENCE_RESULT, false);
                return SEQ_COMPLETE;
            }

            var startTime = DateTime.Now;
            context.PutValue(ContextKey.KEY_SEQUENCE_START_TIME, startTime);

            Log.D("DoCrossLaserShot Sequence CheckInitCondition");
            return SEQ_CAPTURE_NEXT_POSITION;
        }

        private SequenceActionResult CaptureNextPosition(SequenceContext context, string id, SequenceActionResult param)
        {
            int prevRowIdx = -1;   // 직전 처리(Enabled)한 행
            foreach (var scoutNo in _ScoutNoList) {
                if (_ChildBufRowCols[scoutNo].Enabled) {
                    var stopWatchCapture = new Stopwatch();
                    stopWatchCapture.Start();

                    Log.I(string.Format($"Start No.{scoutNo + 1} Field Scan"));
                    double xPos = _ChildBufRowCols[scoutNo].MotorPos.X + _ChildBufRowCols[scoutNo].MotorPosOffset.X;   //카메라 위치에 대한 보정값 입력
                    double yPos = _ChildBufRowCols[scoutNo].MotorPos.Y + _ChildBufRowCols[scoutNo].MotorPosOffset.Y;   //카메라 위치에 대한 보정값 입력

                    Log.I(string.Format($"Move No.{scoutNo + 1} Field Scan Pos(X,Y:{xPos},{yPos})"));

                    // 1. Scanner Jump
                    _Scanner.JumpAbs(xPos, yPos);

                    // 2.SleepTime after Scanner Jump (행 전환=RowChange, 같은 행=SameRow)
                    int curRowIdx = _ChildBufRowCols[scoutNo].RowIdx;
                    bool isRowChange = (prevRowIdx != -1) && (curRowIdx != prevRowIdx);
                    int moveSleepTime = isRowChange
                        ? _SystemParameter.ScoutRowChangeSleepMs
                        : _SystemParameter.ScoutSameRowSleepMs;   //원본: int moveSleepTime = 150;//100;//50;
                    Thread.Sleep(moveSleepTime);
                    Log.I(string.Format($"Complete No.{scoutNo + 1} ScannerMove Position"));

                    // 3. ChildImage Grab
                    Log.I(string.Format($"Start Grab No.{scoutNo + 1} Field Scan Image"));
                    var stopWatchGrab = Stopwatch.StartNew();
                    object image = null;
                    _CameraAgent.GrabSync(_CameraDisplay, ref image);
                    stopWatchGrab.Stop();
                    Log.I(string.Format($"Complete Grab No.{scoutNo + 1} Field Scan Image. Grab Completed : {stopWatchGrab.ElapsedMilliseconds} msec"));

                    MIL_INT destX = _ChildBufRowCols[scoutNo].Rect.X;
                    MIL_INT destY = _ChildBufRowCols[scoutNo].Rect.Y;

                    // 4. Copy ChildImage to ParentImage
                    MIL.MbufCopyClip((MIL_ID)image, _ScannerFieldImage, destX, destY);

                    #region GrabImage 저장 Test
                    //string grabImageFilePath = Path.Combine(ConfigurationManager.AppSettings.Get("DATA_PATH"), "_vision", "FieldScanImage", "Test",
                    //                                        string.Format($"{DateTime.Now.Year}_{DateTime.Now.Month}_{DateTime.Now.Day}/{_ScoutNo + 1}_{DateTime.Now.ToString("HHmmss")}.png"));
                    //MilEnvironment.SaveImage(grabImageFilePath, (MIL_ID)image);
                    #endregion

                    stopWatchCapture.Stop();
                    Log.I(string.Format($"Complete No.{scoutNo + 1} Field Scan, Time: {stopWatchCapture.ElapsedMilliseconds} msec"));

                    prevRowIdx = curRowIdx;   // 처리(Enabled)한 위치만 갱신
                }
            }
            return SequenceAction.Ok();
        }
        private SequenceActionResult SaveImageFile(SequenceContext context, string id, SequenceActionResult param)
        {
            //6. 스캔필드 완료 후 초기 위치 0,0로 이동
            _Scanner.JumpAbs(0, 0, eFunctionType.COMMAND);
            Log.I(string.Format($"Complete Scanner mover Origin"));

            if (_InitParam.IsRealTimeScout) {
                // real time 이면, 그냥 로그만 한줄 찍자.
                Log.I(string.Format("When Real Time Scout Seq is running, Pass Save Image File"));

            } else {
                Log.I(string.Format($"Start to store completed field scan image"));

                //4.스캐너 필드 이미지 파일 저장
                // TODO: Path 설정하는 것 따로 만들어서 넣기. 여기저기 흩어져 있음...
                string fieldScanImageFile = Path.Combine(ConfigurationManager.AppSettings.Get("DATA_PATH"), "_vision", "FieldScanImage",
                    string.Format($"{DateTime.Now.Year}_{DateTime.Now.Month}_{DateTime.Now.Day}/{_ScoutNo + 1}_{DateTime.Now.ToString("HHmmss")}.PNG"));
                _SystemParameter.ScannerFieldImageFile = fieldScanImageFile;

                //5.압축하여 저장
                ImageUtil.SaveFieldScanImageJPG(_ScannerFieldImage, _SystemParameter.ScannerFieldImageFile, 10, false);

                //7. 캔버스 필드 이미지 업데이트(AutoRun, Teaching 화면 업데이트), SystemparameterILSOS의 ScannerFieldImageFile의 경로를 변경해주면 Teaching에서 자동 변경 됨
                context.Settings.Save(CommonSettingIds.SystemParameter, _SystemParameter, "SeqScout");
            }

            return SequenceAction.Ok();
        }

        private SequenceActionResult SaveResultData(SequenceContext context, string id, SequenceActionResult param)
        {
            // 결과 Data 를 업데이트 함.
            context.PutValue(ContextKey.KEY_SEQUENCE_RESULT, true);
            return SequenceAction.Ok();
        }

        private SequenceActionResult WriteCompletionPLCBit(SequenceContext context, string id, SequenceActionResult param)
        {
            // 완료 비트 write 하는 Node 추가 필요. format 유지, SeqScoutNormal 없음

            Log.D("SeqMarkGrid Sequence WriteCompletionPLCBit");
            return SequenceAction.Ok();
        }

        private SequenceActionResult Complete(SequenceContext context, string id, SequenceActionResult param)
        {
            RestoreDeviceStatus();
            Log.I("ScoutNormal Complete");

            context.TryGetValue(ContextKey.KEY_SEQUENCE_RESULT, out bool seqResult);
            SeqScoutNormal_OutParam result = new SeqScoutNormal_OutParam() { Success = seqResult };

            if (seqResult == false) {
                AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutNormal_ResultFailed);
            }

            Log.D("ScoutNormal Sequence Complete");
            return SequenceAction.Ok(result);
        }

        private bool RestoreDeviceStatus()
        {
            if (_InitParam.IsRealTimeScout) {
                if (_ScannerFieldImage != null) {
                    Status.GetProcessingWeldingData().ScannerFiledImage = _ScannerFieldImage;
                }
            } else {

                //8. ScannerFieldImage, _GrabImage Buffer Free
                MIL.MbufFree(_ScannerFieldImage);
                //MIL.MbufFree(_GrabImage);
            }

            //Camera Live 시작
            //Scout 완료 후 Camera Live는 불필요
            //_CameraAgent.StartLive(_CameraDisplay);

            _StopWatchTotal.Stop();
            Log.I(string.Format($"Total Field Scan Time([s]: {(float)_StopWatchTotal.ElapsedMilliseconds / 1000})"));

            return true;
        }

        private void OnForceError(SequenceContext arg1, string arg2)
        {
            Log.E("SeqScoutNormal Sequence is stopped");
        }

        private void OnStopped(SequenceContext obj)
        {
            Log.E("SeqScoutNormal Sequence is stopped");
        }

    }

    public class SeqScoutNormal_InitParam : InitParam
    {
        public bool IsRealTimeScout { get; set; }
        public ScoutSeqParams ScoutSeqParams { get; set; }

    }

    public class SeqScoutNormal_OutParam : OutParam
    {

    }
}