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

using System.Threading;

using SWModules;
using Microsoft.SPOT;

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;
        private MotherBoard mb;

        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, QRecordType.Elmo, processMessage, portName, baudRate, parity, dataBits, stopBits, readTimeout, writeTimeout, useSTX, STX, ETX);
            //IOPort.ReleaseDefaultDataHandler();
           mb = MotherBoard.Instance;
        }
        
        private static byte[] ElmoStop = UTF8Encoding.UTF8.GetBytes("ST;"),
                                ElmoMotorOn = UTF8Encoding.UTF8.GetBytes("MO=1;"),
                                ElmoMotorOff = UTF8Encoding.UTF8.GetBytes("MO=0;"),
                                setEchoOff = UTF8Encoding.UTF8.GetBytes("EO=0;"),
                                setEchoOn = UTF8Encoding.UTF8.GetBytes("EO=1;"),
                                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();

            mb.EnableChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            Thread.Sleep(1000);

            stopMotor();
            //sendEchoOn();
            
            sendEchoOff();
            Thread.Sleep(1000);

            getSerialNumber();
            Thread.Sleep(1000);

            EngrLogger.Comment("Setting Elmo current and acc limits");
            setCurrentAndAcLimits();

            Thread.Sleep(1000);

            getStats();
      
            return (0);
        }

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

        public bool sendJV(int jogVelocityCPS)
        {
            EngrLogger.Comment("BE::sendJV: " + 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 void getStats()
        {
            getVelocity();
            Thread.Sleep(200);
            getActiveCurrent();
            Thread.Sleep(200);
            getReactiveCurrent();
            Thread.Sleep(200);
            getSR();
            Thread.Sleep(200);
            getCL1();
            Thread.Sleep(200);
            getPL1();
            Thread.Sleep(200);
            getPL2();
            Thread.Sleep(200);
            getAccDeacc();
        }

         private static byte[] getCL1Cmd = UTF8Encoding.UTF8.GetBytes("CL[1];");
        private int getCL1()
        {

            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo SR command");
            IOPort.Write(getCL1Cmd, 0, getCL1Cmd.Length);
            return (0);
        }

        private static byte[] getPL1Cmd = UTF8Encoding.UTF8.GetBytes("PL[1];");
        private int getPL1()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo SR command");
            IOPort.Write(getPL1Cmd, 0, getPL1Cmd.Length);
            return (0);
        }

        private static byte[] getPL2Cmd = UTF8Encoding.UTF8.GetBytes("PL[2];");
        private int getPL2()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo SR command");
            IOPort.Write(getPL2Cmd, 0, getPL2Cmd.Length);
            return (0);
        }

        private int getSR()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo SR command");
            IOPort.Write(ElmoStatusRegister, 0, ElmoStatusRegister.Length);
            return (0);
        }

        private static byte[] activeCurrent = UTF8Encoding.UTF8.GetBytes("ID;");
        private int getActiveCurrent()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Active Current command");
            IOPort.Write(activeCurrent, 0, activeCurrent.Length);
            return (0);
        }

        private static byte[] reactiveCurrent = UTF8Encoding.UTF8.GetBytes("IQ;");
        private int getReactiveCurrent()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Reactive Current command");
            IOPort.Write(reactiveCurrent, 0, reactiveCurrent.Length);
            return (0);
        }

        private static byte[] velocity = UTF8Encoding.UTF8.GetBytes("VX;");
        private int getVelocity()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Main Encoder Velocity command");
            IOPort.Write(velocity, 0, velocity.Length);
            return (0);
        }

        private static byte[] getUM = UTF8Encoding.UTF8.GetBytes("UM;");
        public int getUnitMode()
        {
            IOPort.DiscardOutBuffer();
            IOPort.DiscardInBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Main Encoder Velocity command");
            IOPort.Write(getUM, 0, getUM.Length);
            return (0);
        }

        private static byte[] setUMSpeed = UTF8Encoding.UTF8.GetBytes("UM = 2;");
        public int setUnitModeSpeed()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Main Encoder Velocity command");
            IOPort.Write(setUMSpeed, 0, setUMSpeed.Length);
            return (0);
        }

        private static byte[] getAcc = UTF8Encoding.UTF8.GetBytes("AC;");
        private static byte[] getDeacc = UTF8Encoding.UTF8.GetBytes("DC;");
        public int getAccDeacc()
        {
            IOPort.DiscardOutBuffer();
            EngrLogger.writeToColumns("getting Elmo acceleration limit");
            IOPort.Write(getAcc, 0, getAcc.Length);

            EngrLogger.writeToColumns("getting Elmo deacceleration limit");
            IOPort.Write(getDeacc, 0, getDeacc.Length);

            return (0);
        }

        private static byte[] cl1 = UTF8Encoding.UTF8.GetBytes("CL[1];");
        private static byte[] pl1 = UTF8Encoding.UTF8.GetBytes("PL[1];");
        private static byte[] pl2 = UTF8Encoding.UTF8.GetBytes("PL[2];");
        public int readCurrentLimits()
        {
            IOPort.DiscardOutBuffer();

            EngrLogger.writeToColumns("Sending Elmo CL[1] command");
            IOPort.Write(cl1, 0, cl1.Length);
            Thread.Sleep(100);

            EngrLogger.writeToColumns("Sending Elmo PL[1] command");
            IOPort.Write(pl1, 0, pl1.Length);
            Thread.Sleep(100);

            EngrLogger.writeToColumns("Sending Elmo PL[2] command");
            IOPort.Write(pl2, 0, pl2.Length);

            return (0);
        }

        private static byte[] cl1_47 = UTF8Encoding.UTF8.GetBytes("CL[1]=4.7;");
        public int setCurrentLimit47()
        {
            IOPort.DiscardOutBuffer();

            EngrLogger.writeToColumns("Sending Elmo CL[1]=4.7 command");
            IOPort.Write(cl1_47, 0, cl1_47.Length);

            return (0);
        }

        private const string curLimit65 = "CL[1]=6.5;";
        private static byte[] cl1_65 = UTF8Encoding.UTF8.GetBytes(curLimit65);
        public int setCurrentLimit65()
        {
            IOPort.DiscardOutBuffer();

            EngrLogger.writeToColumns("Sending Elmo CL[1]=6.5 command");
            IOPort.Write(cl1_65, 0, cl1_65.Length);

            return (0);
        }


        private static byte[] cl1_476 = UTF8Encoding.UTF8.GetBytes("CL[1]=4.76;");
        public int setCurrentLimit476()
        {
            IOPort.DiscardOutBuffer();

            EngrLogger.writeToColumns("Sending Elmo CL[1]=4.76 command");
            IOPort.Write(cl1_476, 0, cl1_476.Length);

            return (0);
        }

        private static byte[] pl1_135 = UTF8Encoding.UTF8.GetBytes("PL[1]=13.5;");
        private static byte[] pl2_2 = UTF8Encoding.UTF8.GetBytes("PL[2]=2;");
        public int setCurrentLimits476_135()
        {
            IOPort.DiscardOutBuffer();

            EngrLogger.writeToColumns("Sending Elmo CL[1]=4.76 command");
            IOPort.Write(cl1_476, 0, cl1_476.Length);

            Thread.Sleep(1000);

            EngrLogger.writeToColumns("Sending Elmo PL1[1]=13.5 command");
            IOPort.Write(pl1_135, 0, pl1_135.Length);

            Thread.Sleep(1000);

            EngrLogger.writeToColumns("Sending Elmo PL2[1]=2 command");
            IOPort.Write(pl2_2, 0, pl2_2.Length);

            return (0);
        }

        public void setCurrentAndAcLimits()
        {
            if (mb.QueryChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel))
            {
                setAccDeacc1e9();
                setCurrentLimits476_135();
                getStats();
            }
            else
            {
                mb.EnableChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
                Thread.Sleep(1000);
                setAccDeacc1e9();
                setCurrentLimits476_135();
                getStats();
                mb.DisableChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            }
        }

        private static byte[] snCmd = UTF8Encoding.UTF8.GetBytes("SN;");
        public int getSerialNumber()
        {
            IOPort.DiscardOutBuffer();
            //EngrLogger.writeToColumns("Sending Elmo Main Encoder Velocity command");
            IOPort.Write(snCmd, 0, snCmd.Length);
            return (0);
        }

        //TODO P0 need to make this value part of MissionConfig
        private StringBuilder sbTemp2 = new StringBuilder(64);
        private const string accelLimit = "1e9";
        public int setAccDeacc1e9()
        {
            IOPort.DiscardOutBuffer();

            sbTemp.Clear();
            sbTemp.Append("AC[1]=");
            sbTemp.Append(accelLimit);
            sbTemp.Append(";");
            IOPort.Write(Encoding.UTF8.GetBytes(sbTemp.ToString()), 0, sbTemp.Length);

            sbTemp2.Clear();
            sbTemp2.Append("Setting Elmo Acceleration limit = ");
            sbTemp2.Append(sbTemp);
            EngrLogger.writeToColumns(sbTemp2); 
            
            sbTemp.Clear();
            sbTemp.Append("DC=");
            sbTemp.Append(accelLimit);
            sbTemp.Append(";");
            IOPort.Write(Encoding.UTF8.GetBytes(sbTemp.ToString()), 0, sbTemp.Length);
            
            sbTemp2.Clear();
            sbTemp2.Append("Setting Elmo Deacceleration limit = ");
            sbTemp2.Append(sbTemp);
            EngrLogger.writeToColumns(sbTemp2); 

            return (0);
        }


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

            EngrLogger.writeToColumns("Sending Elmo Echo Off command");
            IOPort.Write(setEchoOff, 0, setEchoOff.Length);

            return (0);
        }

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

            EngrLogger.writeToColumns("Sending Elmo Echo On command");
            IOPort.Write(setEchoOn, 0, setEchoOn.Length);

            return (0);
        }

        private static StringBuilder sbTemp = new StringBuilder(128);
        private void processMessage(QRecord qRecord)
        {
            sbTemp.Clear();
            sbTemp.Append("Processing Twitter message: ");
            for (int i = 0; i < qRecord.length; i++ )
            {
                if (qRecord.byteArray[i] > 127)
                    qRecord.byteArray[i] = 126;
            }
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
            EngrLogger.writeToColumns(0, EngrLiterals.ColumnNums.dateTime, qRecord.timeStamp,
                                          EngrLiterals.ColumnNums.state, qRecord.state,
                                          EngrLiterals.ColumnNums.comment, "Twitter msg",
                                          EngrLiterals.ColumnNums.pumpMotor, qRecord.byteArray);
        }
    }
}