// ENGR 2450 Semester Project - Solving matrix problems

#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd_0x27 (0x27, 16, 2); // set the LCD address for a 16 chars and 2 lines display
LiquidCrystal_I2C lcd_0x3F (0x3F, 16, 2); // set the LCD address for a 16 chars and 2 lines display
LiquidCrystal_I2C lcd = lcd_0x27;
byte disp_freq = 2; // in Hz
int disp_delay = 1000 / disp_freq;
long disp_time = 0;

// Setup I/O pins
int button_pin = 4;
int buzzer_pin = 12;
int B1_pin = A1;
int B2_pin = A2;
int B3_pin = A3;
int A[3][3] = {
    {15, -3, -1},
    {-3, 18, -6},
    {-4, -1, 12}
};
int b[3];
float c[3];

// Gauss-Seidel Function
void gaussSeidel(float tolerance = 0.01, int maxIter = 100) {
    float c_old[3];
    float sum;
    int iter = 0;

    do {
        for (int i = 0; i < 3; ++i) {
            c_old[i] = c[i];
        }

        // Update c1
        sum = (b[0] - A[0][1] * c[1] - A[0][2] * c[2]) / A[0][0];
        c[0] = sum;

        // Update c2
        sum = (b[1] - A[1][0] * c[0] - A[1][2] * c[2]) / A[1][1];
        c[1] = sum;

        // Update c3
        sum = (b[2] - A[2][0] * c[0] - A[2][1] * c[1]) / A[2][2];
        c[2] = sum;

        iter++;
    } while ((abs(c[0] - c_old[0]) > tolerance ||
              abs(c[1] - c_old[1]) > tolerance ||
              abs(c[2] - c_old[2]) > tolerance) && iter < maxIter);
}

// Adjust B3 for a rise in C1
void adjustB3ForC1Rise(float targetRise) {
    float initialC1 = c[0];
    float stepSize = 10;  // Adjust this value if needed
    float minB3 = 0;
    float maxB3 = 5000;
    float tolerance = 0.1;
    float currentB3 = map(analogRead(B3_pin), 0, 1023, minB3, maxB3);

    while (abs(c[0] - initialC1 - targetRise) > tolerance) {
        map(analogRead(B3_pin), 0, 1023, minB3, maxB3);
        gaussSeidel();
        if (c[0] < initialC1 + targetRise) {
            minB3 = currentB3;
            currentB3 += stepSize;
        } else {
            maxB3 = currentB3;
            currentB3 -= stepSize;
        }
        stepSize /= 2;
    }
    lcd.setCursor(0, 1);
    lcd.print("B3: ");
    lcd.print(currentB3);
}

// Measure difference in C3 after changing inputs
void measureC3Difference() {
    float initialC3 = c[2];
    b[0] -= 500; // Reduce inputs to reactors 1 and 2
    b[1] -= 250;
    gaussSeidel();
    float diffC3 = c[2] - initialC3;

    lcd.setCursor(0, 1);
    lcd.print("Diff C3: ");
    lcd.print(diffC3);
}

void setup() {
    pinMode(B1_pin, INPUT);
    pinMode(B2_pin, INPUT);
    pinMode(B3_pin, INPUT);
    pinMode(button_pin, INPUT_PULLUP);
    pinMode(buzzer_pin, OUTPUT);

    // Test to see which LCD screen is attached
    Wire.begin();
    byte address = 63; // This is 0x3F
    Wire.beginTransmission(address);
    byte error = Wire.endTransmission();
    if (error == 0) // If it is there, assume it is the only one
        lcd = lcd_0x3F;

    // Initialize LCD
    lcd.init();
    lcd.begin(16, 2);
    lcd.setBacklight(HIGH);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Numerical Method");
    lcd.setCursor(0, 1);
    lcd.print("Semester Project");
    digitalWrite(buzzer_pin, 1);
    delay(100);
    digitalWrite(buzzer_pin, 0);
    delay(2500); // Wait 2.5sec before starting...
    lcd.clear();

    // Output initial message to Serial Monitor
    // Serial.println("Starting the numerical method simulation...");
}

void loop() {
    static bool disp_screen = 0; // This is to pick which info to display...

    if (disp_screen == 0) { // This means it is on the "b" screen
        b[0] = map(analogRead(B1_pin), 0, 1023, 0, 5000);
        b[1] = map(analogRead(B2_pin), 0, 1023, 0, 5000);
        b[2] = map(analogRead(B3_pin), 0, 1023, 0, 5000);
    } else { // Displaying the "c" screen
        gaussSeidel();
    }

    if (millis() > disp_time) {
        disp_time = disp_time + disp_delay;
        if (disp_screen == 0) {
            lcd.setCursor(0, 0);
            lcd.print("B1: ");
            lcd.setCursor(3, 0);
            lcd.print(b[0]);
            lcd.setCursor(8, 0);
            lcd.print("B2:");
            lcd.print(b[1]);
            lcd.setCursor(0, 1);
            lcd.print("B3: ");
            lcd.setCursor(3, 1);
            lcd.print(b[2]);
        } else if (disp_screen == 1) {
            lcd.setCursor(0, 0);
            lcd.print("C1: ");
            lcd.setCursor(3, 0);
            lcd.print(c[0]);
            lcd.setCursor(8, 0);
            lcd.print("C2:");
            lcd.print(c[1]);
            lcd.setCursor(0, 1);
            lcd.print("C3: ");
            lcd.setCursor(3, 1);
            lcd.print(c[2]);
        }
    }

    if (!digitalRead(button_pin)) {
        digitalWrite(buzzer_pin, 1);
        delay(50);
        digitalWrite(buzzer_pin, 0);
        delay(100);
        disp_screen = !disp_screen;
    }

    // c) Increase in B3 for 10 g/m³ rise in c1: -94.5811
    // Reduction in C3 after reducing B1 and B2: 15.2850
}