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

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

namespace MFConsoleApplication1
{
    static public class Elmo
    {
        //private static char ElmoTerminatingChar = ';';
        private static string ElmoRxString = "";

        private const string ElmoStop = "ST;",
                                ElmoMotorOn = "MO=1;",
                                ElmoMotorOff = "MO=0;",
                                ElmoJogVelocity = "JV=",
                                ElmoBegin = "BG;",
                                ElmoVelocityMode = "UM=2;";

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

        public static int initSerialPort(String PortName)
        {
            ElmoPort.ReadTimeout = 1000;
            ElmoPort.Open();
            ElmoPort.DiscardInBuffer();
            ElmoPort.DiscardOutBuffer();

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

            return (0);
        }

        public static bool sendJV(double jogVelocityCPS)
        {
            byte[] ElmoTxBytes;

            string ElmoJVString = "0.0";

            ElmoJVString = ElmoJogVelocity + jogVelocityCPS.ToString("F0") + ";" + "BG;";
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJVString);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

            //eventually check for proper receive response

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

        public static int initElmo(String PortName)
        {
            byte[] ElmoTxBytes;

            initSerialPort(PortName);

            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);

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

        static void ElmoPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            char rxChar;

            while (ElmoPort.BytesToRead > 0)
            {
                rxChar = (char)ElmoPort.ReadByte();
                Debug.Print(rxChar.ToString());
                ElmoRxString = ElmoRxString + rxChar.ToString();
                //if (rxChar == ElmoTerminatingChar)
                //    ElmoReadEvent.Set();
            }
        }

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