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

namespace CPF
{
    public class PositionSensorReadException : System.Exception
    {
        public PositionSensorReadException() : base() { }
        public PositionSensorReadException(string message) : base(message) { }
        public PositionSensorReadException(string message, System.Exception inner) : base(message, inner) { }
    }

    class PositionSensor
    {
        private SPI.Configuration   _a_SPI_Config;
        private SPI                 _a_SPI_Interface;
        private byte[]              _txData;
        private byte[]              _rxData;
        private const double        _countsIn_MM = 0.0005;
        private const double        _movementLimitIn_MM = 1.0;
        private double              _previousPositionIn_MM;
        private int                 _errors;
    
        public PositionSensor()
        {        
            _a_SPI_Config = new SPI.Configuration(Cpu.Pin.GPIO_Pin1, false, 0, 0, true, false, 200, SPI.SPI_module.SPI1);
            _a_SPI_Interface = new SPI(_a_SPI_Config);
            _txData = new byte[3];
            _rxData = new byte[3];
            _txData[0] = _txData[1] = _txData[2] = 0;  // Zero _txData for good hygiene
            _errors = 0;
            _previousPositionIn_MM = UncheckedPosition();
        }

        public double Position
        { 
            get 
            {
                double positionIn_MM;
                positionIn_MM = UncheckedPosition();
                if (System.Math.Abs(positionIn_MM - _previousPositionIn_MM) > _movementLimitIn_MM)
                {
                    _errors++; // Moving more than one 1mm per sample is not reasonable
                    throw new PositionSensorReadException(); // ("Position Sensor Read Exception");  // TO DO:  the "new" will generate garbage, fix it (later)
                }
                else
                {
                    _previousPositionIn_MM = positionIn_MM;
                    return positionIn_MM;
                }
            }
        }

        private double UncheckedPosition()
        {
            double positionIn_MM;
            _a_SPI_Interface.WriteRead(_txData, _rxData);
            int positionInCounts = (((_rxData[0] & 0x7F) << 16) | ((_rxData[1] & 0xFF) << 8) | (_rxData[2] & 0xFF)) * 2;
            positionIn_MM = _countsIn_MM * positionInCounts; //in mm
            return positionIn_MM;
        }

        public int Errors { get { return _errors; } }

    }
}
