#include <SPI.h>
#include <Wire.h>
#include <ESP8266WiFi.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MQTT.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// WiFi credentials
const char * ssid = "XXXXXXXXXX";
const char * password = "XXXXXXXXXX";
// MQTT Broker
const char *mqtt_broker = "core-mosquitto";
const char *topic = "esp32/robot_esp";
const char *mqtt_username = "XXXXXXXXXX"; // for homeassistant
const char *mqtt_password = "XXXXXXXXXX"; // for homeassistant
const int mqtt_port = 1883;
// MQTT Network
const char* mqtt_server = "192.168.0.62"; // this would be the internal server i would like to connect


WiFiClient espClient;
MQTTClient mqttClient;

void setup_wifi() {
  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();
  Serial.println("WiFi connected");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}
void setup_mqtt() {
  // Set up the MQTT client
  Serial.print("\nconnecting...");
  unsigned long startAttemptTime = millis();
  while (!mqttClient.connect("wYh0FE4G3wM-kM6Zh", "public", "public")) {
    Serial.print(".");
    delay(1000);
    if (millis() - startAttemptTime > 10000) { // Timeout after 10 seconds
      Serial.println("\nFailed to connect to MQTT broker. Restarting...");
      ESP.restart(); // Restart the ESP8266
    }
  }

  Serial.println("\nconnected!");

}
void messageReceived(String &topic, String &payload) {
  Serial.println("incoming: " + topic + " - " + payload);

  // Note: Do not use the client in the callback to publish, subscribe or
  // unsubscribe as it may cause deadlocks when other things arrive while
  // sending and receiving acknowledgments. Instead, change a global variable,
  // or push to a queue and handle it in the loop after calling `client.loop()`.
}
void setup() {
  Serial.begin(9600);
  delay(1000);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  setup_wifi(); // Connect to WiFi
  Serial.println("Connected to WIFI.");
  mqttClient.begin("public.cloud.shiftr.io", espClient);
  mqttClient.onMessage(messageReceived);
  //setup_mqtt();
  // Commented out because crash
}

// Function to draw scewed rectangle
void draw_scewed_rect(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, int color) {
  display.fillTriangle(x1, y1, x2, y2, x3, y3, color);
  display.fillTriangle(x1, y1, x3, y3, x4, y4, color);
}

void draw_eye(int s = 0, int expression = 0, int w = 24, int h = 35) {
  int x = 32 * (s == 0 ? 1 : 3); // CENTERED x position
  x -= w / 2; // Adjust x position to top left corner
  x += (s == 0 ? 10 : -10); // move towards center
  int y = 32 - (h / 2); // Adjust y position to top left corner

  display.fillRoundRect(x, y, w, h, 5, SSD1306_WHITE);
  switch(expression) {
    case 0: // Normal
      break;
    case 1: // suspicious
      draw_scewed_rect(x - 2, y - 2, x + w + 2, y - 2, x + w + 2, y + 3, x - 2, y + 9, SSD1306_BLACK);
    default:
      break;
  }
}

void loop() {
  // Draw the robot face with a different expression
  display.clearDisplay();
  draw_eye(0, 0); // Left eye
  draw_eye(1, 1); // Right eye with suspicious expression
  display.display();
  delay(2000); // Wait for 2 seconds
  display.clearDisplay();
  draw_eye(0, 0); // Left eye
  draw_eye(1, 0); // Right eye
  display.display();
  delay(2000); // Wait for 2 seconds
}