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

// --------- PIN SERVO (sesuai diagram) ---------
#define SERVO1_PIN 17   // servo1:PWM -> esp:17 (core 0)
#define SERVO2_PIN 18   // servo2:PWM -> esp:18 (core 1)

Servo servo1;
Servo servo2;

// --------- OLED & I2C ---------
TwoWire I2C_oled1 = TwoWire(0);  // OLED1: SCL 13, SDA 14
TwoWire I2C_oled2 = TwoWire(1);  // OLED2: SCL 19, SDA 20

Adafruit_SSD1306 display1(128, 64, &I2C_oled1, -1);
Adafruit_SSD1306 display2(128, 64, &I2C_oled2, -1);

// --------- TAMPILAN OLED ---------
void initDisplays() {
  char line1[32];
  char line2[32];

  display1.clearDisplay();
  display1.setTextSize(1);
  display1.setTextColor(SSD1306_WHITE);
  display1.setCursor(0, 0);
  snprintf(line1, sizeof(line1), "Servo pin %d", SERVO1_PIN);
  display1.println(line1);
  display1.println("running core 0");
  display1.display();

  display2.clearDisplay();
  display2.setTextSize(1);
  display2.setTextColor(SSD1306_WHITE);
  display2.setCursor(0, 0);
  snprintf(line2, sizeof(line2), "Servo pin %d", SERVO2_PIN);
  display2.println(line2);
  display2.println("running di core 1");
  display2.display();
}

// --------- TASK CORE 0 (servo1 di pin 18) ---------
void core0Task(void *pvParameters) {
  int angle = 0;
  bool naik = true;

  for (;;) {
    servo1.write(angle);

    if (naik) {
      angle += 5;
      if (angle >= 180) {
        angle = 180;
        naik = false;
      }
    } else {
      angle -= 5;
      if (angle <= 0) {
        angle = 0;
        naik = true;
      }
    }

    vTaskDelay(20 / portTICK_PERIOD_MS);  // kecepatan sweep
  }
}

// --------- TASK CORE 1 (servo2 di pin 46) ---------
void core1Task(void *pvParameters) {
  int angle = 180;
  bool turun = true;

  for (;;) {
    servo2.write(angle);

    if (turun) {
      angle -= 5;
      if (angle <= 0) {
        angle = 0;
        turun = false;
      }
    } else {
      angle += 5;
      if (angle >= 180) {
        angle = 180;
        turun = true;
      }
    }

    vTaskDelay(20 / portTICK_PERIOD_MS);
  }
}

// --------- SETUP & LOOP ---------
void setup() {
  Serial.begin(115200);

  // Attach servo ke pin, pakai range 500–2400us seperti contoh kamu
  servo1.attach(SERVO1_PIN, 500, 2400);
  servo2.attach(SERVO2_PIN, 500, 2400);

  // I2C OLED1: SDA 14, SCL 13
  I2C_oled1.begin(14, 13);
  // I2C OLED2: SDA 20, SCL 19
  I2C_oled2.begin(20, 19);

  // Init OLED
  if (!display1.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED1 gagal init");
  }
  if (!display2.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED2 gagal init");
  }

  initDisplays();

  // RTOS task per core (1 baris seperti maumu)
  xTaskCreatePinnedToCore(core0Task, "Core0Task", 4096, NULL, 1, NULL, 0);  // core 0
  xTaskCreatePinnedToCore(core1Task, "Core1Task", 4096, NULL, 1, NULL, 1);  // core 1
}

void loop() {
  // Kosong, semua kerja di task RTOS
}