// Pin definitions
const int greenLedPin = 18;  // Green LED for pass      Anolog  A4
const int redLedPin = 17;    // Red LED for fail        Anolog  A3
const int yelLedLedPin = 19;    // yellow LED for busy  Anolog  A5

int sensorPin = A0;   // select the input pin for the sensor    Anolog
int sensorValue = 0;   // variable to store the value coming from the sensor
//int pinValue = 0;  // read digital pin 3 - 10 

// Analog pins for testing cables (A0 to A7)
const int testPins[] = {2, 3, 4, 5, 6, 7, 8};
const int numPins = 7;

//Add a test pin for shorts test
const int testPin = 10;  // test pin for short result
int testPinState = 0;

//relay pin enable
const int relayEnable1 = 9;  //enable the relay when High
const int relayEnable2 = 11;
const int relayEnable3 = 12;
const int relayEnable4 = 13;
const int relayEnable5 = 15;        //Anolog A1
const int relayEnable6 = 16;        //Anolog A2 

// Threshold for determining pass/fail
const int lowerThreshold = 0;  // Lower value
const int upperThreshold = 1;  // Upper value

// Flag to check if the test is in progress
bool testInProgress = false;

int wait = (500);

//--------------------------------------SETUP---------------------------------------
void setup() {
  // Set LED pins as output
  pinMode(greenLedPin, OUTPUT);
  pinMode(redLedPin, OUTPUT);
  pinMode(yelLedLedPin, OUTPUT);

  //Set relay pin as output
  pinMode(relayEnable1, OUTPUT);
  pinMode(relayEnable2, OUTPUT);
  pinMode(relayEnable3, OUTPUT);
  pinMode(relayEnable4, OUTPUT);
  pinMode(relayEnable5, OUTPUT);
  pinMode(relayEnable6, OUTPUT);

  // Initialize all test pins as inputs
  for (int i = 0; i < numPins; i++) {
        pinMode(testPins[i], INPUT);
  }
  //
  pinMode(testPin, INPUT);

  // Initialize LEDs to be off
  digitalWrite(greenLedPin, LOW);
  digitalWrite(redLedPin, LOW);
  digitalWrite(yelLedLedPin, LOW);

 // call the serial coms to print to debug
  Serial.begin(9600);
}
//-----------------------------------MAIN LOOP---------------------------------------------
void loop() {
  // read the value from the sensor (potentiometer or other analog sensor):
    sensorValue = analogRead(sensorPin);
    
  // Check if the Start Test button is pressed
  if (sensorValue == 1023) {       // Button pressed (LOW due to internal pull-up)
    startTest();
  }

  // If the test is in progress, run the test
  if (testInProgress) {
    runTest();
  }
  
}
//-------------------------------START TEST LOOP-------------------------------------------
void startTest() {
  // Start the test and turn on LEDs based on the test result
  testInProgress = true;
  digitalWrite(greenLedPin, LOW);  // Make sure Green LED is off
  digitalWrite(redLedPin, LOW);    // Make sure Red LED is off
  //digitalWrite(relayEnable, LOW); //Make sure the relay is ON
  digitalWrite(yelLedLedPin, HIGH);
  delay(wait);
  runTest();
}
//---------------------------------RUN TEST LOOP1--------------------------------------------
void runTest() {
  bool allPass = true;

  // Test each contact (digital pin)
  for (int i = 0; i < numPins; i++) {
    int pinValue = digitalRead(testPins[i]);

    //debug serial output
    Serial.print("Testing pin: ");
    Serial.print(testPins[i]);
    Serial.print("Pin Value: ");
    Serial.print(pinValue);

    // Check if the value is within the acceptable range
    //if (pinValue < lowerThreshold || pinValue > upperThreshold) //{
      if (pinValue == LOW) {
      allPass = false;  // Fail if any pin doesn't pass
      //Serial.print(sensorValue1);
      break;
   // }
  }
  }
  // Set the LED status based on pass/fail result
  if (allPass) {
    digitalWrite(greenLedPin, HIGH);  // Green LED ON
    digitalWrite(redLedPin, LOW);     // Red LED OFF
    Serial.print("All Pass: ");
    Serial.println(allPass ? "YES" : "NO");
      
    } else {
    digitalWrite(greenLedPin, LOW);   // Green LED OFF
    digitalWrite(redLedPin, HIGH);    // Red LED ON
      }

  // Wait a short time before completing the test
  delay(100);
  //digitalWrite(yelLedLedPin, LOW);     // Busy led OFF
  // After the test, stop the test sequence
  //testInProgress = false;  // uncomment if test ends here
  //delay(wait);
  runTest2();
}
//----------------------------------------------------RUN TEST LOOP2-----------------------------------------------------
void runTest2() {
  bool allPass = true;
  
  // Test contact (digital pin)
  testPinState = digitalRead(testPin);
      
  // Disconnect 5V and Ground from Sil Header
  // digitalWrite(relayEnable1, HIGH);  // Cut power and ground from the sil header!!! Short an IO pin

  // Read testpin state should be LOW
  testPinState = digitalRead(testPin); // Read pin state 1/9 to 2/10

  // Check if the testpin HIGH:
  if (testPinState == HIGH) {
    // IF HIGH FAIL
    digitalWrite(redLedPin, HIGH);  // Work on this
    return;  // Exit the function if failed
  } else {
    // IF LOW PASS
    delay(500);
    digitalWrite(relayEnable1, HIGH); // Enable relay to switch 2/10 to 3/11
    testPinState = digitalRead(testPin); // Read pin state 2/10 to 3/11
    
    if (testPinState == HIGH) {
      // FAIL
      digitalWrite(redLedPin, HIGH);
      return; // Exit the function if failed
    } else {
      // PASS
      delay(500);
      digitalWrite(relayEnable1, LOW);  // Test done, power off relay
      digitalWrite(relayEnable6, HIGH);  // Enable relay to switch 3/11 to 4/12
      testPinState = digitalRead(testPin);  // Read pin state 3/11 to 4/12
      
      if (testPinState == HIGH) {
        // FAIL
        digitalWrite(redLedPin, HIGH);
        return; // Exit the function if failed
      } else {
        // PASS
        delay(500);
        digitalWrite(relayEnable5, HIGH);  // ENABLE RELAY to switch IO
        digitalWrite(relayEnable2, HIGH);  // Enable relay to switch 4/12 to 5/13
        testPinState = digitalRead(testPin);  // Read pin state 4/12 to 5/13
        
        if (testPinState == HIGH) {
          // FAIL
          digitalWrite(redLedPin, HIGH);
          return; // Exit the function if failed
        } else {
          // PASS
          delay(500);
          digitalWrite(relayEnable2, LOW);  // Test done, power off relay
          testPinState = digitalRead(testPin);  // Read pin state 5/13 to 6/14
          
          if (testPinState == HIGH) {
            // FAIL
            digitalWrite(redLedPin, HIGH);
            return; // Exit the function if failed
          } else {
            // PASS
            delay(500);
            digitalWrite(relayEnable3, HIGH);  // Enable relay to switch 6/14 to 7/15
            testPinState = digitalRead(testPin);  // Read pin state 6/14 to 7/15
            
            if (testPinState == HIGH) {
              // FAIL
              digitalWrite(redLedPin, HIGH);
              return; // Exit the function if failed
            } else {
              // PASS
              delay(500);
              digitalWrite(relayEnable3, HIGH);  // Enable relay to switch 6/14 to 7/15
              testPinState = digitalRead(testPin);  // Read pin state 6/14 to 7/15
              
              if (testPinState == HIGH) {
                // FAIL
                digitalWrite(redLedPin, HIGH);
                return; // Exit the function if failed
              } else {
                // PASS
                delay(500);
                digitalWrite(relayEnable4, HIGH);  // Enable relay to switch 7/15 to 8/16
                testPinState = digitalRead(testPin);  // Read pin state 7/15 to 8/16
                
                if (testPinState == HIGH) {
                  // FAIL
                  digitalWrite(redLedPin, HIGH);
                  return; // Exit the function if failed
                } else {
                  // PASS
                  delay(500);
                  allPass = true;
                  digitalWrite(relayEnable4, LOW); // RELAY off
                  digitalWrite(relayEnable5, LOW); // RELAY off
                  digitalWrite(relayEnable6, LOW); // RELAY off
                  digitalWrite(greenLedPin, HIGH); // Green LED ON
                  digitalWrite(redLedPin, LOW);    // Red LED OFF
                  digitalWrite(yelLedLedPin, LOW); // Busy LED OFF
                }
              }
            }
          }
        }
      }
    }
  }
}