// 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 Chroma HV testing fixture


int InterlockSW = 2; //Door Interlock (High when safe)
int EstopSW = 3; //E-Stop (Low when safe)
int PanelSW = 4; //Control Panel on/off (High when safe)
int ChromaStartSW = 6; // User Select Switch to Start Chroma TestRun Line
int ThreePhaseSW = 7; // User Select Switch for 480V

int ChromaSafeOut = 11; // Chroma Safety Line Remote-Inhibit pin 12 on Chroma(High when all safety conditions are mete)
int TestStartLineOut = 12; // Chroma Test Run Line Pin 11 on Chroma (High when test is running)
int ThreePhaseRelaysOut = 9; // Three Phase Relays On/Off Line

// 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;
int G7;

// Tower Light COntrol
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 || G7 == 1) {
    digitalWrite(redTowerLine, LOW);
    digitalWrite(greenTowerLine, HIGH);
    digitalWrite(yellowTowerLine, LOW);
  }
}

void ChromaSaftyLine() {
  //Chroma Safty Line Control
  if (G1 == 1 && G2 == 1 && G3 == 0) {
    digitalWrite(ChromaSafeOut, HIGH);
  } else {
    digitalWrite(ChromaSafeOut, LOW);
  }
}

void ChromaTestLine() {
  //Chroma Test Line Control
  if (G4 == 1 && G5 == 1) {
    digitalWrite(TestStartLineOut, HIGH);
  } else {
    digitalWrite(TestStartLineOut, LOW);
  }
  if (G1 == 1 && G2 == 1 && G3 == 0 && G7 == 1) {
    digitalWrite(ThreePhaseRelaysOut, HIGH);
  } else {
    digitalWrite(ThreePhaseRelaysOut, LOW);
  }
}


void setup() {
  Serial.begin(9600);

  // Switch Inputs
  pinMode(InterlockSW, INPUT);
  pinMode(PanelSW, INPUT);
  pinMode(EstopSW, INPUT);
  pinMode(ChromaStartSW, INPUT);
  pinMode(ThreePhaseSW, INPUT);

  // Control Lines for Chroma
  pinMode(ChromaSafeOut, OUTPUT);
  pinMode(TestStartLineOut, OUTPUT);

  // Tower Light Control
  pinMode(redTowerLine, OUTPUT);
  pinMode(greenTowerLine, OUTPUT);
  pinMode(yellowTowerLine, OUTPUT);

  // Control Line for Reklays
  pinMode(ThreePhaseRelaysOut, OUTPUT);

}

void loop() {
  // Digital Reads for finding Switches and Status Line States
  G1 = digitalRead(InterlockSW);
  G2 = digitalRead(PanelSW);
  G3 = digitalRead(EstopSW);
  G4 = digitalRead(ChromaStartSW);
  G5 = digitalRead(ChromaSafeOut);
  G6 = digitalRead(TestStartLineOut);
  G7 = digitalRead(ThreePhaseSW);
  //Calling Chroma Safty Line Control Function
  ChromaSaftyLine()
  //Calling Chroma Test Line Function
  ChromaTestLine()
  //Calling Tower Light Function
  TowerLight();

  //Calling Serial Print of Switch Postions for Debuging. (Remove slases to run Function)
  //SwitchPos();
  //Calling Serial Print of Tower Lights for Debuging. (Remove slases to run Function)
  //OUTPUTPos();
  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(" Chroma Safe Line: ");
  Serial.print(G5);
  Serial.print(" Test Start Line: ");
  Serial.println(G6);
}

void OUTPUTPos() {
  int red = digitalRead(redTowerLine);
  int yellow = digitalRead(yellowTowerLine);
  int green = digitalRead(greenTowerLine);

  Serial.print("Red Tower: ");
  Serial.print(red);
  Serial.print(" Yellow Tower: ");
  Serial.print(yellow);
  Serial.print(" Green Tower: ");
  Serial.println(green);
}