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

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

using GHI.Hardware.EMX;
using UTILS;
using HSM;

namespace DEV
{
    public class Bellows
    {
        private SynchronizedEventQueue  _anEventQ;

        private SerialPort              _uart;
        private byte[]                  _cmdBuffer;
        private OutputPort              _valveOpen;

        private enum                    Location { Unknown, InTransit, AtDesired, UndershotDesired, OvershotDesired, RetractedTooFar, ExtendedTooFar };
        private PositionSensor          _aPositionSensor;

        private double                  _desiredPosition_MM;
        private double                  _deltaPosition_MM;
        private double                  _extendLimit;
        private double                  _retractLimit;
        private double                  _lowerPositionLimit;
        private double                  _upperPositionLimit;
        private Location                _state;

        private Int32                   _speedIn_CPS;
        private int                     _movement;  // -1 = Retract. 1 = Extend, 0 = No Movement
        private static object           _aLock;

        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[]     _allStop = Encoding.UTF8.GetBytes("ST;M0=0;UM=2;JV=0;M0=1;BG;M0=0;");
        private readonly byte[]     _elmoMoveCommand  = Encoding.UTF8.GetBytes("MO=0;UM=2;MO=1;JV=");

        public Bellows(SynchronizedEventQueue eq, double retractLimit = 36.0, double extendLimit = 70.0)
        {
            _anEventQ = eq;
            _valveOpen = new OutputPort(Pin.IO70, true);
            closeValve();
            _uart = new SerialPort("COM3", 19200, Parity.None, 8, StopBits.One);
            _uart.Open();
            Stop();
            _cmdBuffer = new byte[64];
            _aPositionSensor = new PositionSensor();
            _aLock = new object();
            _aPositionSensor.Callback = ReportPosition;
            _extendLimit = extendLimit;
            _retractLimit = retractLimit;
            _speedIn_CPS = 0;
            _movement = 0;
        }

        public void Start()
        {
            _aPositionSensor.Go();  // TO DO:  Remove this later, for testing only
        }

        private double ReportedPosition
        {
            get { lock (_aLock) { return ReportedPosition;  } }
            set { lock (_aLock) { ReportedPosition = value; } }
        }

        public void SetSpeed(Int32 speedIn_RPM)
        {
            lock (_aLock) { _speedIn_CPS = speedIn_RPM * 2000 / 60; }
        }

        public Int32 GetSpeed()
        {
            Int32 value;
            lock (_aLock) { value = _speedIn_CPS; }
            return value;
        }

        public void MoveToPosition(double desired, double lowerPositionLimit, double upperPositionLimit, double velocity)
        {
            _desiredPosition_MM = desired;
            _lowerPositionLimit = lowerPositionLimit;
            _upperPositionLimit = upperPositionLimit;
            _deltaPosition_MM   = _desiredPosition_MM - ReportedPosition;
            _state = Location.InTransit;
            _movement = System.Math.Sign(_deltaPosition_MM);
            switch (_movement)
            {
                case 1:
                    // Elmo.Extend(velocity);
                    _aPositionSensor.Go();
                    break;

                case 0: // Already there...
                    // Elmo.Stop();
                    break;

                case -1:
                    // Elmo.Retract(velocity);
                    _aPositionSensor.Go();
                    break;

                default: // Can't happen... TO DO: deal with this ugliness
                    break;
            }
        }

        private void ReportPosition(double mm)
        {
            double currentPositionIn_MM;

            //ReportedPosition = mm;
            //currentPositionIn_MM = ReportedPosition;
            BellowsAtDesiredPosition(mm);
#if FUBAR
            switch (Where(currentPositionIn_MM))
            {
                case Location.RetractedTooFar :
                    break;

                case Location.ExtendedTooFar:
                    break;

                case Location.AtDesired :
                    // AtDesiredPosition(_desiredPosition_MM);
                    _aPositionSensor.Halt();
                    break;

                case Location.InTransit :
                    break;

                case Location.OvershotDesired :
                    break;

                case Location.UndershotDesired:
                    break;

                case Location.Unknown:
                    break;

                default :
                    break;
            }
#endif
        }

        private Location Where(double positionIn_MM)
        {
            Location _location;
            if (positionIn_MM < _retractLimit)
                _location = Location.RetractedTooFar;
            else if (positionIn_MM > _extendLimit)
                _location = Location.ExtendedTooFar;
            else if ((_movement > 0) && (positionIn_MM > _upperPositionLimit))
                _location = Location.OvershotDesired;
            else if ((_movement < 0) && (positionIn_MM < _lowerPositionLimit))
                _location = Location.UndershotDesired;
            else if ((positionIn_MM > _lowerPositionLimit) && (positionIn_MM < _upperPositionLimit))
                _location = Location.AtDesired;
            else
                _location = Location.InTransit;

            return _location;
        } 

        public void Extend()
        {
            closeValve();
            Move(GetSpeed());
        }
        public void Retract()
        {
            openValve();
            Move(-GetSpeed());
        }

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

        private void Move(Int32 speed)
        {
            Tools.Fill(ref _cmdBuffer, 0, _cmdBuffer.Length, 0x20);
            Tools.Copy(_elmoMoveCommand, 0, ref _cmdBuffer, 0, _elmoMoveCommand.Length);
            int commandLength = _cmdBuffer.Length + 1 +
                NumberConversions.IntegerToByteArray(ref _cmdBuffer, _cmdBuffer.Length + 1, speed);
            Tools.Copy(_elmoSemicolonBegin, 0, ref _cmdBuffer, 0, _elmoSemicolonBegin.Length);
            commandLength += _elmoSemicolonBegin.Length;
            _uart.Write(_cmdBuffer, 0, commandLength);
        }

        private void openValve()
        {
            _valveOpen.Write(false);
        }

        private void closeValve()
        {
            _valveOpen.Write(true);
        }

        private void BellowsAtDesiredPosition(double mm)
        {
            _anEventQ.Add((Signal)ProfilerEvents.ReachedDesiredPosition, mm);
        }

        private void BellowsExtendedTooFar(double mm)
        {
            _anEventQ.Add((Signal)ProfilerEvents.ExtendedTooFar, mm);
        }

        private void BellowsRetracedTooFar(double mm)
        {
            _anEventQ.Add((Signal)ProfilerEvents.RetractedTooFar, mm);
        }
    }
}