// BLYNK SETUP
// -------------------------
#define BLYNK_TEMPLATE_ID "TMPL6fiTotWrs"
#define BLYNK_TEMPLATE_NAME "Robotic Arm"
#define BLYNK_AUTH_TOKEN "I-GnjPsp0aouLkoU_ne8r-KtGPzosJSM"
/* Comment this out to disable prints and save space */
#define BLYNK_PRINT Serial


#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>
#include <ESP32Servo.h>


// Your WiFi credentials.
// Set password to "" for open networks.
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

BlynkTimer timer;
// Virtual Pins

#define VP_ACTUATOR   V0   // actuator position slider (0–100 mm)
#define VP_ROTATION   V1   // rotation servo slider
#define VP_GRIPPER    V2   // gripper slider
#define VP_AUTOMODE   V3   // auto mode button

bool autoMode = false;

// -------------------------
// HARDWARE PINS
// -------------------------
#define ENA_PIN 14
#define IN1_PIN 27
#define IN2_PIN 26
#define SERVO_PIN 18
#define GRIPPER_PIN 19
#define IR_SENSOR 34

Servo rotationServo;
Servo gripper;

// Actuator stroke -> time mapping
const int ACTUATOR_MAX_TIME = 20000; // 100mm = 20 seconds (corrected from 2000 to 20000)

int currentPosition = 0; // Track current actuator position (0-100)

// -------------------------
// BLYNK INPUTS
// -------------------------

// Rotation Servo (0–180 degrees)
BLYNK_WRITE(VP_ROTATION) {
  int angle = param.asInt();
  rotationServo.write(angle);
}

// Gripper Servo (0 = closed, 90 = open)
BLYNK_WRITE(VP_GRIPPER) {
  int pos = param.asInt();
  gripper.write(pos);
}

// Auto Mode Switch
BLYNK_WRITE(VP_AUTOMODE) {
  autoMode = param.asInt();  // 1 = ON, 0 = OFF
}

// Linear Actuator Slider (0–100 mm) - CORRECTED VERSION
BLYNK_WRITE(VP_ACTUATOR) {
  int targetPosition = param.asInt(); // requested position (0-100)
  
  if(targetPosition > currentPosition) {
    // Move UP - calculate time needed
    int moveDistance = targetPosition - currentPosition;
    int moveTime = map(moveDistance, 0, 100, 0, ACTUATOR_MAX_TIME);
    
    digitalWrite(IN1_PIN, HIGH);
    digitalWrite(IN2_PIN, LOW);
    delay(moveTime);
    stopActuator();
  }
  else if(targetPosition < currentPosition) {
    // Move DOWN - calculate time needed  
    int moveDistance = currentPosition - targetPosition;
    int moveTime = map(moveDistance, 0, 100, 0, ACTUATOR_MAX_TIME);
    
    digitalWrite(IN1_PIN, LOW);
    digitalWrite(IN2_PIN, HIGH);
    delay(moveTime);
    stopActuator();
  }
  
  // Update current position
  currentPosition = targetPosition;
  Blynk.virtualWrite(VP_ACTUATOR, currentPosition); // Sync slider position
}

// -------------------------
// SETUP
// -------------------------
void setup() {
  // Motor pins
  pinMode(ENA_PIN, OUTPUT);
  pinMode(IN1_PIN, OUTPUT);
  pinMode(IN2_PIN, OUTPUT);
  digitalWrite(ENA_PIN, HIGH);

  // Servos
  rotationServo.attach(SERVO_PIN);
  gripper.attach(GRIPPER_PIN);

  // IR Sensor
  pinMode(IR_SENSOR, INPUT);

  // Default gripper closed
  gripper.write(0);

  // Initialize actuator position
  currentPosition = 0;

  // Blynk
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
  
  // Sync initial slider position
  Blynk.virtualWrite(VP_ACTUATOR, currentPosition);
}

// -------------------------
// LOOP
// -------------------------
void loop() {
  Blynk.run();

  if (autoMode) {
    automove();
  }
}

// -------------------------
// AUTO MOVE SEQUENCE
// -------------------------
void automove() {
  // Move actuator fully UP (100 mm)
  moveUp();
  delay(1000);

  // Find object
  findObject();
  delay(1000);

  // Rotate home
  rotationServo.write(0);
  delay(2000);

  // Move down
  moveDown();
  delay(3000);
}

// -------------------------
// FIND OBJECT USING IR
// -------------------------
void findObject() {
  for (int angle = 0; angle <= 180; angle++) {
    rotationServo.write(angle);
    delay(50);

    if (digitalRead(IR_SENSOR) == HIGH) {
      // Pick object
      gripper.write(90);
      delay(800);
      gripper.write(0);
      delay(800);
      break;
    }
  }
}

// -------------------------
// LINEAR ACTUATOR FUNCTIONS
// -------------------------
void moveUp() {
  digitalWrite(IN1_PIN, HIGH);
  digitalWrite(IN2_PIN, LOW);
  delay(ACTUATOR_MAX_TIME);
  stopActuator();
  currentPosition = 100; // Update position after auto move
  Blynk.virtualWrite(VP_ACTUATOR, currentPosition); // Sync slider
}

void moveDown() {
  digitalWrite(IN1_PIN, LOW);
  digitalWrite(IN2_PIN, HIGH);
  delay(ACTUATOR_MAX_TIME);
  stopActuator();
  currentPosition = 0; // Update position after auto move
  Blynk.virtualWrite(VP_ACTUATOR, currentPosition); // Sync slider
}

void stopActuator() {
  digitalWrite(IN1_PIN, LOW);
  digitalWrite(IN2_PIN, LOW);
}