#include <PS2KeyAdvanced.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include "enigma_types.h"
#include "HCuOLED.h"
#include "SPI.h"
#define DATAPIN 4
#define IRQPIN  3
#define CS_DI 10
#define DC_DI 9
#define RST_DI 8
PS2KeyAdvanced keyboard;
HCuOLED HCuOLED(SH1106, CS_DI, DC_DI, RST_DI); // For SH1106 displays (HCMODU0058 & HCMODU0059)
// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

size_t toKeyIndex(const String& input_hex)
{
    size_t index = -1;
    for (size_t i = 0; i < ALPHABET_SIZE; ++i)
    {
        if (key_values[i].hex == input_hex)
        {
            index = i;
            break;
        }
    }

    return index;
}

size_t shift(size_t index)
{
    for (size_t i = 0; i < WHEELS_AMOUNT; ++i)
    {
        index += key_shifts[i];
    }

    return index % ALPHABET_SIZE;
}

void rotate()
{
    ++key_shifts[0];
    if (key_shifts[0] == 26)
    {
        key_shifts[0] = 1;
        ++key_shifts[1];
        if (key_shifts[1] == 26)
        {
            key_shifts[1] = 1;
            ++key_shifts[2];
            if (key_shifts[2] == 26)
            {
                key_shifts[2] = 1;
            }
        }
    }

    Serial.print(key_shifts[0]);
    Serial.print(" ");
    Serial.print(key_shifts[1]);
    Serial.print(" ");
    Serial.print(key_shifts[2]);
    Serial.println(" ");
}

String lcd_output;

void setup()
{
    HCuOLED.Reset();
    keyboard.begin(DATAPIN, IRQPIN);
    Serial.begin(115200);
    // initialize the LCD
    lcd.begin();

    // Turn on the blacklight and print a message.
    lcd.backlight();
}

void loop()
{
    if (!keyboard.available())
        return;

    const uint16_t key = keyboard.read();
    if (key <= 0)
        return;

    const String value = String(key, HEX);
    const size_t index = toKeyIndex(value);
    if (index == -1)
        return;

    rotate();

    const String encoded_letter = key_values[shift(index)].view;
    size_t y = 0;
    for (size_t i = 0; i < WHEELS_AMOUNT; ++i)
    {
        HCuOLED.Cursor(4,y);
        HCuOLED.SetFont(MedProp_11pt);
        HCuOLED.Print(key_shifts[i]);
        y += 20;
    }

    if (lcd_output.length() == 16)
    {
        lcd.setCursor(0, 0);
        lcd_output = "";
    }

    lcd_output = (lcd_output + encoded_letter);

    lcd.clear();
    lcd.print(lcd_output);
}