using System;
using Microsoft.SPOT;

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

using Microsoft.SPOT.Hardware;

using GHI.Hardware.EMX;

namespace miniCPF
{
    public static class Elmo
    {
        private static OutputPort valve = new OutputPort(Pin.IO70, true);
        private static SerialPort ElmoPort = new SerialPort("COM3", 19200, Parity.None, 8, StopBits.One);

        private static byte[] ElmoStop = UTF8Encoding.UTF8.GetBytes("ST;"),
                                ElmoMotorOn = UTF8Encoding.UTF8.GetBytes("MO=1;"),
                                ElmoMotorOff = UTF8Encoding.UTF8.GetBytes("MO=0;"),
                                ElmoJV = UTF8Encoding.UTF8.GetBytes("JV="),
                                ElmoJV0 = UTF8Encoding.UTF8.GetBytes("JV=0;"),
                                ElmoBegin = UTF8Encoding.UTF8.GetBytes("BG;"),
                                ElmoVelocityMode = UTF8Encoding.UTF8.GetBytes("UM=2;"),
                                semicolon = UTF8Encoding.UTF8.GetBytes(";");

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

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

            stopMotor();

            return (0);
        }

        public static bool extending = false;

        public static bool extendBellows(int jogVelocityCPS)
        {
            closeValve();
            sendJV(System.Math.Abs(jogVelocityCPS));
            extending = true;
            retracting = false;
            return (true);
        }

        public static bool retracting = false;

        public static bool retractBellows(int jogVelocityCPS)
        {
            openValve();
            sendJV((-1 * System.Math.Abs(jogVelocityCPS)));
            retracting = true;
            extending = false;
            return (true);
        }

        public static bool valveOpen;

        private static void openValve()
        {
            valve.Write(false);
            valveOpen = true;
        }

        private static void closeValve()
        {
            valve.Write(true);
            valveOpen = false;
        }

        //SendJV Stuff
        private static byte[] TXBytes = new byte[16];
        private static int TXBytesLength = 0;

        private static bool sendJV(int jogVelocityCPS)
        {
            ElmoPort.DiscardOutBuffer();

            ElmoPort.Write(ElmoMotorOn, 0, ElmoMotorOn.Length);
            ElmoPort.Write(ElmoJV, 0, ElmoJV.Length);

            Array.Clear(TXBytes, 0, TXBytes.Length);
            TXBytes = UTF8Encoding.UTF8.GetBytes(jogVelocityCPS.ToString());
            TXBytesLength = Array.IndexOf(TXBytes, 0);
            ElmoPort.Write(TXBytes, 0, TXBytesLength);

            ElmoPort.Write(semicolon, 0, semicolon.Length);
            ElmoPort.Write(ElmoBegin, 0, ElmoBegin.Length);

            motorStopped = false;

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

        public static bool motorStopped = false;

        public static int stopMotor()
        {
            ElmoPort.DiscardOutBuffer();

            ElmoPort.Write(ElmoMotorOn, 0, ElmoMotorOn.Length);
            ElmoPort.Write(ElmoJV0, 0, ElmoJV0.Length);
            ElmoPort.Write(ElmoBegin, 0, ElmoBegin.Length);
            ElmoPort.Write(ElmoStop, 0, ElmoStop.Length);
            ElmoPort.Write(ElmoMotorOff, 0, ElmoMotorOff.Length);

            closeValve();
            motorStopped = true;
            extending = false;
            retracting = false;

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

        //Event handler stuff
        private static byte LF = 10;
        private static byte STX = LF;
        private static byte[] portReadBytes = new byte[128];

        private static int portBytesToRead = 0;

        private static Object ElmoLock = new Object();

        private static StringBuilder sbEventHandler = new StringBuilder(128);

        private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
        {
            //TODO might be able to do this in a way that if we start in the middle of a message, 
            // we can resynch so subsequent reads get the whole message.
            portBytesToRead = ElmoPort.BytesToRead;
            try
            {
                ElmoPort.Read(portReadBytes, 0, portBytesToRead);
            }
            catch
            {
                Debug.Print("Elmo Port Read Exception");          //TODO do something smarter here
            }

            //Really need TODO something smarter here
            //sbEventHandler.Clear();
            //sbEventHandler.Append(UTF8Encoding.UTF8.GetChars(portReadBytes));
            //Debug.Print("Elmo Response: " + sbEventHandler.ToString());
        }

        static void ElmoPort_ErrorReceived(object sender, SerialErrorReceivedEventArgs e)
        {
            Debug.Print("Elmo Port Error Received");//TODO Eventually
        }
    }
}