using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using ORiN2.bCAP;
using ORiN2.Library;

namespace bCapRobotMove
{
    class Program
    {
        /* *** Please change IP to the target RC8 *** */
        //private static string TARGET_RC8_IP = "192.168.0.1";
        private static string TARGET_RC8_IP = "127.0.0.1";

        static void Main(string[] args)
        {
            try
            {
                /* ---- [Preparation] ---- */
                /*
                 * 1. [RC8 side] Change Mode Switch to [AUTO]
                 */

                new bCapRobotMove("TCP:" + TARGET_RC8_IP, 3000, 3);
            }
            catch (Exception cause)
            {
                Console.WriteLine(cause.ToString());
            }
        }
    }

    public class bCapRobotMove
    {
        private bCAPClient m_client = null;

        public bCapRobotMove(String strConn, int timeout, int retry)
        {
            m_client = new bCAPClient(strConn, timeout, retry);
            m_client.Service_Start("WDT=400");

            /* Get controller handle */
            int hCtrl = 0;
            try
            {
                hCtrl = m_client.Controller_Connect("Ctrl", "CaoProv.DENSO.VRC", "localhost", "");

                /* Clear error */
                m_client.Controller_Execute(hCtrl, "ClearError");
                Console.WriteLine("Clear error");

                /* Get robot handle */
                int hRob = 0;
                try
                {
                    hRob = m_client.Controller_GetRobot(hCtrl, "Rob", "");

                    /* Get arm control authority */
                    m_client.Robot_Execute(hRob, "Takearm", new int[] { 0, 1 });
                    Console.WriteLine("TakeArm");

                    /* Motor on */
                    m_client.Robot_Execute(hRob, "Motor", true);
                    Console.WriteLine("Motor ON");

                    /* Change external speed to 20% */
                    m_client.Robot_Execute(hRob, "ExtSpeed", 20.0);
                    Console.WriteLine("ExtSpeed 20%");

                    /* Move to J(j1 angle, j2 angle, ...) */
                    //Console.WriteLine("Move to J(45, 30, 120, 0, -60, 0)");
                    //m_client.Robot_Move(hRob, 1, "@E J(45, 30, 120, 0, -60, 0)", "");
                    //Console.WriteLine("Move done");

                    /* Move to  P(x,y,z,rx,ry,rz,fig) */
                    Console.WriteLine("Move to P(400.0, 0.0, 300.0, -180.0, 89.0, 180.0, 1)");
                    m_client.Robot_Move(hRob, 1, "@E P(400.0, 0.0, 300.0, -180.0, 89.0, 180.0, 1)", "");
                    Console.WriteLine("Move done");

                    /* Move to  P(x,y,z,rx,ry,rz,fig) */
                    Console.WriteLine("Move to P(306.4245, 208.2947, 300.0, -180.0, 89, 180.0, 1)");
                    m_client.Robot_Move(hRob, 1, "@P P(306.4245, 208.2947, 300.0, -180.0, 89.0, 180.0, 1)", "");
                    Console.WriteLine("Move done");

                    /* Move to  P(x,y,z,rx,ry,rz,fig) with NEXT option */
                    Console.WriteLine("Move to P(306.4245, 208.2947, 575.0, -180.0, 89.0, 180.0, 1)");
                    m_client.Robot_Move(hRob, 1, "@E P(306.4245, 208.2947, 575.0, -180.0, 89.0, 180.0, 1)", "Next");
                    bool bDone = false;
                    do
                    {
                        Thread.Sleep(100);
                        /* Check motion complete */
                        bDone = (bool)m_client.Robot_Execute(hRob, "MotionComplete");
                        /* Get current position (x,y,z,rx,ry,rz,fig)*/
                        double[] dCurPos = (double[])m_client.Robot_Execute(hRob, "CurPos", "");
                        Console.WriteLine("CurPos = ({0:0.000},{1:0.000},{2:0.000},{3:0.000},{4:0.000},{5:0.000},{6:0})", dCurPos[0], dCurPos[1], dCurPos[2], dCurPos[3], dCurPos[4], dCurPos[5], dCurPos[6]);
                    } while (!bDone);
                    Console.WriteLine("Move done");

                    /* Motor off */
                    m_client.Robot_Execute(hRob, "Motor", false);
                    Console.WriteLine("Motor OFF");

                    /* Release arm control authority */
                    m_client.Robot_Execute(hRob, "Givearm", null);
                    Console.WriteLine("GiveArm");
                }
                catch (Exception cause)
                {
                    Console.WriteLine(cause.ToString());
                }
                finally
                {
                    if (hRob != 0)
                    {
                        /* Release robot handle */
                        m_client.Robot_Release(hRob);
                    }
                }
            }
            catch (Exception cause)
            {
                Console.WriteLine(cause.ToString());
            }
            finally
            {
                if (hCtrl != 0)
                {
                    /* Disconnect controller */
                    m_client.Controller_Disconnect(hCtrl);
                }
            }

            m_client.Service_Stop();

            Console.ReadLine();
        }
    }
}