uint8_t enrollFingerprint(uint8_t id) {
  int p = -1;
  Serial.print("Waiting for finger to enroll...");
  
  // Retry logic for capturing image
  for (int attempt = 0; attempt < 3; attempt++) {
    p = finger.getImage();
    switch (p) {
      case FINGERPRINT_OK:
        Serial.println("Image taken");
        break;
      case FINGERPRINT_NOFINGER:
        Serial.print(".");
        break;
      case FINGERPRINT_PACKETRECIEVEERR:
        Serial.println("Error receiving packet, retrying...");
        continue;  // Retry on packet error
      case FINGERPRINT_IMAGEFAIL:
        Serial.println("Imaging error, try again.");
        return p;  // Exit on image failure
      default:
        Serial.println("Unknown error");
        return p;
    }
    if (p == FINGERPRINT_OK) break;  // Exit retry loop if image is successfully taken
  }

  // Convert and store the image template
  p = finger.image2Tz(1);
  if (p != FINGERPRINT_OK) return p;
  
  p = finger.storeModel(id);
  if (p == FINGERPRINT_OK)
    Serial.println("Fingerprint enrolled!");
  else
    Serial.println("Failed to store fingerprint.");

  return p;
}

void setup() {
  Serial.begin(115200);
  mySerial.begin(57600, SERIAL_8N1, 16, 17);  // Ensure TX/RX pins are correct
  finger.begin(57600);
  
  if (finger.verifyPassword()) {
    Serial.println("Sensor initialized.");
    enrollFingerprint(1);  // Enroll the first fingerprint
  } else {
    Serial.println("Failed to initialize sensor.");
  }
}

uint8_t enrollFingerprint(uint8_t id) {
  int p = -1;
  Serial.print("Waiting for finger to enroll...");
  
  // Retry logic for capturing image
  for (int attempt = 0; attempt < 3; attempt++) {
    p = finger.getImage();
    switch (p) {
      case FINGERPRINT_OK:
        Serial.println("Image taken");
        break;
      case FINGERPRINT_NOFINGER:
        Serial.print(".");
        break;
      case FINGERPRINT_PACKETRECIEVEERR:
        Serial.println("Error receiving packet, retrying...");
        continue;  // Retry on packet error
      case FINGERPRINT_IMAGEFAIL:
        Serial.println("Imaging error, try again.");
        return p;  // Exit on image failure
      default:
        Serial.println("Unknown error");
        return p;
    }
    if (p == FINGERPRINT_OK) break;  // Exit retry loop if image is successfully taken
  }

  // Convert and store the image template
  p = finger.image2Tz(1);
  if (p != FINGERPRINT_OK) return p;
  
  p = finger.storeModel(id);
  if (p == FINGERPRINT_OK)
    Serial.println("Fingerprint enrolled!");
  else
    Serial.println("Failed to store fingerprint.");

  return p;
}

void setup() {
  Serial.begin(115200);
  mySerial.begin(57600, SERIAL_8N1, 16, 17);  // Ensure TX/RX pins are correct
  finger.begin(57600);
  
  if (finger.verifyPassword()) {
    Serial.println("Sensor initialized.");
    enrollFingerprint(1);  // Enroll the first fingerprint
  } else {
    Serial.println("Failed to initialize sensor.");
  }
}