#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>  // Use ESP32-specific servo library

// OLED display width and height
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET    -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Potentiometer pins
const int N_pin = 34;
const int P_pin = 35;
const int K_pin = 32;

// Servo pins and objects
Servo servoN;
Servo servoP;
Servo servoK;
const int servoN_pin = 13;
const int servoP_pin = 14;
const int servoK_pin = 15;

int previousN_value = -1;
int previousP_value = -1;
int previousK_value = -1;

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

  // OLED setup
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Stop here if OLED fails to initialize
  }
  display.clearDisplay();

  // Attach servos to their pins
  servoN.attach(servoN_pin);
  servoP.attach(servoP_pin);
  servoK.attach(servoK_pin);

  display.display();
  delay(500); // Reduced initial delay
}

void loop() {
  // Read potentiometer values
  int N_value = analogRead(N_pin);
  int P_value = analogRead(P_pin);
  int K_value = analogRead(K_pin);

  // Only update servos if the value has changed significantly
  if (abs(N_value - previousN_value) > 10) {
    int N_angle = map(N_value, 0, 4095, 0, 180);
    servoN.write(N_angle);
    previousN_value = N_value;
  }
  
  if (abs(P_value - previousP_value) > 10) {
    int P_angle = map(P_value, 0, 4095, 0, 180);
    servoP.write(P_angle);
    previousP_value = P_value;
  }
  
  if (abs(K_value - previousK_value) > 10) {
    int K_angle = map(K_value, 0, 4095, 0, 180);
    servoK.write(K_angle);
    previousK_value = K_value;
  }

  // Display values on OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  
  display.setCursor(0, 0);
  display.print("Nitrogen (N): ");
  display.println(N_value);

  display.setCursor(0, 10);
  display.print("Phosphorus (P): ");
  display.println(P_value);

  display.setCursor(0, 20);
  display.print("Potassium (K): ");
  display.println(K_value);

  display.display();

  // Shorter delay to avoid taking too much time in simulation
  delay(300); 
}