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

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHI.Hardware.EMX;

// Slot 9 on FEZ Spider         J1 on SBE41CP
// Pin  1 -- VCC
// Pin  2 -- +5V
// Pin  3 -- IO46            J1.3 Space P only Mark CTD   SBE In      <-- Spider GPIO OUT
// Pin  4 -- IO6/TXD3        J1.4 Commands from Float     SBE UART RX <-- Spider UART TX        
// Pin  5 -- IO7/RXD3        J1.2 CTD Data Out            SBE UART TX --> Spider UART RX                
// Pin  6 -- IO15            J1.1 Data Request            SBE In      <-- Spider GPIO OUT
// Pin  7 -- IO24
// Pin  8 -- IO25
// Pin  9 -- IO27
// Pin 10 -- GND

namespace CPF
{
   class SBE41
    {
        public delegate void    PressureChange(double pressure);
        public PressureChange   Callback { get; set; }

        private OutputPort      _dataRequest;
        private OutputPort      _space_P_OnlyMark_CTD;
        private OutputPort      _serialTX;
        private delegate void   _state(byte[] buffer);
        private SerialPort      _port;
        private Thread          _CTD_Thread;
        private TimerCallback   _timerCallback;
        private Timer           _timer;
        private AutoResetEvent  _sample;


        private byte[]          _lineFeed               = Encoding.UTF8.GetBytes("\r");
        private byte[]          _carriageReturn         = Encoding.UTF8.GetBytes("\n");
        private byte[]          _readPressure           = Encoding.UTF8.GetBytes("fp\r");
        private byte[]          _readPressureTurnMotor  = Encoding.UTF8.GetBytes("fpt\r");
        private byte[]          _getStatus              = Encoding.UTF8.GetBytes("ds\r");
        private byte[]          _powerDown              = Encoding.UTF8.GetBytes("qsr\r");
        private byte[]          _response               = new byte[100];
        private Int32           _responseIndex;

        private void AssertDataRequest()
        {
            _dataRequest.Write(true);
        }

        private void NegateDataRequest()
        {
            _dataRequest.Write(false);
        }

        private void AssertSpace_P_OnlyMark_CTD()
        {
            _space_P_OnlyMark_CTD.Write(false);
        }

        private void NegateSpace_P_OnlyMark_CTD()
        {
            _space_P_OnlyMark_CTD.Write(true);
        }

        private void ForceWakeUp()
        {
            AssertDataRequest();
            AssertSpace_P_OnlyMark_CTD();
            Thread.Sleep(500);
        }

        private void ReleaseWakeUp()
        {
            NegateDataRequest();
            NegateSpace_P_OnlyMark_CTD();
            Thread.Sleep(500);
        }

        private bool GetResponse(bool print)
        {
            Int32 count;
            _responseIndex = 0;
            do 
            {
                count = _port.Read(_response, _responseIndex, 1);
                if (count > 0)
                {
                    // Change line feed into null terminator and end loop
                    if (_response[_responseIndex] == _lineFeed[0])
                    {
                        _response[_responseIndex] = (byte)0;
                        break;
                    }
                    // Remove any embedded new lines for readability
                    if (_response[_responseIndex] != _carriageReturn[0])
                    {
                        _responseIndex = _responseIndex + 1;
                    }
                }
                else
                {
                    return true;
                }
            } while (true);

            if (print == true)
            {
                string s = new string(System.Text.Encoding.UTF8.GetChars(_response));
                Debug.Print(s);
            }

            return false;
        }

        public SBE41()
        {
            _dataRequest = new OutputPort(Pin.IO15, false);
            _space_P_OnlyMark_CTD = new OutputPort(Pin.IO46, true);
            _port = new SerialPort("com4", 9600, Parity.None, 8, StopBits.One);
            _port.ReadTimeout = 5000;
            _port.WriteTimeout = 20; // Timeout.Infinite;
            _port.Open();
            ReleaseWakeUp();
        }

        // Starts listening to the Pressure Sensor
        public void Start(double sampleRateIn_Hz, int delayUntilStartIn_mSecs)
        {

            _CTD_Thread = new Thread(new ThreadStart(GetPressure));
            _sample = new AutoResetEvent(false);
            _CTD_Thread.Start();
            _timerCallback = new TimerCallback(Sample);
            ReleaseWakeUp();
            EnterCommandMode();
            ExitCommandMode();
            int sampleRateIn_mSec = Tools.SampleRateIn_mSec(sampleRateIn_Hz);
            _timer = new Timer(_timerCallback, null, delayUntilStartIn_mSecs, sampleRateIn_mSec);
        }

        // Stops listening to the Pressure Sensor
        public void Stop()
        {
            _timer.Dispose();
            _timer = null;
            _port.Close();
            _CTD_Thread.Suspend();
        }
    
        public void EnterCommandMode()
        {
            ForceWakeUp();
            ReleaseWakeUp();
            GetResponse(true);
            for (int i = 0; i < 10; i++)
            {
                _port.Write(_lineFeed, 0, 1);
                GetResponse(true);
            }
            bool timeout;
            _port.Write(_getStatus, 0, _getStatus.Length);
            do
            {
                timeout = GetResponse(true);
            } while (timeout == false);
        }

        public void ExitCommandMode()
        {
            _port.Write(_powerDown, 0, _powerDown.Length);
            GetResponse(true);
            GetResponse(true);
        }

        public void Sample(object unused)
        {
            _sample.Set();
        }

        public void GetPressure()
        {
            double pressureIn_dBar;          // in Decibars
             while (true)
            {
                 _sample.WaitOne();             
                _port.Close();
                _serialTX = new OutputPort(Pin.IO6, true);
                AssertDataRequest();
                AssertSpace_P_OnlyMark_CTD();
                Thread.Sleep(50);
                NegateDataRequest();
                Thread.Sleep(150);
                _serialTX.Write(false);
                Thread.Sleep(150);
                NegateSpace_P_OnlyMark_CTD();
                _serialTX.Write(true);
                Thread.Sleep(50);
                _serialTX.Dispose();
                _port.Open();
                GetResponse(true);
                Thread.Sleep(100);
                if (Callback != null)
                {
                    string s = new string(System.Text.Encoding.UTF8.GetChars(_response));
                    if (NumberParser.TryParseDouble(s, out pressureIn_dBar))
                    {
                        Callback(pressureIn_dBar);
                    }
                    else
                    {
                        // TODO:  Log "invalid" number
                    }
                }
                // Thread.Sleep(400);
             }
        }
    }
}
