using System;
using System.Collections.Generic;
using System.Net.Sockets;
using System.Linq;
using System.Text;
//using System.Windows.Forms;

namespace EpsonC3Com
{
    public partial class TcpC3Com
    {
        /// <summary>
        /// Initialize robot by default in controller values of speed and acceleration, set power low
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendInit()
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "Init";
                dataMessage = "!" + command + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Initialize robot by speed and acceleration given by parameters, set power
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendInitS(PowerState power, InitPar parameters)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "InitS";
                dataMessage = "!" + command + dEnd;
                dataMessage += ((int)power).ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += parameters.Speed.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += parameters.Accel.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel + parameters.Decel.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += parameters.SpeedS.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += parameters.AccelS.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel + parameters.DecelS.ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Set power motors as Off, Low, High
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendPower(PowerState power)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "Power";
                dataMessage = "!" + command + dEnd;
                dataMessage += ((int)power).ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Close TCP parser in robot
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendClose()
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "Exit";
                dataMessage = "!" + command + dEnd;
                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Read specified digital input, returned to recieved block
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendDI(byte inputNum)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "DI";
                dataMessage = "?" + command + dEnd;
                dataMessage += inputNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Set specified digital output to Off/On
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendDO(byte outputNum, bool outputVal)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "DO";
                dataMessage = "!" + command + dEnd;
                dataMessage += outputNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += (Convert.ToByte(outputVal)).ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Read status of specified digital output
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendDOR(byte outputNum)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;
            try
            {
                String dataMessage;

                string command = "DOR";
                dataMessage = "?" + command + dEnd;
                dataMessage += outputNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                Initialized = true;

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
        
        /// <summary>
        /// Move robot to XYZ position using PTP
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendGo(RobCoord.XYZ inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                    inCoord.CheckCoord(true);

                string command = "Go";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + inCoord.x.Str + dDel + inCoord.y.Str + dDel + inCoord.z.Str + dDel + inCoord.u.Str + dDel + inCoord.v.Str + dDel + inCoord.w.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to XYZ position using PTP, wait in position and move up and check magnet switch status
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendGoM(bool magnet, RobCoord.XYZ inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                    inCoord.CheckCoord(true);

                string command = "GoM";
                dataMessage = "!" + command + dEnd;
                dataMessage += (Convert.ToByte(magnet)).ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += inCoord.x.Str + dDel + inCoord.y.Str + dDel + inCoord.z.Str + dDel + inCoord.u.Str + dDel + inCoord.v.Str + dDel + inCoord.w.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to XYZ position using straight movement
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendMove(RobCoord.XYZ inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                    inCoord.CheckCoord(true);

                string command = "Move";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + inCoord.x.Str + dDel + inCoord.y.Str + dDel + inCoord.z.Str + dDel + inCoord.u.Str + dDel + inCoord.v.Str + dDel + inCoord.w.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to XYZ position using curve trajectory through middle position
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendArc(RobCoord.XYZ destCoord, RobCoord.XYZ middleCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                {
                    destCoord.CheckCoord(true);
                    middleCoord.CheckCoord(true);
                }

                string command = "Arc";
                dataMessage = "!" + command + dEnd;
                dataMessage += destCoord.x.Str + dDel + destCoord.y.Str + dDel + destCoord.z.Str + dDel + destCoord.u.Str + dDel + destCoord.v.Str + dDel + destCoord.w.Str + dDel;
                dataMessage += middleCoord.x.Str + dDel + middleCoord.y.Str + dDel + middleCoord.z.Str + dDel + middleCoord.u.Str + dDel + middleCoord.v.Str + dDel + middleCoord.w.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        public enum PullType {GetAndLeave, LeaveAndGet}

        /// <summary>
        /// Command to go to position A do spec. action, move to position B and do spec. action
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendPullM(RobCoord.XYZ posACoord, RobCoord.XYZ posBCoord, double height, PullType pullType)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                {
                    //posACoord.CheckCoord(true);
                }

                string command = "PullM";
                dataMessage = "!" + command + dEnd;
                dataMessage += ((int)pullType).ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel;
                dataMessage += posACoord.x.Str + dDel + posACoord.y.Str + dDel + posACoord.z.Str + dDel + posACoord.u.Str + dDel + posACoord.v.Str + dDel + posACoord.w.Str + dDel;
                dataMessage += height.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel + posBCoord.x.Str + dDel + posBCoord.y.Str + dDel + posBCoord.z.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to position defined by joint angles using PTP
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendGoJnt(RobCoord.JntAngl inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                    inCoord.CheckCoord(true);

                string command = "GoJnt";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + inCoord.J1.Str + dDel + inCoord.J2.Str + dDel + inCoord.J3.Str + dDel + inCoord.J4.Str + dDel + inCoord.J5.Str + dDel + inCoord.J6.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to position defined by joint angles using straight movement
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendMoveJnt(RobCoord.JntAngl inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                    inCoord.CheckCoord(true);

                string command = "MoveJnt";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + inCoord.J1.Str + dDel + inCoord.J2.Str + dDel + inCoord.J3.Str + dDel + inCoord.J4.Str + dDel + inCoord.J5.Str + dDel + inCoord.J6.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move all robot joints by relative joint angles
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendRotAll(RobCoord.JntAngl inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "RotAll";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + inCoord.J1.Str + dDel + inCoord.J2.Str + dDel + inCoord.J3.Str + dDel + inCoord.J4.Str + dDel + inCoord.J5.Str + dDel + inCoord.J6.Str + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move speciefied robot joint by relative joint angle
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendRotJnt(int inJNum, double inJRot)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "RotJnt";
                dataMessage = "!" + command + dEnd;
                dataMessage += inJNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel + inJRot.ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Set specified memory tool set with tool offsets
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendToolSet(int setNum, RobCoord.XYZ inCoord)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                //if (CheckSendCoord) //provadi kontrolu souradnic pred odeslanim do robotu
                //    inCoord.CheckCoord(true);
                if (setNum > 15)     //Kontrola, ze pozadovana sada je vetsi jak 15 - robot vice neumi
                {
                    setNum = 15;
                    ErrorShow("Unsupported number of tool set:");
                }

                //RobCoord.XYZ inCoord = inCoordIn.ConvertTool();

                if (setNum != 0)    //U sady 0 ignoruje hodnoty offsetu
                {
                    //Nastaveni aktualni sady 
                    string command = "ToolS";
                    dataMessage = "!" + command + dEnd;
                    dataMessage = dataMessage + setNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dDel + inCoord.x.Str + dDel + inCoord.y.Str + dDel + inCoord.z.Str + dDel + inCoord.u.Str + dDel + inCoord.v.Str + dDel + inCoord.w.Str + dEnd;


                    Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                    StartSendingProc(command);
                    streamInst.Write(data, 0, data.Length);

                    InfoShow("Sent" + dataMessage);

                    //vola metodu pro kontrolu odesilani
                    ReadResponse(command);

                    return true;
                }
                else
                {
                    ErrorShow("Tool set num. 0 doesn't support any offsets");
                }
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Set specified memory tool set as active
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendTool(int setNum)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                if (setNum > 15)     //Kontrola, ze pozadovana sada je vetsi jak 15 - robot vice neumi
                {
                    setNum = 15;
                    ErrorShow("Unsupported number of tool set:");
                }

                //Zmena aktualni sady
                string command = "Tool";
                dataMessage = "!" + command + dEnd;
                dataMessage = dataMessage + setNum.ToString(System.Globalization.CultureInfo.InvariantCulture) + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Set Home position with joint angles
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendHomeSet(double[] inJ)
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "HomeS";
                dataMessage = "!" + command + dEnd;

                for (int jnt = 0; jnt < 6; jnt++)
                {
                    dataMessage += inJ[jnt].ToString(System.Globalization.CultureInfo.InvariantCulture);
                    if (jnt != 5)
                        dataMessage += dDel;
                }
                dataMessage += dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Move robot to Home position respecting Home joint order
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendHome()
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "Home";
                dataMessage = "!" + command + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Read current position of robot in XYZ world coordinates
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendCurPosP()
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "CurPosP";
                dataMessage = "?" + command + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }

        /// <summary>
        /// Read current position of robot in joint angles
        /// </summary>
        /// <param name="inputNum"></param>
        /// <returns></returns>
        public bool SendCurPosJ()
        {
            //Jestlize neni otevren 
            if (!Connected)
                return false;

            try
            {
                String dataMessage;

                string command = "CurPosJ";
                dataMessage = "?" + command + dEnd;

                Byte[] data = System.Text.Encoding.ASCII.GetBytes(dataMessage);

                StartSendingProc(command);
                streamInst.Write(data, 0, data.Length);

                InfoShow("Sent" + dataMessage);

                //vola metodu pro kontrolu odesilani
                ReadResponse(command);

                return true;
            }
            catch (SocketException e)
            {
                SendingError(e);
            }
            return false;
        }
    }
}
