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

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

namespace CPF
{
    public class Bellows
    {
        // Called when the desired position has been reached
        public delegate void        Position(double mm);
        public Position             AtDesiredPosition;

        private enum                Location { Unknown, InTransit, AtDesired, BeyondLimits };

        private Thread              _bellowsThread;
        private TimerCallback       _timerCallback;
        private Timer               _timer;
        private AutoResetEvent      _sample;
        private PositionSensor      _aPositionSensor;
        private Pump                _aPump;
        private double              _sampleRateIn_Hz;

        private double              _desiredPosition_MM;
        private double              _deltaPosition_MM;
        private double              _lowerRetractionLimit;
        private double              _upperRetractionLimit;
        private double              _lowerExtensionLimit;
        private double              _upperExtensionLimit;
        private double              _lowerPositionLimit;
        private double              _upperPositionLimit;

        private int                 _delayUntilStartIn_mSecs;
        private int                 _movement;  // -1 = Retract. 1 = Extend, 0 = No Movement
        private Location            _state;

        public Bellows(double sampleRateIn_Hz, int delayUntilStartIn_mSecs, double retractLimit = 20.0, double extendLimit = 100.0)
        {
            _aPositionSensor = new PositionSensor();
            _aPump = new Pump();
            _bellowsThread = new Thread(new ThreadStart(Run));
            _sampleRateIn_Hz = sampleRateIn_Hz;
            _delayUntilStartIn_mSecs = delayUntilStartIn_mSecs;
            _lowerRetractionLimit = retractLimit - 0.1;
            _upperRetractionLimit = retractLimit + 1.0;
            _lowerExtensionLimit = extendLimit - 1.0;
            _upperExtensionLimit = extendLimit + 0.1;
            _movement = 0;
            _state = Location.Unknown;
        }

        // Starts listening to the Pressure Sensor
        private void Start()
        {
            _sample = new AutoResetEvent(false);
            _timerCallback = new TimerCallback(Sample);
            int sampleRateIn_mSec = Tools.SampleRateIn_mSec(_sampleRateIn_Hz);
            _timer = new Timer(_timerCallback, null, _delayUntilStartIn_mSecs, sampleRateIn_mSec);
            _bellowsThread.Start();
        }

        // Stops listening to the Pressure Sensor
        public void Stop()
        {
            _aPump.Stop();
            _state = Location.Unknown;
        }

        public void MoveToPosition(double desired, double lowerPositionLimit, double upperPositionLimit, double velocity)
        {
            _desiredPosition_MM = desired;
            _lowerPositionLimit = lowerPositionLimit;
            _upperPositionLimit = upperPositionLimit;
            _deltaPosition_MM = _desiredPosition_MM - _aPositionSensor.Position;
            _state = Location.InTransit;
            _movement = System.Math.Sign(_deltaPosition_MM);
            switch (_movement)
            {
                case 1:
                    _aPump.Speed(-velocity);
                    Start();
                    break;

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

                case -1:
                    _aPump.Speed(velocity);
                    Start();
                    break;

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

        private void Sample(object unused)
        {
            if (_state == Location.InTransit)
            {
                _sample.Set();
            }
        }

        public void Run()
        {
            double currentPositionIn_MM;
            while (true)
            {
                _sample.WaitOne();
                currentPositionIn_MM= _aPositionSensor.Position;
                Debug.Print("Current Position = " + currentPositionIn_MM.ToString());
                _state = Where(currentPositionIn_MM);
                switch (_state)
                {
                    case Location.BeyondLimits :
                        break;

                    case Location.AtDesired :
                        Stop();
                        if (AtDesiredPosition != null)
                            AtDesiredPosition(_desiredPosition_MM);
                        break;

                    case Location.InTransit :
                        break;

                    case Location.Unknown:
                        break;

                    default :
                        break;
                } 
            }
        }

        private Location Where(double positionIn_MM)
        {
            Location _location;
            if ((positionIn_MM < _lowerRetractionLimit) && (positionIn_MM > _upperRetractionLimit))
                _location = Location.BeyondLimits;
            else if ((positionIn_MM > _lowerPositionLimit) && (positionIn_MM < _upperPositionLimit))
                _location = Location.AtDesired;
            else
                _location = Location.InTransit;
            return _location;
        }
    }
}

