#define HAT_CENTER 8
#define HAT_N 0
#define HAT_NE 1
#define HAT_E 2
#define HAT_SE 3
#define HAT_S 4
#define HAT_SW 5
#define HAT_W 6
#define HAT_NW 7

#define PIN_RESERVED 3

Encoder encoders[4];

// input pins autofilled based on which mpu
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
int inputPins[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11, A12, A13, A14, A15, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53};
Input inputs[66];
int memoryLocationEncoder = 2300;
int memoryLocationMatrix = 2000;
int memoryLocationDeviceID = 4000;
#elif defined(__AVR_ATmega328P__)
int inputPins[] = {A0, A1, A2, A3, A4, A5, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};
Input inputs[18];
int memoryLocationEncoder = 800;
int memoryLocationMatrix = 512;
int memoryLocationDeviceID = 1000;
#elif defined(__AVR_ATmega32U4__)
int inputPins[] = {18, 19, 20, 21, 4, 6, 8, 9, 10, 2, 3, 5, 7, 14, 15, 16};
Input inputs[16];
int memoryLocationEncoder = 800;
int memoryLocationMatrix = 512;
int memoryLocationDeviceID = 1000;
#include "Joystick.h"

Joystick_ Joystick(0x03,
                   JOYSTICK_TYPE_JOYSTICK, 128, 2,
                   true, true, true, true, true, true,
                   true, true);

#elif defined(ESP32)
int inputPins[] = {4, 5, 13, 14, 15, 16, 17, 18, 19, 23, 25, 26, 27, 32, 33, 34}; //, 35, 36, 39};

Input inputs[16];
int memoryLocationEncoder = 800;
int memoryLocationMatrix = 512;
int memoryLocationDeviceID = 1000;

#include "rr_bleHID.h"

#else
int inputPins[] = {2};
Input inputs[1];
int memoryLocationEncoder = 800;
int memoryLocationMatrix = 512;
int memoryLocationDeviceID = 1000;
#endif

#define FORCE_DEFAULTS false

uint8_t matrixRowCount = 0;
uint8_t matrixColCount = 0;
uint8_t matrixRowPins[16];
uint8_t matrixColPins[16];
uint8_t matrixButtonAssignments[256];
uint8_t matrixState[32];

uint8_t hat1_state = HAT_CENTER;
uint8_t hat2_state = HAT_CENTER;

uint8_t hat3_state = HAT_CENTER;
uint8_t hat4_state = HAT_CENTER;
long lastHatChange = 0;

void InitInputs()
{

    delay(1000);
    // Reset EEPROM
    // for (int i = 0; i < 1024; i++)
    // {
    //     Store8BitValue(i, 0);
    // }

    // 100 written in EEPROM address 0 indicates device has been used more than once
    // if not 100, initialize variables and eeprom with default values.
    int firstRun = Read8BitValue(memoryLocationDeviceID + 23);
    //Serial.println(firstRun);

    if (EncodersInited()){
        SetEncodersToDefault();
    }
    

    if (FORCE_DEFAULTS)
    {
        for (int i = 0; i < InputCount(); i++)
        {
            inputs[i].SetPin(inputPins[i]);
            if (i == 8)
            {
                inputs[i].SetPinMode(OUTPUT);
                inputs[i].SetIsAnalog(3);
                inputs[i].SetMinVal(50);
            }
            else
            {
                inputs[i].SetIsAnalog(false);
            }
            inputs[i].InitInput();
        }
        SaveCurrentInputConfig();
        SetEncodersToDefault();
        SetButtonMatrixToDefault();
        CommitButtonMatrixToEEPROM();
    }
    else if (firstRun != 100)
    {
        OnFirstRun();
    }
    else
    {

        // Load deviceName from EEPROM
        for (int i = 0; i < 16; i++)
        {
            deviceName[i] = char(Read8BitValue(memoryLocationDeviceID + i));
        }

        // Load i2cAddress from EEPROM
        // delay(2000);
        // Serial.println("Loading i2caddress...");
        // delay(2000);
        i2c_address = Read8BitValue(memoryLocationDeviceID + 22);

        //Serial.println(i2c_address);

        // Load Inputs from EEPROM
        for (int i = 0; i < InputCount(); i++)
        {
            inputs[i].SetPin(inputPins[i]);
            inputs[i].ReadFromEEPROM(i);
            inputs[i].InitInput();
        }

        // Load Encoders from EEPROM
        for (int i = 0; i < 4; i++)
        {
            encoders[i].ReadFromEEPROM(memoryLocationEncoder, i);
            inputs[encoders[i].pinAidx].SetPinMode(PIN_RESERVED);
            inputs[encoders[i].pinBidx].SetPinMode(PIN_RESERVED);
        }

        LoadButtonMatrixFromEEPROM();
    }

    UpdateMatrixRowColCount();

    // encoders[0].SetPins(4, 11, inputPins[4], inputPins[11]);
    // encoders[0].leftAssignment = 65;
    // encoders[0].rightAssignment = 66;

    if (IsMaster())
    {
#if defined(__AVR_ATmega32U4__)
        Joystick.begin();
        Joystick.setXAxisRange(-32767, 32767);
        Joystick.setYAxisRange(-32767, 32767);
        Joystick.setZAxisRange(-32767, 32767);

        Joystick.setRxAxisRange(-32767, 32767);
        Joystick.setRyAxisRange(-32767, 32767);
        Joystick.setRzAxisRange(-32767, 32767);

        Joystick.setRudderRange(-32767, 32767);
        Joystick.setThrottleRange(-32767, 32767);

#elif defined(ESP32)
        delay(100);
        initBLEDevice();
        delay(100);
#endif
    }
}

void OnFirstRun()
{
    // Write default deviceName to EEPROM
    char defaultName[] = "NEW_DEVICE      ";
    for (int i = 0; i < 16; i++)
    {
        Store8BitValue(memoryLocationDeviceID + i, defaultName[i]);
    }

    // Write default i2c_address to EEPROM
    Store8BitValue(memoryLocationDeviceID + 22, 0);

    // Write Input defaults to EEPROM
    Store8BitValue(memoryLocationDeviceID + 23, 100); // mark first run byte

    for (int i = 0; i < InputCount(); i++)
    {
        inputs[i].SetPin(inputPins[i]);
        inputs[i].SetToDefaults();
        inputs[i].WriteToEEPROM(i);
    }
    SetButtonMatrixToDefault();
    SetEncodersToDefault();
    CommitButtonMatrixToEEPROM();
}

void ResetToDefaults()
{
    uint8_t currentI2CAddress = i2c_address;
    OnFirstRun();

    // rescue i2c address from reset to defaults
    i2c_address = currentI2CAddress;
    Store8BitValue(memoryLocationDeviceID + 22, i2c_address);
}

// Forces encoder EEPROM initiation for devices upgrading from earlier versions
bool EncodersInited(){
    // 804 == first key assignment, never should be 255 if inited
    if (Read8BitValue(memoryLocationEncoder + 4) == 255){
        return true;
    }
    return false;
}

uint8_t matrixCounter = 0;

void UpdateInputs()
{

    for (int i = 0; i < InputCount(); i++)
    {
        if (i != 4 && i != 11)
            inputs[i].UpdateInput();
        // if (i == 12){
        //     Serial.println(inputs[i].GetAssignedInput());
        // }
    }
    for (int i = 0; i < 4; i++)
    {
        encoders[i].UpdateInput();
    }
    //Serial.println(encoders[0].GetPosition());
    //Serial.print("Encoder0: ");

    RecordMatrixState(0, matrixColCount);

    // for (int i = 0; i < 8; i++){
    //     Serial.print(matrixRowPins[i]);
    //     Serial.print(" ");
    // }
    // Serial.println();

#if defined(__AVR_ATmega32U4__)
    if (hat1_state == HAT_CENTER)
    {
        Joystick.setHatSwitch(0, -1);
    }
    else
    {
        Joystick.setHatSwitch(0, hat1_state * 45);
    }
    if (hat2_state == HAT_CENTER)
    {
        Joystick.setHatSwitch(1, -1);
    }
    else
    {
        Joystick.setHatSwitch(1, hat2_state * 45);
        // Serial.print("hat2_state:");
        // Serial.println(hat2_state);
    }

#elif defined(ESP32)
    SetHat(0, hat1_state);
    SetHat(1, hat2_state);
    SetHat(2, hat3_state);
    SetHat(3, hat4_state);
    updateHIDDevice();
#endif

    if (IsMaster())
    {
        SendLocalJoystickStatus();
    }
}

void SetInput(int idx, int property, int value)
{
    inputs[idx].Set(property, value);
}

void SetEncoder(int idx, int property, int value)
{
    encoders[idx].Set(property, value);
    if (property == 1)
    {
        inputs[value].SetPinMode(PIN_RESERVED);
        encoders[idx].Set(3, inputPins[value]);
    }
    else if (property == 2)
    {
        inputs[value].SetPinMode(PIN_RESERVED);
        encoders[idx].Set(4, inputPins[value]);
    }
}

Input GetInput(int idx)
{
    return inputs[idx];
}

Encoder GetEncoder(int idx)
{
    return encoders[idx];
}

void UpdateMatrixRowColCount()
{
    matrixRowCount = 16;
    matrixColCount = 16;
    for (int i = 0; i < 16; i++)
    {
        if (matrixRowPins[i] == 254)
        {
            matrixRowCount = i;
            break;
        }
    }
    for (int i = 0; i < 16; i++)
    {
        if (matrixColPins[i] == 254)
        {
            matrixColCount = i;
            break;
        }
    }
}

uint8_t GetMatrixRowCount()
{
    return matrixRowCount;
}
uint8_t GetMatrixColCount()
{
    return matrixColCount;
}

void SetMatrixRowPin(int idx, int pin)
{
    matrixRowPins[idx] = pin;
    inputs[pin].SetPinMode(PIN_RESERVED);
    UpdateMatrixRowColCount();
}

void SetMatrixColPin(int idx, int pin)
{
    matrixColPins[idx] = pin;
    inputs[pin].SetPinMode(PIN_RESERVED);
    UpdateMatrixRowColCount();
}

void SetMatrixInputAssignment(int idx, int assignment)
{
    if (idx < 256)
    {
        matrixButtonAssignments[idx] = assignment;
    }
}

uint8_t GetMatrixRowPin(int idx)
{
    return matrixRowPins[idx];
}

uint8_t GetMatrixColPin(int idx)
{
    return matrixColPins[idx];
}

uint8_t GetMatrixInputAssignment(int idx)
{
    return matrixButtonAssignments[idx];
}

int GetInputIdxByPin(int pin)
{
    //Serial.print("Looking for pin: ");
    //Serial.println(pin);
    for (int i = 0; i < InputCount(); i++)
    {
        if (inputs[i].GetPin() == pin)
        {
            return i;
        }
    }
    return -1;
}

void ReportInputConfig(int subDeviceIndex)
{
    if (subDeviceIndex == 0)
    {
        // Master device config
        Serial.write(InputCount()); // input count
        for (int i = 0; i < InputCount(); i++)
        {
            inputs[i].ReportConfig();
        }

        for (int i = 0; i < 4; i++)
        {
            encoders[i].ReportConfig();
        }

        ReportButtonMatrixConfig();
    }
    else
    {
        PrintSubdeviceInputConfig(subDeviceIndex - 1);
    }
}

void SaveCurrentInputConfig()
{
    for (int i = 0; i < 16; i++)
    {
        Store8BitValue(memoryLocationDeviceID + i, deviceName[i]);
    }

    // Write default i2c_address to EEPROM
    Store8BitValue(memoryLocationDeviceID + 22, i2c_address);

    for (int i = 0; i < InputCount(); i++)
    {
        inputs[i].WriteToEEPROM(i);
    }

    for (int i = 0; i < 4; i++)
    {
        encoders[i].WriteToEEPROM(memoryLocationEncoder, i);
    }

    CommitButtonMatrixToEEPROM();
}

void PrintInputs()
{
    for (int i = 0; i < InputCount(); i++)
    {
        Serial.print(inputPins[i]);
        Serial.print("\t");
    }
    Serial.println();
}

uint8_t InputCount()
{
    return sizeof(inputPins) / sizeof(int);
}

void ReportConfigReadable(int idx)
{
    inputs[idx].ReportConfigReadable();
}

void ReportValues()
{
    Serial.write("V");
    for (int i = 0; i < InputCount(); i++)
    {
        Serial.write(highByte(inputs[i].GetRawVal()));
        Serial.write(lowByte(inputs[i].GetRawVal()));
        Serial.write(highByte(inputs[i].GetVal()));
        Serial.write(lowByte(inputs[i].GetVal()));
    }

    int deviceCount = SubDeviceCount();
    Serial.write(deviceCount);

    for (int i = 0; i < deviceCount; i++)
    {
        Serial.write(i); // SubDevice index
        //RequestInputInfo(1, i); // SubDevice config
    }

    Serial.write("\r\n");
}

void JoystickInput(uint8_t assignedInput, int value)
{
#if defined(__AVR_ATmega32U4__)
    // HatSwitch Inputs
    if (assignedInput >= 12 && assignedInput < 44)
    {
        // Hat 1
        if (assignedInput < 20)
        {
            if (value == 0 && assignedInput - 12 == hat1_state)
            {
                hat1_state = HAT_CENTER;
                //Serial.println("CENTER");
            }
            else if (value > 0)
            {
                hat1_state = assignedInput - 12;
                //Serial.println(hat1_state);
            }
        }
        else
        {
            if (value == 0 && assignedInput - 12 - 8 == hat2_state)
            {
                hat2_state = HAT_CENTER;
                //Serial.println("CENTER");
            }
            else if (value > 0)
            {
                hat2_state = assignedInput - 12 - 8;
                //Serial.println(hat2_state);
            }
            //Serial.println(hat2_state);
        }
    }
    // Button inputs
    else if (assignedInput > 12 + 31)
    {
        // first 11 inputs are reserved for axes
        // button1 is stored as 1+12 but reported HID as 1
        Joystick.setButton(assignedInput - 12 - 32, value);
    }
    else
    {
        switch (assignedInput)
        {
        case XAXIS:
            Joystick.setXAxis(value);
            break;
        case YAXIS:
            Joystick.setYAxis(value);
            break;
        case ZAXIS:
            Joystick.setZAxis(value);
            break;
        case XROTATION:
            Joystick.setRxAxis(value);
            break;
        case YROTATION:
            Joystick.setRyAxis(value);
            break;
        case ZROTATION:
            Joystick.setRzAxis(value);
            break;
        case RUDDERAXIS:
            Joystick.setRudder(value);
            break;
        case THROTTLEAXIS:
            Joystick.setThrottle(value);
            break;
        case ACCELERATORAXIS:
            Joystick.setAccelerator(value);
            break;
        case BRAKEAXIS:
            Joystick.setBrake(value);
            break;
        case STEERINGAXIS:
            Joystick.setSteering(value);
            break;
        }
    }

#elif defined(ESP32)

    // HatSwitch Inputs
    if (assignedInput > 11 && assignedInput < 12 + 32)
    {
        if (assignedInput < 12 + 8)
        {
            if (value == 0 && assignedInput - 12 == hat1_state)
            {
                hat1_state = HAT_CENTER;
            }
            else if (value > 0 && assignedInput - 12 != hat1_state)
            {
                hat1_state = assignedInput - 12;
            }
        }
        else if (assignedInput < 12 + (8 * 2))
        {
            if (value == 0 && assignedInput - (12 + 8) == hat2_state)
            {
                hat2_state = HAT_CENTER;
            }
            else if (value > 0 && assignedInput - (12 + 8) != hat2_state)
            {
                hat2_state = assignedInput - (12 + 8);
            }
        }
        else if (assignedInput < 12 + (8 * 3))
        {
            if (value == 0 && assignedInput - (12 + 16) == hat3_state)
            {
                hat3_state = HAT_CENTER;
            }
            else if (value > 0 && assignedInput - (12 + 16) != hat3_state)
            {
                hat3_state = assignedInput - (12 + 16);
            }
        }
        else if (assignedInput < 12 + (8 * 4))
        {
            if (value == 0 && assignedInput - (12 + 24) == hat4_state)
            {
                hat4_state = HAT_CENTER;
            }
            else if (value > 0 && assignedInput - (12 + 24) != hat4_state)
            {
                hat4_state = assignedInput - (12 + 24);
            }
        }
    }
    // Button inputs
    else if (assignedInput > 12 + 31)
    {
        // first 11 inputs are reserved for axes
        // button1 is stored as 1+12 but reported HID as 1
        //Joystick.setButton(assignedInput - 12 - 16, value);
        SetButton(assignedInput - 12 - 32, value);
    }
    else
    {
        SetAxis(assignedInput - 1, value);
    }
#endif
}

void SendLocalJoystickStatus()
{
    uint8_t assignedInput = 0;
    for (int i = 0; i < InputCount(); i++)
    {
        if (inputs[i].IsInput())
        {
            JoystickInput(inputs[i].GetAssignedInput(), inputs[i].GetVal());
        }
    }

    // Send Encoder Inputs
    for (int i = 0; i < 4; i++)
    {
        if (encoders[i].pinA == -1 || encoders[i].pinB == -1)
        {
            continue;
        }
        if (encoders[i].ButtonADown())
        {
            JoystickInput(encoders[i].leftAssignment, 1);
        }
        else
        {
            if (millis() - encoders[i].leftPushedTime < encoders[i].holdTime)
            {
                JoystickInput(encoders[i].leftAssignment, 1);
            } else {
                JoystickInput(encoders[i].leftAssignment, 0);
            }
        }
        if (encoders[i].ButtonBDown())
        {
            JoystickInput(encoders[i].rightAssignment, 1);
        }
        else
        {
            if (millis() - encoders[i].rightPushedTime < encoders[i].holdTime)
            {
                JoystickInput(encoders[i].rightAssignment, 1);
            } else {
                JoystickInput(encoders[i].rightAssignment, 0);
            }
        }
    }

    SendMatrixHIDReport();
}

void SetEncodersToDefault()
{
    for (int i = 0; i < 4; i++)
    {
        encoders[i].ToDefault();
        encoders[i].WriteToEEPROM(memoryLocationEncoder, i);
    }
}

void SetButtonMatrixToDefault()
{
    for (int i = 0; i < 16; i++)
    {
        matrixRowPins[i] = 254; // 254 stands in for null
        matrixColPins[i] = 254;
    }

    for (int i = 0; i < 256; i++)
    {
        matrixButtonAssignments[i] = 0;
    }
}

void ReportButtonMatrixConfig()
{
    for (int x = 0; x < 16; x++)
    {
        Serial.write(matrixRowPins[x]);
    }
    for (int x = 0; x < 16; x++)
    {
        Serial.write(matrixColPins[x]);
    }
    for (int x = 0; x < 256; x++)
    {
        Serial.write(matrixButtonAssignments[x]);
    }
}

void CommitButtonMatrixToEEPROM()
{
    for (int x = 0; x < 16; x++)
    {
        Store8BitValue(memoryLocationMatrix + x, matrixRowPins[x]);
    }
    for (int x = 0; x < 16; x++)
    {
        Store8BitValue(memoryLocationMatrix + 16 + x, matrixColPins[x]);
    }
    for (int x = 0; x < 256; x++)
    {
        Store8BitValue(memoryLocationMatrix + 32 + x, matrixButtonAssignments[x]);
    }
}

void LoadButtonMatrixFromEEPROM()
{
    for (int x = 0; x < 16; x++)
    {
        matrixRowPins[x] = Read8BitValue(memoryLocationMatrix + x);
    }
    for (int x = 0; x < 16; x++)
    {
        matrixColPins[x] = Read8BitValue(memoryLocationMatrix + 16 + x);
    }
    for (int x = 0; x < 256; x++)
    {
        matrixButtonAssignments[x] = Read8BitValue(memoryLocationMatrix + 32 + x);
    }
}

// Report current buttn states to serial
void ReportMatrixState()
{
    // iterate the columns
    for (int colIndex = 0; colIndex < 16; colIndex++)
    {
        pinMode(inputPins[matrixRowPins[colIndex]], OUTPUT);
        digitalWrite(inputPins[matrixRowPins[colIndex]], LOW);

        byte rowByte0 = 0;
        byte rowByte1 = 0;

        for (int rowIndex = 0; rowIndex < 16; rowIndex++)
        {

            pinMode(inputPins[matrixColPins[rowIndex]], INPUT_PULLUP);

            if (colIndex < 8)
            {
                if (!digitalRead(inputPins[matrixColPins[rowIndex]]))
                {
                    bitSet(rowByte0, rowIndex);
                }
            }
            else
            {
                if (!digitalRead(inputPins[matrixColPins[rowIndex]]))
                {
                    bitSet(rowByte1, rowIndex - 8);
                }
            }

            pinMode(inputPins[matrixColPins[rowIndex]], INPUT);
        }
        Serial.write(rowByte0);
        Serial.write(rowByte1);

        // disable the column
        pinMode(inputPins[matrixRowPins[colIndex]], INPUT);
    }
}

// Report matrix state over i2c
void ReportMatrixStateI2cOLD(int fromCol, int colCount)
{
    // iterate the columns
    for (int colIndex = fromCol; colIndex < fromCol + colCount; colIndex++)
    {
        pinMode(inputPins[matrixColPins[colIndex]], OUTPUT);
        digitalWrite(inputPins[matrixColPins[colIndex]], LOW);

        byte rowByte0 = 0;
        byte rowByte1 = 0;

        for (int rowIndex = 0; rowIndex < 16; rowIndex++)
        {
            pinMode(inputPins[matrixRowPins[rowIndex]], INPUT_PULLUP);

            if (colIndex < 8)
            {
                if (!digitalRead(inputPins[matrixRowPins[rowIndex]]))
                {
                    bitSet(rowByte0, rowIndex);
                }
            }
            else
            {
                if (!digitalRead(inputPins[matrixRowPins[rowIndex]]))
                {
                    bitSet(rowByte1, rowIndex - 8);
                }
            }

            pinMode(inputPins[matrixRowPins[rowIndex]], INPUT);
        }
        i2cWrite(rowByte0);
        i2cWrite(rowByte1);

        // disable the column
        pinMode(inputPins[matrixColPins[colIndex]], INPUT);
    }
}

// Report matrix state over i2c
void RecordMatrixState(int fromCol, int colCount)
{
    // iterate the columns
    for (int colIndex = fromCol; colIndex < fromCol + colCount; colIndex++)
    {
        pinMode(inputPins[matrixRowPins[colIndex]], OUTPUT);
        digitalWrite(inputPins[matrixRowPins[colIndex]], LOW);

        for (int rowIndex = 0; rowIndex < 16; rowIndex++)
        {
            pinMode(inputPins[matrixColPins[rowIndex]], INPUT_PULLUP);

            if (colIndex < 8)
            {
                bitWrite(matrixState[colIndex * 2], rowIndex, !digitalRead(inputPins[matrixColPins[rowIndex]]));
            }
            else
            {
                bitWrite(matrixState[colIndex * 2 + 1], rowIndex, !digitalRead(inputPins[matrixColPins[rowIndex]]));
            }

            pinMode(inputPins[matrixColPins[rowIndex]], INPUT);
        }

        // disable the column
        pinMode(inputPins[matrixRowPins[colIndex]], INPUT);
    }
}

void ReportMatrixStateI2c(int fromCol, int colCount)
{
    for (int colIndex = fromCol; colIndex < fromCol + colCount; colIndex++)
    {
        i2cWrite(matrixState[colIndex * 2]);
        i2cWrite(matrixState[colIndex * 2 + 1]);
    }
}

void SendMatrixHIDReport()
{

    // for (int i = 0; i < 32; i++)
    // {
    //     Serial.print(matrixState[i]);
    //     Serial.print(" ");
    // }
    // Serial.println();

    for (int colIndex = 0; colIndex < matrixColCount; colIndex++)
    {
        for (int rowIndex = 0; rowIndex < matrixRowCount; rowIndex++)
        {
            if (rowIndex < 8)
            {
                //Serial.print(bitRead(matrixState[colIndex*2], rowIndex));
                JoystickInput(matrixButtonAssignments[colIndex * 16 + rowIndex], bitRead(matrixState[colIndex * 2], rowIndex));
            }
            else
            {
                //Serial.print(bitRead(matrixState[colIndex*2+1], rowIndex));
                JoystickInput(matrixButtonAssignments[colIndex * 16 + rowIndex], bitRead(matrixState[colIndex * 2 + 1], rowIndex));
            }
            //
        }
    }
    //Serial.println();
}