#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 definitions
const int nepaInputPin = 12;
const int nepaOutputPin = 0;
const int relay1Pin = 16;
const int relay2Pin = 4;
const int sensor1Pin = 34;
const int sensor2Pin = 35;

const float sensorMultiplier = 0.185;
const float sensorOffset = 0;

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

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

// Publications
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");

unsigned long lastSensorReadTime = 0;
const unsigned long sensorReadInterval = 5000;

void setup() {
  Serial.begin(115200);

  pinMode(nepaInputPin, INPUT);
  pinMode(nepaOutputPin, OUTPUT);
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);

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

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

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

void loop() {
  mqtt.processPackets(1000);
  mqtt.ping();

  // Relay State Handling
  if (relayStateControl.lastread != NULL) {
    int relayCommand = atoi((char *)relayStateControl.lastread);  // Convert command to integer
    Serial.print("Received relay command: ");
    Serial.println(relayCommand);
    
    // Process the relay commands
    switch (relayCommand) {
      case 0:
        digitalWrite(relay1Pin, LOW);
        digitalWrite(relay2Pin, LOW);
        break;
      case 1:
        digitalWrite(relay1Pin, HIGH);
        digitalWrite(relay2Pin, LOW);
        break;
      case 2:
        digitalWrite(relay1Pin, LOW);
        digitalWrite(relay2Pin, HIGH);
        break;
      case 3:
      default:
        digitalWrite(relay1Pin, HIGH);
        digitalWrite(relay2Pin, HIGH);
        break;
    }
  }

  // Power Control Handling
  if (powerControl.lastread != NULL) {
    String command = (char *)powerControl.lastread;
    Serial.print("Received power command: ");
    Serial.println(command);
    
    if (command == "grid") {
      digitalWrite(nepaOutputPin, LOW);
    } else if (command == "solar") {
      digitalWrite(nepaOutputPin, HIGH);
    } else {
      digitalWrite(nepaOutputPin, !digitalRead(nepaInputPin));
    }
  }

  // Sensor Data Reading & Publishing
  unsigned long currentMillis = millis();
  if (currentMillis - lastSensorReadTime >= sensorReadInterval) {
    lastSensorReadTime = currentMillis;

    int raw1 = analogRead(sensor1Pin);
    int raw2 = analogRead(sensor2Pin);

    float voltage1 = (raw1 * 3.3) / 4095.0;
    float voltage2 = (raw2 * 3.3) / 4095.0;

    float current1 = (voltage1 - sensorOffset) / sensorMultiplier;
    float current2 = (voltage2 - sensorOffset) / sensorMultiplier;

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

  // NEPA Status Handling
  static int lastNepainputState = -1;
  int currentNepainputState = digitalRead(nepaInputPin);

  if (currentNepainputState != lastNepainputState) {
    lastNepainputState = currentNepainputState;
    String timestamp = String(millis());
    String statusMessage = String(currentNepainputState == HIGH ? "NEPA_ON" : "NEPA_OFF") + ", " + timestamp;
    if (!nepaStatusFeed.publish(statusMessage.c_str())) {
      Serial.println("Failed to publish NEPA status");
    } else {
      Serial.println("Published NEPA status: " + statusMessage);
    }
  }
}