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

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

// Servo settings
Servo servo1;
Servo servo2;

// WiFi settings
const char* ssid = "siyaram4g";
const char* password = "qazx1234";

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

  // Initialize OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.display();
  delay(2000);
  display.clearDisplay();

  // Initialize servos
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  servo1.attach(18);
  servo2.attach(19);
  servo1.write(90);
  servo2.write(90);

  // Connect to WiFi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }

  // Print IP address to Serial Monitor
  Serial.println("Connected to WiFi");
  Serial.print("IP Address: ");
  Serial.println(WiFi.localIP());

  // Print message on OLED display
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Hi from Robot 1");
  display.println("IP Address:");
  display.println(WiFi.localIP());
  display.display();
}

void loop() {
  // Example servo movement
  for (int pos = 0; pos <= 180; pos++) {
    servo1.write(pos);
    servo2.write(180 - pos);
    delay(15);
  }
  
  for (int pos = 180; pos >= 0; pos--) {
    servo1.write(pos);
    servo2.write(180 - pos);
    delay(15);
  }
}