#include <ESP32Servo.h>
#include <WiFi.h>
#include <ThingSpeak.h>

// Servo motor objects
Servo baseServo;
Servo armServo;
Servo clawServo;

// Ultrasonic Sensor
const int trigPin = 5;
const int echoPin = 18;

// Buttons
const int pickButton = 27;
const int placeButton = 26;

// WiFi credentials
const char* ssid = "realme C65";
const char* password = "the looser";

// ThingSpeak credentials
unsigned long myChannelNumber = 2714914;
const char * myWriteAPIKey = "11ERKGDFZMFUJ3WR";
WiFiClient client;

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

  // Initialize Servo motors
  baseServo.attach(13);
  armServo.attach(12);
  clawServo.attach(14);
  
  // Initialize Ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Initialize Buttons
  pinMode(pickButton, INPUT_PULLUP);
  pinMode(placeButton, INPUT_PULLUP);
  
  // Connect to Wi-Fi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");

  // Initialize ThingSpeak
  ThingSpeak.begin(client);
}

void loop() {
  if (digitalRead(pickButton) == LOW) {
    // Perform pick action
    pickObject();
  } else if (digitalRead(placeButton) == LOW) {
    // Perform place action
    placeObject();
  }
  
  // Send data to ThingSpeak
  sendDataToThingSpeak();
  delay(2000);
}

void pickObject() {
  // Move base to correct position
  baseServo.write(90);  // Rotate base
  delay(500);
  
  // Move arm down to pick object
  armServo.write(180);  // Lower arm
  delay(500);
  
  // Close claw to pick object
  clawServo.write(0);   // Close claw
  delay(500);
}

void placeObject() {
  // Move base to correct position
  baseServo.write(180);  // Rotate base to place position
  delay(500);
  
  // Move arm down to place object
  armServo.write(90);    // Raise arm
  delay(500);
  
  // Open claw to release object
  clawServo.write(90);   // Open claw
  delay(500);
}

void sendDataToThingSpeak() {
  long distance = getDistance();  // Get distance from ultrasonic sensor
  
  // Write to ThingSpeak
  ThingSpeak.setField(1, distance);  // Log distance data to ThingSpeak
  ThingSpeak.writeFields(myChannelNumber, myWriteAPIKey);
}

long getDistance() {
  // Send a pulse to trigger pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the duration of the echo
  long duration = pulseIn(echoPin, HIGH);
  
  // Calculate distance in cm
  long distance = (duration * 0.034) / 2;
  
  return distance;
}