// Control Panel Switches
int STOP_SWITCH = 2;
int MANUAL_MODE = 8;
int LOCAL_MODE = 9;
int AUTO_MODE = 10;
int POWER_ON = 11;
int ARM_2_QUILL = 46;
int ARM_2_TURRET = 48;
int TURRET_CCW = 50;
int TURRET_CW = 52;
int TOOL_OUT = 49;
int TOOL_IN = 51;
int CLAW_CLOSE = 53;

// Fagor M Codes
int M20_TOOL_OUT = 22;
int M21_TURRET_CW = 24;
int M22_TURRET_CCW = 26;
int M23_TOOL_IN = 28;
int M27_TURRET_HOME = 30;
int RESET_RELAY = 44;

// ATC Control Switches
int ARM_AT_QUILL = 32;
int MIDDLE_CAM = 34;
int ARM_AT_TURRET = 36;
int TURRET_GENEVA_SWITCH = 38;
int OPEN_CLAW_SWITCH = 40;
int TURRET_HOME_SWITCH = 42;

// Air Sol Outputs
int MASTER_AIR = 23;
int ARM_2_TURRET_SOL = 25;
int ARM_2_QUILL_SOL = 27;
int OPEN_CLAW = 29;
int CLOSE_CLAW = 31;
int DRAW_BAR_ENGAGE = 33;
int AIR_RETARD = 35;

// Turret Motor

int WHITE_120 = 16;
int BLACK_120 = 17;
int BLACK_0 = 18;
int BLUE_120 = 19;
int BLUE_0 = 20;
int BROWN_0 = 21;

// PDB Motor
int PDB_RED = 37;
int PDB_BLACK = 39;
int PDB_ORANGE = 41;
int PDB_BROWN = 43;

// Other Vairables
int PDB_Delay_Time = 20; // Time duration between the time the draw bar moves down and the motor starts to run in ms
int PDB_Runtime = 1500; // Runtime for the PDB motor in auto mode. 
int Reset_Timer = 20; // Time of the reset signal back to the Fagor Control


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

// Control Panel Switches
  pinMode(STOP_SWITCH, INPUT_PULLUP);
  pinMode(MANUAL_MODE, INPUT_PULLUP);
  pinMode(LOCAL_MODE, INPUT_PULLUP);
  pinMode(AUTO_MODE, INPUT_PULLUP);
  pinMode(POWER_ON, INPUT_PULLUP);
  pinMode(ARM_2_QUILL, INPUT_PULLUP);
  pinMode(ARM_2_TURRET, INPUT_PULLUP);
  pinMode(TURRET_CCW, INPUT_PULLUP);
  pinMode(TURRET_CW, INPUT_PULLUP);
  pinMode(TOOL_OUT, INPUT_PULLUP);
  pinMode(TOOL_IN, INPUT_PULLUP);
  pinMode(CLAW_CLOSE, INPUT_PULLUP);

// Fagor M Codes
  pinMode(M20_TOOL_OUT, INPUT_PULLUP);
  pinMode(M21_TURRET_CW, INPUT_PULLUP);
  pinMode(M22_TURRET_CCW, INPUT_PULLUP);
  pinMode(M23_TOOL_IN, INPUT_PULLUP);
  pinMode(M27_TURRET_HOME, INPUT_PULLUP);
  pinMode(RESET_RELAY, OUTPUT);

// ATC Control Switches
  pinMode(ARM_AT_QUILL, INPUT_PULLUP);
  pinMode(MIDDLE_CAM, INPUT_PULLUP);
  pinMode(ARM_AT_TURRET, INPUT_PULLUP);
  pinMode(TURRET_GENEVA_SWITCH, INPUT_PULLUP);
  pinMode(OPEN_CLAW_SWITCH, INPUT_PULLUP);
  pinMode(TURRET_HOME_SWITCH, INPUT_PULLUP);

// Air Sol Outputs
  pinMode(MASTER_AIR, OUTPUT);
  pinMode(ARM_2_TURRET_SOL, OUTPUT);
  pinMode(ARM_2_QUILL_SOL, OUTPUT);
  pinMode(OPEN_CLAW, OUTPUT);
  pinMode(CLOSE_CLAW, OUTPUT);
  pinMode(DRAW_BAR_ENGAGE, OUTPUT);
  pinMode(AIR_RETARD, OUTPUT);

// Turret Motor
  pinMode(WHITE_120, OUTPUT);
  pinMode(BLACK_120, OUTPUT);
  pinMode(BLACK_0, OUTPUT);
  pinMode(BLUE_120, OUTPUT);
  pinMode(BLUE_0, OUTPUT);
  pinMode(BROWN_0, OUTPUT);

// PDB Motor
  pinMode(PDB_RED, OUTPUT);
  pinMode(PDB_BLACK, OUTPUT);
  pinMode(PDB_ORANGE, OUTPUT);
  pinMode(PDB_BROWN, OUTPUT);


  // Stop Interrupt


}


void loop() {
  // put your main code here, to run repeatedly:

  if(digitalRead(STOP_SWITCH) == HIGH){
    digitalWrite(MASTER_AIR, HIGH);
  } else{
    digitalWrite(MASTER_AIR, LOW);
  }

  if(digitalRead(MANUAL_MODE) == LOW){
    // Serial.println("Manual Mode");
    manualMode();
  }
  if(digitalRead(LOCAL_MODE) == LOW){
    // Serial.println("Local Mode");
    localMode();
  }
  if(digitalRead(AUTO_MODE) == LOW){
    // Serial.println("Auto Mode");
    autoMode();
  }
}

void manualMode() {



  if(digitalRead(ARM_2_QUILL) == LOW && digitalRead(ARM_AT_QUILL) == HIGH){
    Serial.println("Arm to Quill Button Press");
    arm2Spindle();
  }

  if(digitalRead(ARM_2_TURRET) == LOW && digitalRead(ARM_AT_TURRET) == HIGH){
    Serial.println("Arm to turret button press");
    arm2Turret();
  }

  digitalWrite(CLOSE_CLAW, !digitalRead(CLAW_CLOSE));
  digitalWrite(OPEN_CLAW, digitalRead(CLAW_CLOSE));

  if(digitalRead(TURRET_CW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CW button pressed");
    turretCW();
  }

  if(digitalRead(TURRET_CCW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CCW button pressed");
    turretCCW();
  }

  if(digitalRead(TOOL_IN) == LOW && digitalRead(ARM_AT_QUILL) == LOW){
    Serial.println("Tool In button pressed");
    toolIn();
  }

    if(digitalRead(TOOL_OUT) == LOW && digitalRead(ARM_AT_QUILL) == LOW){
    Serial.println("Tool Out button pressed");
    toolOut();
  }

}

void arm2Spindle() {
  while(true){
    digitalWrite(ARM_2_QUILL_SOL, HIGH);
    if(digitalRead(ARM_AT_QUILL) == LOW){
      digitalWrite(ARM_2_QUILL_SOL, LOW);
      return;
    }
  }
}

void arm2Turret() {
  while(true){
    digitalWrite(ARM_2_TURRET_SOL, HIGH);
    digitalWrite(AIR_RETARD, HIGH);
    if(digitalRead(ARM_AT_TURRET) == LOW){
      digitalWrite(ARM_2_TURRET_SOL, LOW);
      digitalWrite(AIR_RETARD, LOW);
      return;
    }
  }
}

void turretCW() {
  while(true){
    digitalWrite(WHITE_120, HIGH);
    digitalWrite(BLACK_0, HIGH);
    digitalWrite(BLUE_120, HIGH);
    digitalWrite(BROWN_0, HIGH);
    if((digitalRead(TURRET_GENEVA_SWITCH) == LOW) ){
      digitalWrite(WHITE_120, LOW);
      digitalWrite(BLACK_0, LOW);
      digitalWrite(BLUE_120, LOW);
      digitalWrite(BROWN_0, LOW);
      return;
    }
  }
}

void turretCCW() {
  while(true){
    digitalWrite(WHITE_120, HIGH);
    digitalWrite(BLACK_120, HIGH);
    digitalWrite(BLUE_0, HIGH);
    digitalWrite(BROWN_0, HIGH);
    if((digitalRead(TURRET_GENEVA_SWITCH) == LOW) ){
      digitalWrite(WHITE_120, LOW);
      digitalWrite(BLACK_120, LOW);
      digitalWrite(BLUE_0, LOW);
      digitalWrite(BROWN_0, LOW);
      return;
    }
  }
}

void toolIn() {
  digitalWrite(DRAW_BAR_ENGAGE, HIGH);
  delay(PDB_Delay_Time);
  while(digitalRead(TOOL_IN) == LOW){
    digitalWrite(PDB_RED, HIGH);
    digitalWrite(PDB_BROWN, HIGH);
  }
  digitalWrite(PDB_RED, LOW);
  digitalWrite(PDB_BROWN, LOW);
  digitalWrite(DRAW_BAR_ENGAGE, LOW);
  return;
}

void toolOut() {
  digitalWrite(DRAW_BAR_ENGAGE, HIGH);
  delay(PDB_Delay_Time);
  while(digitalRead(TOOL_OUT) == LOW){
    digitalWrite(PDB_BLACK, HIGH);
    digitalWrite(PDB_ORANGE, HIGH);
    digitalWrite(PDB_BROWN, HIGH);
  }
  digitalWrite(PDB_BLACK, LOW);
  digitalWrite(PDB_ORANGE, LOW);
  digitalWrite(PDB_BROWN, LOW);
  digitalWrite(DRAW_BAR_ENGAGE, LOW);
  return;
}

void localMode() {

  if(digitalRead(TURRET_CW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CW button pressed");
    turretCW();
  }

  if(digitalRead(TURRET_CCW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CCW button pressed");
    turretCCW();
  }

  if(digitalRead(TOOL_IN) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    autoToolIn();
  }

  if(digitalRead(TOOL_OUT) == LOW && digitalRead(ARM_AT_QUILL) == LOW){
    autoToolOut();
  }

}

void autoToolIn() {
  do{
    digitalWrite(CLOSE_CLAW, HIGH);
    digitalWrite(OPEN_CLAW, LOW);
    digitalWrite(ARM_2_QUILL_SOL, HIGH);
    if(digitalRead(MIDDLE_CAM) == LOW){
      digitalWrite(DRAW_BAR_ENGAGE, HIGH);
      delay(PDB_Delay_Time);
      digitalWrite(PDB_RED, HIGH);
      digitalWrite(PDB_BROWN, HIGH);
      delay(PDB_Runtime);
      digitalWrite(PDB_RED, LOW);
      digitalWrite(PDB_BROWN, LOW);
      digitalWrite(DRAW_BAR_ENGAGE, LOW);
    }
  } while(digitalRead(ARM_AT_QUILL) == HIGH);
  
  if(digitalRead(ARM_AT_QUILL) == LOW){
    digitalWrite(CLOSE_CLAW, LOW);
    digitalWrite(OPEN_CLAW, HIGH);
    digitalWrite(ARM_2_QUILL_SOL, LOW);
    return;
  }
  
}

void autoToolOut() {
  
    digitalWrite(CLOSE_CLAW, HIGH);
    digitalWrite(OPEN_CLAW, LOW);
    digitalWrite(ARM_2_TURRET_SOL, HIGH);
    digitalWrite(AIR_RETARD, HIGH);
    digitalWrite(DRAW_BAR_ENGAGE, HIGH);
    delay(PDB_Delay_Time);
    digitalWrite(PDB_BLACK, HIGH);
    digitalWrite(PDB_ORANGE, HIGH);
    digitalWrite(PDB_BROWN, HIGH);
    delay(PDB_Runtime);
    digitalWrite(PDB_BLACK, LOW);
    digitalWrite(PDB_ORANGE, LOW);
    digitalWrite(PDB_BROWN, LOW);
    digitalWrite(DRAW_BAR_ENGAGE, LOW);
  while(digitalRead(ARM_AT_TURRET) == HIGH){
    digitalWrite(ARM_2_TURRET_SOL, HIGH);
    digitalWrite(AIR_RETARD, HIGH);
  }

  if(digitalRead(ARM_AT_TURRET) == LOW){
    digitalWrite(CLOSE_CLAW, LOW);
    digitalWrite(OPEN_CLAW, HIGH);
    digitalWrite(ARM_2_TURRET_SOL, LOW);
    digitalWrite(AIR_RETARD, LOW);
    return;
  }
  
}

void autoMode() {

    if(digitalRead(M21_TURRET_CW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CW button pressed");
    turretCW();
    digitalWrite(RESET_RELAY, HIGH);
    delay(Reset_Timer);
    digitalWrite(RESET_RELAY, LOW);
  }

  if(digitalRead(M22_TURRET_CCW) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CCW button pressed");
    turretCCW();
    digitalWrite(RESET_RELAY, HIGH);
    delay(Reset_Timer);
    digitalWrite(RESET_RELAY, LOW);
  }

  if(digitalRead(M23_TOOL_IN) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    autoToolIn();
    digitalWrite(RESET_RELAY, HIGH);
    delay(Reset_Timer);
    digitalWrite(RESET_RELAY, LOW);
  }

  if(digitalRead(M20_TOOL_OUT) == LOW && digitalRead(ARM_AT_QUILL) == LOW){
    autoToolOut();
    digitalWrite(RESET_RELAY, HIGH);
    delay(Reset_Timer);
    digitalWrite(RESET_RELAY, LOW);
  }

  if(digitalRead(M27_TURRET_HOME) == LOW && digitalRead(ARM_AT_TURRET) == LOW){
    Serial.println("Turret CCW button pressed");
    turretHome();
    digitalWrite(RESET_RELAY, HIGH);
    delay(Reset_Timer);
    digitalWrite(RESET_RELAY, LOW);
  }

}

void turretHome() {
  while(true){
    digitalWrite(WHITE_120, HIGH);
    digitalWrite(BLACK_0, HIGH);
    digitalWrite(BLUE_120, HIGH);
    digitalWrite(BROWN_0, HIGH);
    if((digitalRead(TURRET_HOME_SWITCH) == LOW) ){
      digitalWrite(WHITE_120, LOW);
      digitalWrite(BLACK_0, LOW);
      digitalWrite(BLUE_120, LOW);
      digitalWrite(BROWN_0, LOW);
      return;
    }
  }
}