#include <LiquidCrystal_I2C.h>
#include <Servo.h>
String currentSelected = "MK45";
String selectedPitch = "init";
String selectedYaw = "init";

const int moveSpeed = 3;

int statusLED = 23;
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int buttonPin = 13;
bool lastButtonState = LOW;
bool currentButtonState;
bool actionDone = false;


int pitchAngle = 90;
int deadband = 50; //deadzone

int yawAngle = 90;


int mk45Pitch = 90;
int mk45Yaw = 90;
int CRAM1Pitch = 90;
int CRAM1Yaw = 90;
int CRAM2Pitch = 90;
int CRAM2Yaw = 90;


int lastmk45Pitch = 90;
int lastmk45Yaw = 90;
int lastCRAM1Pitch = 90;
int lastCRAM1Yaw = 90;
int lastCRAM2Pitch = 90;
int lastCRAM2Yaw = 90;

   Servo mk45PitchServo;
   Servo mk45YawServo;
   Servo CRAM1PitchServo;
   Servo CRAM1YawServo;
   Servo CRAM2PitchServo;
   Servo CRAM2YawServo;



void setup() {
  pinMode(buttonPin, INPUT_PULLUP);

  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.print("Arleigh Burke");
  delay(1000);
  lcd.clear();

pitchAngle = constrain(pitchAngle, 0, 180);
yawAngle = constrain(pitchAngle, 0, 180);


    mk45PitchServo.attach(2);
    mk45YawServo.attach(3);
    CRAM1PitchServo.attach(4);
    CRAM1YawServo.attach(5);
    CRAM2PitchServo.attach(6);
    CRAM2YawServo.attach(7);

}

void loop() {
  digitalWrite(statusLED, HIGH);
  currentButtonState = digitalRead(buttonPin);


Serial.print(pitchAngle);
Serial.print(" / ");
Serial.println(yawAngle);
  
if(currentSelected == "MK45"){
  pitchAngle = mk45Pitch;
yawAngle = mk45Yaw;

  int joystickValueX = analogRead(A1);
  int joystickValueY = analogRead(A0);


  
  if (joystickValueX < 512 - deadband) {
    pitchAngle -= moveSpeed; 
}
else if (joystickValueX > 512 + deadband) {
  pitchAngle += moveSpeed; 
}

if (joystickValueY < 512 - deadband) {
  yawAngle -= moveSpeed; 
}
else if (joystickValueY > 512 + deadband) {
  yawAngle += moveSpeed; 
}

mk45Pitch = pitchAngle;
mk45Yaw = yawAngle;
mk45PitchServo.write(mk45Pitch);
mk45YawServo.write(mk45Yaw);


  }
else if(currentSelected == "CRAM_1") {
  pitchAngle = CRAM1Pitch;
yawAngle = CRAM1Yaw;


  int joystickValueX = analogRead(A1);
  int joystickValueY = analogRead(A0);


  if (joystickValueX < 512 - deadband) {
    pitchAngle -= moveSpeed; 
}
else if (joystickValueX > 512 + deadband) {
  pitchAngle += moveSpeed; 
}

if (joystickValueY < 512 - deadband) {
  yawAngle -= moveSpeed; 
}
else if (joystickValueY > 512 + deadband) {
  yawAngle += moveSpeed; 
}
CRAM1Pitch = pitchAngle;
CRAM1Yaw = yawAngle;
CRAM1PitchServo.write(CRAM1Pitch);
CRAM1YawServo.write(CRAM1Yaw);
}
else if(currentSelected == "CRAM_2") {
  pitchAngle = CRAM2Pitch;
yawAngle = CRAM2Yaw;


  int joystickValueX = analogRead(A1);
  int joystickValueY = analogRead(A0);


  if (joystickValueX < 512 - deadband) {
    pitchAngle -= moveSpeed; 
}
else if (joystickValueX > 512 + deadband) {
  pitchAngle += moveSpeed; 
}

if (joystickValueY < 512 - deadband) {
  yawAngle -= moveSpeed; 
}
else if (joystickValueY > 512 + deadband) {
  yawAngle += moveSpeed; 
}
CRAM2Pitch = pitchAngle;
CRAM2Yaw = yawAngle;

CRAM2PitchServo.write(CRAM2Pitch);
CRAM2YawServo.write(CRAM2Yaw);
}
else {
  selectedPitch = "ERROR";
  selectedYaw = "ERROR";

}



  if (lastButtonState == HIGH && currentButtonState == LOW && !actionDone) {
    digitalWrite(statusLED, LOW);
    changeWeaponry();
    actionDone = true;
  }

  if (currentButtonState == HIGH) {
    actionDone = false;
  }

  lastButtonState = currentButtonState;

  lcd.setCursor(0, 0);
  lcd.print("                ");  
  lcd.setCursor(0, 0);
  lcd.print(currentSelected);

  lcd.setCursor(0, 1);
  lcd.print("                ");  
  lcd.setCursor(0, 1);
  lcd.print(yawAngle);
  lcd.setCursor(6, 1);
  lcd.print(pitchAngle);


}

void changeWeaponry() {
  if (currentSelected == "MK45") {
    currentSelected = "CRAM_1";
  } else if (currentSelected == "CRAM_1") {
    currentSelected = "CRAM_2";
  } else if (currentSelected == "CRAM_2") {
    currentSelected = "MK45";
  } else {
    currentSelected = "ERROR";
  }

}