// Demonstrate custom formatting and scanning using the '%Z' and '%z'
// codes.  

// A custom Z print handler that expects a pointer to a char, and
// prints it out twice, for robustness.
void ZPrinter(u8 face, void * arg, bool alt, int width, bool zerofill) {
  API_ASSERT_NONNULL(arg);      // Make sure we got an arg
  char c = * (char*) arg;       // Get the underlying char
  facePrint(face,c,BYTE);       // Print it once
  facePrint(face,c,BYTE);       // Print it twice
}

// Our custom Z packet scanner.  Reads a doubled byte.
bool ZScanner(u8 * packet, void * arg, bool alt, int width) {
  u32 ch1, ch2;

  // Try for two bytes, make sure they match
  if (!packetRead(packet,ch1,BYTE)) return false;
  if (!packetRead(packet,ch2,BYTE)) return false;
  if (ch1 != ch2) return false;

  API_ASSERT_NONNULL(arg);      // Make sure we got an arg
  char * p = (char*) arg;       // Cast to right pointer type
  *p = ch1;                     // Stash the char
  return true;                  // Done.
}

void doubleDouble(u8 * packet) {
  char c;

  // Establish our custom scanner and analyze the packet.
  // Note that the %Z code does _NOT_ increment the matches count!
  if (packetScanf(packet,"%Zd%z\n", ZScanner, &c) != 3) return;

  // Echo the packet using the custom printer
  facePrintf(packetSource(packet),"%Zd%z\n", ZPrinter, &c); // Print result
}

void setup() {
  Body.reflex('d',doubleDouble);
}

void loop() {
  /* nothing to do */
}

