#include "mbed.h"
#include "Keypad.h"


Keypad::Keypad(
    uint8_t nRows,
    DigitalIn rows[],
    uint8_t nColumns,
    DigitalOut cols[],
    int debounce_ms):
  _rows(),
  _columns(),
  _debounce(debounce_ms),
  _started(false),
  _nrows(nRows),
  _ncols(nColumns)
{
  printf("Setup Keypad: %dx%d\r\n", nColumns, nRows);
  for(uint8_t c=0; c<nColumns; c++)
	{
    _columns.push_back(cols[c]);
    _columns[c].write(0);
  }

  for(uint8_t r=0; r<nRows; r++)
	{
    _rows.push_back(rows[r]);
    _rows[r].mode(PullDown);
  }
  _keys = new uint8_t[_nrows*_ncols];
}

Keypad::~Keypad()
{
  delete _keys;
}

void Keypad::run()
{
  uint8_t col=0;
  uint8_t row;

  for (row=0; row<(_ncols*_nrows); row++)
    _keys[row] = 0;

  while(_started)
  {
	// activate column col
    _columns[col].write(1);
    ThisThread::sleep_for(5ms);
    for(row=0; row<_nrows; row++)
    {
      if (_rows[row].read())
      {
		if (_keys[row + _nrows*col] < 2)  // ensure only one key is pressed in this column
        {
          _keys[row + _nrows*col]++;
          if ((_keys[row + _nrows*col] == 2) && _kp_callback)
            _kp_callback.call(row, col);
        }
      }
      else
      {
        if (_keys[row + _nrows*col] > 0)
        {
          _keys[row + _nrows*col]--;
          if ((_keys[row + _nrows*col] == 0) && _kr_callback)
            _kr_callback.call(row, col);
        }
      }
      }
    _columns[col].write(0);
    col = (col+1) % _ncols;
    ThisThread::sleep_for(5ms);
  }
}

void Keypad::attach(const keyevent_callback_t& kp_callback,
					const keyevent_callback_t& kr_callback)
{
  _kp_callback = kp_callback;
  _kr_callback = kr_callback;
}

void Keypad::start(void)
{
  if (_started)
    return;
  _started = true;
  _loop.start(callback(this, &Keypad::run));
}

void Keypad::stop(void)
{
  _started = false;
}
