#include <WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"

// Adafruit IO credentials
#define IO_USERNAME  "BenSmiling"
#define IO_KEY       "aio_ZdwN60vmbTT3QaYyHt6VRdxzuB5F"

// Wi-Fi credentials
#define WLAN_SSID     "Wokwi-GUEST"
#define WLAN_PASS     ""

// Pin assignments
const int ledPin = 13;
const int nepaInputPin = 12;
const int nepaOutputPin = 0;
const int relay1Pin = 16;
const int relay2Pin = 4;
const int relaySwitchPin = 17;  // Relay for switching sensors
const int sharedSensorPin = 34; // Shared analog input pin

const float sensorMultiplier = 0.185;
const float sensorOffset = 2.5;

WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, "io.adafruit.com", 1883, IO_USERNAME, IO_KEY);

Adafruit_MQTT_Subscribe powerControl = Adafruit_MQTT_Subscribe(&mqtt, IO_USERNAME "/feeds/ACTIVE_POWER");
Adafruit_MQTT_Subscribe relayStateControl = Adafruit_MQTT_Subscribe(&mqtt, IO_USERNAME "/feeds/RELAY_STATES");

Adafruit_MQTT_Publish nepaStatusFeed = Adafruit_MQTT_Publish(&mqtt, IO_USERNAME "/feeds/NEPA_STATUS");
Adafruit_MQTT_Publish outputPowerFeed = Adafruit_MQTT_Publish(&mqtt, IO_USERNAME "/feeds/OUTPUT_POWER");

float lastSensor1Value = -1;
float lastSensor2Value = -1;

unsigned long lastSensorReadTime = 0;
const unsigned long sensorReadInterval = 3000;
bool readingSensor1 = true;

void setup() {
  Serial.begin(115200);
  pinMode(nepaInputPin, INPUT);
  pinMode(nepaOutputPin, OUTPUT);
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(relaySwitchPin, OUTPUT);
  pinMode(ledPin, OUTPUT);

  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFi connected");

  mqtt.subscribe(&powerControl);
  mqtt.subscribe(&relayStateControl);

  while (!mqtt.connected()) {
    Serial.print("Connecting to Adafruit IO...");
    if (mqtt.connect()) {
      Serial.println("connected!");
    } else {
      Serial.println("failed, retrying...");
      delay(5000);
    }
  }
}

float readSharedSensor() {
  delay(100); // Settle time after switching
  int raw = analogRead(sharedSensorPin);
  float voltage = raw * (3.3 / 4095.0);
  return (voltage - sensorOffset) / sensorMultiplier;
}

void loop() {
  if (!mqtt.connected()) {
    while (!mqtt.connected()) {
      Serial.print("Reconnecting to Adafruit IO...");
      if (mqtt.connect()) {
        Serial.println("reconnected!");
        mqtt.subscribe(&powerControl);
        mqtt.subscribe(&relayStateControl);
      } else {
        Serial.print("failed, retrying in 5 seconds...");
        delay(5000);
      }
    }
  }

  mqtt.processPackets(1000);

  unsigned long currentMillis = millis();
  if (currentMillis - lastSensorReadTime >= sensorReadInterval) {
    lastSensorReadTime = currentMillis;

    // Alternate between sensors
    digitalWrite(relaySwitchPin, readingSensor1 ? LOW : HIGH); // LOW = sensor1, HIGH = sensor2
    float current = readSharedSensor();

    if (readingSensor1) {
      Serial.printf("Sensor 1 Current: %.3f A\n", current);
      lastSensor1Value = current;
    } else {
      Serial.printf("Sensor 2 Current: %.3f A\n", current);
      lastSensor2Value = current;
    }

    String message = String(lastSensor1Value, 3) + ", " + String(lastSensor2Value, 3);
    if (!outputPowerFeed.publish(message.c_str())) {
      Serial.println("Failed to publish OUTPUT_POWER");
    } else {
      Serial.println("Published OUTPUT_POWER: " + message);
    }

    readingSensor1 = !readingSensor1; // Switch for next cycle
  }

  // Power control logic
  if (powerControl.lastread != NULL) {
    String command = (char *)powerControl.lastread;
    if (command == "grid") digitalWrite(nepaOutputPin, LOW);
    else if (command == "solar") digitalWrite(nepaOutputPin, HIGH);
    else digitalWrite(nepaOutputPin, !digitalRead(nepaInputPin));
  }

  // Relay state control
  if (relayStateControl.lastread != NULL) {
    int relayCommand = atoi((char *)relayStateControl.lastread);
    digitalWrite(relay1Pin, relayCommand & 1);
    digitalWrite(relay2Pin, relayCommand & 2);
  }

  // NEPA status reporting
  static int lastNepastate = -1;
  int currentState = digitalRead(nepaOutputPin);
  if (currentState != lastNepastate) {
    lastNepastate = currentState;
    String message = String(currentState == LOW ? "NEPA_ON" : "NEPA_OFF") + ", " + String(millis());
    if (!nepaStatusFeed.publish(message.c_str())) {
      Serial.println("Failed to publish NEPA status");
    } else {
      Serial.println("Published NEPA status: " + message);
    }
  }
}