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

//Not in Actual Code
int joystickPin[4] = {34, 35, 39, 36};
int joystickVal[4] = {0, 0, 0, 0};

unsigned long curMillis;
unsigned long readIntervalMillis = 100;
unsigned long lastReadMillis;

// number of degrees per movement
int servoMove = 5;

//Joystick parameter
int potCentre = 1840; 
int potDeadRange = 300;

// Servo parament
Servo servos[4];
int servoPin[] = {26, 27, 14, 12};
int servoPos[] = {90, 90, 90, 90};
int servoMax[] = {180, 150, 180, 180};
int servoMin[] = {0, 90, 0, 0};

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

  //Attach all the pin to the servo
  for(int i=0;i<sizeof(servos)/sizeof(Servo); i++){
    servos[i].attach(servoPin[i]);
  }

  //Don't put in the actual Code
  for(int i=0;i<sizeof(joystickPin)/sizeof(int); i++){
    pinMode(joystickPin[i], INPUT);
  }

}

void loop() {
  //Don't put in Code, This part is fetch from server
  for(int i=0; i<sizeof(joystickPin)/sizeof(int); i++){
    joystickVal[i] = analogRead(joystickPin[i]);
  }

  //increment or decrement the each servoPos if the joystickVal is above or below Threshold
  curMillis = millis();
  if (curMillis - lastReadMillis >= readIntervalMillis) {
    lastReadMillis += readIntervalMillis;

    for(int i=0;i<sizeof(servos)/sizeof(Servo); i++){
      if (joystickVal[i] > potCentre + potDeadRange) {
        servoPos[i] += servoMove;
      }
      if (joystickVal[i] < potCentre - potDeadRange) {
        servoPos[i] -= servoMove;
      }
    }
    
    //Don't let the servoPos go below or above its given min or  max
    checkDeadZone();
  } 

  //move each servo to each servo position
  moveServo();

}

void moveServo() {
  //Move Servo
  for(int i = 0; i<sizeof(servos)/sizeof(Servo); i++){
    servos[i].write(servoPos[i]);
  }
}

void checkDeadZone(){

 // check that the values are within limits
  for(int i=0; i < sizeof(servoPos)/sizeof(int); i++){
    if (servoPos[i] > servoMax[i]) {
      servoPos[i] = servoMax[i];
    }
    if(servoPos[i] < servoMin[i]) {
      servoPos[i] = servoMin[i];
    }
  }

}