#define _REAL





using System;

using System.Collections.Generic;

using System.Text;

using System.Windows.Forms;

using System.Threading;

//using YETlib;

using System.Runtime.InteropServices;



namespace PinMarker.MotionWrappers

{

    public class YmcWrapper : MotionObject 

    {

        IntPtr gGlobalPtr;

        IntPtr hDevice;

        IntPtr hMark, hReset, hHome, hAlarm = IntPtr.Zero, hErrorCode;

        static readonly int CountsFactor = 2048;

        static readonly int SpeedCountsFactor = 2048*60;

        int lastError = -1;



        //IntPtr hAir, hX, hY, hF;     

        public virtual event ErrorStatusChanged OnErrorStatusChanged;





        private IntPtr hNumMoves;

        private IntPtr hRun, hStop;





        public int GetCountsFactor(int ax)

        {

            return CountsFactor;

        }



        public int GetSpeedCountsFactor(int ax)

        {

            return SpeedCountsFactor;

        }



        public void SetResetEvent(AutoResetEvent _stopEvent)

        {

            

        }



        public YmcWrapper()

        {



        }



        public int Init()

        {

#if _REAL

            // Logger.GetInstance().Log("Trying to create yetlib.Init()");

            try

            {

                



                int rc = 0;

                unsafe

                {

                    COM_DEVICE* ComDevice = stackalloc COM_DEVICE[1];

                    ComDevice[0].ComDeviceType = 4; // COMDEVICETYPE_PCI_MODE;

                    ComDevice[0].PortNumber = 1;

                    ComDevice[0].CpuNumber = 1;

                    ComDevice[0].NetworkNumber = 0;

                    ComDevice[0].StationNumber = 0;

                    ComDevice[0].UnitNumber = 0;

                    ComDevice[0].IPAddress = 0;

                    ComDevice[0].Timeout = 10000;



                    

                    /*GCHandle arr = GCHandle.Alloc(ComDevice, GCHandleType.Pinned);                   

                    rc = fnTestDll((COM_DEVICE*)arr.AddrOfPinnedObject().ToPointer(), out gGlobalPtr);

                    rc = fnTestDll1((COM_DEVICE*)arr.AddrOfPinnedObject().ToPointer(), out gGlobalPtr);                   

                    //rc = ymcOpenController((COM_DEVICE*)arr.AddrOfPinnedObject().ToPointer(), out gGlobalPtr);

                    */



                    rc = ymcOpenController(ComDevice, out gGlobalPtr);                   



                    Logger.GetInstance().Log("ymcOpenController done: rc=" + rc.ToString());

                    Logger.GetInstance().Log("ymcOpenController done: gGlobalPtr=" + gGlobalPtr.ToString());

                                    

                    //rc = ymcSetAPITimeoutValue(30000);

                    Logger.GetInstance().Log("ymcSetAPITimeoutValue done: rc=" + rc.ToString());





                    rc = ymcClearAllAxes();

                    Logger.GetInstance().Log("ymcClearAllAxes done: rc=" + rc.ToString());



                    //rc = InitAxis();



                    ymcGetRegisterDataHandle("MB100013", out hReset);

                    ymcGetRegisterDataHandle("MB100010", out hHome);



                    ymcGetRegisterDataHandle("OB00224", out hMark);



                    /*ymcGetRegisterDataHandle("ML10050", out hAir);

                    ymcGetRegisterDataHandle("ML10052", out hX);

                    ymcGetRegisterDataHandle("ML10054", out hY);

                    ymcGetRegisterDataHandle("ML10056", out hF);

                     */

//                    ymcGetRegisterDataHandle("ML10050", out hMoveBuf);

                    ymcGetRegisterDataHandle("MW10041", out hNumMoves);

                    ymcGetRegisterDataHandle("MB100011", out hRun);

                    ymcGetRegisterDataHandle("MB100012", out hStop);



                    ymcGetRegisterDataHandle("MB100050", out hAlarm);

                    ymcGetRegisterDataHandle("MW10000", out hErrorCode);



                }

                // int rc = yetlib.Init();

               

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("Init() - Exception: " + e.Message);

            }

           

#endif

            // Logger.GetInstance().Log("int rc = yetlib.Init(); done");

            return 0;

        }



        public int Close()

        {

#if _REAL

            // yetlib.Close();

            try

            {

                ymcCloseController(gGlobalPtr);

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("ymcCloseController - Exception: " + e.Message);

            }

#endif

            return 0;

        }



        int GetHandleToRegister(String sRegister, ref IntPtr Handle)

        {

#if _REAL

            try

            {

                int rc = ymcGetRegisterDataHandle(sRegister, out Handle);

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("GetHandleToRegister - Exception: " + e.Message);

            }

#endif



            return 0;

        }



        int SetMemory(IntPtr Handle, int nCount, int[] values)

        {

#if _REAL

            try

            {

                unsafe

                {

                    GCHandle arr = GCHandle.Alloc(values, GCHandleType.Pinned);

                    int rc = ymcSetRegisterData(Handle, nCount, (int*)arr.AddrOfPinnedObject().ToPointer());

                }

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("SetMemory - Exception: " + e.Message);

            }

#endif

            return 0;

        }



        int GetMemory(IntPtr Handle, int nCount, ref int[] values, ref int nOutCount)

        {

#if _REAL

            try

            {

                unsafe

                {

                    int rc = ymcGetRegisterData(Handle, nCount, out values[0], out nOutCount);

                    //  Logger.GetInstance().Log("GetMemory: " + Handle.ToString() + " rc= " + rc.ToString());

                    return rc;

                }

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("GetMemory - Exception: " + e.Message);

            }

#endif

            return 0;

         

        }



        /*

        public int InitAxis()

        {

            int rc = 0;

            try

            {               

                unsafe

                {

                    IntPtr* hAxis = stackalloc IntPtr[2];

                    rc = ymcDeclareAxis(1, 0, 3, 1, 1, 1, "AXIS001", out hAxis[0]);

                    rc = ymcDeclareAxis(1, 0, 3, 2, 2, 1, "AXIS002", out hAxis[1]);



                    rc = ymcDeclareDevice(2, hAxis, out hDevice);

                }

            }

            catch (Exception e)

            {

                Logger.GetInstance().Log("ymcDeclareAxis - Exception: " + e.Message);

                return rc;

            }



            return rc;

        }

        */



        public long MoveTo(double X, double Y, int Speed, int Acc, int Dec)

        {

            const int MTYPE_ABSOLUTE = 1;

            const int VTYPE_UNIT_PAR = 0;

            const int ATYPE_UNIT_PAR = 0;

            const int FTYPE_S_CURVE = 0;

            //const int PHYSICALAXIS = 0;

            const int DATATYPE_IMMEDIATE = 0;

            const short DISTRIBUTION_COMPLETED = 0;

            //const short COMMAND_STARTED = 2;

           



            long rc = 0;



            try

            {

                unsafe

                {

                    MOTION_DATA[] motData = new MOTION_DATA[2];

                    POSITION_DATA[] posData = new POSITION_DATA[2];                                       



                    motData[0].CoordinateSystem = motData[1].CoordinateSystem = 0;

                    motData[0].MoveType = motData[1].MoveType = MTYPE_ABSOLUTE;

                    motData[0].VelocityType = motData[1].VelocityType = VTYPE_UNIT_PAR;

                    motData[0].AccDecType = motData[1].AccDecType = ATYPE_UNIT_PAR;

                    motData[0].FilterType = motData[1].FilterType = FTYPE_S_CURVE;

                    motData[0].DataType = motData[1].DataType = 0;



                    motData[0].MaxVelocity = motData[1].MaxVelocity = 1428800;

                    motData[0].Acceleration = motData[1].Acceleration = Acc;

                    motData[0].Deceleration = motData[1].Deceleration = Dec;

                    motData[0].FilterTime = motData[1].FilterTime = 0;

                    motData[0].Velocity = motData[1].Velocity = Speed;

       

                    //rc = ymcGetAxisHandle(PHYSICALAXIS, 1, 0, 3, 1, 0, null, out hAxis[0]);

                    //rc = ymcGetAxisHandle(PHYSICALAXIS, 1, 0, 3, 2, 0, null, out hAxis[1]);



                    

                    posData[0].DataType = DATATYPE_IMMEDIATE;

                    posData[0].PositionData = Convert.ToInt32(X * CountsFactor);



                    posData[1].DataType = DATATYPE_IMMEDIATE;

                    posData[1].PositionData = Convert.ToInt32(Y * CountsFactor);





                    //Call ymcMoveLinear.

                    rc = ymcMoveLinear(hDevice, motData, posData, 0, null, DISTRIBUTION_COMPLETED, 0);



                    Logger.GetInstance().Log("MoveTo - Done, rc= " + rc.ToString());



                    //rc = ymcMoveLinear(hDevice, motData, posData, 0, null, COMMAND_STARTED, 0);

                }

            }

            catch (Exception e)

            {                

                Logger.GetInstance().Log("MoveTo - Exception: " + e.Message);

                return rc;

            }

            return 0;

        }



        public long Home()

        {

            int[] v = new int[1];

            v[0] = 1;

            SetMemory(hHome, 1, v);

            Thread.Sleep(10);

            v[0] = 0;

            SetMemory(hHome, 1, v);



            return 0;

        }



        public long Reset()

        {

            int[] v = new int[1];

            v[0] = 1;

            SetMemory(hReset, 1, v);

            Thread.Sleep(10);

            v[0] = 0;

            SetMemory(hReset, 1, v);



            return 0;

        }



        public long Stop()

        {

            int[] v = new int[1];

            v[0] = 1;

            SetMemory(hStop, 1, v);         



            return 0;

        }



        public bool IsProgramEnded()

        {

            int[] d = new int[1];

            int nout = 0;



            try

            {

                GetMemory(hRun, 1, ref d, ref nout);



                return d[0] == 0;

            }

            catch (Exception e)

            {

                MessageBox.Show(e.Message);

                return false;

            }



            return false;

        }



        public long SendMotionsBuffer(List<Motion> values, int nMotions)

        {



            // Create the correct buffer

          /*  double tx, ty;



            int[] motBuff = new int[values.Count * 4];



            for (int i = 0; i < gpPrev.path.Count; i++)

            {

                if (backgroundWorker1.CancellationPending)

                {

                    MessageBox.Show("Cancelled");

                    break;

                }



                if (gpPrev.path[i].Type == MachLine.LineType.Connection)

                    motBuff[i * 4] = 0;

                else

                    motBuff[i * 4] = 1;



                tx = gpPrev.path[i].Xe;

                ty = -gpPrev.path[i].Ye;

                //tx = Convert.ToSingle(txtXmm.Text) - gpPrev.path[i].Xe;

                //ty = Convert.ToSingle(txtYmm.Text) - gpPrev.path[i].Ye;

                //ty = gpPrev.path[i].Ye;

                motBuff[i * 4 + 1] = (int)(tx * motion.GetCountsFactor(0));

                motBuff[i * 4 + 2] = (int)(ty * motion.GetCountsFactor(1));

                motBuff[i * 4 + 3] = (int)(Speed * motion.GetSpeedCountsFactor(0));



                //Console.WriteLine(i.ToString() + ":" + tx.ToString() + ", " + ty.ToString());

            }

            */

            return 0;

        }



        public long SendMotionsBuffer(int[] values, int nMotions)

        { 

            int[] d = new int[1];

            const int MaxReg = 100;

            d[0] = nMotions;

            try

            {                

                SetMemory(hNumMoves, 1, d);

                        

                int nFirstReg = 0;

                while (nFirstReg < nMotions*4)

                {

                    int count = Math.Min(MaxReg, nMotions*4-nFirstReg);

                    int[] temp = new int[count];



                    for (int k = 0; k < count; k++)

                        temp[k] = values[nFirstReg+k];



                    String S = "ML" + (10050 + nFirstReg*2).ToString();

                    IntPtr hMoveBuf;

                    ymcGetRegisterDataHandle(S, out hMoveBuf);

                    SetMemory(hMoveBuf, count, temp);



                    nFirstReg += count;

                }

                

                d[0] = 1;

                SetMemory(hRun, 1, d);

            }

            catch (Exception e)

            {

                MessageBox.Show(e.Message);

            }



            return 0;

        }



        public int MarkerUpDown(int nVal)

        {

            int[] v = new int[1];

            v[0] = 0;

            return SetMemory(hMark, 1, v);

        }



        public int ReadError()

        {            

            int[] d = new int[1];

            int count = 0;



            if (hAlarm != IntPtr.Zero)

                GetMemory(hAlarm, 1, ref d, ref count);

            else

                d[0] = 0; // sim 1 to make read error work



            if (d[0] != 0)

            {

                GetMemory(hErrorCode, 1, ref d, ref count);

                if (lastError != d[0])

                {

                    lastError = d[0];

                    OnErrorStatusChanged(this, lastError);

                }

                return lastError;

            }

            else

            {

                if (lastError != 0)

                {

                    lastError = 0;

                    OnErrorStatusChanged(this, lastError);

                }

            }

            return 0;

        }



#if _REAL



        #region Imports

        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcSetRegisterData(

            IntPtr dh, int N, int* data);



        [DllImport("ymcPCAPI.dll")]

        public static extern int ymcGetRegisterData(

            IntPtr dh, int N, out int data, out int nCount);



        [DllImport("ymcPCAPI.dll")]

        public static extern int ymcGetRegisterDataHandle(

            String pRegisterName,

            out IntPtr hRegisterData);



        [DllImport("ymcPCAPI.dll")]

        public static extern int ymcClearAllAxes();

   

        [DllImport("ymcPCAPI.dll")]

        public static extern int ymcCloseController(IntPtr hCont);



        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]

        public struct COM_DEVICE

        {

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort ComDeviceType;

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort PortNumber;

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort CpuNumber;

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort NetworkNumber;

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort StationNumber;

            [MarshalAs(UnmanagedType.U2, SizeConst = 2)]

            public ushort UnitNumber;

            [MarshalAs(UnmanagedType.U4, SizeConst = 4)]

            public uint IPAddress;

            [MarshalAs(UnmanagedType.U4, SizeConst = 4)]

            public uint Timeout;

        }





        /*[DllImport("TestDll.dll")]

        public static extern unsafe int fnTestDll1

            (COM_DEVICE* data, out IntPtr dh);



        [DllImport("TestDll.dll")]

        public static extern unsafe int fnTestDll(COM_DEVICE* data, out IntPtr dh);

        */



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcOpenController

            (COM_DEVICE* data, out IntPtr dh);



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcSetAPITimeoutValue

            (long dh);



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcGetAxisHandle

            (

            ushort SpecifyType,

            ushort RackNo,

            ushort SlotNo,

            ushort SubslotNo,

            ushort AxisNo,

            ushort LogicalAxisNo,

            String	pAxisName,

            out IntPtr pAxis);



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcDeclareDevice(

            Int16 AxisNum,

            IntPtr* pAxis,

            out IntPtr phDevice);



        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]

        public struct MOTION_DATA

        {

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 CoordinateSystem;

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 MoveType;

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 VelocityType;

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 AccDecType;

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 FilterType;

            [MarshalAs(UnmanagedType.I2, SizeConst = 2)]

            public Int16 DataType;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 MaxVelocity;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 Acceleration;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 Deceleration;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 FilterTime;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 Velocity;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 ApproachVelocity;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 CreepVelocity;

        }



        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]

        public struct POSITION_DATA

        {

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 DataType;

            [MarshalAs(UnmanagedType.I4, SizeConst = 4)]

            public Int32 PositionData;

        }



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcMoveLinear(

            IntPtr					hDevice,

            MOTION_DATA[]			lpMotionData,

            POSITION_DATA[]          lpPos,

            Int32					hMoveIO,

            byte*					pObjectName,

            Int16					WaitForCompletion,

            Int32					SystemOption

            );



        [DllImport("ymcPCAPI.dll")]

        public static extern unsafe int ymcDeclareAxis(

            Int16 RackNo,

            Int16 SlotNo,

            Int16 SubslotNo,

            Int16 AxisNo,

            Int16 LogicalAxisNo,

            Int16 AxisType,

            String		pAxisName,

            out IntPtr pAxis);

    



        #endregion

#endif

    }

}