using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Runtime.InteropServices;
namespace ConsoleApplication2
{
    enum RobotMode
    {
        ROBOT_MODE_MANUAL,	//手动低速模式
        ROBOT_MODE_AUTO,	//自动模式
        ROBOT_MODE_MANUFAST	//手动高速模式
    };
    enum ProgramState
    {
        PROG_STATE_NOT_LOAD,	//程序未加载
        PROG_STATE_RUNNING,		//运行中
        PROG_STATE_PAUSE,		//暂停
        PROG_STATE_STOP			//停止
    };
    enum SpeedRatioType
    {
        SPEED_RATIO_JOG,	//手动速度倍率
        SPEED_RATIO_PROG	//程序速度倍率
    };
    public struct RobotPos
    {
        public double x;
        public double y;
        public double z;
        public double a;
        public double b;
        public double c;
        public int cfg;
        public int turn;
        public double ej1;
        public double ej2;
        public double ej3;
        public double ej4;
        public double ej5;
        public double ej6;
    };

    class Program
    {
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern uint GetMaxConnectionCount();
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern uint GetCurrentConnectionCount();    
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern IntPtr CreateCRobotAPI(uint index);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern void DestoryCRobotAPI(IntPtr pRobot);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int EnableApiControl(IntPtr pRobot, byte enable);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int ConnectRobot(IntPtr pRobot, string robot_addr);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern void DisconnectRobot(IntPtr pRobot);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int PowerOn(IntPtr pRobot);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int SetControlMode(IntPtr pRobot, RobotMode mode);  
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int GetPos(IntPtr pRobot, ref RobotPos robot_pos);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int CheckLinePosValid(IntPtr pRobot, RobotPos start_pos, IntPtr target_pos, uint target_pos_size, uint tool_index, uint wobj_index, byte[] valid, uint valid_size);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int GetAlarmList(IntPtr pRobot, uint[] list, uint list_size);
        [DllImport("RobotAPI.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern int LoadProgram(IntPtr pRobot, string file_path);

        static int Main(string[] args)
        {
            Console.WriteLine("Program start");

            //获取最大允许连接数
            uint max_connection_count = GetMaxConnectionCount();
            Console.WriteLine("max_connection_count: {0}", max_connection_count);
            
            //测试建立5个连接
            IntPtr[] robot = new IntPtr[5] { IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero };
            for (uint i = 0; i < 5; i++)
            {
                robot[i] = CreateCRobotAPI(i);

            }
            int res = ConnectRobot(robot[1], "192.168.0.101");
            if (res != 0)
            {
                Console.WriteLine("robot[1] connect failed");
            }
            res = ConnectRobot(robot[2], "192.168.0.102");
            if (res != 0)
            {
                Console.WriteLine("robot[2] connect failed");
            }
            //测试删除无用连接
            DestoryCRobotAPI(robot[4]);

            //获取现存连接数
            uint curr_connection_count = GetCurrentConnectionCount();
            Console.WriteLine("curr_connection_count: {0}", curr_connection_count);

            byte enable = 1;
            res = EnableApiControl(robot[1], enable);
            if (res != 0)
            {
                Console.WriteLine("EnableApiControl faileded!");
                return 0;
            }
            res = PowerOn(robot[1]);
            if (res != 0)
            {
                Console.WriteLine("PowerOn failed!");
                return 0;
            }
            RobotPos pos = new RobotPos();
            RobotMode mode = RobotMode.ROBOT_MODE_AUTO;
            int setcontrolmode = SetControlMode(robot[1], mode);
            Console.WriteLine("setcontrolmode: {0}", setcontrolmode);
            res = GetPos(robot[1], ref pos);
            if (res != 0)
            {
                Console.WriteLine("GetPos failed!");
                return 0;
            }

            uint target_pos_size = 3;
            uint tool_index = 3;
            uint wobj_index = 2;
            uint valid_size = 5;
            RobotPos pos1 = new RobotPos()
            {
                x = 1.0,
                y = 1.0,
                z = 1.0,
                a = 1.0,
                b = 1.0,
                c = 1.0,
                cfg = 1,
                turn = 1,
                ej1 = 2.0,
                ej2 = 1.0,
                ej3 = 1.0,
                ej4 = 1.0,
                ej5 = 1.0,
                ej6 = 3.0
            };
            RobotPos pos2 = new RobotPos()
            {
                x = 2.0,
                y = 2.0,
                z = 2.0,
                a = 2.0,
                b = 2.0,
                c = 2.0,
                cfg = 2,
                turn = 2,
                ej1 = 2.0,
                ej2 = 2.0,
                ej3 = 2.0,
                ej4 = 2.0,
                ej5 = 2.0,
                ej6 = 2.0
            };
            RobotPos[] f = new RobotPos[] { pos1, pos2 };
            IntPtr g = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(RobotPos)) * f.Length);
            Marshal.StructureToPtr(f[0], g, false);
            Marshal.StructureToPtr(f[1], g + Marshal.SizeOf(typeof(RobotPos)), false);

            byte[] valid_vec = { 1, 1, 1, 0, 0 };
            res = CheckLinePosValid(robot[1], pos1, g, target_pos_size, tool_index, wobj_index, valid_vec, valid_size);
            if (res != 0)
            {
                Console.WriteLine("CheckLinePosValid failed!");
                return 0;
            }
            Console.WriteLine("pos1.x={0}, pos1.ej1={1}, pos1.ej6={2}", pos1.x, pos1.ej1, pos1.ej6);
            Console.WriteLine("Sizeof(RobotPos): {0}", Marshal.SizeOf(typeof(RobotPos)));

            uint[] list = { 1, 2, 3, 4, 5 };
            uint list_size = 10;
            res = GetAlarmList(robot[1], list, list_size);
            if (res != 0)
            {
                Console.WriteLine("GetAlarmList failed!");
                return 0;
            }

            string file_path = "C:/testLoadProgram/";
            res = LoadProgram(robot[1], file_path);
            if (res != 0)
            {
                Console.WriteLine("LoadProgram failed!");
                return 0;
            }
            Console.WriteLine("Program over!");
            while (true) { }
        }
    }
}
