// Program to controll the HV testing fixture
// Rev-2: Updated names of switches and control lines for more clearity


// Pins Used to controll the safety of the Croma HV testing fixture

int InterlockSW = 5; //Door Interlock (High when safe)
int PanelSW = 6; //Control Panel on/off (High when safe)
int EstopSW = 7; //E-Stop (Low when safe)
int CromaSafe = 8; // Croma Safety Line (High when all safety conditions are mete)
int TestStartSW = 2; // User Test_Start command
int TestStartLine = 11; // Croma Test Run Line (High when test is running)

// LightTower Pins
int redTowerLine = 22;
int greenTowerLine = 26;
int yellowTowerLine = 24;

// Variables used to check condition of switches
int G1;
int G2;
int G3;
int G4;
int G5;
int G6;

void TowerLight() {

  if (G5 == 0 && G6 == 0) {
    digitalWrite(redTowerLine, HIGH);
    digitalWrite(greenTowerLine, LOW);
    digitalWrite(yellowTowerLine, LOW);
  }
  if (G5 == 1 && G6 == 0) {
    digitalWrite(redTowerLine, LOW);
    digitalWrite(greenTowerLine, LOW);
    digitalWrite(yellowTowerLine, HIGH);
  }
  if (G5 == 1 && G6 == 1) {
      digitalWrite(redTowerLine, LOW);
    digitalWrite(greenTowerLine, HIGH);
    digitalWrite(yellowTowerLine, LOW);
  }
}

void setup() {
  Serial.begin(9600);
  // Switch Inputs
  pinMode(InterlockSW, INPUT);
  pinMode(PanelSW, INPUT);
  pinMode(EstopSW, INPUT);
  pinMode(TestStartSW, INPUT);
  // Control Lines for Croma
  pinMode(CromaSafe, OUTPUT);
  pinMode(TestStartLine, OUTPUT);
  // Tower Light Control
  pinMode(redTowerLine, OUTPUT);
  pinMode(greenTowerLine, OUTPUT);
  pinMode(yellowTowerLine, OUTPUT);

}

void loop() {

  G1 = digitalRead(InterlockSW);
  G2 = digitalRead(PanelSW);
  G3 = digitalRead(EstopSW);
  G4 = digitalRead(TestStartSW);
  G5 = digitalRead(CromaSafe);
  G6 = digitalRead(TestStartLine);

  if (G1 == 1 && G2 == 1 && G3 == 0) {
    digitalWrite(CromaSafe, HIGH);
  } else {
    digitalWrite(CromaSafe, LOW);
  }

  if (G4 == 1 && G5 == 1) {
    digitalWrite(TestStartLine, HIGH);
  } else {
    digitalWrite(TestStartLine, LOW);
  }
  TowerLight();
  //SwitchPos();
  delay(100);

}

void SwitchPos() {
  Serial.print("Door Interlock: ");
  Serial.print(G1);
  Serial.print(" Panel Switch: ");
  Serial.print(G2);
  Serial.print(" Estop Switch (Needs to be Low): ");
  Serial.print(G3);
  Serial.print(" Test Start Switch: ");
  Serial.print(G4);
  Serial.print(" Croma Safe Line: ");
  Serial.print(G5);
  Serial.print(" Test Start Line: ");
  Serial.println(G6);

}