#define BLYNK_TEMPLATE_ID "xxxxxxxxxxxx"
#define BLYNK_TEMPLATE_NAME "xxxxxxxxxxxxxx"
#define BLYNK_AUTH_TOKEN "xxxxxxxxxxxxxxx"

#define BLYNK_PRINT Serial

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

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



char WIFI_SSID[] = "xxxxxxxxxxxxx";
char WIFI_PASS[] = "xxxxxxxxxxxxxx";

// ── L298N pins ────────────────────────────────────────────────────
#define IN1  26
#define IN2  27
#define IN3  13
#define IN4  12
#define ENA  25
#define ENB  14

// ── Speed 0–255, very slow ────────────────────────────────────────
#define SPEED_A  170
#define SPEED_B  165

// ── PCA9685 ───────────────────────────────────────────────────────
Adafruit_PWMServoDriver pca = Adafruit_PWMServoDriver(0x40);

#define SERVO_PWM_FREQ  50
#define SERVO_MIN_US    500
#define SERVO_MAX_US    2500
#define TICK_US         4.8828f

#define CH_SHOULDER_L  0
#define CH_SHOULDER_R  1
#define CH_SLIDER      2
#define CH_ELBOW_L     3
#define CH_ELBOW_R     4

#define STEP_DEG  1.0f
#define STEP_MS   18

struct ServoTarget { uint8_t ch; float target; };
float currentPos[16];

// ── PWM speed write — works on ESP32 core 2.x AND 3.x ────────────
void setSpeed(uint8_t pin, uint8_t speed) {
  // ledcAttach is available on core 3.x
  // On core 2.x use ledcSetup+ledcAttachPin instead
  // This version uses ledcAttach (core 3.x)
  ledcAttach(pin, 1000, 8);       // 1000Hz, 8-bit
  ledcWrite(pin, speed);
}

// ── Motor A ───────────────────────────────────────────────────────
void motorA(int dir) {
  if (dir > 0)      { digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH); }
  else if (dir < 0) { digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);  }
  else              { digitalWrite(IN1, LOW);  digitalWrite(IN2, LOW);  }
  setSpeed(ENA, dir != 0 ? SPEED_A : 0);
}

// ── Motor B ───────────────────────────────────────────────────────
void motorB(int dir) {
  if (dir > 0)      { digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);  }
  else if (dir < 0) { digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH); }
  else              { digitalWrite(IN3, LOW);  digitalWrite(IN4, LOW);  }
  setSpeed(ENB, dir != 0 ? SPEED_B : 0);
}

void stopMotors()    { motorA(0);  motorB(0);  Serial.println(">> STOP");         }
void driveForward()  { motorA(1);  motorB(1);  Serial.println(">> ROTATE RIGHT");      }
void driveBackward() { motorA(-1); motorB(-1); Serial.println(">> ROTATE LEFT");     }
void rotateLeft()    { motorA(-1); motorB(1);  Serial.println(">> FORWARD");  }
void rotateRight()   { motorA(1);  motorB(-1); Serial.println(">> BACKWARD"); }

// ── Servo helpers ─────────────────────────────────────────────────
uint16_t angleToPulse(float deg) {
  float us = SERVO_MIN_US + (deg / 180.0f) * (SERVO_MAX_US - SERVO_MIN_US);
  return (uint16_t)(us / TICK_US);
}
void setServoInstant(uint8_t ch, float deg) {
  deg = constrain(deg, 0, 180);
  pca.setPWM(ch, 0, angleToPulse(deg));
  currentPos[ch] = deg;
}
void moveServo(uint8_t ch, float target) {
  target = constrain(target, 0, 180);
  float pos = currentPos[ch], start = pos;
  while (abs(target - pos) > 0.01f) {
    pos += (target > start) ? STEP_DEG : -STEP_DEG;
    if ((target > start && pos > target) || (target < start && pos < target)) pos = target;
    pca.setPWM(ch, 0, angleToPulse(pos));
    delay(STEP_MS);
  }
  currentPos[ch] = target;
}
void moveServos(ServoTarget targets[], uint8_t count) {
  float maxSteps = 0;
  for (uint8_t i = 0; i < count; i++) {
    float s = abs(targets[i].target - currentPos[targets[i].ch]) / STEP_DEG;
    if (s > maxSteps) maxSteps = s;
  }
  float startPos[16];
  for (uint8_t i = 0; i < count; i++) startPos[targets[i].ch] = currentPos[targets[i].ch];
  for (uint16_t step = 0; step <= (uint16_t)maxSteps; step++) {
    float t = maxSteps > 0 ? (float)step / maxSteps : 1.0f;
    for (uint8_t i = 0; i < count; i++) {
      uint8_t ch = targets[i].ch;
      pca.setPWM(ch, 0, angleToPulse(startPos[ch] + t * (targets[i].target - startPos[ch])));
    }
    delay(STEP_MS);
  }
  for (uint8_t i = 0; i < count; i++) currentPos[targets[i].ch] = targets[i].target;
}

// ── Poses ─────────────────────────────────────────────────────────
void poseCenter() {
  Serial.println(">> CENTER");
  ServoTarget t[] = {{CH_SHOULDER_L,90},{CH_SHOULDER_R,90},{CH_ELBOW_L,90},{CH_ELBOW_R,90}};
  moveServos(t, 4);
}
void poseRight() {
  Serial.println(">> RIGHT POSE");
  ServoTarget t[] = {{CH_SHOULDER_L,180},{CH_SHOULDER_R,0},{CH_ELBOW_L,0},{CH_ELBOW_R,180}};
  moveServos(t, 4);
}
void poseLeft() {
  Serial.println(">> LEFT POSE");
  ServoTarget t[] = {{CH_SHOULDER_L,0},{CH_SHOULDER_R,180},{CH_ELBOW_L,180},{CH_ELBOW_R,0}};
  moveServos(t, 4);
}

void executeCommand(char cmd) {
  switch(cmd) {
    case '0': poseRight();    break;
    case '1': poseCenter();   break;
    case '2': poseLeft();     break;
    case 'w': driveForward(); break;
    case 's': driveBackward();break;
    case 'a': rotateLeft();   break;
    case 'd': rotateRight();  break;
    case 'x': stopMotors();   break;
  }
}

// ── Blynk handlers ────────────────────────────────────────────────
BLYNK_WRITE(V0) { if (param.asInt() == 1) poseCenter(); }
BLYNK_WRITE(V1) { if (param.asInt() == 1) poseRight();  }
BLYNK_WRITE(V2) { if (param.asInt() == 1) poseLeft();   }

BLYNK_WRITE(V3) {
  int angle = constrain(param.asInt(), 0, 180);
  Serial.print(">> SLIDER ch2: "); Serial.println(angle);
  moveServo(CH_SLIDER, angle);
}

BLYNK_WRITE(V4) {
  if (param.asInt() == 1) rotateLeft();
  else stopMotors();
}
BLYNK_WRITE(V5) {
  if (param.asInt() == 1) rotateRight();
  else stopMotors();
}
BLYNK_WRITE(V6) {
  if (param.asInt() == 1) driveBackward();
  else stopMotors();
}
BLYNK_WRITE(V7) {
  if (param.asInt() == 1) driveForward();
  else stopMotors();
}

// ── Setup ─────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  while (!Serial) { delay(10); }

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  stopMotors();

  pca.begin();
  pca.setOscillatorFrequency(27000000);
  pca.setPWMFreq(SERVO_PWM_FREQ);
  delay(100);

  setServoInstant(CH_SHOULDER_L, 180);
  setServoInstant(CH_SHOULDER_R, 0);
  setServoInstant(CH_SLIDER,     0);
  setServoInstant(CH_ELBOW_L,    0);
  setServoInstant(CH_ELBOW_R,    180);

  Wire.begin(21,22);
  if(!display.begin(SSD1306_SWITCHCAPVCC,0x3C)){Serial.println("OLED failed");while(true);} 
  display.clearDisplay();display.setTextSize(2);display.setTextColor(SSD1306_WHITE);display.setCursor(10,0);display.println("ROBOT");display.setTextSize(1);display.setCursor(10,25);display.println("Connecting...");display.display();

  Blynk.begin(BLYNK_AUTH_TOKEN, WIFI_SSID, WIFI_PASS);
  display.clearDisplay();display.setTextSize(2);display.setCursor(10,0);display.println("ROBOT");display.setTextSize(1);display.setCursor(10,25);display.println("WiFi Connected");display.setCursor(10,40);display.println("Blynk Ready");display.display();

  Serial.println("─────────────────────────────────");
  Serial.println("Blynk connected!");
  Serial.println("V0=CENTER  V1=RIGHT  V2=LEFT");
  Serial.println("V3=SLIDER ch2 (0-180)");
  Serial.println("V4=ROT.LEFT  V5=ROT.RIGHT");
  Serial.println("V6=BACKWARD  V7=FORWARD");
  Serial.println("Serial: w/s/a/d=drive  0/1/2=poses  x=stop");
  Serial.println("─────────────────────────────────");
}

// ── Loop ──────────────────────────────────────────────────────────
void loop() {
  Blynk.run();

  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd != '\r' && cmd != '\n' && cmd != ' ') executeCommand(cmd);
  }
}