// Pin definitions for the FlexiForce sensors
#define FLEXI_FORCE_PIN_1 A0
#define FLEXI_FORCE_PIN_2 A1
#define FLEXI_FORCE_PIN_3 A2

// Pin definitions for other digital pins
#define PIN_P_1 6
#define PIN_P_2 5
#define PIN_P_3 3

// Constants for size of arrays and threshold
#define ARRAY_SIZE 9
#define THRESHOLD 90 //sucess at 30 and 50 too but 90 was more consistent

// Arrays to hold sensor readings and boolean states
int valArray[ARRAY_SIZE];
int valInitArray[ARRAY_SIZE];
bool boolArray[ARRAY_SIZE];

// Variable to hold the sum of boolean states
int sum = 0;

// Initial setup routine
void setup() {
  // Begin serial communication
  Serial.begin(9600);
  
  // Set pin modes to output
  pinMode(PIN_P_1, OUTPUT);
  pinMode(PIN_P_2, OUTPUT);
  pinMode(PIN_P_3, OUTPUT);

  // Initialize pins and values
  initializePins();
  initializeValues();
}

// Main loop routine
void loop() {
  // Initialize pins and update values
  initializePins();
  updateValues();

  // Update boolean array and print it
  updateBoolArray();
  printBoolArray();
  
  // Check conditions of walls
  checkWallConditions();
  
  // Print new line
  Serial.println();
}

// Function to initialize pins to low
void initializePins() {
  digitalWrite(PIN_P_1,LOW);
  digitalWrite(PIN_P_2,LOW);
  digitalWrite(PIN_P_3,LOW);
}

// Function to initialize values by reading from sensors
void initializeValues() {
  updateValuesWithPinHigh(PIN_P_3, valInitArray, 6, 0, 3);
  updateValuesWithPinHigh(PIN_P_2, valInitArray, 7, 1, 4);
  updateValuesWithPinHigh(PIN_P_1, valInitArray, 8, 2, 5);
}

// Function to update values by reading from sensors
void updateValues() {
  updateValuesWithPinHigh(PIN_P_3, valArray, 6, 0, 3);
  updateValuesWithPinHigh(PIN_P_2, valArray, 7, 1, 4);
  updateValuesWithPinHigh(PIN_P_1, valArray, 8, 2, 5);
}

// Function to read sensor values with the given pin set high
void updateValuesWithPinHigh(int pin, int* array, int index1, int index2, int index3) {
  digitalWrite(pin, HIGH);
  array[index1] = analogRead(FLEXI_FORCE_PIN_1);
  array[index2] = analogRead(FLEXI_FORCE_PIN_2);
  array[index3] = analogRead(FLEXI_FORCE_PIN_3);
  digitalWrite(pin, LOW);
}

// Function to update boolean array based on threshold
void updateBoolArray() {
  sum = 0;
  for(int k = 0; k < ARRAY_SIZE; k++){
    if(valArray[k] > valInitArray[k] + THRESHOLD) {
      boolArray[k] = 1;
      sum++;
    } else {
      boolArray[k] = 0;
    }
  }
}

// Function to print the boolean array
void printBoolArray() {
  for(int i = 0; i < 3; i++) {
    for(int j = 0; j < 3; j++) {
      Serial.print(boolArray[3*i + j]);
      Serial.print("\t");
    }
    Serial.println();
  }
}

// Function to check conditions of walls based on boolean array
void checkWallConditions() {
  // Check conditions based on sum of boolean states
  // Print the corresponding wall condition
   if(sum == 3){
    if(boolArray[0] + boolArray[1] + boolArray[2] == 3){
      Serial.println("Wall Top");
    }else if(boolArray[3] + boolArray[4] + boolArray[5] == 3){
      Serial.println("Wall Middle Hori");
    }else if(boolArray[6] + boolArray[7] + boolArray[8] == 3){
      Serial.println("Wall Bottom");
    }
    if(boolArray[0] + boolArray[3] + boolArray[6] == 3){
      Serial.println("Wall Left");
    }else if(boolArray[1] + boolArray[4] + boolArray[7] == 3){
      Serial.println("Wall Middle Vert ");
    }else if(boolArray[2] + boolArray[5] + boolArray[8] == 3){
      Serial.println("Wall Right");
    }
  }else if(sum == 4) {
    if(boolArray[0] + boolArray[1] + boolArray[3] + boolArray[4] == 4){
      Serial.println("Bottom Right Edge");
    }else if(boolArray[1] + boolArray[2] + boolArray[4] + boolArray[5] == 4){
      Serial.println("Bottom Left edge");
    }else if(boolArray[6] + boolArray[7] + boolArray[3] + boolArray[4] == 4){
      Serial.println("Top Right Edge");
    }else if(boolArray[8] + boolArray[7] + boolArray[5] + boolArray[4] == 4){
      Serial.println("Top Left Edge");
    }
  }else if(sum == 5) {
    if(boolArray[0] + boolArray[1] + boolArray[3] + boolArray[4] == 0){
      Serial.println("Bottom Right Corner");
    }else if(boolArray[1] + boolArray[2] + boolArray[4] + boolArray[5] == 0){
      Serial.println("Bottom Left Corner");
    }else if(boolArray[6] + boolArray[7] + boolArray[3] + boolArray[4] == 0){
      Serial.println("Top Right Corner");
    }else if(boolArray[8] + boolArray[7] + boolArray[5] + boolArray[4] == 0){
      Serial.println("Top Left Corner");
    }
  }else if(sum == 6){
    if(boolArray[0] + boolArray[1] + boolArray[2] == 0){
      Serial.println("Top Edge");
    }else if(boolArray[3] + boolArray[4] + boolArray[5] == 0){
      Serial.println("Middle Hori Edge");
    }else if(boolArray[6] + boolArray[7] + boolArray[8] == 0){
      Serial.println("Bottom Edge");
    }
    if(boolArray[0] + boolArray[3] + boolArray[6] == 0){
      Serial.println("Left Edge");
    }else if(boolArray[1] + boolArray[4] + boolArray[7] == 0){
      Serial.println("Middle Vert Edge ");
    }else if(boolArray[2] + boolArray[5] + boolArray[8] == 0){
      Serial.println("Right Edge");
    }
  }

}