// Transform 4x4 Servo Digit Display - MIT
#include <ESP32Servo.h>
#include <math.h>

// ------------------------ Servo PIN declaration ------------------------

#define servo_0_0_pin 16
#define servo_0_1_pin 4
#define servo_0_2_pin 2
#define servo_0_3_pin 15

#define servo_1_0_pin 19
#define servo_1_1_pin 18
#define servo_1_2_pin 5
#define servo_1_3_pin 17

#define servo_2_0_pin 26
#define servo_2_1_pin 25
#define servo_2_2_pin 33
#define servo_2_3_pin 32

#define servo_3_0_pin 13
#define servo_3_1_pin 12
#define servo_3_2_pin 14
#define servo_3_3_pin 27

// ------------------------ Matrix setup ---------------------------------

const int numRows = 4;
const int numCols = 4;

const int minAngle = 0;    // OFF
const int maxAngle = 90;   // ON

Servo servos[numRows][numCols];

int servoPins[numRows][numCols] = {
  {servo_0_0_pin, servo_0_1_pin, servo_0_2_pin, servo_0_3_pin},
  {servo_1_0_pin, servo_1_1_pin, servo_1_2_pin, servo_1_3_pin},
  {servo_2_0_pin, servo_2_1_pin, servo_2_2_pin, servo_2_3_pin},
  {servo_3_0_pin, servo_3_1_pin, servo_3_2_pin, servo_3_3_pin}
};

// ------------------------ Digit bitmaps (0–9) --------------------------

const int DIGITS[10][4][4] = {
  { {1,1,1,1}, {1,0,0,1}, {1,0,0,1}, {1,1,1,1} }, // 0
  { {0,1,0,0}, {1,1,0,0}, {0,1,0,0}, {1,1,1,0} }, // 1
  { {1,1,1,1}, {0,0,0,1}, {1,1,1,1}, {1,0,0,0} }, // 2
  { {1,1,1,1}, {0,0,1,1}, {0,0,1,1}, {1,1,1,1} }, // 3
  { {1,0,0,1}, {1,0,0,1}, {1,1,1,1}, {0,0,0,1} }, // 4
  { {1,1,1,1}, {1,0,0,0}, {1,1,1,1}, {0,0,0,1} }, // 5
  { {1,1,1,1}, {1,0,0,0}, {1,1,1,1}, {1,0,0,1} }, // 6
  { {1,1,1,1}, {0,0,0,1}, {0,0,1,0}, {0,1,0,0} }, // 7
  { {1,1,1,1}, {1,0,0,1}, {1,1,1,1}, {1,0,0,1} }, // 8
  { {1,1,1,1}, {1,0,0,1}, {1,1,1,1}, {0,0,0,1} }  // 9
};

// ------------------------ Functions ------------------------------------

void clearDisplay() {
  for (int r = 0; r < numRows; r++) {
    for (int c = 0; c < numCols; c++) {
      servos[r][c].write(minAngle);
    }
  }
}

void showDigit(int digit) {
  if (digit < 0 || digit > 9) return;

  for (int r = 0; r < numRows; r++) {
    for (int c = 0; c < numCols; c++) {
      int angle = DIGITS[digit][r][c] ? maxAngle : minAngle;
      servos[r][c].write(angle);
      delay(5); // small smoothing delay
    }
  }
}

// ------------------------ Setup ----------------------------------------

void setup() {
  Serial.begin(115200);
  Serial.println("4x4 Servo Digit Display Ready");
  Serial.println("Send digits 0–9 via Serial Monitor");

  for (int r = 0; r < numRows; r++) {
    for (int c = 0; c < numCols; c++) {
      servos[r][c].attach(servoPins[r][c]);
    }
  }

  clearDisplay();
}

// ------------------------ Loop -----------------------------------------

void loop() {
  if (Serial.available()) {
    char ch = Serial.read();

    if (ch >= '0' && ch <= '9') {
      int digit = ch - '0';
      Serial.print("Displaying: ");
      Serial.println(digit);
      showDigit(digit);
    }
  }
}