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

LiquidCrystal_I2C lcd(0x27, 16, 2);

extern "C" void matrixMultiply(int* A, int* B, int* result);

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  int A[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  int B[9] = {9, 8, 7, 6, 5, 4, 3, 2, 1};
  int result[9] = {0};

  // Display Matrix A
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Matrix A:");
  for (int i = 0; i < 3; i++) {
    lcd.setCursor(0, 1);
    lcd.print(A[i * 3]);
    lcd.print(" ");
    lcd.print(A[i * 3 + 1]);
    lcd.print(" ");
    lcd.print(A[i * 3 + 2]);
    delay(1000);
    lcd.clear();
  }

  // Display Matrix B
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Matrix B:");
  for (int i = 0; i < 3; i++) {
    lcd.setCursor(0, 1);
    lcd.print(B[i * 3]);
    lcd.print(" ");
    lcd.print(B[i * 3 + 1]);
    lcd.print(" ");
    lcd.print(B[i * 3 + 2]);
    delay(1000);
    lcd.clear();
  }

  lcd.print("Multiplying...");
  delay(1000);

  // Call assembly function
  matrixMultiply(A, B, result);

  // Display Result
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Result:");
  for (int i = 0; i < 3; i++) {
    lcd.setCursor(0, 1);
    lcd.print(result[i * 3]);
    lcd.print(" ");
    lcd.print(result[i * 3 + 1]);
    lcd.print(" ");
    lcd.print(result[i * 3 + 2]);
    delay(1000);
    lcd.clear();
  }

  Serial.println("Matrix Multiplication Result:");
  for (int i = 0; i < 3; i++) {
    Serial.print(result[i * 3]);
    Serial.print(" ");
    Serial.print(result[i * 3 + 1]);
    Serial.print(" ");
    Serial.println(result[i * 3 + 2]);
  }
}

void loop() {
  // Nothing here
}