#include <SPI.h>
#include <LedControl.h>

#define DIN_PIN   11
#define CS_PIN    10
#define CLK_PIN   13

#define DIN_PIN2  9
#define CS_PIN2   8
#define CLK_PIN2  7

LedControl lc = LedControl(DIN_PIN, CLK_PIN, CS_PIN, 4); // DIN, CLK, CS, number of displays
LedControl lc2 = LedControl(DIN_PIN2, CLK_PIN2, CS_PIN2, 4);
const int maxCars = 10;
const int maxX = 1920;
const int maxY1 = 800; // Constraint for ymin
const int maxY = 1080;
unsigned int x1[maxCars], y1[maxCars], x2[maxCars], y2[maxCars];
int carCount = 0;
bool speedEntered = false;
int vehicleSpeed = 0;

void setup() {
  for (int i = 0; i < 4; i++) {
    lc.shutdown(i, false);
    lc.setIntensity(i, 8);
    lc.clearDisplay(i);

    lc2.shutdown(i, false);
    lc2.setIntensity(i, 8);
    lc2.clearDisplay(i);

    for (int row = 0; row < 8; row++) {
      for (int col = 0; col < 8; col++) {
        lc.setLed(i, 7 - row, 7 - col, true);
        lc2.setLed(i, 7 - row, 7 - col, true);
      }
    }
  }

  Serial.begin(9600);
  Serial.println("Enter speed as 'v speed_value' or coordinates for up to 10 cars in the format: 'w x1 y1 x2 y2' ");
}

void loop() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim(); // Remove any leading/trailing whitespace
    Serial.print("Received input: ");
    Serial.println(input);

    parseInput(input);

    if (carCount > 0) {
      applyConstraints();
      filterValidEntries();
      sortCoordinates();
      mergeCoordinates();
      printCoordinates("Merged Coordinates:");
      controlLEDs();
    } else {
      Serial.println("Speed limit less than 40, ADB is in manual mode");
      keepLEDsOn();
    }
  }
}

void parseInput(String input) {
  carCount = 0;
  int index = 0;
  char* token = strtok(input.c_str(), " ");
  while (token != NULL && carCount < maxCars) {
    if (strcmp(token, "v") == 0) {  // Check for speed input
      token = strtok(NULL, " ");
      vehicleSpeed = atoi(token); // Store the speed value
      Serial.print("Speed set to: ");
      Serial.println(vehicleSpeed);
      
      // If speed is greater than 40, expect coordinates
      if (vehicleSpeed > 40) {
        Serial.println("Speed exceeds 40, please enter coordinates for up to 6 cars in the format: 'w x1 y1 x2 y2'");
        // Wait for coordinates input
        while (carCount == 0) {
          if (Serial.available() > 0) {
            String coordsInput = Serial.readStringUntil('\n');
            Serial.print("Received coordinates input: ");
            Serial.println(coordsInput);
            parseCoordinates(coordsInput);
          }
        }
      }
      return; // Stop further processing if only speed is provided
    } else if (strcmp(token, "w") == 0) {  // Check for coordinate input
      parseCoordinates(input);  // Parse coordinates
      return;
    }
    token = strtok(NULL, " ");
  }
}


    // if (!speedEntered) {
    //   if (input.startsWith("v ")) {
    //     speed = input.substring(2).toInt();
    //     Serial.print("Entered speed: ");
    //     Serial.println(speed);

    //     if (speed > 40) {
    //       speedEntered = true;
    //       Serial.println("Speed is greater than 40 km/h. Enter coordinates for up to 6 cars in the format: w x1 y1 x2 y2");
    //     } else {
    //       Serial.println("Speed is less than or equal to 40 km/h. LEDs will remain on (Manual mode).");
    //       // Keep LEDs on (manual mode)
    //       keepLEDsOn();
    //     }
    //   } else {
    //     Serial.println("Invalid input. Please enter speed in the format 'v <value>'.");
    //   }
    // } else {
    //   parseCoordinates(input);
    //   if (carCount > 0) {
    //     applyConstraints();
    //     filterValidEntries();
    //     sortCoordinates();
    //     mergeCoordinates();
    //     printCoordinates("Merged Coordinates:");
    //     controlLEDs();
    //   } else {
    //     Serial.println("Invalid input. Please enter coordinates in the correct format.");
    //   }
    //}
  //}
//}

void parseCoordinates(String input) {
  carCount = 0;
  int index = 0;
  char* token = strtok(input.c_str(), " ");
  while (token != NULL && carCount < maxCars) {
    if (strcmp(token, "w") == 0) {
      if (index != 0) {
        carCount++;
        index = 0;
      }
      token = strtok(NULL, " ");
      continue;
    }

    if (strcmp(token, "x1") == 0) {
      token = strtok(NULL, " ");
      x1[carCount] = atoi(token);
      index++;
    } else if (strcmp(token, "y1") == 0) {
      token = strtok(NULL, " ");
      y1[carCount] = atoi(token);
      index++;
    } else if (strcmp(token, "x2") == 0) {
      token = strtok(NULL, " ");
      x2[carCount] = atoi(token);
      index++;
    } else if (strcmp(token, "y2") == 0) {
      token = strtok(NULL, " ");
      y2[carCount] = atoi(token);
      index++;
    }
    token = strtok(NULL, " ");
  }
  carCount++;
}

void applyConstraints() {
  for (int i = 0; i < carCount; i++) {
    if (x1[i] >= maxX || x2[i] >= maxX || y1[i] > maxY1 || x2[i] <= x1[i]) {
      x1[i] = 0;
      y1[i] = 0;
      x2[i] = 0;
      y2[i] = 0;
    }
  }
}

void filterValidEntries() {
  int validCount = 0;
  for (int i = 0; i < carCount; i++) {
    if (x1[i] != 0 || y1[i] != 0 || x2[i] != 0 || y2[i] != 0) {
      x1[validCount] = x1[i];
      y1[validCount] = y1[i];
      x2[validCount] = x2[i];
      y2[validCount] = y2[i];
      validCount++;
    }
  }
  carCount = validCount;
}

void sortCoordinates() {
  for (int i = 0; i < carCount - 1; i++) {
    for (int j = 0; j < carCount - i - 1; j++) {
      if (x1[j] > x1[j + 1]) {
        swap(j, j + 1);
      }
    }
  }
}

void mergeCoordinates() {
  int mergedCount = 0;
  for (int i = 0; i < carCount; i++) {
    if (x1[i] != 0 || y1[i] != 0 || x2[i] != 0 || y2[i] != 0) {
      int mergeStart = x1[i];
      int mergeEnd = x2[i];
      for (int j = i + 1; j < carCount; j++) {
        if (x1[j] <= mergeEnd) {
          mergeEnd = max(mergeEnd, x2[j]);
          x1[j] = 0;
          y1[j] = 0;
          x2[j] = 0;
          y2[j] = 0;
        }
      }
      x1[mergedCount] = mergeStart;
      y1[mergedCount] = y1[i];
      x2[mergedCount] = mergeEnd;
      y2[mergedCount] = y2[i];
      mergedCount++;
    }
  }
  carCount = mergedCount;
}

void swap(int i, int j) {
  int temp;

  temp = x1[i];
  x1[i] = x1[j];
  x1[j] = temp;

  temp = y1[i];
  y1[i] = y1[j];
  y1[j] = temp;

  temp = x2[i];
  x2[i] = x2[j];
  x2[j] = temp;

  temp = y2[i];
  y2[i] = y2[j];
  y2[j] = temp;
}

void printCoordinates(const char* title) {
  Serial.println(title);
  for (int i = 0; i < carCount; i++) {
    Serial.print("Car ");
    Serial.print(i + 1);
    Serial.print(": x1=");
    Serial.print(x1[i]);
    Serial.print(", y1=");
    Serial.print(y1[i]);
    Serial.print(", x2=");
    Serial.print(x2[i]);
    Serial.print(", y2=");
    Serial.println(y2[i]);
  }
}

void controlLEDs() {
  int numSets = 8; // Number of column sets (since each set is now 4 columns)
  int setWidth = maxX / (numSets * 2); // Width of each set (16 sets in total, 8 per matrix)

  unsigned int binaryValue = 0x0000; // 16-bit binary value with all LEDs on (0000 0000 0000 0000)

  for (int i = 0; i < carCount; i++) {
    int colMin = x1[i] / setWidth; // Calculate starting set
    int colMax = x2[i] / setWidth; // Calculate ending set

    Serial.print("Car ");
    Serial.print(i + 1);
    Serial.print(": Controlling sets from ");
    Serial.print(colMin);
    Serial.print(" to ");
    Serial.println(colMax);

    for (int col = colMin; col <= colMax; col++) {
      binaryValue |= (1U << col); // Set the bits for the columns covered by the car to 1 (LEDs off)
    }
  }

  Serial.print("Final Binary value: ");
  printBinaryValue(binaryValue);

  unsigned int decimalValue = binaryValue;
  Serial.print("Decimal value: ");
  Serial.println(decimalValue);

  // Control LEDs using the binary value
  controlLEDs1(binaryValue);
}

void printBinaryValue(unsigned int value) {
  char binaryString[17]; // 16 bits + null terminator
  for (int i = 15; i >= 0; i--) {
    binaryString[15 - i] = (value & (1 << i)) ? '1' : '0';
  }
  binaryString[16] = '\0';
  Serial.println(binaryString);
}

void controlLEDs1(unsigned int number) {
  int maxRows = 8;

  // If the number is 0xFFFF, turn off all LEDs
  if (number == 0xFFFF) {
    for (int device = 0; device < 4; device++) {
      for (int row = 0; row < maxRows; row++) {
        for (int col = 0; col < 8; col++) {
          lc.setLed(device, row, col, false);
          lc2.setLed(device, row, col, false);
        }
      }
    }
    return;
  }

  // Iterate through each bit
  for (int set = 0; set < 16; set++) {
    bool turnOff = !bitRead(number, set); // Read the bit value (0 or 1) and invert it

    int firstColumn = set * 4; // Calculate the first column of the set
    int lastColumn = firstColumn + 3; // Calculate the last column of the set

    for (int row = 0; row < maxRows; row++) {
      for (int col = firstColumn; col <= lastColumn; col++) {
        int deviceIndex = col / 8; // Determine the device for the column
        int columnIndex = col % 8; // Determine the column index within the device
        if (deviceIndex < 4) {
          lc.setLed(deviceIndex, row, columnIndex, turnOff); // Control the LED in lc
        } else {
          lc2.setLed(deviceIndex - 4, row, columnIndex, turnOff); // Control the LED in lc2
        }
      }
    }
  }
}

void keepLEDsOn() {
  for (int device = 0; device < 4; device++) {
    for (int row = 0; row < 8; row++) {
      for (int col = 0; col < 8; col++) {
        lc.setLed(device, row, col, true);
        lc2.setLed(device, row, col, true);
      }
    }
  }
}