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

using System.Threading;

using SWModules;

namespace HWModules
{
    //TODO P2 probably need to make this a singleton if we want to process responses from the Twitter

    public class ElmoTwitter
    {
        private NativeUART IOPort;

        public ElmoTwitter(string id, string portName, int baudRate, Parity parity, int dataBits, StopBits stopBits, int readTimeout, int writeTimeout, bool useSTX, byte STX, byte ETX)
        {
            IOPort = new NativeUART(id, processMessage, portName, baudRate, parity, dataBits, stopBits, readTimeout, writeTimeout, useSTX, STX, ETX);
            
        }

        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;"),
                                ElmoStatusRegister = UTF8Encoding.UTF8.GetBytes("SR;"),
                                semicolon = UTF8Encoding.UTF8.GetBytes(";");

        public int init()
        {
            if (!IOPort.IsOpen)
            {
                IOPort.Open();
            }

            IOPort.DiscardInBuffer();
            IOPort.DiscardOutBuffer();

            stopMotor();
            
            return (0);
        }

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

        public bool sendJV(int jogVelocityCPS)
        {
            IOPort.DiscardOutBuffer();

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

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

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

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

        public int stopMotor()
        {
            IOPort.DiscardOutBuffer();

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

            return (0);       
        }

        public int sendSR()
        {
            IOPort.DiscardOutBuffer();

            //EngrLogger.writeToColumns("Sending Elmo SR command");
            IOPort.Write(ElmoStatusRegister, 0, ElmoStatusRegister.Length);

            return (0);
        }

        // TODO Stub 
        private void processMessage(QRecord qRecord)
        {
            //EngrLogger.writeToColumns("Processing Elmo Twitter Message stub");
        }
    }
}