using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Project
{
    public struct sPositionData
    {
        public double position;
        public double acc;
        public double dcc;
        public double speed;
        public Boolean isError;
        public string message;
        public void Clear()
        {
            position = 0;
            acc = 0;
            dcc = 0;
            speed = 0;
            isError = true;
            message = "Not Set";
        }
    }
    public static partial class Util_Mot
    {

        #region "Get Axis Position"

        public static sPositionData GetAxPXPos(eAxisPXPos pos) { return GetAxPos(eAxis.X_PICKER, (int)pos); }
        public static sPositionData GetAxPZPos(eAxisPZPos pos) { return GetAxPos(eAxis.Z_PICKER, (int)pos); }

        public static sPositionData GetAxPLMPos(eAxisPLMovePos pos) { return GetAxPos(eAxis.PL_MOVE, (int)pos); }
        public static sPositionData GetAxPLZPos(eAxisPLUPDNPos pos) { return GetAxPos(eAxis.PL_UPDN, (int)pos); }

        public static sPositionData GetAxPRMPos(eAxisPRMovePos pos) { return GetAxPos(eAxis.PR_MOVE, (int)pos); }
        public static sPositionData GetAxPRZPos(eAxisPRUPDNPos pos) { return GetAxPos(eAxis.PR_UPDN, (int)pos); }

        public static sPositionData GetAxPTPos(eAxisPTPos pos) { return GetAxPos(eAxis.Z_THETA, (int)pos); }

        public static sPositionData GetAxPos(eAxis axis, int pos)
        {
            return GetAxpos((int)axis, pos);
        }
        public static sPositionData GetAxpos(int axis, int pos)
        {
            var retval = new sPositionData();
            retval.Clear();
            if (Pub.Result.mModel == null || Pub.Result.mModel.isSet == false)
            {
                retval.message = "모션 모델이 설정되어 있지 않습니다";
                return retval;
            }
            var data = Pub.Result.mModel.Position[axis][pos];
            if (data.index == -1)
            {
                retval.message = string.Format("축:{0})의 위치:{1} 의 값이 존재하지 않습니다", axis, pos);// "모션 모델이 설정되어 있지 않습니다";
                return retval;
            }

            retval.position = data.value;
            retval.speed = data.speed;
            retval.acc = data.acc;
            if (data.dcc < 1) retval.dcc = retval.acc;
            else retval.dcc = data.dcc;

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

            retval.isError = false;
            retval.message = string.Empty;
            return retval;
        }

        #endregion

        #region "Get Position Match"


        public static Boolean getPositionMatch(eAxisPXPos pos, double offset = 0.1)
        {
            var posData = GetAxPXPos(pos);
            return getPositionMatch(eAxis.X_PICKER, posData.position, offset);
        }

        public static Boolean getPositionMatch(eAxisPZPos pos, double offset = 0.1)
        {
            var posData = GetAxPZPos(pos);
            return getPositionMatch(eAxis.Z_PICKER, posData.position, offset);
        }
        public static Boolean getPositionMatch(eAxisPTPos pos, double offset = 0.1)
        {
            var posData = GetAxPTPos(pos);
            return getPositionMatch(eAxis.Z_THETA, posData.position, offset);
        }

        public static Boolean getPositionMatch(eAxisPLMovePos pos, double offset = 0.1)
        {
            var posData = GetAxPLMPos(pos);
            return getPositionMatch(eAxis.PL_MOVE, posData.position, offset);
        }

        public static Boolean getPositionMatch(eAxisPLUPDNPos pos, double offset = 0.1)
        {
            var posData = GetAxPLZPos(pos);
            return getPositionMatch(eAxis.PL_UPDN, posData.position, offset);
        }

        public static Boolean getPositionMatch(eAxisPRMovePos pos, double offset = 0.1)
        {
            var posData = GetAxPRMPos(pos);
            return getPositionMatch(eAxis.PR_MOVE, posData.position, offset);
        }

        public static Boolean getPositionMatch(eAxisPRUPDNPos pos, double offset = 0.1)
        {
            var posData = GetAxPRZPos(pos);
            return getPositionMatch(eAxis.PR_UPDN, posData.position, offset);
        }

        #endregion

        #region "Get Position Name"

        public static ePickYPosition GetPKX_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.X_PICKER;
            if (Pub.mot.IsInit == false) return ePickYPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePickYPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePickYPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKON).position;
            if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKON;

            PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKOFFR).position;
            if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKOFFR;

            PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKOFFL).position;
            if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKOFFL;

			PosT = Util_Mot.GetAxPXPos(eAxisPXPos.READYL).position;
            if (MatchPosition(Pos, PosT)) return ePickYPosition.READYL;

            PosT = Util_Mot.GetAxPXPos(eAxisPXPos.READYR).position;
            if (MatchPosition(Pos, PosT)) return ePickYPosition.READYR;

            if (Pub.mot.isLimitN[motIndex]) return ePickYPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePickYPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePickYPosition.HOME;
            else return ePickYPosition.UNKNOWN;
        }
        public static ePickZPosition GetPKZ_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.Z_PICKER;
            if (Pub.mot.IsInit == false) return ePickZPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePickZPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePickZPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKOFFR).position;
            if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKOFFR;

            PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKOFFL).position;
            if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKOFFL;

            PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKON).position;
            if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKON;

            PosT = Util_Mot.GetAxPZPos(eAxisPZPos.READY).position;
            if (MatchPosition(Pos, PosT)) return ePickZPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return ePickZPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePickZPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePickZPosition.HOME;
            else return ePickZPosition.UNKNOWN;
        }
        public static eThetaPosition GetPT_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.Z_THETA;
            if (Pub.mot.IsInit == false) return eThetaPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return eThetaPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return eThetaPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPTPos(eAxisPTPos.READY).position;
            if (MatchPosition(Pos, PosT)) return eThetaPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return eThetaPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return eThetaPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return eThetaPosition.HOME;
            else return eThetaPosition.UNKNOWN;
        }

        public static ePrintYPosition GetPLM_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.PL_MOVE;
            if (Pub.mot.IsInit == false) return ePrintYPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePrintYPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintYPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTH07).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH07;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTH13).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH13;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTL07).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL07;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTL13).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL13;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTM07).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTM07;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTM13).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTM13;

            PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.READY).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return ePrintYPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePrintYPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePrintYPosition.HOME;
            else return ePrintYPosition.UNKNOWN;
        }

        public static ePrintYPosition GetPRM_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.PR_MOVE;
            if (Pub.mot.IsInit == false) return ePrintYPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePrintYPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintYPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTH07).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH07;

            PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTL07).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL07;

            PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTH13).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH13;

            PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTL13).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL13;


            PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.READY).position;
            if (MatchPosition(Pos, PosT)) return ePrintYPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return ePrintYPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePrintYPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePrintYPosition.HOME;
            else return ePrintYPosition.UNKNOWN;
        }

        public static ePrintZPosition GetPLZ_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.PL_UPDN;
            if (Pub.mot.IsInit == false) return ePrintZPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePrintZPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintZPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.PICKOFF).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKOFF;

            PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.PICKON).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKON;

            PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.READY).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return ePrintZPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePrintZPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePrintZPosition.HOME;
            else return ePrintZPosition.UNKNOWN;
        }

        public static ePrintZPosition GetPRZ_PosName(double Pos = -1)
        {
            //홈을 잡지 않았다면 오류로 처리함\
            //eYPPosition retval = eYPPosition.Unknown;

            var motIndex = (int)eAxis.PR_UPDN;
            if (Pub.mot.IsInit == false) return ePrintZPosition.ERROR; //200213
            if (Pub.mot.isHomeSet[motIndex] == false) return ePrintZPosition.ERROR;
            if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintZPosition.ERROR;

            //위치가 입력되지 않았다면 현재 위치를 사용한다
            if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];

            var PosT = 0.0;

            //지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
            PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.PICKOFF).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKOFF;

            PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.PICKON).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKON;

            PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.READY).position;
            if (MatchPosition(Pos, PosT)) return ePrintZPosition.READY;

            if (Pub.mot.isLimitN[motIndex]) return ePrintZPosition.LIMITN;
            else if (Pub.mot.isLimitP[motIndex]) return ePrintZPosition.LIMITP;
            else if (Pub.mot.isOrg[motIndex]) return ePrintZPosition.HOME;
            else return ePrintZPosition.UNKNOWN;
        }

        #endregion

        /// <summary>
        /// Z-L축이 안전위치에있는가?(Ready 보다 위에있으면 안전위치이다)
        /// </summary>
        /// <returns></returns>
        public static Boolean isAxisSaftyZone(eAxis axis, int allowoffset = 2)
        {
            //홈을 잡지 않았다면 오류로 처리함
            var motIndex = (int)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.X_PICKER)
            {
                //피커는 안전위이가 2개 있다
                readypos = GetAxPXPos(eAxisPXPos.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.Z_PICKER) readypos = GetAxPZPos(eAxisPZPos.READY);
            else if (axis == eAxis.Z_THETA) readypos = GetAxPTPos(eAxisPTPos.READY);
            else if (axis == eAxis.PL_MOVE) readypos = GetAxPLMPos(eAxisPLMovePos.READY);
            else if (axis == eAxis.PR_MOVE) readypos = GetAxPRMPos(eAxisPRMovePos.READY);
            else if (axis == eAxis.PR_UPDN) readypos = GetAxPRZPos(eAxisPRUPDNPos.READY);
            else if (axis == eAxis.PL_UPDN) readypos = GetAxPLZPos(eAxisPLUPDNPos.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_;
            if (!Move_Validation(axis, out errorMessage)) retval = false;    //이동이 불가한 경우 체크
            else if (axis == eAxis.X_PICKER)
            {
                //Z축 홈이 필요하다
                var zFHome = Pub.mot.isHomeSet[(int)eAxis.Z_PICKER];
                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 축 홈이 필요 합니다";
                    retval = false;
                }
            }
            else if (axis == eAxis.PR_MOVE)   //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
            {
                if (Pub.mot.isHomeSet[(int)eAxis.PR_UPDN] == false)
                {
                    errorMessage = "PRINT-R Z 축 홈이 필요 합니다";
                    retval = false;
                }
            }
            return retval;
        }

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

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

            return true;
        }

    }
}