#include <Servo.h>
#include <WiFi.h>  // Use WiFi.h for ESP32
#include <ThingSpeak.h>

// ThingSpeak credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const unsigned long channelID = 2648183; // Replace with your ThingSpeak Channel ID
const char* writeAPIKey = "G2WHKHMPE8KFLEY6";  // Replace with your Write API Key

// Setup joysticks
const int JOY_R_VERT = 34;  // Use GPIO pins specific to ESP32
const int JOY_R_HORIZ = 35;
const int JOY_L_VERT = 32;
const int JOY_L_HORIZ = 33;

// Setup motors
const int CLAW = 13;   // GPIO pins for ESP32
const int SHOULDER = 12;
const int ELBOW = 14;
const int BASE = 27;

// Setup servo objects
Servo clawMotor;
Servo shoulderMotor;
Servo elbowMotor;
Servo baseMotor;

WiFiClient client;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Connect to Wi-Fi
   if(WiFi.status() != WL_CONNECTED) {
     Serial.print("attempting to connect");
  while (WiFi.status() != WL_CONNECTED) {
    WiFi.begin(ssid, password);
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected");
}


  // Connect to ThingSpeak
  ThingSpeak.begin(client);

  // Initialize motors
  clawMotor.attach(CLAW);
  shoulderMotor.attach(SHOULDER);
  elbowMotor.attach(ELBOW);
  baseMotor.attach(BASE);

  // Reset the arm to its default position
  setMotorPosition(clawMotor, 0);
  setMotorPosition(shoulderMotor, 0);
  setMotorPosition(elbowMotor, 0);
  setMotorPosition(baseMotor, 90);
}

void loop() {
  int motorSpeed = 5;

  // Read joystick values
  int clawAmt = getJoystickPosition(JOY_L_HORIZ); 
  int clawPos = clawMotor.read() + motorSpeed * clawAmt;
  setMotorPosition(clawMotor, clawPos);

  int baseAmt = getJoystickPosition(JOY_R_HORIZ);
  int basePos = baseMotor.read() + motorSpeed * baseAmt;
  setMotorPosition(baseMotor, basePos);

  int shoulderAmt = getJoystickPosition(JOY_R_VERT);
  int shoulderPos = shoulderMotor.read() + motorSpeed * shoulderAmt;
  setMotorPosition(shoulderMotor, shoulderPos);

  int elbowAmt = getJoystickPosition(JOY_L_VERT);
  int elbowPos = elbowMotor.read() + motorSpeed * elbowAmt;
  setMotorPosition(elbowMotor, elbowPos);

  // Update ThingSpeak
  ThingSpeak.setField(1, clawPos);
  ThingSpeak.setField(2, basePos);
  ThingSpeak.setField(3, shoulderPos);
  ThingSpeak.setField(4, elbowPos);
  
  // Write to ThingSpeak
  int responseCode = ThingSpeak.writeFields(channelID, writeAPIKey);
  if(responseCode == 200){
    Serial.println("Channel update successful.");
  } else {
    Serial.println("Failed to update channel. Response code: " + String(responseCode));
  }
  
  // Wait 20 seconds to avoid exceeding the rate limit
  delay(20000);
}

int getJoystickPosition(int pin){
  int amount = analogRead(pin);
  int result = 0;
  if(amount > 600){
    result = -1;
  } else if (amount < 450){
    result = 1;
  }
  return result;
}

void setMotorPosition(Servo motor, int position){
  if(position < 0){
    position = 0;
  } else if(position > 180){
    position = 180;
  }
  motor.write(position);  
}