#pragma warning disable IDE1006 // 명명 스타일
using arDev.MOT;
using AR;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Project
{
	public static partial class MOT
	{

		/// <summary>
		/// 상대위치로 모터를 이동합니다
		/// </summary>
		/// <param name="value"></param>
		/// <param name="src"></param>
		public static void Rotation(double value,string src)
		{
			//모터회전
			var thpos = MOT.GetPTPos(ePTLoc.READY); //적용할 속도값용
			MOT.Move(eAxis.Z_THETA, value, thpos.Speed, thpos.Acc, true);
			PUB.log.AddI($"Theta Rorate:{value},Reason:{src}");
		}

		public static Boolean CheckMotionLimitError()
		{
			for (short i = 0; i < PUB.mot.DeviceCount; i++)
			{
				if (PUB.mot.IsUse(i) == false) continue; //미사용축은 제외한다.
														 //if (SETTING.System.UseAxis(i) == false) continue;
														 //if (SETTING.System.UseOriginSignal(i) == false) continue;
				if (PUB.mot.IsLimit(i)) return true;
			}
			return false;

		}

		#region "Check Motion Position"

		public static Boolean CheckMotionPos(TimeSpan stepTime, sPositionData posdata, string source, Boolean useInterLocak = true, int timeoutSec = 0)
		{
			if (posdata.Axis < 0) throw new Exception("CheckMotionPos:Motion number not found");
			//return CheckMotionPos((eAxis)posdata.Axis, stepTime, posdata.Position, posdata.Speed, posdata.Acc, posdata.Dcc, source, useInterLocak, timeoutSec, posdata.inpositionrange);
			return CheckMotionPos((eAxis)posdata.Axis, stepTime, posdata.Position, posdata.Speed, posdata.Acc, posdata.Dcc, source, useInterLocak, timeoutSec, posdata.inpositionrange);
		}


		public static Boolean CheckMotionPos(eAxis Motaxis, TimeSpan stepTime, double pos, double speed, double acc, double dcc, string source, Boolean useInterLocak = true, int timeoutSec = 0, float inpaccr = 0f)
		{
			var axis = (short)Motaxis;

			//타임아웃 적용 191213
			if (timeoutSec == 0) timeoutSec = AR.SETTING.Data.Timeout_MotionCommand;

			//accr 범위가 지정되잇다며 ㄴ그것을 사용한다
			if (inpaccr == 0f)
				inpaccr = 0.1f;// SETTING.System.INPAccurary(axis);


			//X축을 그립위치까지 이동함
			if (PUB.mot.IsInp(axis) && PUB.mot.IsMotion(axis) == false)
			{
				var offset = Math.Abs(PUB.mot.GetCmdPos(axis) - pos);
				var offsetReal = Math.Abs(PUB.mot.GetCmdPos(axis) - PUB.mot.GetActPos(axis));

				//커맨드위치는 오차가 없어야 한다
				if (offset > 0.001 || offsetReal > inpaccr) //220214 0.01)
				{
					//모션을 옴겨야하는 상황인데 최대시간을 초과했다면 오류로 처리한다
					if (timeoutSec != -1 && stepTime.TotalSeconds >= timeoutSec)
					{
						PUB.Result.SetResultTimeOutMessage(Motaxis, eECode.MOT_CMD, eNextStep.PAUSE, source, pos, PUB.mot.ErrorMessage);
						return false;
					}

					if (MOT.Move(Motaxis, pos, speed, acc, false, true, useInterLocak) == false)
					{
						Console.WriteLine("move error {0},pos={1} => {2}", Motaxis, pos, PUB.mot.ErrorMessage);
						return false;
					}

					//모션을 이동시켰으니 False 처리한다
					//return false;
				}
			}

			//축이 이동중이면 처리하지 않는다.
			if (PUB.mot.IsInp(axis) == false || PUB.mot.IsMotion(axis) == true)
			{
				return false;
			}

			//X축실위치값 확인
			if (MOT.getPositionMatch(axis, pos, inpaccr) == false)
			{
				return false;
			}


			return true;
		}


		#endregion


		#region "Get Axis Position"

		public static sPositionData GetPXPos(ePXLoc pos) { return GetAxPos(eAxis.PX_PICK, (int)pos); }
		public static sPositionData GetPZPos(ePZLoc pos) { return GetAxPos(eAxis.PZ_PICK, (int)pos); }
		public static sPositionData GetLMPos(eLMLoc pos) { return GetAxPos(eAxis.PL_MOVE, (int)pos); }
		public static sPositionData GetLZPos(eLZLoc pos) { return GetAxPos(eAxis.PL_UPDN, (int)pos); }
		public static sPositionData GetRMPos(eRMLoc pos) { return GetAxPos(eAxis.PR_MOVE, (int)pos); }
		public static sPositionData GetRZPos(eRZLoc pos) { return GetAxPos(eAxis.PR_UPDN, (int)pos); }
		public static sPositionData GetPTPos(ePTLoc pos) { return GetAxPos(eAxis.Z_THETA, (int)pos); }


		#endregion



		/// <summary>
		/// 지정된 축의 위치값에 속한 이름을 반환합니다.
		/// 특정 구간에 속하지 않은 경우 UNKNOWN 을 반환 합니다
		/// </summary>
		/// <param name="axis"></param>
		/// <param name="Pos"></param>
		/// <returns></returns>
		public static String GetPosName(eAxis axis, double Pos = -1, double CheckOffset = 0.1)
		{
			//홈을 잡지 않았다면 오류로 처리함\
			//eYPPosition retval = eYPPosition.Unknown;

			var motIndex = (short)axis;

			if (PUB.mot.IsInit == false) return "ERROR";// (T)ePickYPosition.ERROR; //200213
			if (PUB.mot.IsHomeSet(motIndex) == false) return "ERROR";//ePickYPosition.ERROR;
			if (PUB.Result == null || PUB.Result.isSetmModel == false) return "ERROR";//return ePickYPosition.ERROR;

			if (PUB.mot.IsUse(motIndex) == false) return "DISABLE";
			else if (PUB.mot.IsServAlarm(motIndex)) return "SVALM";
			else if (PUB.mot.IsServOn(motIndex) == false) return "SVOFF";
			else if (PUB.mot.IsLimitN(motIndex)) return "LIMITN";
			else if (PUB.mot.IsLimitP(motIndex)) return "LIMITP";
			else if (PUB.mot.IsLimitSWN(motIndex)) return "LIMITN(SW)";
			else if (PUB.mot.IsLimitSWP(motIndex)) return "LIMITP(SW)";
			else if (PUB.mot.IsOrg(motIndex)) return "HOME";
			else
			{
				//특정위치에있는지 확인한다.  match 명령사용
				if (Pos == -1) Pos = PUB.mot.GetActPos(motIndex);
				var PosT = 0.0;

				//해당 모션이 속한 좌표를 모두 가져온다
				var pts = PUB.Result.mModel.Position[motIndex].OrderBy(t => t.value).ToList();

				//각위치별값을 확인한다.
				for (int i = 0; i < pts.Count; i++)
				{
					var pValue = pts[i];
					if (Math.Abs(pValue.value - Pos) <= CheckOffset)
					{
						//해당위치에 있다i
						if (axis == eAxis.PX_PICK) return ((ePXLoc)i).ToString();
						else if (axis == eAxis.PZ_PICK) return ((ePZLoc)i).ToString();
						else if (axis == eAxis.Z_THETA) return ((ePTLoc)i).ToString();

						else if (axis == eAxis.PL_MOVE) return ((eLMLoc)i).ToString();
						else if (axis == eAxis.PL_UPDN) return ((eLZLoc)i).ToString();

						else if (axis == eAxis.PR_MOVE) return ((eRMLoc)i).ToString();
						else if (axis == eAxis.PR_UPDN) return ((eRZLoc)i).ToString();

						else return $"P#{i}";
					}
				}

				//위치를 찾지 못했다
				if (Math.Abs(Pos) <= CheckOffset) return "ZERO";
				else return "UNKNOWN";
			}
		}


		/// <summary>
		/// Z-L축이 안전위치에있는가?(Ready 보다 위에있으면 안전위치이다)
		/// </summary>
		/// <returns></returns>
		public static Boolean isAxisSaftyZone(eAxis axis, int allowoffset = 2)
		{
			//홈을 잡지 않았다면 오류로 처리함
			var motIndex = (short)axis;
			if (PUB.mot.IsInit == false) return false;
			if (PUB.mot.IsHomeSet(motIndex) == false) return false;
			if (PUB.Result == null || PUB.Result.isSetmModel == false) return false;

			sPositionData readypos;
			if (axis == eAxis.PX_PICK)
			{
				//피커는 안전위이가 2개 있다
				readypos = GetPXPos(ePXLoc.PICKON);
				//var readypos1 = GetAxPXPos(eAxisPXPos.ReadyR);
				//var offset1 = getPositionOffset(axis, readypos.position);
				//var offset2 = getPositionOffset(axis, readypos1.position);
				//if (offset1 < allowoffset || offset2 < allowoffset ) return true;    //오차가 2미만이라면 안전하다
				//return false;
			}
			else if (axis == eAxis.PZ_PICK) readypos = GetPZPos(ePZLoc.READY);
			else if (axis == eAxis.Z_THETA) readypos = GetPTPos(ePTLoc.READY);
			else if (axis == eAxis.PL_MOVE) readypos = GetLMPos(eLMLoc.READY);
			else if (axis == eAxis.PR_MOVE) readypos = GetRMPos(eRMLoc.READY);
			else if (axis == eAxis.PR_UPDN) readypos = GetRZPos(eRZLoc.READY);
			else if (axis == eAxis.PL_UPDN) readypos = GetLZPos(eLZLoc.READY);
			else return false;

			var offset = getPositionOffset(axis, readypos.Position);
			if (offset < allowoffset) return true;    //오차가 2미만이라면 안전하다
			return false;
		}

		private static Boolean Home_Validation(eAxis axis, out string errorMessage)
		{
			Boolean retval = true;
			//   int axis = (int)axis_;

			var CW = false; //홈검색은 항상 뒤로 움직인다.
			if (!Move_Validation(axis, CW, out errorMessage)) retval = false;    //이동이 불가한 경우 체크
			else if (axis == eAxis.PX_PICK)
			{
				//Z축 홈이 필요하다
				var zFHome = PUB.mot.IsHomeSet((int)eAxis.PZ_PICK);
				var zRHome = PUB.mot.IsHomeSet((int)eAxis.Z_THETA);
				if (zFHome == false || zRHome == false)
				{
					//errorMessage = "Z-PICKER,Z-THETA 축 홈이 필요 합니다";
					//retval = false;
				}
			}
			else if (axis == eAxis.PL_MOVE)   //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
			{
				if (PUB.mot.IsHomeSet((int)eAxis.PL_UPDN) == false)
				{
					errorMessage = "PRINT-L Z axis home is required";
					retval = false;
				}
			}
			else if (axis == eAxis.PR_MOVE)   //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
			{
				if (PUB.mot.IsHomeSet((int)eAxis.PR_UPDN) == false)
				{
					errorMessage = "PRINT-R Z axis home is required";
					retval = false;
				}
			}
			return retval;
		}

		//private static Boolean Move_Validation(eAxis axis, out string errorMessage)
		//{
		//	errorMessage = string.Empty;

		//	if (DIO.IsEmergencyOn() == true)
		//	{
		//		errorMessage = ("비상정지 상태일때에는 움직일 수 없습니다.");
		//		return false;
		//	}

		//	return true;
		//}
		private static Boolean Move_Validation(eAxis axis, Boolean MoveCW, out string errorMessage)
		{
			errorMessage = string.Empty;

			if (DIO.IsEmergencyOn() == true)
			{
				errorMessage = ("Cannot move when in emergency stop state.");
				return false;
			}

			//홈이 잡힌상태일때
			if (PUB.mot.IsHomeSet((short)axis))
			{
				if (axis == eAxis.PX_PICK)
				{
					var Pos = MOT.GetPXPos(ePXLoc.PICKON);
					var PosOffset = MOT.getPositionOffset(Pos);

					if(AR.SETTING.Data.Log_Debug)
					PUB.logDbg.Add($"X-axis jog center offset:{PosOffset:N3}");

					if (MoveCW==false && PosOffset < -1) //좌측위치로 이동하는 경우
					{
					   	if(PUB.mot.IsHomeSet((int)eAxis.PL_MOVE))
						{
							var PosY = MOT.GetLMPos(eLMLoc.READY);
							var PosYOffse = MOT.getPositionOffset(PosY);
							if(PosYOffse < -1)
							{
								//프린터 Y축이 더 안쪽으로 들어와있으니 충돌할 수 있다.
								errorMessage = "Possible collision with Print(L) axis";
								return false;
							}
						}
					}
					else if(MoveCW && PosOffset > 1)	//우측으로 이동하는 경우
					{
						if (PUB.mot.IsHomeSet((int)eAxis.PR_MOVE))
						{
							var PosY = MOT.GetRMPos(eRMLoc.READY);
							var PosYOffse = MOT.getPositionOffset(PosY);
							if (PosYOffse < -1)
							{
								//프린터 Y축이 더 안쪽으로 들어와있으니 충돌할 수 있다.
								errorMessage = "Possible collision with Print(R) axis";
								return false;
							}
						}
					}
				}
			}

			return true;
		}

        #region "Common Util"

        /// <summary>
        /// 지정한 축의 모델정보에서 해당 위치값을 찾아 반환 합니다
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="pos"></param>
        /// <returns></returns>
        private static sPositionData GetAxPos(eAxis axis, int pos)
        {
            return GetAxpos((short)axis, pos);
        }
        private static sPositionData GetAxpos(short axis, int pos)
        {
            var retval = new sPositionData();
            retval.Clear();
            if (PUB.Result.mModel == null || PUB.Result.mModel.isSet == false)
            {
                retval.Message = "Motion model is not set";
                return retval;
            }

            if(axis >= PUB.Result.mModel.Position.Count)
            {
                retval.Message = $"Motion model (axis) information not found ({axis}/{PUB.Result.mModel.Position.Count})";
                return retval;
            }

            if (pos >= PUB.Result.mModel.Position[axis].Length)
            {
                retval.Message = $"Motion model (position) information not found ({pos}/{PUB.Result.mModel.Position[axis].Length})";
                return retval;
            }

            var data = PUB.Result.mModel.Position[axis][pos];
            if (data.index == -1)
            {
                retval.Message = string.Format("Value for axis:{0} position:{1} does not exist", axis, pos);
                return retval;
            }
            retval.Axis = axis; //220301
            retval.Position = data.value;
            retval.Speed = data.speed;
            retval.Acc = data.acc;


            //환경설정에서 저속모드로 설정했다면 지정된 속도로만 처리한다
            if (AR.SETTING.Data.Enable_SpeedLimit == true)
                retval.Speed = Math.Min(retval.Speed, AR.SETTING.Data.LimitSpeed);

            ////시스템설정의 속도 체크 220524
            //var maxspeed = SETTING.System.GetMaxSpeed;
            //var motidx = (int)axis;
            //if (motidx >= 0 && motidx < maxspeed.Length && maxspeed[motidx] > 0)
            //{
            //    retval.Speed = Math.Min(retval.Speed, maxspeed[motidx]);
            //}

            ////시스템설정의 가속도체크
            //var maxAcc = SETTING.System.GetMaxAcc;
            //if (motidx >= 0 && motidx < maxAcc.Length && maxAcc[motidx] > 0)
            //{
            //    retval.Acc = Math.Min(retval.Acc, maxAcc[motidx]);
            //}

            if (data.dcc < 1) retval.Dcc = retval.Acc;
            else retval.Dcc = data.dcc;

            //retval.isError = false;
            retval.Message = string.Empty;
            return retval;
        }

        /// <summary>
        /// 지정축의 모션을 중지 합니다.
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="reason"></param>
        /// <param name="estop"></param>
        public static void Stop(eAxis axis, string reason, Boolean estop = false)
        {
            if (PUB.mot.IsInit == false) return;
            if (PUB.mot.IsServAlarm((short)axis)) return;
            PUB.mot.MoveStop(reason, (short)axis, estop);

        }

        //지정한 옵셋범위에 들어있는지 체크합니다.
        static Boolean MatchPosition(double cpos, double tpos, double diff = 0.1)
        {
            var offset = Math.Abs(tpos - cpos);
            return offset <= diff;
        }
        public static double getPositionOffset(eAxis axis, double cmdPos)
        {
            return getPositionOffset((short)axis, cmdPos);
        }

        /// <summary>
        /// 현재위치에서 지정된 위치를 뺀 값입니다. +가 반환되면 현재 위치가 지정위치보다 멀리 있다는 뜻입니다.
        /// </summary>
        /// <param name="Pos"></param>
        /// <returns></returns>
        public static double getPositionOffset(sPositionData Pos)
        {
            return getPositionOffset(Pos.Axis, Pos.Position);
        }
        /// <summary>
        /// 현재위치에서 지정된 위치를 뺀 값입니다. +가 반환되면 현재 위치가 지정위치보다 멀리 있다는 뜻입니다.
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="cmdPos"></param>
        /// <returns></returns>
        public static double getPositionOffset(short axis, double cmdPos)
        {
            var coffset = (PUB.mot.GetActPos(axis) - cmdPos);
            return coffset;// return coffset < offset;
        }

        public static bool getPositionMatch(eAxis axis, double cmdPos, double offset = 0.1)
        {
            return getPositionMatch((short)axis, cmdPos, offset);
        }

        public static bool getPositionMatch(sPositionData pos, double offset = 0.1)
        {
            return getPositionMatch(pos.Axis, pos.Position, offset);
        }


        public static bool getPositionMatch(short axis, double cmdPos, double offset = 0.1)
        {
            var actpos = PUB.mot.GetActPos(axis);
            var coffset = Math.Abs(actpos - cmdPos);
            return coffset <= offset;
        }

        public static List<string> GetActiveLockList(eAxis axis, CInterLock Lck)
        {
            var locklist = new List<string>();

            if (Lck.IsEmpty() == false)
            {
                for (int i = 0; i < Lck.Length; i++)
                {
                    if (Lck.get(i))
                    {
                        var vStr = $"[{i}]";
                        vStr = ((eILock)i).ToString();
                        locklist.Add(vStr);
                    }
                }
            }
            return locklist;
        }

        public static Boolean Home(string reason, eAxis axis_, Boolean useValidcheck = true)
        {
            string Message;
            int axis = (int)axis_;
            if (useValidcheck == true && !Home_Validation(axis_, out Message))
            {
                PUB.mot.RaiseMessage(string.Format("[{0}-Axis] Move Error : {1}", axis, Message), true);
                return false;
            }

            var HomespdH = PUB.system_mot.GetHomeSpeedHigh;
            var HomespdL = PUB.system_mot.GetHomeSpeedLow;
            var homespdA = PUB.system_mot.GetHomeSpeedAcc;
            var useOrgSensor = PUB.system_mot.GetUseOrigin;

            //Pub.mot.Home(1, arDev.AzinAxt.eMotionDirection.Negative, arDev.AzinAxt.eSoftLimitStopMode.SStop, COMM.SETTING.Data.HZSpeed, COMM.SETTING.Data.HZAcc);
            //Boolean useOrgSensor = !disable_org[axis]; // (axis_ != eAxis.Marking); //A센서는 ORG가 없으므로 NE센서만으로 처리해야 함
            return PUB.mot.Home(reason, (short)axis, MOTION_DIRECTION.Negative,
                STOPTYPE.EStop,
                HomespdH[axis],
                HomespdL[axis],
                homespdA[axis],
                useOrgSensor[axis]);
        }


        private static DateTime[] MotCmdtime = new DateTime[] { DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now };
        private static double[] MotCmdPos = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };

        public static Boolean Move(eAxis axis, sPositionData posdata)
        {
            return Move(axis, posdata.Position, posdata.Speed, posdata.Acc);
        }

        public static Boolean Move(sPositionData posdata)
        {
            return Move((eAxis)posdata.Axis, posdata);
        }

        public static Boolean Move(eAxis axis, double pos_mm, double speed, double acc = 1000, Boolean relative = false, Boolean validchk = true, Boolean UserInterLock = true)
        {

            //너무빠른시간 동작하지 못하게 한다
            if (MotCmdPos[(int)axis] == pos_mm)
            {
                var ts = DateTime.Now - MotCmdtime[(int)axis];
                if (ts.TotalMilliseconds < 1000)
                {
                    //너무 빠르게 재시도하지 않게 한다 210115
                    Console.WriteLine("mot command skip : " + axis.ToString() + "pos:" + pos_mm.ToString() + " too short");
                    MotCmdtime[(int)axis] = DateTime.Now.AddMilliseconds(-1500);
                    return true;
                }
                else
                {
                    MotCmdtime[(int)axis] = DateTime.Now;
                    MotCmdPos[(int)axis] = pos_mm;
                }
            }
            else
            {
                MotCmdtime[(int)axis] = DateTime.Now;
                MotCmdPos[(int)axis] = pos_mm;
            }

            //이동유효성검사
            string Message;
            if (validchk == true)
            {
                //현재위치보다 작으면 ccw
                var isCW = pos_mm >= PUB.mot.GetActPos((short)axis);
                if (!Move_Validation(axis, isCW, out Message))
                {
                    PUB.log.AddE(string.Format("[{0}-Axis] Move Error : {1}", axis, Message));
                    //Pub.mot.RaiseMessage(, true);
                    return false;
                }
            }

            //해당 축 ILOCK체크
            if (UserInterLock)
            {
                //lock이 걸린경우 lock 걸린 항목을 모두 확인하면 좋다
                var ilock = PUB.iLock[(int)axis];
                if (ilock.IsEmpty() == false)
                {
                    var locklist = MOT.GetActiveLockList(axis, ilock);
                    PUB.mot.ErrorMessage = $"{ilock.Tag} Interlock(" + string.Join(",", locklist) + ")";
                    //PUB.Result.SetResultMessage(eResult.MOTION, eECode.INTERLOCK, eNextStep.PAUSE, PUB.mot.ErrorMessage);
                    return false;
                }
            }

            var pos_pulse = pos_mm;// *10;
            var result = PUB.mot.Move((short)axis, pos_pulse, speed, acc, acc, relative, false, validchk);
            if (!result) PUB.mot.RaiseMessage("Move(X) Error : " + PUB.mot.ErrorMessage, true);
            return result;
        }


        public static Boolean JOG(short _Axis, MOTION_DIRECTION Dir, double _Vel, double _Acc, double _Dec, Boolean sCurve = false, Boolean EnableValidCheck = true, Boolean UserInterLock = true)
        {
            eAxis axis = (eAxis)_Axis;

            //웨이퍼가 감지되는 상태일때 OPEN 되어있다면 이동하지 못하게 한다
            if (EnableValidCheck == true)
            {
                var curpost = PUB.mot.GetActPos(_Axis);
                var IsMoveCW = false;

                IsMoveCW = Dir == MOTION_DIRECTION.Positive;

                if (!Move_Validation(axis, IsMoveCW, out string Message))
                {
                    PUB.log.AddE(string.Format("[{0}-Axis] JOG Error : {1}", axis, Message));
                    return false;
                }
            }
            else PUB.log.AddAT($"Validation check disabled during jog movement (axis:{axis})");

            //해당 축 ILOCK체크
            if (UserInterLock)
            {
                //lock이 걸린경우 lock 걸린 항목을 모두 확인하면 좋다
                if (PUB.iLock[_Axis].IsEmpty() == false)
                {
                    var locklist = MOT.GetActiveLockList(axis, PUB.iLock[_Axis]);
                    PUB.mot.ErrorMessage = $"{axis} Interlock(" + string.Join(",", locklist) + ")";
                    return false;
                }
            }
            else PUB.log.AddAT($"Interlock check disabled during jog movement (axis:{axis})");


            return PUB.mot.JOG(_Axis, Dir, _Vel, _Acc, sCurve, EnableValidCheck);
        }




        #endregion
    }
}