#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// ---------------- OLED ----------------
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ---------------- PINS ----------------

// Voltage Sensors (Left pots)
#define V1 34
#define V2 35
#define V3 32

// Current Sensors (Right pots)
#define C1 27
#define C2 25
#define C3 26

// Relays
#define R1 18
#define R2 19
#define R3 5

// LEDs
#define LED1 13
#define LED2 12
#define LED3 14

// ---------------- VARIABLES ----------------
int activePhase = 0;
int lastPhase = 0;

// Minimum valid level (adjust if needed)
int minThreshold = 100;

// ---------------- SETUP ----------------
void setup() {
  Serial.begin(115200);

  // I2C for OLED
  Wire.begin(21, 22);

  // Relay Pins
  pinMode(R1, OUTPUT);
  pinMode(R2, OUTPUT);
  pinMode(R3, OUTPUT);

  // LED Pins
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);

  // Turn OFF all initially
  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);

  digitalWrite(LED1, LOW);
  digitalWrite(LED2, LOW);
  digitalWrite(LED3, LOW);

  // OLED Init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED FAIL");
    while (1);
  }

  display.clearDisplay();
  display.display();
}

// ---------------- LOOP ----------------
void loop() {

  // 🔹 Read Sensors
  int v1 = analogRead(V1);
  int v2 = analogRead(V2);
  int v3 = analogRead(V3);

  int c1 = analogRead(C1);
  int c2 = analogRead(C2);
  int c3 = analogRead(C3);

  // 🔹 Calculate sums
  int sum1 = v1 + c1;
  int sum2 = v2 + c2;
  int sum3 = v3 + c3;

  // 🔥 Find Highest Phase
  if (sum1 >= sum2 && sum1 >= sum3) {
    activePhase = 1;
  } 
  else if (sum2 >= sum1 && sum2 >= sum3) {
    activePhase = 2;
  } 
  else {
    activePhase = 3;
  }

  // 🔹 No Phase Condition
  if (sum1 < minThreshold && sum2 < minThreshold && sum3 < minThreshold) {
    activePhase = 0;
  }

  // 🔥 Stability Control (Avoid Flicker)
  if (activePhase != lastPhase) {
    delay(200);
    lastPhase = activePhase;
  }

  // 🔥 Relay Control (ONLY ONE ON)
  digitalWrite(R1, activePhase == 1 ? HIGH : LOW);
  digitalWrite(R2, activePhase == 2 ? HIGH : LOW);
  digitalWrite(R3, activePhase == 3 ? HIGH : LOW);

  // 🔥 LED Control
  digitalWrite(LED1, activePhase == 1 ? HIGH : LOW);
  digitalWrite(LED2, activePhase == 2 ? HIGH : LOW);
  digitalWrite(LED3, activePhase == 3 ? HIGH : LOW);

  // 🔹 Serial Monitor
  Serial.println("-------------");
  Serial.print("P1: "); Serial.print(sum1);
  Serial.print(" | P2: "); Serial.print(sum2);
  Serial.print(" | P3: "); Serial.println(sum3);

  Serial.print("Active Phase: ");
  Serial.println(activePhase);

  // 🔥 OLED Display
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  display.setCursor(0, 0);
  display.print("P1: "); display.print(sum1);

  display.setCursor(0, 10);
  display.print("P2: "); display.print(sum2);

  display.setCursor(0, 20);
  display.print("P3: "); display.print(sum3);

  display.setCursor(0, 40);
  if (activePhase == 0) {
    display.print("NO PHASE");
  } else {
    display.print("ACTIVE: P");
    display.print(activePhase);
  }

  display.display();

  delay(300);
Serial.println("Sending data to Firebase...");
}