/*
    @file:      FIRMWARE.cpp
    @author:    Erik Tóth
    @contact:   etoth@tsn.at
    @date:      2025-10-26
    @brief:     Firmware for MCU
*/

#include "FIRMWARE.h"

bool isNotKey(Key k)
{
    if((k.row == NOT_A_KEY.row) && (k.col == NOT_A_KEY.col)) return true;
    else return false;
}

bool isEqualKey(Key k1, Key k2)
{
    if((k1.row == k2.row) && (k1.col == k2.col)) return true;
    else return false;
}

Keyboard::Keyboard(uint8_t nRows, uint8_t nCols, uint8_t *pinsRow, uint8_t *pinsCol)
{
    _nRows = nRows;
    _nCols = nCols;
    _pinsRow = pinsRow;
    _pinsCol = pinsCol;

    _nActiveKeys = 0;
    _nSticky = 2;

    for(uint8_t i = 0; i < _nRows; i++)
    {
        for(uint8_t j = 0; j < _nCols; j++)
        {
            _keyState[i][j] = false;
            _keyStateLatest[i][j] = false;
            _lastChangeTime[i][j] = 0;
        }
    }
}

void Keyboard::begin()
{
    for(int i = 0; i < _nRows; i++) pinMode(_pinsRow[i], INPUT_PULLDOWN);
    for(int i = 0; i < _nCols; i++) pinMode(_pinsCol[i], OUTPUT);
}

void Keyboard::update()
{
    unsigned long now = millis();
    for(uint8_t i = 0; i < _nCols; i++)
    {
        digitalWrite(_pinsCol[i], HIGH);
        for(uint8_t j = 0; j < _nRows; ++j) 
        {
            bool reading = (digitalRead(_pinsRow[j]) == HIGH);

            if(reading != _keyStateLatest[i][j])
            {
                _keyStateLatest[i][j] = reading;
                _lastChangeTime[i][j] = now;
            }

            if((now - _lastChangeTime[i][j]) > MS_DEBOUNCE)
            {
                if(reading != _keyState[i][j])
                {
                    _keyState[i][j] = reading;

                    if(reading) _addActiveKey(i, j);
                    else _removeActiveKey(i, j);
                }
            }
        }
        digitalWrite(_pinsCol[i], LOW);
    }
    if((_nActiveKeys == 1) && _inQueue(NOT_A_KEY)) _nActiveKeys = 0;
}

int Keyboard::getQueueLength()
{
    return _nActiveKeys;
}

Key Keyboard::getQueue(uint8_t index)
{
    if(index < _nActiveKeys) return _activeKeys[index];
    else return NOT_A_KEY;
}

bool Keyboard::_inQueue(uint8_t row, uint8_t col)
{
    for(uint8_t i = 0; i < _nActiveKeys; i++) 
    {
        if((_activeKeys[i].row == row) && (_activeKeys[i].col == col)) return true;
    }
    return false;
}

bool Keyboard::_inQueue(Key k)
{
    for(uint8_t i = 0; i < _nActiveKeys; i++) 
    {
        if(_isEqualKey(_activeKeys[i], k)) return true;
    }
    return false;
}

bool Keyboard::_isNotKey(Key k)
{
    return isNotKey(k);
}

bool Keyboard::_isEqualKey(Key k1, Key k2)
{
    return isEqualKey(k1, k2);
}

void Keyboard::_addActiveKey(uint8_t row, uint8_t col)
{
    if(_inQueue(NOT_A_KEY))
    {
        for(int i = 0; i < _nSticky; i++)
        {
            if(_isNotKey(_activeKeys[i]))
            {
                _activeKeys[i] = {row, col};
                return;
            }
        }
    }
    else if((_nActiveKeys < N_MAX_QUEUE) && !(_inQueue(row, col)))
    {
        _activeKeys[_nActiveKeys++] = {row, col};
    }
    else return;
}

void Keyboard::_removeActiveKey(uint8_t row, uint8_t col)
{
    bool notKeyReplaced = true;

    for(uint8_t i = 0; i < _nActiveKeys; i++) 
    {
        if((_activeKeys[i].row == row) && (_activeKeys[i].col == col)) 
        {
            if(i < _nSticky)
            {
                _activeKeys[i] = NOT_A_KEY;
                notKeyReplaced = false;
            }            

            if((_isNotKey(_activeKeys[i])) && (_nActiveKeys-1 >= _nSticky)) 
            {
                _activeKeys[i] = _activeKeys[_nSticky];
                notKeyReplaced = true;
            }

            for(uint8_t j = i; j < _nActiveKeys-1; j++) 
            {     
                if(j >= _nSticky) _activeKeys[j] = _activeKeys[j + 1];
            }

            if(notKeyReplaced || (i > _nSticky)) _nActiveKeys--;
            else if(_isNotKey(_activeKeys[_nSticky-1])) _nActiveKeys--;

            return;
        }
    }
}