#include "LedControl.h"

// Initialize the LedControl object for 4 connected modules
LedControl lc = LedControl(12, 11, 10, 4);

// Define pins for ultrasonic sensor
const int echoPin = 3; // Echo pin for the ultrasonic sensor
const int trigPin = 4; // Trigger pin for the ultrasonic sensor

// 8x8 dot matrix representations of numbers 0-9
byte numbers[10][8] = {
    {0x3C, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3C}, // 0
    {0x08, 0x18, 0x38, 0x08, 0x08, 0x08, 0x08, 0x7E}, // 1
    {0x3C, 0x42, 0x40, 0x20, 0x10, 0x08, 0x04, 0x7E}, // 2
    {0x3C, 0x42, 0x40, 0x30, 0x40, 0x40, 0x42, 0x3C}, // 3
    {0x20, 0x30, 0x28, 0x24, 0x7E, 0x20, 0x20, 0x20}, // 4
    {0x7E, 0x02, 0x1E, 0x20, 0x40, 0x40, 0x42, 0x3C}, // 5
    {0x3C, 0x02, 0x02, 0x1E, 0x22, 0x42, 0x42, 0x3C}, // 6
    {0x7E, 0x40, 0x20, 0x10, 0x08, 0x08, 0x08, 0x08}, // 7
    {0x3C, 0x42, 0x42, 0x3C, 0x42, 0x42, 0x42, 0x3C}, // 8
    {0x3C, 0x42, 0x42, 0x3C, 0x40, 0x40, 0x40, 0x3C}  // 9
};

unsigned long lastMeasurementTime = 0;
const int measurementInterval = 400; // Interval for distance measurements
const int animationInterval = 40; // Refresh interval for animations
unsigned long lastAnimationTime = 0;
int targetColumnsLit = 0;
int currentCol = 0;

void setup() {
  Serial.begin(9600);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);

  // Initialize all LED modules
  for (int i = 0; i < 4; i++) {
    lc.shutdown(i, false);
    lc.setIntensity(i, 8);
    lc.clearDisplay(i);
  }
}

void loop() {
  // Measure distance
  if (millis() - lastMeasurementTime >= measurementInterval) {
    lastMeasurementTime = millis();
    float distance = readDistance();
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    // Update the displayed number based on the measured distance
    int displayValue = constrain((int)distance, 0, 99);
    displayNumber(displayValue);

    // Update the target number of columns to light up based on distance
    targetColumnsLit = calculateColumnsToLight(distance);
  }

  // Update the animation display
  updateAnimation();
}

float readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  float distance = pulseIn(echoPin, HIGH) / 58.00;
  return distance > 400 ? 400 : distance; // Limit max measurement distance
}

void displayNumber(int number) {
  lc.clearDisplay(2); // Clear display module 2
  lc.clearDisplay(3); // Clear display module 3
  if (number < 10) {
    displayDigit(2, number); // Display a single digit on module 3
  } else {
    displayDigit(2, number % 10); // Unit digit on module 2
    displayDigit(3, number / 10); // Tens digit on module 3
  }
}

void displayDigit(int module, int digit) {
  for (int row = 0; row < 8; row++) {
    byte reversed = needsMirror(digit) ? reverseByte(numbers[digit][row]) : numbers[digit][row];
    lc.setRow(module, row, reversed);
  }
}

bool needsMirror(int digit) {
  return (digit == 2 || digit == 3 || digit == 4 || digit == 5 || digit == 6 || digit == 7 || digit == 9);
}

byte reverseByte(byte b) {
  b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
  b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
  b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
  return b;
}

int calculateColumnsToLight(float distance) {
  return constrain(map(distance, 21, 0, 0, 18), 0, 16);
}

void updateAnimation() {
  if (millis() - lastAnimationTime >= animationInterval) {
    lastAnimationTime = millis();
    if (currentCol < targetColumnsLit) {
      lightUpColumn(currentCol, true);
      currentCol++;
    } else if (currentCol > targetColumnsLit) {
      currentCol--;
      lightUpColumn(currentCol, false);
    }
  }
}

void lightUpColumn(int col, bool state) {
  int matrixIndex = 1 - (col / 8);
  int colIndex = col % 8;
  for (int row = 0; row < (col / 2) + 1; row++) {
    lc.setLed(matrixIndex, 7 - row, colIndex, state);
  }
}