#include <Servo.h>

Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo
Servo myservo3;  // create servo object to control a servo
Servo myservo4;  // create servo object to control a servo

int potpin1 = 0;  // analog pin used to connect the potentiometer servo 1
int potpin2 = 1;  // analog pin used to connect the potentiometer servo 2
int potpin3 = 2;  // analog pin used to connect the potentiometer servo 3
int potpin4 = 3;  // analog pin used to connect the potentiometer servo 4

int btn1 = 3; //Digital pin click stick 1
int btn2 = 2; //Digital pin click stick 2

int val;    // variable to read the value from the analog pin
int curPosServo1 = 512;
int curPosServo2 = 512;
int curPosServo3 = 512;
int curPosServo4 = 512;

void setup() {
  myservo1.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo2.attach(10);  // attaches the servo on pin 9 to the servo object
  myservo3.attach(11);
  myservo4.attach(6);
  
  pinMode(btn1, INPUT);   
  pinMode(btn2, INPUT);   

  Serial.begin(115200);
}

void loop() {


  leerStick(potpin1, myservo1, curPosServo1, false);
  leerStick(potpin2, myservo2, curPosServo2, false);
  leerStick(potpin3, myservo3, curPosServo3, false);
  leerStick(potpin4, myservo4, curPosServo4, false);

  leerJoystickClick(1);
  leerJoystickClick(2);


  delay(15);                           // waits for the servo to get there
}


void leerJoystickClick(int stick) {
  int buttonPin;

  if (stick == 1){
    buttonPin = btn1;
  }else if (stick == 2){
    buttonPin = btn2;
  }

  int buttonState = digitalRead(buttonPin);

  // check if the pushbutton is pressed. If it is, the buttonState is HIGH:
  if (buttonState == HIGH) {
    if (stick == 1) {
      curPosServo1 = 512;
      curPosServo2 = 512;
    } else if (stick == 2) {
      curPosServo3 = 512;
      curPosServo4 = 512;
    } 
  } 
  
}

void leerStick(int stick, Servo servo, int &curPos, bool invertir) {
  val = analogRead(stick);  
  if(val > 1000){
    movUp(curPos, invertir);
  }
  else if (val < 400) {
    movDown(curPos, invertir);
  }
  moverServo(servo, curPos);
}

void moverServo(Servo servo, int curPos){
  int val2  = map(curPos, 0, 1023, 0, 180);
  servo.write(val2);                  // sets the servo position according to the scaled value
}

void movUp(int &curPos, bool invertir) {
  if (invertir){
    curPos += 10;
  }
  else {
    curPos -= 10;
  }
  delay(50); 
}

void movDown(int &curPos, bool invertir) {
  if (invertir){
    curPos -= 10;
  }
  else {
    curPos += 10;
  }
  delay(50); 
}