using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using NLog;
using SmartSEMControl;

namespace MeasureThread
{
    class Locate : ILocate
    {
        struct PositionRecord
        {
           public float xs;
           public float ys;
           public float xpCur;
           public float ypCur;
            public float magCur;
        }
        private PositionRecord pr=new PositionRecord();
        NLog.Logger log;
        private LocateParam prm;
        private ISEMControl iSEM;
        const float Z_Min = 0;
        const float Z_Max = 0.05f;
        const float fSateyZ = (float)0.007;//单位是米
        public Locate(ISEMControl iSEM)
        {
            this.log = NLog.LogManager.GetCurrentClassLogger();

           
            this.iSEM = iSEM ?? throw new ArgumentNullException(nameof(iSEM));
        }

        public bool LocateCutHolePosition(MeasureData.CutHole currHole, bool ifTilt)
        {
            if (ifTilt)//如果是倾斜样品台，移动Z轴
            {
                //0、先移动Z轴到比较低的位置上。
                if (!MoveZAxis(fSateyZ, Locate.ZAxisDirection.down))
                {

                    log.Error("测量线程报错：样品台Z轴移动失败,Z移动" + fSateyZ.ToString(), false);

                    return false;
                }


            }

            //1、移动样品台到第一个观测点，先移动R轴，再移动XY轴

            if (!MoveRAxis(currHole.Position.R))
            {

                log.Error("测量线程报错：样品台R轴移动失败,R为" + currHole.Position.ToString(), false);

                //continue;
                return false;
            }


            //2、移动XY轴
            // lo.MoveToPixByMoveStage(currHole.Position.X,currHole.Position.Y)
            if (!iSEM.MoveStageXY(currHole.Position.X, currHole.Position.Y))
            {

                log.Error("测量线程报错：样品台XY轴移动失败,(X,Y)为"
                    + currHole.Position.X.ToString() + ","
                    + currHole.Position.Y.ToString() + ")", false);

                return false;
            }
            while (true)
            {
                Thread.Sleep(1000);

                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            //判断是否停止进程
           

            //3、恢复Z轴
            if (ifTilt)
            {
                //恢复Z轴 

                if (!MoveZAxisByAbs(currHole.Position.Z))
                {

                    log.Error("测量线程报错：样品台Z轴移动失败,Z为" + currHole.Position.Z.ToString(), false);

                    return false;
                }

            }

           

            //4、设置WD
            iSEM.SetWorkingDistance(currHole.Position.WD);
            Thread.Sleep(1000);
            return true;


        }

        public bool RotateStageByDeltaR(float deltaR)
        {
            float Zpos = iSEM.GetStageAtZ();
            Thread.Sleep(200);

            log.Info("测量线程：当前Z轴位置" + Zpos.ToString() + "m", true);
            Locate lo = new Locate(iSEM);
            //选择之前，先降Z轴
            lo.MoveZAxis(fSateyZ, Locate.ZAxisDirection.down);

           

            iSEM.SetStageDeltaR(deltaR);
            while (true)
            {
                Thread.Sleep(1000);

                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            Thread.Sleep(1000);
            if (!MoveZAxisByAbs(Zpos))
            {
                log.Error("测量线程报错：样品台Z回到安全位置失败", false);
                return false;
            }

            return true;


        }
        public void RecordCurrentBeamShiftAndStagePosition()
        {
            #region 记录初始设置的BeamShift的百分比和样品台XY位置
            pr. xs = iSEM.GetBeamShiftX();
            Thread.Sleep(200);
            pr. ys = iSEM.GetBeamShiftY();
            Thread.Sleep(200);
            pr. xpCur = iSEM.GetStageAtX();
            Thread.Sleep(200);
            pr. ypCur = iSEM.GetStageAtY();
            Thread.Sleep(200);
            pr.magCur = iSEM.GetMagnification();
            Thread.Sleep(500);
            log.Info("原来的光束偏移量x = " + pr.xs.ToString()
                + "y=" + pr.ys.ToString() + ","
                + "原来样品台的位置x= " +pr. xpCur.ToString()
                + "y=" + pr.ypCur.ToString(), true);
            #endregion

        }
        public void RestoreLastBeamShiftAndStagePosition()
        {
            #region 恢复到拍照的状态
            log.Info("恢复到拍照的状态！");
            iSEM.SetStageGotoX(pr.xpCur);
            while (true)
            {
                Thread.Sleep(1000);

                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            iSEM.SetStageGotoY(pr.ypCur);
            while (true)
            {
                Thread.Sleep(1000);

                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }


            //恢复BeamShift的百分比值
            iSEM.SetBeamShiftX(pr.xs);
            Thread.Sleep(200);
            iSEM.SetBeamShiftY(pr.ys);
            Thread.Sleep(200);

            //恢复原来的放大倍数
            log.Info("恢复原来的放大倍数！ " + pr.magCur);
            if (!iSEM.SetMagnification(pr.magCur))
            {
                log.Error("恢复放大倍数失败", false);
                return;
            }
            float cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));

            #endregion

        }
        public enum ZAxisDirection
        {
            up=0,
            down=1
        }
        public bool MoveZAxis(float value, ZAxisDirection d )
        {
            if (value < Z_Min || value> Z_Max)
            {
                log.Info("Z轴位置设置超出范围.", true);
                return false;
            }
            float curZ = iSEM.GetStageAtZ();
            float targetZ;
            if (d == ZAxisDirection.up)
            {
                targetZ = curZ + value;
            }
            else
            {
                targetZ = curZ - value;
            }

            log.Info("将Z轴位置指定到：" + value.ToString(), true);
            if (!iSEM.SetStageGotoZ(targetZ))
            {
                log.Error("样品台Z轴移到位置失败", false);
                return false;
            }


            while (true)
            {
                Thread.Sleep(1000);
                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            Thread.Sleep(200);
            iSEM.CmdFocusScanSpeed("CMD_SCANRATE5");
            //add by sun 2020-12-15 拍截面图调用蔡司接口时，要保证扫描速度5以上，自动亮度对比度功能关闭 end
            Thread.Sleep(200);
            float cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));
           
            return true;
        }
        public bool MoveRAxis(float value)
        {
            if (!iSEM.SetStageGotoR(value))
            {
                
                return false;
            }
            while (true)
            {
                Thread.Sleep(1000);
                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            Thread.Sleep(200);
            iSEM.CmdFocusScanSpeed("CMD_SCANRATE5");//CmdFocusRate(5);
            //add by sun 2020-12-15 拍截面图调用蔡司接口时，要保证扫描速度5以上，自动亮度对比度功能关闭 end
            Thread.Sleep(200);
            float cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));
            return true;
          

        }

        public bool MoveZAxisByAbs(float value)
        {
            if (value < Z_Min || value > Z_Max)
            {
                log.Info("Z轴位置设置超出范围.", true);
                return false;
            }
         

            log.Info("将Z轴位置指定到：" + value.ToString(), true);
            if (!iSEM.SetStageGotoZ(value))
            {
                log.Error("样品台Z轴移到位置失败", false);
                return false;
            }


            while (true)
            {
                Thread.Sleep(1000);
                if (iSEM.GetStageIs() == 0)
                {
                    break;
                }
            }
            Thread.Sleep(200);
            iSEM.CmdFocusScanSpeed("CMD_SCANRATE5");//CmdFocusRate(5);
            //add by sun 2020-12-15 拍截面图调用蔡司接口时，要保证扫描速度5以上，自动亮度对比度功能关闭 end
            Thread.Sleep(200);
            float cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));
            return true;
           
        }

        //移动到像素位置，这里要靠移动样品台实现，当位移量小于3um时，不予移动
     private   bool MoveToPixByMoveStage()
        {
            float xc = prm.PositionX;
            float yc = prm.PositionY;
            //单位是m/pix
            float XpixSize = iSEM.GetPixelSize();
            log.Info("X像素尺寸=" + XpixSize.ToString() + "m/pixel", true);
            float YpixSize = iSEM.GetPixelSize();///(float)MParam.PixelSizeCor);
            log.Info("Y像素尺寸=" + YpixSize.ToString() + "m/pixel", true);

            Thread.Sleep(500);
            //0:width, 1:height
            int[] imageSize = iSEM.GetImageStore();

            int width = imageSize[0] / 2;
            int height = imageSize[1] / 2;

            log.Info("目标像素是(" + xc.ToString() + "," + yc.ToString() + ")", true);
            log.Info("中心像素是(" + width.ToString() + "," + height.ToString() + ")", true);

            float deltX = (xc - (float)width) * XpixSize;
            float deltY = (yc - (float)height) * YpixSize;

            log.Info("x位移量 = " + deltX.ToString() + "m", true);
            log.Info("y位移量 = " + deltY.ToString() + "m", true);

            float xpCur = iSEM.GetStageAtX();
            float ypCur = iSEM.GetStageAtY();

            log.Info("当前位置（" + xpCur.ToString() + "," + ypCur.ToString() + "),单位m", true);

            float xpNew = xpCur - deltX;
            float ypNew = ypCur + deltY;

            log.Info("目标位置（" + xpNew.ToString() + "," + ypNew.ToString() + "),单位m", true);

                log.Info("X方向移动样品台", true);

                if (!iSEM.SetStageGotoX(xpNew))
                {
                    return false;
                }

                //判断是否移动完成
                while (true)
                {
                    Thread.Sleep(1000);
                    if (iSEM.GetStageIs() == 0)
                    {
                        break;
                    }
                }
          

        
                log.Info("Y方向移动样品台", true);

                if (!iSEM.SetStageGotoY(ypNew))
                {
                    return false;
                }

                //判断是否移动完成
                while (true)
                {
                    Thread.Sleep(1000);
                    if (iSEM.GetStageIs() == 0)
                    {
                        break;
                    }
                }
          

            return true;
        }
        //移动到像素位置
      private  bool MoveToPix()
        {
            float xc = prm.PositionX;
            float yc = prm.PositionY;
            //单位是m/pix
            float XpixSize = iSEM.GetPixelSize();

            log.Info("X像素尺寸=" + XpixSize.ToString() + "m/pixel", true);
            float YpixSize = (XpixSize / prm.PixelSize_Y_cur);
            log.Info("Y像素尺寸=" + YpixSize.ToString() + "m/pixel", true);

            Thread.Sleep(200);
            //0:width, 1:height
            int[] imageSize = iSEM.GetImageStore();

            int width = imageSize[0] / 2;
            int height = imageSize[1] / 2;

            log.Info("目标像素是(" + xc.ToString() + "," + yc.ToString() + ")", true);
            log.Info("中心像素是(" + width.ToString() + "," + height.ToString() + ")", true);

            float deltX = (xc - (float)width) * XpixSize;
            float deltY = (yc - (float)height) * YpixSize;

            log.Info("x位移量 = " + deltX.ToString() + "m", true);
            log.Info("y位移量 = " + deltY.ToString() + "m", true);

            float xpCur = iSEM.GetStageAtX();
            Thread.Sleep(200);
            float ypCur = iSEM.GetStageAtY();

            log.Info("当前位置（" + xpCur.ToString() + "," + ypCur.ToString() + "),单位m", true);

            float xpNew = xpCur - deltX;
            float ypNew = ypCur + deltY;

            log.Info("目标位置（" + xpNew.ToString() + "," + ypNew.ToString() + "),单位m", true);

            //计算最大偏移量
            float beamXCur = iSEM.GetBeamShiftX();//单位是%
            Thread.Sleep(200);
            iSEM.SetBeamShiftX(100);
            Thread.Sleep(200);
            float beamXMax = iSEM.GetBeamOffsetX();//单位是m
            Thread.Sleep(200);
            iSEM.SetBeamShiftX(beamXCur);
            Thread.Sleep(200);
            //计算光束偏移值：
            float beamX = iSEM.GetBeamOffsetX();
            log.Info("当前X方向光束偏移量=" + beamX.ToString() + "m", true);

            beamX = (-deltX + beamX);
            log.Info("X方向光束偏移量应为=" + beamX.ToString() + "m", true);

            if (((beamX <= beamXMax) && (beamX >= 0)) ||
                ((beamX >= -beamXMax) && (beamX < 0)))
            {
                log.Info("X方向移动光束", true);
                float beamXShift = beamX * 100 / beamXMax;
                log.Info("X方向光束偏移量应为=" + beamXShift.ToString() + "%", true);
                if (!iSEM.SetBeamShiftX(beamXShift))//if (!iSEM.SetBeamOffsetX(beamX))
                {
                    log.Info("X方向光束偏移量" + beamX.ToString() + "m失败", true);
                    return false;
                }
            }
            else if (deltX > 0.000003 || deltX < -0.000003)//大于3um使用移动样品台实现
            {
                log.Info("X方向移动样品台", true);

                if (!iSEM.SetStageGotoX(xpNew))
                {
                    return false;
                }

                //判断是否移动完成
                while (true)
                {
                    Thread.Sleep(1000);
                    if (iSEM.GetStageIs() == 0)
                    {
                        break;
                    }
                }
            }

            float beamY = iSEM.GetBeamOffsetY();
            log.Info("当前Y方向光束偏移量=" + beamY.ToString() + "m", true);

            beamY = (-deltY + beamY);
            log.Info("Y方向光束偏移量应为=" + beamY.ToString() + "m", true);

            //计算最大偏移量
            float beamYCur = iSEM.GetBeamShiftY();//单位是%
            Thread.Sleep(200);
            iSEM.SetBeamShiftY(100);
            Thread.Sleep(200);
            float beamYMax = iSEM.GetBeamOffsetY();//单位是m
            Thread.Sleep(200);
            iSEM.SetBeamShiftY(beamYCur);
            Thread.Sleep(200);
            if (((beamY <= beamYMax) && (beamY >= 0)) ||
                ((beamY >= -beamYMax) && (beamY < 0)))
            {
                log.Info("Y方向移动光束", true);
                float beamYShift = beamY * 100 / beamYMax;
                log.Info("Y方向光束偏移量" + beamYShift.ToString() + "%", true);
                //if(!iSEM.SetBeamOffsetY(beamY))
                if (!iSEM.SetBeamShiftY(beamYShift))
                {
                    log.Info("Y方向光束偏移量" + beamY.ToString() + "m失败", true);
                    return false;
                }
            }
            else if (deltY > 0.000003 || deltY < -0.000003)//大于3um使用移动样品台实现
            {
                log.Info("Y方向移动样品台", true);

                if (!iSEM.SetStageGotoY(ypNew))
                {
                    return false;
                }

                //判断是否移动完成
                while (true)
                {
                    Thread.Sleep(1000);
                    if (iSEM.GetStageIs() == 0)
                    {
                        break;
                    }
                }

            }

            return true;
        }

        public bool MoveToPixByMoveStage(float x, float y)
        {
            if (prm == null)
            {
                prm = new LocateParam();
            }
            this.prm.PositionX = x;
            this.prm.PositionY = y;
            
             MoveToPixByMoveStage();
            Thread.Sleep(200);
            Thread.Sleep(200);
            iSEM.CmdFocusScanSpeed("CMD_SCANRATE5");//CmdFocusRate(5);
            //add by sun 2020-12-15 拍截面图调用蔡司接口时，要保证扫描速度5以上，自动亮度对比度功能关闭 end
            Thread.Sleep(200);
            float cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));
            return true;
           

        }

        public bool MoveToPix(float x,float y,float pixelSize_Y_cur)
        {
            if (prm == null)
            {
                prm = new LocateParam();
            }
            this.prm.PositionX = x;
            this.prm.PositionY = y;
            this.prm.PixelSize_Y_cur = pixelSize_Y_cur;
             MoveToPix();
            Thread.Sleep(200);
            iSEM.CmdFocusScanSpeed("CMD_SCANRATE5");//CmdFocusRate(5);
            //add by sun 2020-12-15 拍截面图调用蔡司接口时，要保证扫描速度5以上，自动亮度对比度功能关闭 end
            Thread.Sleep(200);
            float  cycle_time = iSEM.GetCycleTime();
            Thread.Sleep(100 + Convert.ToInt32(cycle_time));
            return true;
        }
        public LocateParam GetParam()
        {
            if (prm == null)
            {

                prm = new LocateParam();
            }
            return prm;
        }
    }
}
