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

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

using GHI.Pins;

namespace abstractState
{
    public class Elmo
    {
        //On 2015 March 14 the following line was being used.  This line is incorrect and the next line assigning PC0 is the correct pin mapping
        //I have no idea how this happened
        //private static OutputPort valve = new OutputPort(G400.PB14, true);
        private static OutputPort valve = new OutputPort(G400.PC0, 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)
        {
            if (!valveOpen)
            {
                openValve();
                Thread.Sleep(500);
            }
            sendJV(System.Math.Abs(jogVelocityCPS));
            extending = true;
            retracting = false;
            return (true);
        }

        public static bool retracting = false;

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

        public static bool valveOpen;

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

        public 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
        }

        private static double[] CN0254Volts = new double[8];
        private static StringBuilder sbTemp = new StringBuilder(128);
        private static DateTime timeNow = new DateTime();

        public static void exerciseBellows()
        {
            double bellowsPosition = double.NaN;

            sbTemp.Clear();
            sbTemp.Append("Extending Bellows");
            Elmo.extendBellows(configFile.ABRetractJV);
            for (int i = 0; i < 25; i++)
            {
                CN0254Volts = CN0254.scanAll();
                timeNow = DateTime.Now;
                bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                EngrLogger.ColumnNums.state, Program.CurrentState,
                                                EngrLogger.ColumnNums.comment, sbTemp,
                                                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();

            sbTemp.Clear();
            sbTemp.Append("Retracting Bellows");
            Elmo.retractBellows(configFile.ABRetractJV);
            for (int i = 0; i < 25; i++)
            {
                CN0254Volts = CN0254.scanAll();
                timeNow = DateTime.Now;
                bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                EngrLogger.ColumnNums.state, Program.CurrentState,
                                                EngrLogger.ColumnNums.comment, sbTemp,
                                                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();
        }

        //return 1 when in within allowable error, 0 when still moving, -1 if error
        public static int moveBellowsToPosition(double targetPosition, int speedCPS, double allowableError)
        {
            //ensure input values are acceptable
            //target position
            if ((targetPosition > configFile.bellowsUpperLimit) || (targetPosition < configFile.bellowsLowerLimit))
                return(-1);

            //speed
            if ((speedCPS > 100000) || (speedCPS < 0))
                return (-1);

            //pudate (local) bellowsPosition value so as not to rely on potentially old value put through Queue
            double bellowsPosition = double.NaN;
            CN0254Volts = CN0254.scanAll();
            bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;

            //bellows at the correct position?
            if (System.Math.Abs(bellowsPosition - targetPosition) < allowableError)
            {
                Elmo.stopMotor();           //stopMotor closes valve and sets Elmo bool flags appropriately, 

                return (1);
            }
            
            //bellows not at targetPosition, move within range at defined speed.
            else
            {
                //extend case
                if (bellowsPosition < targetPosition)
                {
                    extendBellows(speedCPS);                    
                }

                //retract case
                if (bellowsPosition > targetPosition)
                {
                    retractBellows(speedCPS);
                }

                //return 0 to indicate movement not complete
                return (0);

            }
        }
    }
}