Untitled

using System;
using System.IO;
using SystemTypes;
using AcqIPC;
using Camtek.CoordinateSystems;
using CamtekSystem;
using FocusFramesProcessors;
using AutoFocusScanner;
using FocusMap.FocusFrameProcessorInterfaces;
using MachineDataTypesLib;
using MachineSrv;
using ManagedMil.Buffer;
using Camtek.WaferInfo.Tools;
using System.Threading;
using System.Runtime.InteropServices;
using CamtekSystem.Tracing;
using AutoFocus.Common;
using CamtekSystem.MMF.ConsumersAndProducers;
using CamtekSystem.MMF;

namespace AutoFocus
{
    public class AreaCameraAutoFocusActivity : DisposableObject, ILiveAutofocusActivity
    {
        private SImageShift _focusRectangleSize;
        private readonly MachineConnector _machineConnector;
        private readonly IMachineObj _machine;
        private readonly IAreaCamera _camera;
        private readonly AcqIpcConnector _acqIpcConnector;

        // Steps        
        private readonly ZShift<StageVCS> _focusStep;

        private readonly double _fineFocusStepInDoFFactor = 1;
        private readonly ZShift<StageVCS> _fineFocusStep;

        private readonly double _coarseStepInDoFFactor = 10;
        private readonly ZShift<StageVCS> _coarseFocusStep;
        private  int _minFramesCount = 5;

        private readonly double _motorAccDecDistanceUm = 200;

        // Fine Ranges
        private readonly double _fineFocusRange1Um = 300;
        private readonly ZShift<StageVCS> _fineFocusRange1;

        private readonly double _fineFocusRange2InCoarseStepFactor = 2;
        private readonly ZShift<StageVCS> _fineFocusRange2;

        private readonly double _fineScoreThreashold = 90;

        // Coarse Range
        private readonly bool _tryFullRange;
        private ZCoord<StageVCS> _minFocusLimitZ;
        private ZCoord<StageVCS> _maxFocusLimitZ;

        private readonly ZCoord<ChuckVCS> _defaultCoarseMinOffsetAboveChuckUm = new ZCoord<ChuckVCS>(-1500);
        private readonly ZCoord<ChuckVCS> _defaultCoarseMaxOffsetAboveChuckUm = new ZCoord<ChuckVCS>(4500);

        private readonly ZShift<StageVCS> _defaultCoarseRangeAroundWaferThicknessUm = 1000;

        private readonly ZCoord<StageVCS> _coarseMinLimit;
        private readonly ZCoord<StageVCS> _coarseMaxLimit;

        private readonly double _coarseRangeEquivalenceFactor = 1.5;

        // FFP
        private PeakDetectorRtp _ffpParameters;
        private IMilImageToGrayScaleTranslator _colorToGrayTanslator;
        private const string FfpParametersConfigurationFile = @"c:\bis\data\apps\LiveAutoFocus.ini";
        private readonly string BaseFfpConfigurationPath = $@"c:\Falcon\Data\\Machine\{Environment.MachineName}\Cameras\";
        private readonly string _debugOuputPath = "";
        private readonly int _debugMode = 0;

        // Focus Fusion
        private int DefaultBlockSize = 3;
        private SmoothColorFusionOperator DefaultFocusOperator = SmoothColorFusionOperator.VarLocal;
        private GrayFocusFusionOperator DefaultFocusColorOperator = GrayFocusFusionOperator.VarLocal;

        // Simulator
        private string _frameStorageRootPath;

        // Processing
        private Thread _autoFocusThread;
        private SStageZCoord _autoFocusThreadResult;
        private AutoFocusScanner.AutoFocusScanner _scanner;
        private bool _bCancel;
        private string _lastError;
        private bool _bCompletion;        

        [Flags]
        private enum RangeResult
        {
            Regular = 0x01,
            MemoryLimitHit = 0x02,
            SoftwareLimitHit = 0x04,
            CoarseLimitHit = 0x08,
        }

        private byte[] _mask;
        private readonly FocusType _focusType;

        public AreaCameraAutoFocusActivity(SImageShift focusRectangleSize, SChuckZCoord startPosAboveChuck,bool useChuckSurface, FocusType focusType = FocusType.BestImage)
            : this(focusRectangleSize, false, useChuckSurface, focusType)
        {
            Trace.LiveAutoFocus.Flow("Create LiveAutoFocusActivity({0}, {1});", focusRectangleSize, startPosAboveChuck);

            SChuckPlaneCoord currentChuckPos = _machine.GetCurrentChuckPos();
            ZCoord<StageVCS> startZ = Converter.Convert( 
                _machine.GetZPostionInStageCoord(currentChuckPos, startPosAboveChuck,useChuckSurface));

            ZShift<StageVCS> coarseRange = _defaultCoarseRangeAroundWaferThicknessUm;

            _coarseMinLimit = ZCoord<StageVCS>.Max(_minFocusLimitZ, startZ - coarseRange * 0.5);
            _coarseMaxLimit = ZCoord<StageVCS>.Min(_maxFocusLimitZ, startZ + coarseRange * 0.5);
        }

        public AreaCameraAutoFocusActivity(SImageShift focusRectangleSize, SChuckZCoord customMinCoarseOffsetAboveChuck, SChuckZCoord customMaxCoarseOffsetAboveChuck,bool useChuckSurface, FocusType focusType = FocusType.BestImage)
            : this(focusRectangleSize, false, useChuckSurface, focusType)
        {
            Trace.LiveAutoFocus.Flow("Create LiveAutoFocusActivity({0}, {1}, {2})", focusRectangleSize, customMinCoarseOffsetAboveChuck, customMaxCoarseOffsetAboveChuck);

            SChuckPlaneCoord currentChuckPos = _machine.GetCurrentChuckPos();

            ZCoord<StageVCS> curstomMin = 
                Converter.Convert(_machine.GetZPostionInStageCoord(currentChuckPos, customMinCoarseOffsetAboveChuck, useChuckSurface));

            ZCoord<StageVCS> curstomMax =
                Converter.Convert(_machine.GetZPostionInStageCoord(currentChuckPos, customMaxCoarseOffsetAboveChuck, useChuckSurface));

            _coarseMinLimit = ZCoord<StageVCS>.Max(_minFocusLimitZ, ZCoord<StageVCS>.Min(curstomMin, curstomMax));
            _coarseMaxLimit = ZCoord<StageVCS>.Min(_maxFocusLimitZ, ZCoord<StageVCS>.Max(curstomMin, curstomMax));
        }

        public AreaCameraAutoFocusActivity(SImageShift focusRectangleSize, bool tryFullRange,bool useChuckSurface, FocusType focusType = FocusType.BestImage)
        {
            Trace.LiveAutoFocus.Flow("Create LiveAutoFocusActivity({0}, false);", focusRectangleSize);

            _focusType = focusType;

            _focusRectangleSize = focusRectangleSize;

            _autoFocusThread = null;
            _autoFocusThreadResult = new SStageZCoord();

            _machineConnector = new MachineConnector();
            _machine = _machineConnector.MachineObject;
            _camera = _machineConnector.AreaCamera;
            if (_camera == null || _camera.Type == eCAMERA_TYPE.NO_CAMERA)
            {
                throw new NullReferenceException("LiveAutoFocusActivity: Cannot create active camera object!");
            }

            _acqIpcConnector = new AcqIpcConnector(AcqIpcProcMode.Remote)
            {
                DefultTaskPoolType = AcqTaskPoolType.Process
            };
            _acqIpcConnector.SetAcqDataSize((int)(_camera.size.x * _camera.size.y * _camera.CameraDigitizer.bands));
            _acqIpcConnector.Reset();

            _fineScoreThreashold = IniFileHelper.GetIni("FocusRouteRtp", "FineScoreThreshold", FfpParametersConfigurationFile, _fineScoreThreashold);
            _fineFocusStepInDoFFactor = IniFileHelper.GetIni("FocusRouteRtp", "FineFocusStepInDoF_Factor", FfpParametersConfigurationFile, _fineFocusStepInDoFFactor);
            _coarseStepInDoFFactor = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    GetAutoFocusParameter("CoarseFocusStepInDoF_Factor_x", _coarseStepInDoFFactor) :
                    IniFileHelper.GetIni("FocusRouteRtp", "CoarseFocusStepInDoF_Factor", FfpParametersConfigurationFile, _coarseStepInDoFFactor);
            _motorAccDecDistanceUm = IniFileHelper.GetIni("FocusRouteRtp", "MotorAccDecDistance_Um", FfpParametersConfigurationFile, _motorAccDecDistanceUm);

            _fineFocusRange1Um = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    GetAutoFocusParameter("FocusScanRange_x", _fineFocusRange1Um) :
                    IniFileHelper.GetIni("FocusRouteRtp", "FineFocusRange1_Um", FfpParametersConfigurationFile, _fineFocusRange1Um);
            _fineFocusRange2InCoarseStepFactor = FineFocusRange2InCoarseStepFactor();

            _defaultCoarseMinOffsetAboveChuckUm = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    new ZCoord<ChuckVCS>(GetAutoFocusParameter("DefaultCoarseMinOffsetAboveChuck_Um_x", _defaultCoarseMinOffsetAboveChuckUm.Z)) :
                    new ZCoord<ChuckVCS>(IniFileHelper.GetIni("FocusRouteRtp", "DefaultCoarseMinOffsetAboveChuck_Um", FfpParametersConfigurationFile, _defaultCoarseMinOffsetAboveChuckUm.Z));
            _defaultCoarseMaxOffsetAboveChuckUm = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    new ZCoord<ChuckVCS>(GetAutoFocusParameter("DefaultCoarseMaxOffsetAboveChuck_Um_x", _defaultCoarseMaxOffsetAboveChuckUm.Z)) :
                    new ZCoord<ChuckVCS>(IniFileHelper.GetIni("FocusRouteRtp", "DefaultCoarseMaxOffsetAboveChuck_Um", FfpParametersConfigurationFile, _defaultCoarseMaxOffsetAboveChuckUm.Z));

            _defaultCoarseRangeAroundWaferThicknessUm = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    new ZShift<StageVCS>(GetAutoFocusParameter("DefaultCoarseRangeAroundWaferThickness_Um_x", _defaultCoarseRangeAroundWaferThicknessUm.Z)) :
                    new ZShift<StageVCS>(IniFileHelper.GetIni("FocusRouteRtp", "DefaultCoarseRangeAroundWaferThickness_Um", FfpParametersConfigurationFile, _defaultCoarseRangeAroundWaferThicknessUm.Z));

            _coarseRangeEquivalenceFactor = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA || _camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA) ?
                    GetAutoFocusParameter("CoarseRangeEquivalenceFactor_x", _coarseRangeEquivalenceFactor) :
                    IniFileHelper.GetIni("FocusRouteRtp", "CoarseRangeEquivalenceFactor", FfpParametersConfigurationFile, _coarseRangeEquivalenceFactor);

            _debugOuputPath = IniFileHelper.GetIni("PeakDetectorRtp", "SaveDebugInfo", FfpParametersConfigurationFile, _debugOuputPath);
            _debugOuputPath.Trim();

            _debugMode = IniFileHelper.GetIni("DebugMode", "Enable", FfpParametersConfigurationFile, _debugMode);

            _frameStorageRootPath = IniFileHelper.GetIni("Simulator", "SaveFramesRootPath", FfpParametersConfigurationFile, "");

            ZCoord<StageVCS> minMotorZStage = new ZCoord<StageVCS>(_camera.Motor.MinPos + _motorAccDecDistanceUm);
            ZCoord<StageVCS> maxMotorZStage = new ZCoord<StageVCS>(_camera.Motor.MaxPos - _motorAccDecDistanceUm);
            _minFocusLimitZ = ZCoord<StageVCS>.Min(minMotorZStage, maxMotorZStage);
            _maxFocusLimitZ = ZCoord<StageVCS>.Max(minMotorZStage, maxMotorZStage);
            _tryFullRange = tryFullRange;
            
            _focusStep = new ZShift<StageVCS>(_camera.CurrentFocusStep);            
            _fineFocusStep = _focusStep * _fineFocusStepInDoFFactor;
            ZShift<StageVCS> maxCoarseFocusStep = IniFileHelper.GetIni("FocusRouteRtp", "MaxCoarseStep",
                                                                       FfpParametersConfigurationFile, 500);            
            _coarseFocusStep = (_focusStep * _coarseStepInDoFFactor).Z  < maxCoarseFocusStep.Z ?_focusStep * _coarseStepInDoFFactor : maxCoarseFocusStep.Z ;

            _fineFocusRange1 = new ZShift<StageVCS>(_fineFocusRange1Um);
            _fineFocusRange2 = new ZShift<StageVCS>(_coarseFocusStep.Z * _fineFocusRange2InCoarseStepFactor);
            
            Trace.LiveAutoFocus.Info("Focus stp: {0} for Mag: {1}", _focusStep, _camera.Zoom[eZOOM_RING_TYPE.NUM_OF_ZOOM_RINGS].Value);
            Trace.LiveAutoFocus.Info("Fine Focus Step: {0}", _fineFocusStep);
            Trace.LiveAutoFocus.Info("Coarse Focus Step: {0}", _coarseFocusStep);

            Trace.LiveAutoFocus.Info("Fine Score Threashold: {0}", _fineScoreThreashold);
            Trace.LiveAutoFocus.Info("Fine Focus Step In DoF Factor: {0}", _fineFocusStepInDoFFactor);
            Trace.LiveAutoFocus.Info("Coarse Step In DoF Factor: {0}", _coarseStepInDoFFactor);
            Trace.LiveAutoFocus.Info("Try Full Range: {0}", _tryFullRange);

            Trace.LiveAutoFocus.Info("Fine Focus Range 1: {0}", _fineFocusRange1);
            Trace.LiveAutoFocus.Info("Fine Focus Range 2: {0}", _fineFocusRange2);

            Trace.LiveAutoFocus.Info("Default Coarse Focus Min Offset Above Chuck (um): {0}", _defaultCoarseMinOffsetAboveChuckUm);
            Trace.LiveAutoFocus.Info("Default Coarse Focus Max Offset Above Chuck (um): {0}", _defaultCoarseMaxOffsetAboveChuckUm);
            Trace.LiveAutoFocus.Info("Default Coarse Range Around Wafer Thickness (um): {0}", _defaultCoarseRangeAroundWaferThicknessUm);


            SChuckPlaneCoord currentChuckPos = _machine.GetCurrentChuckPos();

            ZCoord<StageVCS> lowPos = Converter.Convert(
                _machine.GetZPostionInStageCoord(currentChuckPos, 
                                Converter.Convert(_defaultCoarseMinOffsetAboveChuckUm), useChuckSurface));

            ZCoord<StageVCS> highPos = Converter.Convert(
                _machine.GetZPostionInStageCoord(currentChuckPos, 
                                Converter.Convert(_defaultCoarseMaxOffsetAboveChuckUm), useChuckSurface));

            _coarseMinLimit = ZCoord<StageVCS>.Min(lowPos, highPos);
            _coarseMaxLimit = ZCoord<StageVCS>.Max(lowPos, highPos);
        }

        private double GetAutoFocusParameter(string paramName, double defaultValue)
        {
            double value = 0;
            string fileName = "calib.ini";
            string fullConfigurationPath = Path.Combine(GetFfpConfigurationPath(), fileName);
            string paramNamePerMag = paramName + _camera.Zoom[eZOOM_RING_TYPE.NUM_OF_ZOOM_RINGS].Value.ToString("F2");
            value = IniFileHelper.GetIni(paramNamePerMag, "Value", fullConfigurationPath, defaultValue);
            return value;
        }

        private string GetFfpConfigurationPath()
        {
            string cameraType = (_camera.Type == eCAMERA_TYPE.HIGH_MAG_CAMERA) ? "HighMag" : "VeryHighMag";
            string configurationPath = Path.Combine(BaseFfpConfigurationPath, cameraType);
            return configurationPath;
        }

        private double FineFocusRange2InCoarseStepFactor()
        {
            switch (_camera.Type)
            {
                case eCAMERA_TYPE.HIGH_MAG_CAMERA:
                    return GetAutoFocusParameter("FineFocusRange2InCoarseStep_Factor_x", 2);
                case eCAMERA_TYPE.VERYHIGH_MAG_CAMERA:
                    return GetAutoFocusParameter("FineFocusRange2InCoarseStepColor_Factor_x", 6);
                case eCAMERA_TYPE.IR_SCAN_CAMERA:
                    return IniFileHelper.GetIni("FocusRouteRtp", "FineFocusRange2InCoarseStepIRScan_Factor", FfpParametersConfigurationFile, 4);
                default:
                    return IniFileHelper.GetIni("FocusRouteRtp", "FineFocusRange2InCoarseStep_Factor", FfpParametersConfigurationFile, 2);
            }
        }

        public void BeginAsyncFocus()
        {
            _autoFocusThreadResult.z = 0;
            _autoFocusThread = new Thread(FocusThread);
            _autoFocusThread.Start();
        }

        private void FocusThread ()
        {
            try
            {
                _autoFocusThreadResult = Focus();
            }
            catch (Exception e)
            {
                Trace.LiveAutoFocus.Error("Faild to Focus. Exception: {0}", e);
                _lastError = "AutoFocus Failed in Focus";
                Trace.LiveAutoFocus.Error("Faild to Focus. Exception: {0}", _lastError);
                _bCompletion = false;
            }
            finally
            {

                if (AsyncFocusCompleted != null)
                {
                    AsyncFocusCompleted(_autoFocusThreadResult, _bCompletion);
                }
            }
        }

        public void Cancel()
        {
            _bCancel = true;
            if(null != _scanner)
            {
                _scanner.Cancel();
            }
        }

        public bool WaitForAsyncFocusCompleted(int millisecondsTimeout, [Out] out SStageZCoord focusPositionAboveChuck, [Out] out bool bSuccess)
        {
            bool threadExited = _autoFocusThread.Join(millisecondsTimeout);
            focusPositionAboveChuck = _autoFocusThreadResult;
            bSuccess = _bCompletion;
            return threadExited;
        }

        public event AsyncFocusCompletedHandler AsyncFocusCompleted;

        public SStageZCoord Focus()
        {
            ZCoord<StageVCS> positionBeforeAutoFocus = new ZCoord<StageVCS>();
            IFocusFrameProcessor focusFrameProcessor = null;
			bool bNeedRestoreToLive = false;
            try
            {
                //int frameSizeX = (int)_camera.size.x;
                //int frameSizeY = (int)_camera.size.y;
                SImageShift processingFrameSize = new SImageShift()
                {
                    x = Math.Min(1024, _camera.size.x),
                    y = Math.Min(1024, _camera.size.y)
                };

                _mask = CreateFfpMask(_focusRectangleSize, processingFrameSize);
                _ffpParameters = InitFppParameters(out _colorToGrayTanslator);

                int focusOperatorFine1 = IniFileHelper.GetIni("PeakDetectorRtp", "Operator", FfpParametersConfigurationFile, 1);
                _ffpParameters.Operator = GetFfpOperator(focusOperatorFine1);


                if(_ffpParameters.Operator== FocusOperator.FoucsDistanceEstimator)
				{
                    string fdeModelFilePath = GetFdeModelFilePath(_camera);
                    FdeParameters fdeParameters = FdeParameters.ReadFromFile($"{fdeModelFilePath}");
                    _ffpParameters.FdeParameters = fdeParameters;
                }

                _ffpParameters.FocusRange = _camera.CurrentFocusRange;
                _ffpParameters.DebugFileFullPath = String.IsNullOrEmpty(_debugOuputPath) ? "" : Path.Combine(_debugOuputPath, "fineFocusRange_1.txt");

                focusFrameProcessor = CreateProcessor(_debugMode, (int)processingFrameSize.x, (int)processingFrameSize.y, _mask, _ffpParameters, _colorToGrayTanslator);                

                positionBeforeAutoFocus = new ZCoord<StageVCS>(_camera.Motor.Position);
                ZCoord<StageVCS> bestKnownFocus = positionBeforeAutoFocus;

                //Fine around the current position
                ZCoord<StageVCS> start;
                ZCoord<StageVCS> stop;

                RangeResult rangeResult = TryCreateScanRange(bestKnownFocus, _fineFocusRange1, _fineFocusStep, 1, out start, out stop);

                if (_bCancel)
                {
                    _lastError = "Auto-Focus failed: user Cancel";
                    Trace.LiveAutoFocus.Warning("Faild to Focus. Exception: {0}", _lastError);
                    _bCompletion = false;
                    return Converter.Convert(positionBeforeAutoFocus);
                }
                AddXYPositionToFrameStorageRootPath();
                double score;

                if (_camera.IsLiveVideoActive)
                {
                    _camera.StopLiveVideo();
                    bNeedRestoreToLive = true;
                }

                using (_scanner = AutoFocusScannerFactory.CreateScanner(start, stop, _fineFocusStep,
                                                                 string.IsNullOrEmpty(_frameStorageRootPath) ? _frameStorageRootPath : Path.Combine(_frameStorageRootPath, "fineFocusRange_1")))
                {
                    Trace.LiveAutoFocus.Flow("Start scanning fine range first time. Start Z: {0}, Stop Z: {1}, Step: {2}", start, stop, _fineFocusStep);
                    bestKnownFocus = _scanner.Focus(focusFrameProcessor, processingFrameSize, out score);
                    Trace.LiveAutoFocus.Flow("Finish scanning fine range first time. Score Threshold: {0}, Score : {1}, Focus: {2}", _fineScoreThreashold, score, bestKnownFocus);
                }

                SaveBestImage(focusFrameProcessor, "FineFocus1.tif");

                if (_focusType == FocusType.Fusion)
                {
                    _bCompletion = true;

                    SendBestImage(focusFrameProcessor);

                    MoveCameraTo(positionBeforeAutoFocus);
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                    focusFrameProcessor?.DisposeResources();
                    return Converter.Convert(positionBeforeAutoFocus);
                }

                if (score >= _fineScoreThreashold)
                {
                    //Fine peak found
                    _bCompletion = true; 

                    MoveCameraTo(bestKnownFocus);
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                    focusFrameProcessor?.DisposeResources();
                    return Converter.Convert(bestKnownFocus);
                }

                //Coarse scan between coarse limits
                score = 0;
                ZShift<StageVCS> coarseRange = ZShift<StageVCS>.Min(_coarseMaxLimit - _coarseMinLimit, GetFramesMemoryLimit() * _coarseFocusStep);
                ZCoord<StageVCS> coarseCenter = _coarseMinLimit + 0.5 * coarseRange;

                if (((rangeResult & RangeResult.CoarseLimitHit) != 0) && !_tryFullRange && NeerCoarseCenter(coarseCenter, start, stop))
                {
                    //No need to try coarse, the previous fine scan was the coarse equivalent (for example for mag x1.0)
                    _bCompletion = true;
                    MoveCameraTo(positionBeforeAutoFocus);
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                    focusFrameProcessor?.DisposeResources();
                    return Converter.Convert(positionBeforeAutoFocus);
                }

                rangeResult = RangeResult.Regular;
                bool continueCoarseScans = !_bCancel;
                ZCoord<StageVCS> bestCoarseKnownFocus = bestKnownFocus;

                int coarseScanIndex = 0;
                int coarseScore = IniFileHelper.GetIni("FocusRouteRtp", "CoarseScore", FfpParametersConfigurationFile, 55);
                while (continueCoarseScans && (score <= coarseScore) && ((rangeResult & RangeResult.SoftwareLimitHit) == 0))
                {
                    continueCoarseScans = _tryFullRange && !_bCancel;
                    rangeResult = TryCreateScanRange(coarseCenter, coarseRange, _coarseFocusStep, 1, out start, out stop);

                    int focusOperator4CoarseScan = IniFileHelper.GetIni("PeakDetectorRtp", "CoarseOperator", FfpParametersConfigurationFile, 6);
                    _ffpParameters.Operator = GetFfpOperator(focusOperator4CoarseScan);
                    
                    _ffpParameters.FocusRange = _camera.CurrentFocusRange;
                    _ffpParameters.DebugFileFullPath = String.IsNullOrEmpty(_debugOuputPath) ? "" : Path.Combine(_debugOuputPath, "corseFocusRange" + coarseScanIndex + ".txt");
                    _ffpParameters.PyramidLevel = 0;

                    focusFrameProcessor?.DisposeResources();
                    focusFrameProcessor = CreateProcessor(_debugMode, (int)processingFrameSize.x, (int)processingFrameSize.y, _mask, _ffpParameters, _colorToGrayTanslator);
                    using (_scanner = AutoFocusScannerFactory.CreateScanner(start, stop, _coarseFocusStep,
                        string.IsNullOrEmpty(_frameStorageRootPath) ? _frameStorageRootPath : Path.Combine(_frameStorageRootPath, string.Format("coarse_range_", coarseScanIndex))))
                    {
                        Trace.LiveAutoFocus.Flow("Start scanning coarse range. Start Z: {0}, Stop Z: {1}, Step: {2}", start, stop, _coarseFocusStep);
                        bestCoarseKnownFocus = _scanner.Focus(focusFrameProcessor, processingFrameSize, out score);
                        Trace.LiveAutoFocus.Flow("Finish scanning coarse range. Score : {0}, Focus: {1}", score, bestKnownFocus);
                    }

                    coarseCenter += coarseRange - _coarseFocusStep * 2.5;
                    coarseScanIndex++;

                    string imageName = "CoarseFocus" + coarseScanIndex + ".tif";
                    SaveBestImage(focusFrameProcessor, imageName);
                }
                if (score == 0 || _bCancel)
                {
                    //Auto-Focus failed: No Fine peak could be found
                    _lastError = "Auto-Focus failed: No Fine peak could be found";
                    Trace.LiveAutoFocus.Warning("Faild to Focus. Exception: {0}", _lastError);
                    _bCompletion = false;
                    MoveCameraTo(positionBeforeAutoFocus);
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                    focusFrameProcessor?.DisposeResources();
                    return Converter.Convert(positionBeforeAutoFocus);
                }

                // Fine around the found peak
                TryCreateScanRange(bestCoarseKnownFocus, _fineFocusRange2, _fineFocusStep, -1, out start, out stop);

                int focusOperatorFine2 = IniFileHelper.GetIni("PeakDetectorRtp", "Operator", FfpParametersConfigurationFile, 1);
                _ffpParameters.Operator = GetFfpOperator(focusOperatorFine2);
                
                _ffpParameters.FocusRange = _camera.CurrentFocusRange;
                _ffpParameters.DebugFileFullPath = String.IsNullOrEmpty(_debugOuputPath) ? "" : Path.Combine(_debugOuputPath, "fineFocusRange_2.txt");
                focusFrameProcessor?.DisposeResources();
                focusFrameProcessor = CreateProcessor(_debugMode, (int)processingFrameSize.x, (int)processingFrameSize.y, _mask, _ffpParameters, _colorToGrayTanslator);
                using (_scanner = AutoFocusScannerFactory.CreateScanner(start, stop, _fineFocusStep,
                    string.IsNullOrEmpty(_frameStorageRootPath) ? _frameStorageRootPath : Path.Combine(_frameStorageRootPath, "fineFocusRange_2")))
                {
                    Trace.LiveAutoFocus.Flow("Start scanning fine range second time. Start Z: {0}, Stop Z: {1}, Step: {2}", start, stop, _fineFocusStep);
                    bestKnownFocus = _scanner.Focus(focusFrameProcessor, processingFrameSize, out score);
                    Trace.LiveAutoFocus.Flow("Finish scanning fine range second time. Score Threshold: {0}, Score : {1}, Focus: {2}", _fineScoreThreashold, score, bestKnownFocus);
                }


                SaveBestImage(focusFrameProcessor, "FineFocus2.tif");


                if (score >= _fineScoreThreashold)
                {
                    //Fine peak found
                    _bCompletion = true;
                    MoveCameraTo(bestKnownFocus);
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                    focusFrameProcessor?.DisposeResources();
                    return Converter.Convert(bestKnownFocus);
                }
                //Auto-Focus failed: No Fine peak could be found
                _lastError = "Auto-Focus failed: No Fine peak could be found";
                Trace.LiveAutoFocus.Warning("Faild to Focus. Exception: {0}", _lastError);
                _bCompletion = false;
                MoveCameraTo(bestCoarseKnownFocus);
                if (bNeedRestoreToLive)
                {
                    Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                    _camera.StartLiveVideo(ref sMatrix);
                }
                focusFrameProcessor?.DisposeResources();
                return Converter.Convert(bestCoarseKnownFocus);
            }
            catch (Exception e)
            {
                Trace.LiveAutoFocus.Error("Faild to Focus. Exception: {0}", e);
                _lastError = "AutoFocus Failed in Focus";
                _bCompletion = false;
                try
                {
                    if (bNeedRestoreToLive)
                    {
                        Cfc2_DataDictionary.STransformMatrix sMatrix = _machine.StageToTableMatrix;
                        _camera.StartLiveVideo(ref sMatrix);
                    }
                }
                catch (Exception ex)
                {
                    Trace.LiveAutoFocus.Error("Faild to restore live video. Exception: {0}", ex);
                }
                return Converter.Convert(positionBeforeAutoFocus);
            }
            finally
            {
                focusFrameProcessor?.DisposeResources();
            }
        }

        private void AddXYPositionToFrameStorageRootPath()
        {
            Coord<ChuckCS> chuckXY = Converter.Convert( _machine.GetCurrentChuckPos());
            string frameStorageName = string.Format(@"{0:F0}_{1:F0}", chuckXY.X, chuckXY.Y);
            eCAMERA_TYPE type = _machine.active_camera;
            _frameStorageRootPath = string.IsNullOrEmpty(_frameStorageRootPath)
                                        ? _frameStorageRootPath : Path.Combine(_frameStorageRootPath, type.ToString());
            _frameStorageRootPath = string.IsNullOrEmpty(_frameStorageRootPath)
                                        ? _frameStorageRootPath : Path.Combine(_frameStorageRootPath, frameStorageName);
            
           
        }

        private void MoveCameraTo(ZCoord<StageVCS> bestKnownFocus)
        {
            DateTime startWait = DateTime.Now;
            bool bTimeOut = false;
            if(_camera.Motor.IsMoving)
            {
                while(_camera.Motor.IsMoving && !bTimeOut)
                {
                    Thread.Sleep(10);
                    bTimeOut = (DateTime.Now - startWait).TotalMilliseconds > 30000;
                }
                Trace.LiveAutoFocus.Info($"MoveCameraTo exit loop while moving after {(DateTime.Now- startWait).TotalMilliseconds} msec");
            }
            if(!bTimeOut)
            {
                _camera.Motor.MoveTo(bestKnownFocus.Z);
            }
            else
            {
                Trace.LiveAutoFocus.Error("MoveCameraTo fail because of timeout of moving");
            }
        }

        private bool NeerCoarseCenter( ZCoord<StageVCS> coarseCenter, ZCoord<StageVCS> start, ZCoord<StageVCS> stop)
        {
            return ((start.Z + stop.Z) / 2 > (coarseCenter.Z - _fineFocusStep.Z) && (start.Z + stop.Z) / 2 < (coarseCenter.Z + _fineFocusStep.Z));
        }

        private IFocusFrameProcessor CreateProcessor(int debugMode, int frameSizeX, int frameSizeY, byte[] focusMask, PeakDetectorRtp rtp, IMilImageToGrayScaleTranslator colorToGrayTranslator)
        {
            eCAMERA_TYPE typeOfCamera = _camera.Type;

            switch (_focusType)
            {
                case FocusType.Fusion:

                    if (typeOfCamera == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA)
                    {
                        SmoothColorFusionRtp fusionRtp = new SmoothColorFusionRtp();

                        fusionRtp.FrameSizeX = (int)_camera.size.x;
                        fusionRtp.FrameSizeY = (int)_camera.size.y;

                        fusionRtp.BlockSize = DefaultBlockSize;
                        fusionRtp.focusOperator = DefaultFocusOperator;

                        return new SmoothColorFusion(fusionRtp);
                    }
                    else
                    {
                        FocusFusionRtp fusionRtp = new FocusFusionRtp();

                        fusionRtp.FrameSizeX = (int)_camera.size.x;
                        fusionRtp.FrameSizeY = (int)_camera.size.y;

                        fusionRtp.BlockSize = DefaultBlockSize;
                        fusionRtp.focusOperator = DefaultFocusColorOperator;

                        return new FocusFusionGray(fusionRtp);
                    }
                    
                case FocusType.BestImage:
                    if (debugMode == 1 && colorToGrayTranslator == null)
                    {
                        return new FocusBestImagePDGray(frameSizeX, frameSizeY, focusMask, rtp);
                    }

                    if (debugMode == 1 && colorToGrayTranslator != null)
                    {
                        return new FocusBestImagePDColor(frameSizeX, frameSizeY, focusMask, rtp, colorToGrayTranslator);
                    }

                    return new FocusProcessorLive(frameSizeX, frameSizeY, focusMask, rtp, colorToGrayTranslator);
            }            

            return new FocusProcessorLive(frameSizeX, frameSizeY, focusMask, rtp, colorToGrayTranslator);
        }

        private void SaveBestImage(IFocusFrameProcessor focusProcessor, string imageName)
        {
            string imagePath = String.IsNullOrEmpty(_debugOuputPath) ? "" : Path.Combine(_debugOuputPath, imageName);
            if (!String.IsNullOrEmpty(_debugOuputPath) && !Directory.Exists(_debugOuputPath))
            {
                try
                {
                    Directory.CreateDirectory(_debugOuputPath);
                }
                catch (Exception e)
                {
                    Trace.LiveAutoFocus.Error("Faild CreateDirectory: {0}", e);
                    imagePath = "";
                }
            }
            
            var bestImageProvider = focusProcessor as IBestImageFocusFrameProcessor;

            if (bestImageProvider != null)
            {
                MilImage bestImage = bestImageProvider.GetBestImage();
                bestImage.Save(imagePath);

                if (_focusRectangleSize.x < bestImage.SizeX && _focusRectangleSize.y < bestImage.SizeY)
                {
                    int offsetX = (int)(bestImage.SizeX - _focusRectangleSize.x) / 2;
                    int offsetY = (int)(bestImage.SizeY - _focusRectangleSize.y) / 2;
                    using (var afRoi = bestImage.GetChild(offsetX, offsetY, (int)_focusRectangleSize.x, (int)_focusRectangleSize.y))
                    {
                        afRoi.Save(imagePath.Substring(0, imagePath.Length - 4) + "_roi.tif");
                    }
                }
            }
        }

        private byte[] GetBufferFormImage(MilImage image)
        {

            byte[] result = null;
            if (_camera.Type == eCAMERA_TYPE.VERYHIGH_MAG_CAMERA)
            {
                var bestImage = image as MilImageColor<byte>;

                if (bestImage != null)
                {
                    result = bestImage.Get1D();
                }
            }
            else
            {
                var bestImage = image as MilImage<byte>;

                if (bestImage != null)
                {
                    result = bestImage.Get1D();
                }
            }

            return result;
        }

        private void SendBestImage(IFocusFrameProcessor focusProcessor)
        {
            var bestImageProvider = focusProcessor as IBestImageFocusFrameProcessor;

            if (bestImageProvider != null)
            {
                var bestImage = bestImageProvider.GetBestImage();

                if (bestImage != null)
                {
                    byte[] buffer = GetBufferFormImage(bestImage);

                    if (buffer != null)
                    {
                        var producer = new ByteArrayDataProducer(buffer, buffer.Length);

                        ConnectToMMF();

                        if (_mMf == null)
                            throw new Exception("Invalid memory map file.");

                        using (MmfData mmfData = _mMf.GetMmfData(Win32Mmf.AccessRight.Write, 0, buffer.Length))
                        {
                            if (mmfData == null)
                                throw new Exception("Error while getting memory map file data.");

                            mmfData.WriteToMmf(producer);
                        }

                        DisconnectFromMMF();
                    }
                }
            }
        }


        private CamtekSystem.MMF.MemoryMappedFile _mMf;
        private const string LIVE_VIDEO_MMF_NAME = "LiveVideo_data_Buffer";

        private void ConnectToMMF()
        {
            if (!MemoryMappedFileFactory.Exist(LIVE_VIDEO_MMF_NAME))
            {
                throw new Exception("Live video memory map file does not exists.");
            }

            _mMf = MemoryMappedFileFactory.Open(LIVE_VIDEO_MMF_NAME, false);

            if (_mMf == null)
                throw new Exception("Error while opening the live video memory map file.");
        }

        private void DisconnectFromMMF()
        {
            if (_mMf == null)
                throw new Exception("Invalid state (memory map file is null).");

            _mMf.Dispose();
            _mMf = null;
        }


        private PeakDetectorRtp InitFppParameters(out IMilImageToGrayScaleTranslator translator)
        {
            // Fill parameters considering defaults
            var PeakDetectorRtp = new PeakDetectorRtp
            {
               DebugFileFullPath = IniFileHelper.GetIni("PeakDetectorRtp", "SaveDebugInfo", FfpParametersConfigurationFile, @""),
            };

            PeakDetectorRtp.PyramidLevel = 0;
           

            eCAMERA_TYPE typeOfCamera = _camera.Type;
            if (typeOfCamera != eCAMERA_TYPE.VERYHIGH_MAG_CAMERA)
            {
                translator = null;
                return PeakDetectorRtp;
            }

            var colorTranslator = IniFileHelper.GetIni("Color", "Translator", FfpParametersConfigurationFile, 3);
            switch (colorTranslator)
            {
                case 0:
                    translator = new ColorToBlue();
                    break;
                case 1:
                    translator = new ColorToGreen();
                    break;
                case 2:
                    translator = new ColorToRed();
                    break;
                case 3:
                    translator = new ColorToLuminance();
                    break;
                default:
                    translator = null;
                    break;
            }
            return PeakDetectorRtp;
        }

        private FocusOperator GetFfpOperator(int focusOperator)
        {
            FocusOperator ffpOperator;
            switch(focusOperator)
            {
                case 0:
                default:
                    ffpOperator = FocusOperator.NormalizedVarLocal;
                    break;
                case 1:
                    ffpOperator = FocusOperator.VarLocal;
                    break;
                case 2:
                    ffpOperator = FocusOperator.Grad;
                    break;
                case 3:
                    ffpOperator = FocusOperator.SmoothedVar;
                    break;
                case 4:
                    ffpOperator = FocusOperator.SmoothedVarMultiPeakLow;
                    break;
                case 5:
                    ffpOperator = FocusOperator.SmoothedVarMultiPeakHigh;
                    break;
                case 6:
                    ffpOperator = FocusOperator.GradCoarse;
                    break;
                case 9:
                    ffpOperator = FocusOperator.FoucsDistanceEstimator;
                    break;
            }
            return ffpOperator;
        }

        private byte[] CreateFfpMask(SImageShift rectangle, SImageShift processingFrameSize)
        {
            int frameSizeX = (int)processingFrameSize.x; // _camera.size.x;
            int frameSizeY = (int)processingFrameSize.y; // _camera.size.y;
            int frameSize = frameSizeX * frameSizeY;

            byte[] maskBuffer = new byte[frameSize];

            int left = (int)(frameSizeX / 2 - rectangle.x / 2);
            int top = (int)(frameSizeY / 2 - rectangle.y / 2);

            int right = (int)(left +rectangle.x);
            int bottom =(int)(top +rectangle.y);

            for (int iCol = 0; iCol < frameSizeX; iCol++)
            {
                for (int iRow = 0; iRow < frameSizeY; iRow++)
                {
                    if (iCol >= left && iCol < right && iRow >= top && iRow < bottom)
                    {
                        maskBuffer[iCol + iRow * frameSizeX] = 1;
                    }
                    else
                    {
                        maskBuffer[iCol + iRow * frameSizeX] = 0;
                    }
                }
            }

            return maskBuffer;
        }

        private RangeResult TryCreateScanRange(ZCoord<StageVCS> center, ZShift<StageVCS> range, ZShift<StageVCS> step, int direction, out ZCoord<StageVCS> start, out ZCoord<StageVCS> stop)
        {

            RangeResult result = RangeResult.Regular;

            ZShift<StageVCS> minRange = _minFramesCount * step;
            range = ZShift<StageVCS>.Max(minRange, range);

            int framesMemoryLimit = GetFramesMemoryLimit();
            if (range.Abs().Z >= (framesMemoryLimit * step).Z) // memory limit hit
            {
                range = framesMemoryLimit * step;
                result |= RangeResult.MemoryLimitHit;
            }

            if (range.Abs().Z >= (_maxFocusLimitZ - _minFocusLimitZ).Abs().Z) // software limits hit
            {
                range = _maxFocusLimitZ - _minFocusLimitZ;
                result |= RangeResult.SoftwareLimitHit;
            }

            if ((_coarseRangeEquivalenceFactor * range).Z >= (_coarseMaxLimit - _coarseMinLimit).Abs().Z) // the range is coarse equivalent
            {
                range = ZShift<StageVCS>.Max(_coarseMaxLimit - _coarseMinLimit, range);
                result |= RangeResult.CoarseLimitHit;
            }

            ZCoord<StageVCS> min = center - 0.5 * range; // low software limit hit
            if (min.Z <= _minFocusLimitZ.Z)
            {
                SetStartStopByDirection(_minFocusLimitZ, _minFocusLimitZ + range, direction, out start, out stop);
                return result | RangeResult.SoftwareLimitHit;
            }

            ZCoord<StageVCS> max = center + 0.5 * range; // high software limit hit
            if (max.Z >= _maxFocusLimitZ.Z)
            {
                SetStartStopByDirection(_maxFocusLimitZ - range, _maxFocusLimitZ, direction, out start, out stop);
                return result | RangeResult.SoftwareLimitHit;
            }

            // no bound
            SetStartStopByDirection(min, max, direction, out start, out stop);

            return result;
        }

        private static void SetStartStopByDirection(ZCoord<StageVCS> min, ZCoord<StageVCS> max, int direction, out ZCoord<StageVCS> start, out ZCoord<StageVCS> stop)
        {
            if (direction >= 0)
            {
                start = min;
                stop = max;
            }
            else
            {
                start = max;
                stop = min;
            }
        }

        private int GetFramesMemoryLimit()
        {
            return _acqIpcConnector.GetFreeMemoryInFrames(AcqMemoryType.FrameData);
        }


        public string GetLastErrorMessage()
        {
            if (!string.IsNullOrEmpty((_lastError)))
            {
                Trace.LiveAutoFocus.Warning("Faild to Focus. Exception: {0}", _lastError);
            }
            return _lastError;
        }

        private string GetFdeModelFilePath(IAreaCamera camera)
        {
           
            double mag = camera.Zoom[eZOOM_RING_TYPE.NUM_OF_ZOOM_RINGS].Value;
            string retval = $@"c:\Falcon\Data\Machine\{Environment.MachineName}\Cameras\{camera.Name}\FdeModels\Model_x{mag:0.00}";  

            return retval;
        }
    }
}