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

#include "FIRMWARE.h"

// ==================== Helper-Functions ====================

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::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 col = 0; col < _nCols; col++)
    {
        digitalWrite(_pinsCol[col], HIGH);
        for(uint8_t row = 0; row < _nRows; ++row) 
        {
            bool reading = (digitalRead(_pinsRow[row]) == HIGH);

            if(reading != _keyStateLatest[row][col])
            {
                _keyStateLatest[row][col] = reading;
                _lastChangeTime[row][col] = now;
            }

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

                    if(reading) _addActiveKey(row, col);
                    else _removeActiveKey(row, col);
                }
            }
        }
        digitalWrite(_pinsCol[col], 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;
        }
    }
}

// ==================== CV ====================

CV::CV(Adafruit_MCP4728 *dac, TwoWire *wire, uint8_t nCV, MCP4728_channel_t *cvChannelMap, uint16_t *keyToVoltage, uint8_t row, uint8_t col)
{
    _dac = dac;
    _wire = wire;
    _nCV = nCV;
    _row = row;
    _col = col;
    _keyToVoltage = keyToVoltage;

    for(uint8_t i = 0; i < N_MAX_DAC_CH; i++)
    {
        _cvChannelMap[i] = i < _nCV ? cvChannelMap[i] : (MCP4728_channel_t)(0); 
    }
}

bool CV::begin(uint8_t pinSDA, uint8_t pinSCL)
{
    if((_wire->begin(pinSDA, pinSCL) && _dac->begin(96U, _wire)))
    {
        clearAll();
        return true;
    }
    else return false;
}

void CV::setVoltage(uint8_t cvIndex, uint16_t mV)
{
    if(cvIndex >= _nCV) return;
    MCP4728_channel_t ch = _cvChannelMap[cvIndex];
    _dac->setChannelValue(ch, map(mV, 0, 2048, 0, 4095), MCP4728_VREF_INTERNAL);
}

void CV::setVoltage(uint8_t cvIndex, Key k)
{
    if(cvIndex >= _nCV) return;
    if(isNotKey(k)) setVoltage(cvIndex, 0);
    else setVoltage(cvIndex, _keyToVoltage[_getKeyToVoltageIndex(k)]);
}

void CV::clearAll()
{
    for(uint8_t i = 0; i < _nCV; i++) setVoltage(i, 0);
}

uint8_t CV::_getKeyToVoltageIndex(uint8_t row, uint8_t col)
{
    return (row*_col + col);
}

uint8_t CV::_getKeyToVoltageIndex(Key k)
{
    return (k.row*_col + k.col); 
}

// ==================== SequencerBlock ====================

/*!
*   @param maxDurationMS maximum loop duration of recording in milliseconds
*   @param maxStepCount maximum number of steps that can be recorded
*/
SequencerBlock::SequencerBlock(uint16_t maxDurationMS, uint16_t maxStepCount)
{
    _maxDurationMS = maxDurationMS;
    _maxStepCount = maxStepCount;
    _stepCount = 0;
    _currentStep = 0;
    _isRecording = false;
    _isPlaying = false;
    _loop = false;
    _lastVoltageCh1 = 0;
    _lastVoltageCh2 = 0;
    _recordStartTime = 0;
    _lastStepTime = 0;
    _playStartTime = 0;
    _stepStartTime = 0;
}

void SequencerBlock::startRecord()
{
    if(_isPlaying) stopPlay();
    
    clear();
    _isRecording = true;
    _recordStartTime = millis();
    _lastStepTime = _recordStartTime;
    _lastVoltageCh1 = 0xFFFF;  // Ungültiger Wert zum Triggern des ersten Steps
    _lastVoltageCh2 = 0xFFFF;
}

void SequencerBlock::stopRecord()
{
    if(!_isRecording) return;
    
    _finishCurrentStep();
    _isRecording = false;
}

void SequencerBlock::addStep(uint16_t voltage_ch1, uint16_t voltage_ch2)
{
    if(!_isRecording) return;
    if(timeLimitReached()) 
    {
        stopRecord();
        return;
    }
    
    unsigned long now = millis();
    
    // Hat sich die Spannung geändert?
    bool voltageChanged = (voltage_ch1 != _lastVoltageCh1) || (voltage_ch2 != _lastVoltageCh2);
    
    if(voltageChanged)
    {
        // Vorherigen Step abschließen (wenn vorhanden)
        if(_stepCount > 0)
        {
            _finishCurrentStep();
        }
        
        // Neuen Step beginnen
        if(_canAddStep())
        {
            _sequence[_stepCount].voltage_ch1 = voltage_ch1;
            _sequence[_stepCount].voltage_ch2 = voltage_ch2;
            _sequence[_stepCount].duration = 0;
            _stepCount++;
            
            _lastStepTime = now;
            _lastVoltageCh1 = voltage_ch1;
            _lastVoltageCh2 = voltage_ch2;
        }
    }
    else
    {
        // Gleiche Spannung - Duration des aktuellen Steps aktualisieren
        if(_stepCount > 0)
        {
            _sequence[_stepCount - 1].duration = now - _lastStepTime;
        }
    }
}

bool SequencerBlock::isRecording()
{
    return _isRecording;
}

void SequencerBlock::startPlay()
{
    if(_stepCount == 0) return;
    if(_isRecording) stopRecord();
    
    _isPlaying = true;
    _currentStep = 0;
    _playStartTime = millis();
    _stepStartTime = _playStartTime;
}

void SequencerBlock::stopPlay()
{
    _isPlaying = false;
    _currentStep = 0;
}

void SequencerBlock::update()
{
    if(!_isPlaying || _stepCount == 0) return;
    
    unsigned long now = millis();
    unsigned long elapsed = now - _stepStartTime;
    
    // Prüfen ob aktueller Schritt abgelaufen ist
    if(elapsed >= _sequence[_currentStep].duration)
    {
        _currentStep++;
        
        // Sequenz-Ende erreicht?
        if(_currentStep >= _stepCount)
        {
            if(_loop)
            {
                _currentStep = 0;
                _stepStartTime = now;
            }
            else
            {
                stopPlay();
                return;
            }
        }
        else
        {
            _stepStartTime = now;
        }
    }
}

bool SequencerBlock::isPlaying()
{
    return _isPlaying;
}

void SequencerBlock::clear()
{
    _stepCount = 0;
    _currentStep = 0;
    _lastVoltageCh1 = 0;
    _lastVoltageCh2 = 0;
    
    for(uint8_t i = 0; i < _MAX_SEQUENCE_STEPS; i++)
    {
        _sequence[i].voltage_ch1 = 0;
        _sequence[i].voltage_ch2 = 0;
        _sequence[i].duration = 0;
    }
}

void SequencerBlock::setLoop(bool loop)
{
    _loop = loop;
}

bool SequencerBlock::timeLimitReached()
{
    if(!_isRecording) return false;
    
    unsigned long now = millis();
    unsigned long elapsed = now - _recordStartTime;
    
    return (elapsed >= _maxDurationMS);
}

bool SequencerBlock::stepLimitReached()
{
    return (_stepCount >= _maxStepCount);
}

uint16_t SequencerBlock::getStepCount()
{
    return _stepCount;
}

uint16_t SequencerBlock::getCurrentVoltageCh1()
{
    if(!_isPlaying || _stepCount == 0) return 0;
    if(_currentStep >= _stepCount) return 0;
    
    return _sequence[_currentStep].voltage_ch1;
}

uint16_t SequencerBlock::getCurrentVoltageCh2()
{
    if(!_isPlaying || _stepCount == 0) return 0;
    if(_currentStep >= _stepCount) return 0;
    
    return _sequence[_currentStep].voltage_ch2;
}

uint16_t SequencerBlock::getTotalDuration()
{
    uint16_t total = 0;
    for(uint8_t i = 0; i < _stepCount; i++)
    {
        total += _sequence[i].duration;
    }
    return total;
}

void SequencerBlock::_finishCurrentStep()
{
    if(_stepCount == 0) return;
    
    unsigned long now = millis();
    uint16_t duration = now - _lastStepTime;
    
    _sequence[_stepCount - 1].duration = duration;
}

bool SequencerBlock::_canAddStep()
{
    if(_stepCount >= _maxStepCount) return false;
    if(_stepCount >= _MAX_SEQUENCE_STEPS) return false;
    if(timeLimitReached()) return false;
    
    return true;
}