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

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

namespace CPF
{
    public class Pump
    {
        private readonly byte[] _elmoStop               = Encoding.UTF8.GetBytes("ST;");
        private readonly byte[] _elmoMotorOn            = Encoding.UTF8.GetBytes("M0=1;");
        private readonly byte[] _elmoMotorOff           = Encoding.UTF8.GetBytes("M0=0;");
        private readonly byte[] _elmoJogVelocity        = Encoding.UTF8.GetBytes("JV=");
        private readonly byte[] _elmoJogVelocityZero    = Encoding.UTF8.GetBytes("JV=0;");
        private readonly byte[] _elmoBegin              = Encoding.UTF8.GetBytes("BG;");
        private readonly byte[] _elmoSemicolonBegin     = Encoding.UTF8.GetBytes(";BG;");
        private readonly byte[] _elmoVelocityMode       = Encoding.UTF8.GetBytes("UM=2;");
        private readonly byte[] _allStop                = Encoding.UTF8.GetBytes("ST;M0=0;UM=2;JV=0;M0=1;BG;M0=0;");
        private SerialPort      _uart;
        private byte[]          _cmdBuffer;

        public Pump()
        {
            _uart = new SerialPort("COM2", 19200, Parity.None, 8, StopBits.One);
            _uart.Open();
            _cmdBuffer = new byte[32];
            Stop();
        }

        public void Speed(double velocity)
        {   
            // Zero fill the command buffer for good hygiene
            Tools.Fill(ref _cmdBuffer, 0, _cmdBuffer.Length, NumberConversions.Zero);

            // Fill in the command with "MO=1;JV=<num>;BG;"
            Tools.Copy(_elmoMotorOn, 0, ref _cmdBuffer, 0, _elmoMotorOn.Length);
            Tools.Copy(_elmoJogVelocity, 0, ref _cmdBuffer, _elmoMotorOn.Length, _elmoJogVelocity.Length);
            int length = NumberConversions.IntegerToByteArray(ref _cmdBuffer, (_elmoMotorOn.Length + _elmoJogVelocity.Length), (int) velocity);
            Tools.Copy(_elmoSemicolonBegin, 0, ref _cmdBuffer, 
                      (_elmoMotorOn.Length + _elmoJogVelocity.Length + length), _elmoSemicolonBegin.Length);
            _uart.Write(_cmdBuffer, 0, (_elmoMotorOn.Length + _elmoJogVelocity.Length + length + _elmoSemicolonBegin.Length));
        }

        public void Stop()
        {
             _uart.Write(_allStop, 0, _allStop.Length);
        }
    }
}
