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

/*
This demonstration makes the path motion that is the same as an infinite symbol.
    . .      . .
  .     .  .     .
 .       .        .
  .     .  .     .
    . .      . .
*/
namespace bCapSlaveMove
{
    class Program
    {
        static void Main(string[] args)
        {
            try
            {
                /* ---- [Preparation] ---- */
                /*
                 * 1. [RC8 side] Add [b-CAP Slave] option license
                 * 2. [RC8 side] Change Mode Switch to [AUTO]
                 */

                new bCapSlaveMove("TCP:127.0.0.1", 3000, 3);
            }
            catch (Exception cause)
            {
                Console.WriteLine(cause.ToString());
            }

        }
    }

    public class bCapSlaveMove
    {
        private static int PERIOD = 100;
        private static int AMPLITUDE = 10;
        private static int TIMES = 3;

        private bCAPClient m_client = null;

        public bCapSlaveMove(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 first pose */
                    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");

                    /* Get current angle */
                    double[] dJntInit = (double[])m_client.Robot_Execute(hRob, "CurJnt", "");

                    /* Copy current angle to temporary */
                    double[] dJntTmp = new double[dJntInit.Length];
                    for (int i = 2; i < dJntInit.Length; i++)
                        dJntTmp[i] = dJntInit[i];

                    /* Start slave mode (Mode 2, J Type) */
                    m_client.Robot_Execute(hRob, "slvChangeMode", 0x202);
                    Console.WriteLine("Start slave mode (Mode 2, J Type)");

                    for (int j = 0; j < TIMES; j++)
                    {
                        for (int i = 0; i < PERIOD*2; i++)
                        {
                            if (i <= PERIOD)
                            {
                                dJntTmp[0] = dJntInit[0] + i / 10.0;
                                dJntTmp[1] = dJntInit[1] + AMPLITUDE * Math.Sin(2 * Math.PI * i / PERIOD);
                            }
                            else
                            {
                                dJntTmp[0] = dJntInit[0] + (PERIOD * 2 - i) / 10.0;
                                dJntTmp[1] = dJntInit[1] - AMPLITUDE * Math.Sin(2 * Math.PI * (PERIOD * 2 - i) / PERIOD);
                            }
                            m_client.Robot_Execute(hRob, "slvMove", dJntTmp);
                        }
                    }

                    /* Stop robot */
                    m_client.Robot_Execute(hRob, "slvMove", dJntTmp);

                    /* Stop slave mode */
                    m_client.Robot_Execute(hRob, "slvChangeMode", 0);
                    Console.WriteLine("Change to normal mode");

                    /* 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();
        }
    }

}