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

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

using GHI.Pins;

namespace CPF
{
    public static class Elmo
    {
        //RAPTOR Board
        private static OutputPort openCommand = new OutputPort(G400.PC0, true);
        private static InputPort readyStatus = new InputPort(G400.PC1, true, Port.ResistorMode.Disabled);
        private static InputPort openedStatus = new InputPort(G400.PC2, true, Port.ResistorMode.Disabled);
        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();
            closeValve();

            return (0);
        }

        public static bool extending = false;

        public static bool extendBellows(int jogVelocityCPS)
        {
            if (!SV.BellowsAtUpperLimit)
            {
                if (!isValveOpen())
                {
                    openValve();
                    Thread.Sleep(1500);
                }
                sendJV(System.Math.Abs(jogVelocityCPS));
                extending = true;
                retracting = false;
                return (true);
            }
            else
            {
                EngrLogger.writeToColumns("ERROR: Elmo.extendBellows: Bellows at upper limit, can't extend any more");
                return (false);
            }
        }

        public static bool retracting = false;

        public static bool retractBellows(int jogVelocityCPS)
        {

            if (!SV.BellowsAtLowerLimit)
            {
                if (!isValveOpen())
                {
                    openValve();
                    Thread.Sleep(1500);
                }
                sendJV((-1 * System.Math.Abs(jogVelocityCPS)));
                retracting = true;
                extending = false;
                return (true);
            }
            else
            {
                EngrLogger.writeToColumns("ERROR: Elmo.retractBellows: Bellows at lower limit, can't retract any more");
                return (false);
            }
        }

        public static bool isValveOpen()
        {
            return (!openedStatus.Read());
        }

        public static bool valveOpen;

        public static void openValve()
        {
            EngrLogger.writeToColumns("Starting valve open");
            openCommand.Write(false);
            valveOpen = true;
        }

        public static void closeValve()
        {
            EngrLogger.writeToColumns("Starting valve close");
            openCommand.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);

            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 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 int exerciseBellows()
        {
            double bellowsPosition = double.NaN;

            sbTemp.Clear();
            sbTemp.Append("Extending Bellows");
            Elmo.extendBellows(configFile.ABRetractJV);
            for (int i = 0; i < 10; i++)
            {
                timeNow = DateTime.Now;
                //CN0254Volts = CN0254.scanAll();
                //bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                bellowsPosition = ADC2485.readBellowsPosition();
                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 < 10; i++)
            {
                timeNow = DateTime.Now;
                //CN0254Volts = CN0254.scanAll();
                //bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                bellowsPosition = ADC2485.readBellowsPosition();
                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);
        }

        //return 1 when in within allowable error, 0 when still moving, -1 if error
        //TODO if the speed is too high, and allowableError too small, the bellows could possibly oscillate about the targetPosition. Address this.
        //impliment a counter that will slow the motor if it passes and needs to reverse to achieve desired position?
        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);

            if (SV.BellowsAtLowerLimit || SV.BellowsAtUpperLimit)
                return (-1);

            //bellows at the correct position?
            if (System.Math.Abs(Program.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
                if (Program.bellowsPosition < targetPosition)
                {
                    extendBellows(speedCPS);
                }

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

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