#include <Servo.h>
//scrap this one, didnt work either

//servo objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

//joystick pins
int joy1X = A0; // Joystick 1 X-axis
int joy1Y = A1; // Joystick 1 Y-axis
int joy2X = A2; // Joystick 2 X-axis
int joy2Y = A3; // Joystick 2 Y-axis

//  pushbutton pin
int pushButtonPin = 2;

// pushbutton pins
int selectButton1Pin = 9;    
int selectButton2Pin = 10;   

// initial servo pos
int pos1, pos2, pos3, pos4, pos5, pos6;

// Define the dead zone threshold
const int deadZone = 10;

void setup() {
  // Attach servos to digi pins
  servo1.attach(3); 
  servo2.attach(4); 
  servo3.attach(5); 
  servo4.attach(6); 
  servo5.attach(7); 
  servo6.attach(8); 

  // Set initial servo pos
  pos1 = 90;
  pos2 = 90;
  pos3 = 90;
  pos4 = 90;
  pos5 = 90;
  pos6 = 90;

  pinMode(selectButton1Pin, INPUT_PULLUP);
  pinMode(selectButton2Pin, INPUT_PULLUP);

  // pushbutt pin with pull-up resistor
  pinMode(pushButtonPin, INPUT_PULLUP);
}

void loop() {
  // Read joystick values
  int joy1XVal = analogRead(joy1X);
  int joy1YVal = analogRead(joy1Y);
  int joy2XVal = analogRead(joy2X);
  int joy2YVal = analogRead(joy2Y);

  // Check and update servo pos based on deadzore
  if (abs(joy1XVal - 512) > deadZone) {
    pos1 = map(joy1XVal, 0, 1023, 0, 180);
    servo1.write(pos1);
  }

  if (abs(joy1YVal - 512) > deadZone) {
    pos2 = map(joy1YVal, 0, 1023, 0, 180); 
    servo2.write(pos2);
  }

  if (abs(joy2XVal - 512) > deadZone) {
    pos3 = map(joy2XVal, 0, 1023, 0, 180); 
    servo3.write(pos3);
  }

  if (abs(joy2YVal - 512) > deadZone) {
    pos4 = map(joy2YVal, 0, 1023, 0, 180);
    servo4.write(pos4);
  }

  // servo5 joystick button control
  if (digitalRead(selectButton1Pin) == LOW) { 
    pos5 -= 10; // Move left
    if (pos5 < 0) pos5 = 0; 
    servo5.write(pos5);
    delay(200);
  }

  if (digitalRead(selectButton2Pin) == LOW) { 
    pos5 += 10; 
    if (pos5 > 180) pos5 = 180; 
    servo5.write(pos5);
    delay(200); 
  }

  // Control servo6 with a pushbutt
  if (digitalRead(pushButtonPin) == LOW) { 
    servo6.write(120);
    delay(2000);       
    servo6.write(90);  
  }
}