using System;
using System.Collections;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Timers;

// for DllImport
using System.Runtime.InteropServices;

// for ORiN
using ORiN2.interop.CAO;

namespace JoyStick
{
    public partial class frmMain : Form
    {
        private delegate void JoyStick_Control(CaoMessage msg);  // Delegate function for processing OnMessage event

        // Read ini file with kernelwin32
        [DllImport("KERNEL32.DLL")]
        private static extern uint GetPrivateProfileString(string lpAppName, string lpKeyName, string lpDefault, StringBuilder lpReturnedString, uint nSize, string lpFileName);
        
        // Write ini file with kernelwin32
        [DllImport("KERNEL32.DLL")]
        private static extern uint WritePrivateProfileString(string lpAppName, string lpKeyName, string lpString, string lpFileName);

        // Cao objects
        private CaoEngine m_caoEng;
        private CaoWorkspace m_caoWs;

        // Cao objects for controlling robot controller
        private CaoController m_caoCtrl;
        private CaoRobot m_caoRobo;
        private CaoVariable m_CurAng;
        private CaoVariable m_CurPos;
        private CaoVariable m_caoRoboBusy;
        
        // Cao object for controlling joystick
        private CaoController m_caoJoystickCtrl;
        private static string m_conAppName = "JoyStick";

        // Current angle and position
        private float[] m_fJnt; // Type J
        private float[] m_fPos; // Type P

        private int m_iManualInterporation; // Move type (1: Type J, 2: Type P)
        private int m_iManualType;  // -1: Minus button is pressed, 1: Plus button is pressed
        private int m_iManualJoint; // Index of pressed button
        private int m_iButtons; // Index of pressed button

        // Ini file name
        private string m_strIniFileName;

        public frmMain()
        {
            InitializeComponent();
        }

        /*
         * On_Click btnConn
         * 
         * If controller object already exists, then execute Disconnect
         * else execute Connect
         */
        private void btnConn_Click(object sender, EventArgs e)
        {
            if (m_caoCtrl != null)
            {
                Disconnect();
            }
            else
            {
                Connect();
            }
        }

        /* 
         * Connect controller and get robot object
         */
        private void Connect()
        {
            try
            {
                string sConn;
                sConn = cboConn.Text;

                m_caoCtrl = m_caoWs.AddController("", "CaoProv.DENSO.RC8", "", sConn);
                m_caoRobo = m_caoCtrl.AddRobot("RC8", "");

                // Get arm control authority
                m_caoRobo.Execute("Takearm", "");

                // Motor on
                //m_caoRobo.Execute("Motor", new string[2] { "1", "0" });

                // Get variable objects for watching current angle and posture
                m_CurAng = m_caoRobo.AddVariable("@CURRENT_ANGLE", "");
                m_CurPos = m_caoRobo.AddVariable("@CURRENT_POSITION", "");

                // Get variable object for watching robot status
                m_caoRoboBusy = m_caoRobo.AddVariable("@BUSY_STATUS", "");

                btnConn.Text = "&Disconnect";
                ControlEnabled(true);   // Enable components

                AddConnHistory(cboConn.Text);

                InitCurPos();   // Get current angle and posture
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);

                Disconnect();
            }
        }

        /*
         * Disconnect controller
         */
        private void Disconnect()
        {
            DisconnectJoystick();

            try
            {
                // Release robot object
                if (m_caoRobo != null)
                {
                    // Release robot variable objects
                    if (m_CurAng != null)
                    {
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(m_CurAng);
                        m_CurAng = null;
                    }

                    if (m_CurPos != null)
                    {
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(m_CurPos);
                        m_CurPos = null;
                    }

                    if (m_caoRoboBusy != null)
                    {
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoRoboBusy);
                        m_caoRoboBusy = null;
                    }

                    // Motor off
                    //m_caoRobo.Execute("Motor", new string[2] { "0", "0" });

                    // Release arm control authority
                    m_caoRobo.Execute("Givearm", "");

                    System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoRobo);
                    m_caoRobo = null;
                }

                // Release controller object
                if (m_caoCtrl != null)
                {
                    //OK
                    // 1. CaoControllersオブジェクトから対象コントローラの参照を取り除く
                    CaoControllers ctrls = m_caoWs.Controllers;
                    ctrls.Remove(m_caoCtrl.Name);

                    // 2. 対象コントローラの参照を取り除く
                    System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoCtrl);
                    m_caoCtrl = null;

                    // 3. 1.で取得したCaoControllersの参照を取り除く
                    System.Runtime.InteropServices.Marshal.ReleaseComObject(ctrls);
                    ctrls = null;

                    //NG
                    // 1. CaoControllersオブジェクトから対象コントローラの参照を取り除く
                    //m_caoWs.Controllers.Remove(m_caoCtrl.Index);

                    // 2. 対象コントローラの参照を取り除く
                    //System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoCtrl);
                    //m_caoCtrl = null;
                }

                btnConn.Text = "&Connect";
                ControlEnabled(false);  // Disable components
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }
        // On_Change chkJoystick
        private void chkJoystick_CheckedChanged(object sender, EventArgs e)
        {
            try
            {
                if (!chkJoystick.Checked)
                {
                    DisconnectJoystick();
                }
                else
                {
                    ConnectJoystick();
                }
            }
            catch (Exception ex)
            {
                DisconnectJoystick();
                chkJoystick.Checked = false;

                MessageBox.Show(ex.Message);
            }
        }

        // Connect JoyStick
        private void ConnectJoystick()
        {
            m_caoJoystickCtrl = m_caoWs.AddController("JoyCtrl", "CaoProv.DENSO.Joystick", "", "ID=" + txtJoystickID.Text);
            m_caoJoystickCtrl.OnMessage += new _ICaoControllerEvents_OnMessageEventHandler(OnMessage);

            // Connected JoyStick.
            pbxJoystick.BackColor = Color.GreenYellow;
        }

        // Disconnect JoyStick
        private void DisconnectJoystick()
        {
            // Stop the timer1.
            timer1.Enabled = false;

            if (m_caoJoystickCtrl != null)
            {
                //OK
                // 1. CaoControllersオブジェクトから対象Joystickコントローラの参照を取り除く
                CaoControllers ctrls = m_caoWs.Controllers;
                ctrls.Remove(m_caoJoystickCtrl.Name);

                // 2. 対象JoyStickコントローラの参照を取り除く
                System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoJoystickCtrl);
                m_caoJoystickCtrl = null;

                // 3. 1.で取得したCaoControllersの参照を取り除く
                System.Runtime.InteropServices.Marshal.ReleaseComObject(ctrls);
                ctrls = null;
            }
            chkJoystick.Checked = false;
            pbxJoystick.BackColor = Color.Silver;
        }

        // Executed by OnMessage event
        private void JoyStick_Event(CaoMessage msg)
        {
            try
            {
                m_iManualInterporation = cboManualInterporation.SelectedIndex + 1;

                int iValue;
                double dValue;
                
                switch (msg.Number)
                {
                    case 1: // "BUTTONS" '[8]...[2][1] each bit stands for each button
                        Int32.TryParse(msg.Value.ToString(), out iValue);

                        // If pressed "button 10", then change interporation
                        if ((iValue & 0x200) != 0)
                        {
                            if (cboManualInterporation.SelectedIndex != 0)
                            {
                                // 1 - JOINT
                                cboManualInterporation.SelectedIndex = 0;
                            }
                            else
                            {
                                // 2 - X-Y
                                cboManualInterporation.SelectedIndex = 1;
                            }
                        }
                        m_iButtons = iValue;
                        break;
                    case 2:
                    case 3: // "@X", "@Y"
                        Double.TryParse(msg.Value.ToString(), out dValue); // X = -1.0 〜 +1.0
                        if ((msg.Number == 3) && (m_iManualInterporation == 2))
                        {
                            dValue = -1 * dValue;
                        }

                        System.Diagnostics.Debug.WriteLine("@X/Y: " + dValue.ToString());

                        if (dValue == 1.0) // Right
                        {
                            m_iManualType = 1; // 1: Plus button is pressed
                            if (m_iButtons != 0)
                            {
                                MoveManualCtrl(m_iManualType, m_iButtons);
                            }
                        }
                        else if (dValue == -1.0) //Left
                        {
                            m_iManualType = -1; // -1: Minus button is pressed
                            if (m_iButtons != 0)
                            {
                                MoveManualCtrl(m_iManualType, m_iButtons);
                            }
                        }
                        else if (dValue == 0) // else
                        {
                            m_iManualType = 0; //Do not move
                            MoveManualCtrl(m_iManualType, m_iButtons);
                        }
                        break;

                    case 6:
                        // arrow key up->right->down->left = 0 -> 9000 -> 18000 -> 27000
                        // not pressed = 65535

                        Int32.TryParse(msg.Value.ToString(), out iValue);
                        System.Diagnostics.Debug.WriteLine("@POV: " + iValue.ToString());

                        if (m_iManualInterporation == 2)
                        {
                            if ((iValue == 9000) || (iValue == 0)) // right or up
                            {
                                m_iManualType = 1; // 1: Plus button is pressed
                                if (m_iButtons != 0)
                                {
                                    MoveManualCtrl(m_iManualType, m_iButtons);
                                }
                            }
                            else if ((iValue == 27000) || (iValue == 18000)) // left or down
                            {
                                m_iManualType = -1; // -1: Minus button is pressed
                                if (m_iButtons != 0)
                                {
                                    MoveManualCtrl(m_iManualType, m_iButtons);
                                }
                            }
                            else
                            {
                                m_iManualType = 0; //Do not move
                                MoveManualCtrl(m_iManualType, m_iButtons);
                            }
                        }
                        else
                        {
                            if (iValue == 9000 || iValue == 18000) // right or down
                            {
                                m_iManualType = 1; // 1: Plus button is pressed
                                if (m_iButtons != 0)
                                {
                                    MoveManualCtrl(m_iManualType, m_iButtons);
                                }
                            }
                            else if (iValue == 27000 || iValue == 0) // left or up
                            {
                                m_iManualType = -1; // -1: Minus button is pressed
                                if (m_iButtons != 0)
                                {
                                    MoveManualCtrl(m_iManualType, m_iButtons);
                                }
                            }
                            else
                            {
                                m_iManualType = 0; // Do not move
                                MoveManualCtrl(m_iManualType, m_iButtons);
                            }
                        }
                        break;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        private void MoveManualCtrl(int iDirection, int iValue)
        {
            Boolean bMove = false;

            m_iManualInterporation = ((int)cboManualInterporation.SelectedIndex) + 1;

            if (iDirection != 0)
            {
                // Press button1
                if ((iValue & 0x1) != 0)
                {
                    m_iManualJoint = 0;
                    bMove = true;
                }
                // Press button2
                else if ((iValue & 0x2) != 0)
                {
                    m_iManualJoint = 1;
                    bMove = true;
                }
                // Press button3
                else if ((iValue & 0x4) != 0)
                {
                    m_iManualJoint = 2;
                    bMove = true;
                }
                // Press button4
                else if ((iValue & 0x8) != 0)
                {
                    m_iManualJoint = 3;
                    bMove = true;
                }
                // Press button5
                else if ((iValue & 0x10) != 0)
                {
                    m_iManualJoint = 4;
                    bMove = true;
                }
                // Press button6
                else if ((iValue & 0x20) != 0)
                {
                    m_iManualJoint = 5;
                    bMove = true;
                }
                // Press button7
                else if ((iValue & 0x40) != 0)
                {
                    m_iManualJoint = 6;
                    if (m_iManualInterporation != 2)
                    {
                        bMove = true;
                    }
                }
                // Press button8
                else if ((iValue & 0x80) != 0)
                {
                    m_iManualJoint = 7;
                    if (m_iManualInterporation != 2)
                    {
                        bMove = true;
                    }
                }
                else
                {
                    timer1.Enabled = false;
                }

                if (bMove)
                {
                    if (chkInching.Checked == true)
                    {
                        timer1_Tick(null, null);
                    }
                    else
                    {
                        timer1.Enabled = true;
                    }
                }
            }
            else
            {
                timer1.Enabled = false;
            }
        }

        /*
         * Get current angle and posture
         */
        private void InitCurPos()
        {
            double[] tmpCurPose;

            if (m_CurAng != null)
            {
                tmpCurPose = (double[])m_CurAng.Value;

                // Get current angle
                m_fJnt[0] = (float)tmpCurPose[0];
                m_fJnt[1] = (float)tmpCurPose[1];
                m_fJnt[2] = (float)tmpCurPose[2];
                m_fJnt[3] = (float)tmpCurPose[3];
                m_fJnt[4] = (float)tmpCurPose[4];
                m_fJnt[5] = (float)tmpCurPose[5];
                m_fJnt[6] = (float)tmpCurPose[6];
                m_fJnt[7] = (float)tmpCurPose[7];
            }

            if (m_CurPos != null)
            {
                tmpCurPose = (double[])m_CurPos.Value;

                // Get current position
                m_fPos[0] = (float)tmpCurPose[0];
                m_fPos[1] = (float)tmpCurPose[1];
                m_fPos[2] = (float)tmpCurPose[2];
                m_fPos[3] = (float)tmpCurPose[3];
                m_fPos[4] = (float)tmpCurPose[4];
                m_fPos[5] = (float)tmpCurPose[5];
                m_fPos[6] = (float)tmpCurPose[6];
            }
        }

        /*
         * Move robot
         */
        private void timer1_Tick(object sender, EventArgs e)
        {
            // If robot status is free, then move robot
            if ((bool)m_caoRoboBusy.Value == false)
            {
                // Get current cboManualInterporation value
                m_iManualInterporation = cboManualInterporation.SelectedIndex + 1;

                double dDev;
                double.TryParse(lblDevVal.Text, out dDev);

                try
                {
                    // Move with Type P
                    if (m_iManualInterporation != 2)
                    {
                        if (m_iManualJoint < 8)
                        {
                            m_fJnt[m_iManualJoint] = m_fJnt[m_iManualJoint] + (float)(m_iManualType * dDev);

                            m_caoRobo.Move(m_iManualInterporation, new object[] { m_fJnt, "J", "@P" }, "");
                        }
                    }
                    else // Move with Type J
                    {
                        if (m_iManualJoint < 7)
                        {
                            m_fPos[m_iManualJoint] = m_fPos[m_iManualJoint] + (float)(m_iManualType * dDev);

                            m_caoRobo.Move(m_iManualInterporation, new object[] { m_fPos, "P", "@P" }, "");
                        }
                    }
                }
                catch (Exception ex)
                {
                    timer1.Enabled = false;

                    m_iManualType = 0;
                    m_iManualJoint = 0;

                    InitCurPos();

                    MessageBox.Show(ex.Message);
                }
            }
        }

        /*
         * Get path of .ini file
         */
        private string AppPath()
        {
            return System.IO.Path.GetDirectoryName(Application.ExecutablePath);
        }

        /*
         * Add current controller path to history of connection
         */
        private void AddConnHistory(string strConn)
        {
            int iCurSel;
            iCurSel = cboConn.FindStringExact(strConn, -1);

            if (iCurSel == -1)
            {
                cboConn.Items.Insert(0, strConn);
            }
            else
            {
                cboConn.Items.RemoveAt(iCurSel);
                cboConn.Items.Insert(0, strConn);
            }

            if (cboConn.Items.Count > 10)
            {
                cboConn.Items.RemoveAt(10);
            }

            cboConn.SelectedIndex = 0;
        }

        /*
         * Load histroy of connection and value of deviation
         */
        private void LoadHistory()
        {
            int intLoopCount;
            string strConn;

            for (intLoopCount = 1; intLoopCount <= 10; intLoopCount++)
            {
                strConn = GetPrivateProfile(m_conAppName, "ConnHistory" + intLoopCount.ToString(), "");

                if (strConn.Length > 0)
                {
                    cboConn.Items.Add(strConn);
                }
            }

            if (cboConn.Items.Count == 0)
            {
                cboConn.Items.Add("server=192.168.0.1");
            }

            cboConn.SelectedIndex = 0;

            strConn = GetPrivateProfile(m_conAppName, "Deviation", "0.1");
            lblDevVal.Text = strConn;
        }

        /*
         * Write history of connection (Max 10 data)
         */
        private void SaveHistory()
        {
            int intLoopCount;

            if (cboConn.Items.Count > 0)
            {
                IEnumerator iEnum = cboConn.Items.GetEnumerator();
                iEnum.Reset();

                for (intLoopCount = 1; intLoopCount <= 10 && iEnum.MoveNext(); intLoopCount++)
                {
                    WritePrivateProfile(m_conAppName, "ConnHistory" + intLoopCount.ToString(), iEnum.Current as string);
                }
            }

            WritePrivateProfile(m_conAppName, "Deviation", lblDevVal.Text);
        }

        /*
         * Read parameters from .ini file
         */
        public string GetPrivateProfile(string Section, string EntryName, string Default)
        {
            StringBuilder tmp = new StringBuilder(255);
            uint strlen;

            strlen = GetPrivateProfileString(Section, EntryName, Default, tmp, (uint)tmp.Capacity, m_strIniFileName);

            if (strlen > 0)
                return (tmp.ToString());
            else
                return (Default);
        }

        /*
         * Write parameters to .ini file
         */
        public void WritePrivateProfile(string Section, string EntryName, string tmp)
        {
            WritePrivateProfileString(Section, EntryName, tmp, m_strIniFileName);
        }

        private void frmMain_Load(object sender, EventArgs e)
        {
            try
            {
                m_fJnt = new float[8];
                m_fPos = new float[7];

                // Create CaoEngine
                m_caoEng = new CaoEngine();

                // Get CaoWorkspace
                m_caoWs = m_caoEng.Workspaces.Item(0);

                // Initialize cao objects as null
                m_caoCtrl = null;
                m_caoRobo = null;
                m_CurAng = null;
                m_CurPos = null;
                m_caoRoboBusy = null;
                m_caoJoystickCtrl = null;

                // Read ini file
                m_strIniFileName = AppPath() + "\\Connect.ini";
                LoadHistory();

                cboManualInterporation.SelectedIndex = 0;
                m_iManualInterporation = cboManualInterporation.SelectedIndex + 1;

                m_iManualType = 0;
                m_iManualJoint = 0;

                ControlEnabled(false);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        /*
        * On_MouseDown btnMinus or btnPlus
        */
        private void btn_MouseDown(object sender, EventArgs e)
        {
            string btnName;
            btnName = ((System.Windows.Forms.Button)sender).Name;

            // Pressed plus button
            if (string.Compare(btnName.Substring(0, btnName.Length - 1), "btnPlus") == 0)
            {
                m_iManualType = 1;
            }
            else // Pressed minus button
            {
                m_iManualType = -1;
            }

            // Get index of pressed button
            int.TryParse(btnName.Substring(btnName.Length - 1, 1), out m_iManualJoint);

            if (chkInching.Checked == true)
            {
                timer1_Tick(null, null);
            }
            else
            {
                timer1.Enabled = true;
            }
        }

        // On_MouseUp btnMinus or btnPlus
        private void btn_MouseUp(object sender, EventArgs e)
        {
            m_iManualType = 0;
            m_iManualJoint = 0;

            timer1.Enabled = false;
        }

        /*
         * On_Change cboManualInterporation
         */
        private void cboManualInterporation_SelectedIndexChanged(object sender, EventArgs e)
        {
            // Get current angle and posture
            if (m_iManualInterporation != cboManualInterporation.SelectedIndex + 1)
            {
                InitCurPos();
            }
        }

        /*
         * Show help
         */
        private void btnHelp_Click(object sender, EventArgs e)
        {
            MessageBox.Show("[JOINT]" + "\n" +
            " J<n> : Button [<n>] + [<-]/[->]" + "\n" + "\n" +
            "[X-Y]" + "\n" +
            " X:    Button [1] + [<-] / [->]" + "\n" +
            " Y:    Button [2] + [<-] / [->]" + "\n" +
            " Z:    Button [3] + [<-] / [->]" + "\n" +
            " RX:   Button [4] + [<-] / [->]" + "\n" +
            " RY:   Button [5] + [<-] / [->]" + "\n" +
            " RZ:   Button [6] + [<-] / [->]"
             , "Help", System.Windows.Forms.MessageBoxButtons.OK);
        }

        /*
         * On_Click btnUpdate
         */
        private void btnUpdate_Click(object sender, EventArgs e)
        {
            double dVal;

            string strVal;
            strVal = txtDevCng.Text;

            // If strVal can not convert to double, goto catch process
            if (double.TryParse(strVal, out dVal) == false)
            {
                MessageBox.Show("Can not set this value for inching.", "Error", System.Windows.Forms.MessageBoxButtons.OK);
            }
            else
            {
                // Update lblDevVal
                lblDevVal.Text = dVal.ToString();
            }
        }

        // Catch OnMessage event
        private void OnMessage(CaoMessage pICaoMsg)
        {
            try
            {
                // Create JoyStick_Control object
                JoyStick_Control j_ctrl = new JoyStick_Control(JoyStick_Event);
                Invoke(j_ctrl, pICaoMsg);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }
        private void frmMain_FormClosing(object sender, FormClosingEventArgs e)
        {
            SaveHistory();
        }

        private void frmMain_FormClosed(object sender, FormClosedEventArgs e)
        {
            try
            {
                Disconnect();

                System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoWs);
                m_caoWs = null;

                System.Runtime.InteropServices.Marshal.ReleaseComObject(m_caoEng);
                m_caoEng = null;
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }
        /*
 * Control components
 *  
 * bEnabled = true:  Enable components
 * bEnabled = false; Disable components
 * 
 */
        private void ControlEnabled(Boolean bEnable)
        {
            // Interporation
            cboManualInterporation.Enabled = bEnable;

            // Joystick
            chkJoystick.Enabled = bEnable;
            txtJoystickID.Enabled = bEnable;

            // button
            btnMinus0.Enabled = bEnable;
            btnPlus0.Enabled = bEnable;
            btnMinus1.Enabled = bEnable;
            btnPlus1.Enabled = bEnable;
            btnMinus2.Enabled = bEnable;
            btnPlus2.Enabled = bEnable;
            btnMinus3.Enabled = bEnable;
            btnPlus3.Enabled = bEnable;
            btnMinus4.Enabled = bEnable;
            btnPlus4.Enabled = bEnable;
            btnMinus5.Enabled = bEnable;
            btnPlus5.Enabled = bEnable;
            btnMinus6.Enabled = bEnable;
            btnPlus6.Enabled = bEnable;
            btnMinus7.Enabled = bEnable;
            btnPlus7.Enabled = bEnable;

            // Deviation
            txtDevCng.Enabled = bEnable;
            btnUpdate.Enabled = bEnable;
            chkInching.Enabled = bEnable;
        }

    }
}