using System;
using Microsoft.SPOT;

using System.Threading;
using System.IO.Ports;
using System.Text;

using Microsoft.SPOT.Hardware;

namespace miniCPF
{
    public static class Elmo
    {
        private static byte[] ElmoStop = UTF8Encoding.UTF8.GetBytes("ST;"),
                                ElmoMotorOn = UTF8Encoding.UTF8.GetBytes("MO=1;"),
                                ElmoMotorOff = UTF8Encoding.UTF8.GetBytes("MO=0;"),
                                ElmoJogVelocity = UTF8Encoding.UTF8.GetBytes("JV="),
                                ElmoBegin = UTF8Encoding.UTF8.GetBytes("BG;"),
                                ElmoVelocityMode = UTF8Encoding.UTF8.GetBytes("UM=2;"),
                                semicolon = UTF8Encoding.UTF8.GetBytes(";");

        //private static StringBuilder sb = new StringBuilder(64);

        private static SerialPort ElmoPort = new SerialPort("COM2", 19200, Parity.None, 8, StopBits.One);

        private static byte[] TXBytes = new byte[64];
        private static int TXBytesLength = 0;

        public static int init()
        {
            ElmoPort.ReadTimeout = 1000;
            ElmoPort.Open();
            ElmoPort.DiscardInBuffer();
            ElmoPort.DiscardOutBuffer();

            ElmoPort.DataReceived += new SerialDataReceivedEventHandler(ElmoPort_DataReceived);
            ElmoPort.ErrorReceived += new SerialErrorReceivedEventHandler(ElmoPort_ErrorReceived);

            stopBEMotor();

            return (0);
        }

        public static bool sendJV(double jogVelocityCPS)
        {
            //sb.Clear();
            //sb.Append(ElmoMotorOn);
            //sb.Append(ElmoJogVelocity);
            //sb.Append(jogVelocityCPS.ToString("F0"));
            //sb.Append(";");
            //sb.Append(ElmoBegin);
            //TXBytes = Encoding.UTF8.GetBytes(sb.ToString());
            //TXBytesLength = Array.IndexOf(TXBytes, 0);

            ElmoPort.Write(ElmoMotorOn, 0, ElmoMotorOn.Length);
            ElmoPort.Write(ElmoMotorOn, 0, ElmoJogVelocity.Length);
            TXBytes = UTF8Encoding.UTF8.GetBytes(jogVelocityCPS.ToString());
            ElmoPort.Write(semicolon, 0, semicolon.Length);
            ElmoPort.Write(ElmoBegin, 0, ElmoBegin.Length);

            ElmoPort.DiscardOutBuffer();
            ElmoPort.Write(TXBytes, 0, TXBytesLength);
            //eventually check for proper receive response

            return (true); //TODO need to do error checking on .Write and Elmo response eventually and return false on error
        }

        public static int stopBEMotor()
        {
            //ElmoPort.DiscardInBuffer();
            //ElmoPort.DiscardOutBuffer();
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoStop);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Elmo Motor off
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Set velocity mode
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoVelocityMode);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Set jog velocity to 0
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJogVelocity + "0;");
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Turn Motor on
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOn);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Begin motion
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoBegin);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            ////Elmo motor off
            //ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff);
            //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

 //           EngrLog.writeLog(Program.DateTimeNowMilliSeconds() + "Type\tEvent\tComment\tElmo motor stopped");

            return (0);         //TODO Eventually deal with errors
        }

        static void ElmoPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            //TODO Eventually
        }

        static void ElmoPort_ErrorReceived(object sender, SerialErrorReceivedEventArgs e)
        {
            //TODO Eventually
        }


    }
}