#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// LCD Configuration (change address if needed)
LiquidCrystal_I2C lcd(0x27, 20, 4);  // 0x27 is common, try 0x3F if not working

// Button Configuration
const int buttonPin = PC13;  // Nucleo user button
unsigned long lastButtonPressTime = 0;
int buttonPressCount = 0;
bool displayOn = true;

// Keypad Configuration
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

// STM32 pin mapping for keypad
byte rowPins[ROWS] = {PB3, PB4, PB5, PB6};   // Rows
byte colPins[COLS] = {PB7, PB8, PB9, PB10};  // Columns

// Circular buffer for display
const int bufferSize = 20;
char displayBuffer[bufferSize + 1] = {0};
int bufferIndex = 0;
bool bufferWrapped = false;

void setup() {
  // Initialize serial for debugging
  Serial.begin(9600);
  
  // Initialize LCD with retries
  bool lcdInitialized = false;
  for(int i = 0; i < 3; i++) {
    lcd.init();
    if(lcd.begin()) {
      lcdInitialized = true;
      break;
    }
    delay(200);
  }
  
  if(lcdInitialized) {
    lcd.backlight();
    lcd.print("System Ready");
    Serial.println("LCD initialized");
  } else {
    Serial.println("LCD failed");
    while(1); // Halt if LCD fails
  }

  // Button setup
  pinMode(buttonPin, INPUT_PULLUP);

  // Keypad setup
  for(int r = 0; r < ROWS; r++) {
    pinMode(rowPins[r], INPUT_PULLUP);
  }
  for(int c = 0; c < COLS; c++) {
    pinMode(colPins[c], INPUT_PULLUP);
  }

  // Remap I2C pins if needed (STM32 specific)
  Wire.setSDA(PA10);
  Wire.setSCL(PA9);
  Wire.begin();
}

void loop() {
  processButton();
  processKeypad();
  delay(50);  // Small delay for debouncing
}

void processButton() {
  static bool lastButtonState = HIGH;
  static unsigned long lastDebounceTime = 0;
  int buttonState = digitalRead(buttonPin);

  // Check for state change
  if (buttonState != lastButtonState) {
    lastDebounceTime = millis();
  }

  // Debounce check
  if ((millis() - lastDebounceTime) > 50) {
    // Button pressed (LOW because of pull-up)
    if (buttonState == LOW && lastButtonState == HIGH) {
      handleButtonPress();
    }
  }
  lastButtonState = buttonState;
}

void handleButtonPress() {
  unsigned long currentTime = millis();
  
  // Check for triple press within 3 seconds
  if ((currentTime - lastButtonPressTime) < 3000) {
    buttonPressCount++;
  } else {
    buttonPressCount = 1;
  }
  
  lastButtonPressTime = currentTime;

  // Toggle display
  displayOn = !displayOn;
  if (displayOn) {
    lcd.display();
    lcd.backlight();
    updateDisplay(); // Refresh display when turning on
    Serial.println("Display ON");
  } else {
    lcd.noDisplay();
    lcd.noBacklight();
    Serial.println("Display OFF");
  }

  // Clear display on triple press
  if (buttonPressCount >= 3) {
    clearDisplay();
    buttonPressCount = 0;
    Serial.println("Display cleared");
  }
}

void processKeypad() {
  static bool keypadReady = true;
  
  // Wait for all keys to be released
  if (!keypadReady) {
    for (int r = 0; r < ROWS; r++) {
      pinMode(rowPins[r], OUTPUT);
      digitalWrite(rowPins[r], LOW);
      
      for (int c = 0; c < COLS; c++) {
        if (digitalRead(colPins[c]) == LOW) {
          return; // Key still pressed
        }
      }
      
      pinMode(rowPins[r], INPUT_PULLUP);
    }
    keypadReady = true;
    return;
  }

  // Scan keypad
  for (int r = 0; r < ROWS; r++) {
    pinMode(rowPins[r], OUTPUT);
    digitalWrite(rowPins[r], LOW);
    
    for (int c = 0; c < COLS; c++) {
      if (digitalRead(colPins[c]) == LOW) {
        delay(20); // Debounce
        if (digitalRead(colPins[c]) == LOW) {
          char key = keys[r][c];
          Serial.print("Key pressed: ");
          Serial.println(key);
          addToBuffer(key);
          keypadReady = false;
        }
      }
    }
    
    pinMode(rowPins[r], INPUT_PULLUP);
  }
}

void addToBuffer(char key) {
  displayBuffer[bufferIndex] = key;
  bufferIndex = (bufferIndex + 1) % bufferSize;
  
  if (bufferIndex == 0) {
    bufferWrapped = true;
  }
  
  updateDisplay();
}

void updateDisplay() {
  if (!displayOn) return;
  
  char tempBuffer[bufferSize + 1] = {0};
  
  if (bufferWrapped) {
    // Handle wrapped buffer
    memcpy(tempBuffer, &displayBuffer[bufferIndex], bufferSize - bufferIndex);
    memcpy(tempBuffer + (bufferSize - bufferIndex), displayBuffer, bufferIndex);
  } else {
    // Simple case
    memcpy(tempBuffer, displayBuffer, bufferIndex);
  }
  
  lcd.clear();
  lcd.print(tempBuffer);
}

void clearDisplay() {
  memset(displayBuffer, 0, sizeof(displayBuffer));
  bufferIndex = 0;
  bufferWrapped = false;
  
  if (displayOn) {
    lcd.clear();
  }
}