//##########################################################################################################################################################################################################################
//### Initialize
//##########################################################################################################################################################################################################################

//MODULE 2 - WIRES
//#############################################################################################################
const int analogPins[6] = {A1, A2, A3, A4, A5, A6};
const int ledPin = LED_BUILTIN;
const int threshold = 1000;//Threshold to determine if wire is disconnected

// Define constants for wire colors
const int RED = 0;
const int BLUE = 1;
const int WHITE = 2;
const int YELLOW = 3;
const int BLACK = 4;
const int UNKNOWN = 5;

// Define expected analog value ranges for each color with tolerance
const int RED_MIN = 85, RED_MAX = 103;
const int BLUE_MIN = 169, BLUE_MAX = 199;
const int WHITE_MIN = 304, WHITE_MAX = 349;
const int YELLOW_MIN = 485, YELLOW_MAX = 536;
const int BLACK_MIN = 686, BLACK_MAX = 772;

int wireColors[6];
int numberOfWires;
int correctWire = -1;

//##########################################################################################################################################################################################################################
//### SETUP
//##########################################################################################################################################################################################################################

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

  //MODULE 2 - WIRES
  // Initialize wire pins as inputs
  pinMode(ledPin, OUTPUT);

    // Read initial wire colors
    numberOfWires = 0;
    for (int i = 0; i < 6; i++) {
        int value = analogRead(analogPins[i]);
      	Serial.print(value);
      	Serial.print(": ");
        if (value < threshold) {
            wireColors[numberOfWires] = getColor(value);
            Serial.print("Wire ");
            Serial.print(numberOfWires);
            Serial.print(" color: ");
            Serial.println(wireColors[numberOfWires]);
            numberOfWires++;
        }
    }

    // Determine which wire to cut
    determineCorrectWire();

    Serial.print("Number of wires: ");
    Serial.println(numberOfWires);
    Serial.print("Correct wire to cut: ");
    Serial.println(correctWire);
}

//##########################################################################################################################################################################################################################
//### FUNCTIONS
//##########################################################################################################################################################################################################################

//#############################################################################################################
//### MODULE 2 - WIRES
//#############################################################################################################
// Function to map analog values to wire colors
int getColor(int analogValue) {
    if (analogValue >= RED_MIN && analogValue <= RED_MAX) return RED;
    else if (analogValue >= BLUE_MIN && analogValue <= BLUE_MAX) return BLUE;
    else if (analogValue >= WHITE_MIN && analogValue <= WHITE_MAX) return WHITE;
    else if (analogValue >= YELLOW_MIN && analogValue <= YELLOW_MAX) return YELLOW;
    else if (analogValue >= BLACK_MIN && analogValue <= BLACK_MAX) return BLACK;
    else return UNKNOWN;
}

// Function to find the correct wire to cut based on the given rules
void determineCorrectWire() {
    int redCount = 0, blueCount = 0, yellowCount = 0, whiteCount = 0, blackCount = 0;

    for (int i = 0; i < numberOfWires; i++) {
        switch (wireColors[i]) {
            case RED: redCount++; break;
            case BLUE: blueCount++; break;
            case WHITE: whiteCount++; break;
            case YELLOW: yellowCount++; break;
            case BLACK: blackCount++; break;
            default: break;
        }
    }

    // Serial number's last digit (for simplicity, we'll use a fixed value)
    int serialLastDigit = 3; // Example serial number's last digit

    if (numberOfWires == 3) {
        if (redCount == 0) correctWire = 1;
        else if (wireColors[2] == WHITE) correctWire = 2;
        else if (blueCount > 1) correctWire = 2; // Last blue wire
        else correctWire = 2;
    }
    else if (numberOfWires == 4) {
        if (redCount > 1 && serialLastDigit % 2 != 0) correctWire = 3; // Last red wire
        else if (wireColors[3] == YELLOW && redCount == 0) correctWire = 0;
        else if (blueCount == 1) correctWire = 0;
        else if (yellowCount > 1) correctWire = 3;
        else correctWire = 1;
    }
    else if (numberOfWires == 5) {
        if (wireColors[4] == BLACK && serialLastDigit % 2 != 0) correctWire = 3;
        else if (redCount == 1 && yellowCount > 1) correctWire = 0;
        else if (blackCount == 0) correctWire = 1;
        else correctWire = 0;
    }
    else if (numberOfWires == 6) {
        if (yellowCount == 0 && serialLastDigit % 2 != 0) correctWire = 2;
        else if (yellowCount == 1 && whiteCount > 1) correctWire = 3;
        else if (redCount == 0) correctWire = 5;
        else correctWire = 3;
    }
}


//##########################################################################################################################################################################################################################
//### Loop
//##########################################################################################################################################################################################################################

void loop() {
  
  //MODULE 2 - WIRES
  //#############################################################################################################
  // Monitor for disconnection of the correct wire
    if (correctWire != -1) {
        int value = analogRead(analogPins[correctWire]);
        if (value > threshold) {
            digitalWrite(ledPin, HIGH); // Turn on LED if correct wire is disconnected
        } else {
            digitalWrite(ledPin, LOW); // Turn off LED otherwise
        }
    }

    delay(100);


}