// Put your name and CWID here

#include <Servo.h>

#define JS_BUTTON_PIN A0
#define JS_HORIZONTAL_PIN A2
#define JS_VERTICAL_PIN A1

#define GREY_BUTTON_PIN 3

#define BLUE_LED_PIN 2
#define RED_LED_PIN 10
#define SWITCH_PIN 12
#define RIGHTSERVO_PIN 9
#define LEFTSERVO_PIN 6


Servo rightServo;
Servo leftServo;

int rightServoPos = 90;
int leftServoPos = 90;


void setup() {
  rightServo.attach(RIGHTSERVO_PIN);
  leftServo.attach(LEFTSERVO_PIN);
  rightServo.write(rightServoPos);
  leftServo.write(leftServoPos);
  pinMode(SWITCH_PIN, INPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(BLUE_LED_PIN, OUTPUT);
  pinMode(GREY_BUTTON_PIN, INPUT_PULLUP);
  pinMode(JS_BUTTON_PIN, INPUT_PULLUP);
}


void loop() {
  int sliderSwitch;
  int updownValue, leftrightValue, zValue;

  sliderSwitch = digitalRead(SWITCH_PIN);
  if (sliderSwitch == HIGH) {
    digitalWrite(BLUE_LED_PIN, LOW);
    digitalWrite(RED_LED_PIN, HIGH);
    
    if (digitalRead(GREY_BUTTON_PIN) == LOW) {  // button centers the servo
      rightServoPos = 90;
      leftServoPos = 90;
      delay(100);
    }

  }  else if (sliderSwitch == LOW) {  // use the joystick

    digitalWrite(BLUE_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, LOW);

    updownValue = analogRead(JS_HORIZONTAL_PIN);
    leftrightValue = analogRead(JS_VERTICAL_PIN);
    zValue = digitalRead(JS_BUTTON_PIN);
  
    if (zValue == LOW) {  // the joystick center button is pressed
      rightServo.write(90);
      leftServo.write( 90);
      //digitalWrite(BLUE_LED_PIN, LOW);
      delay(500);
      //digitalWrite(BLUE_LED_PIN, HIGH);
      rightServo.write(rightServoPos);
      leftServo.write( leftServoPos);
    }
 
    if (updownValue == 0) {
      rightServoPos = rightServoPos + 1;
      leftServoPos = leftServoPos - 1;
    }
    
    if (updownValue == 1023) {
      rightServoPos = rightServoPos - 1;
      leftServoPos = leftServoPos + 1;
    }

    if (leftrightValue > 525) {
      rightServoPos = rightServoPos - 1;
      leftServoPos = leftServoPos - 1;
    } 
    
    if (leftrightValue < 500) {
      rightServoPos = rightServoPos + 1;
      leftServoPos = leftServoPos + 1;
    }
  }

  rightServoPos = constrain(rightServoPos, 0, 180);
  leftServoPos = constrain(leftServoPos, 0, 180);
  rightServo.write(rightServoPos);
  leftServo.write( leftServoPos);
  delay(10);
}