// Ethan Clark A20330956
#include <Servo.h>

#define HORZ_PIN A3
#define VERT_PIN A5
#define BUTTON_PIN 3
#define RED_LED_PIN 2
#define GREEN_LED_PIN 10
#define SWITCH_PIN 12
#define DIAL_PIN A0
#define RIGHTSERVO_PIN 9
#define LEFTSERVO_PIN 6

Servo rightServo; 
Servo leftServo;

int rightServoStartPosition = 90; 
int leftServoStartPosition = 90; 
bool leftHandMode = false; 

float scale = 0.01; 
int servoIncrement = 1;

void setup() {
  pinMode(BUTTON_PIN, INPUT); 
  pinMode(RED_LED_PIN, OUTPUT); 
  pinMode(GREEN_LED_PIN, OUTPUT); 
  pinMode(SWITCH_PIN, INPUT_PULLUP); 
  
  rightServo.attach(RIGHTSERVO_PIN);
  leftServo.attach(LEFTSERVO_PIN);
  
  rightServo.write(rightServoStartPosition);
  leftServo.write(leftServoStartPosition);
}

void moveServo(Servo& servo, int targetPosition) {
  int currentPosition = servo.read();
  int direction = (targetPosition > currentPosition) ? 1 : -1;
  
  for (int pos = currentPosition; pos != targetPosition; pos += direction) {
    servo.write(pos);
    delay(5); 
  }
}

void moveServos(Servo& left, int leftTarget, Servo& right, int rightTarget) {
  int leftCurrent = left.read();
  int rightCurrent = right.read();
  
  int leftDirection = (leftTarget > leftCurrent) ? 1 : -1;
  int rightDirection = (rightTarget > rightCurrent) ? 1 : -1;
  
  while (leftCurrent != leftTarget || rightCurrent != rightTarget) {
    if (leftCurrent != leftTarget) {
      leftCurrent += leftDirection;
      left.write(leftCurrent);
    }
    
    if (rightCurrent != rightTarget) {
      rightCurrent += rightDirection;
      right.write(rightCurrent);
    }
    
    delay(5);
  }
}

void loop() {
  int switchState = digitalRead(SWITCH_PIN);
  
  if (switchState == LOW) { 
    leftHandMode = true; 
    digitalWrite(RED_LED_PIN, HIGH); 
    digitalWrite(GREEN_LED_PIN, LOW); 
    
    int vertPosition = analogRead(VERT_PIN);
    int horzPosition = analogRead(HORZ_PIN);
    
    int upThreshold = 300;
    int downThreshold = 700; 
    int leftThreshold = 300; 
    int rightThreshold = 700; 
    
    int newLeftPosition = leftServo.read();
    int newRightPosition = rightServo.read();
    
    bool joystickMoved = false;
    
    if (vertPosition < upThreshold) {
      newLeftPosition--;
      newRightPosition++;
      joystickMoved = true;
    }
    else if (vertPosition > downThreshold) {
      newLeftPosition++;
      newRightPosition--;
      joystickMoved = true;
    }
    if (horzPosition < leftThreshold) {
      newLeftPosition++;
      newRightPosition++;
      joystickMoved = true;
    }
    if (horzPosition > rightThreshold) {
      newLeftPosition--;
      newRightPosition--;
      joystickMoved = true;
    }
    
    if (joystickMoved) {
      newLeftPosition = constrain(newLeftPosition, 0, 180);
      newRightPosition = constrain(newRightPosition, 0, 180);
      
      moveServos(leftServo, newLeftPosition, rightServo, newRightPosition);
    }
    
    if (digitalRead(BUTTON_PIN) == HIGH) {
      leftServo.write(leftServoStartPosition);
      rightServo.write(rightServoStartPosition);
    }
  } else { 
    leftHandMode = false;
    digitalWrite(GREEN_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, LOW);
    
    int potValue = analogRead(DIAL_PIN);
    
    int rightServoPosition = map(potValue, 0, 1023, 0, 180);
    
    rightServoPosition = constrain(rightServoPosition, 0, 180);
    
    moveServo(rightServo, rightServoPosition);
  }
}