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

// A sample struct.  We will always pass a pointer to a TwoD to our
// custom Z handlers.
// Our I/O format for a TwoD with x==3 and y==-2 will be '(3,-2)'.
struct TwoD {                   // Simple 2D coords based on shorts
  s16 x;                        // +-32K x range
  s16 y;                        // +-32K y range
};

// Our custom Z print handler.  In this example we care about only the
// 'face' and 'arg' arguments, but a more sophisticated handler might
// look at the other arguments as well, which provide more information
// about the requested formatting.  (For example:
//   Format code      alt      width     zerofill
//       %z          false       0        false
//       %3z         false       3        false
//       %07z        false       7        true
//       %70z        false      70        false
//       %#z         true        0        false
//       %#0z        true        0        true
// )
void TwoDZPrinter(u8 face, void * arg, bool alt, int width, bool zerofill) {
  API_ASSERT_NONNULL(arg);      // Make sure we got an arg
  TwoD t = * (TwoD*) arg;       // Cast to the struct we require
  facePrint(face,"(");          // Start the coored format
  facePrint(face,(long) t.x);   // Print the x component, signed
  facePrint(face,",");          // Separate the coords
  facePrint(face,(long) t.y);   // Print the y component, signed
  facePrint(face,")");          // End the coord format
}

// Our custom Z packet scanner.  Inverts TwoDZPrinter.  Returns true
// iff all went well.
bool TwoDZScanner(u8 * packet, void * arg, bool alt, int width) {
  u32 ch;
  int x, y;

  // First, check formatting and collect all the data
  if (!packetRead(packet,ch,BYTE) || ch != '(') return false;
  if (!packetRead(packet,x,DEC) || x < S16_MIN || x > S16_MAX) return false;
  if (!packetRead(packet,ch,BYTE) || ch != ',') return false;
  if (!packetRead(packet,y,DEC) || y < S16_MIN || y > S16_MAX) return false;
  if (!packetRead(packet,ch,BYTE) || ch != ')') return false;

  // As a convenience, this scanner allows passing a null pointer as a
  // flag meaning 'read but then discard a TwoD'.  If we didn't care
  // about that, do API_ASSERT_NONNULL(arg) here instead of 'if'.

  if (arg) {                    // If they gave us a pointer
    TwoD * tp = (TwoD*) arg;    // Use it
    tp->x = x;
    tp->y = y;
  }
  return true;                  // Success!
}

void negateCoord(u8 * packet) {
  TwoD c;                       

  // Establish our custom scanner and analyze the packet.
  // Note that the %Z code does _NOT_ increment the matches count!
  if (packetScanf(packet,"%Zn%z\n", TwoDZScanner, &c) != 3) {
    logNormal("Failed at %d\n",packetCursor(packet)); // Say where the parse died..
    return;
  }
  c.x = -c.x;                   // Negate the coord 
  c.y = -c.y;                   // they gave us

  // Establish our custom printer and format a response.  
  // Note that we must pass a _POINTER_ to the value that is to be
  // sent to the custom z printer!
  facePrintf(packetSource(packet),"n%Z%z\n", TwoDZPrinter, &c); // Print result
}

void setup() {
  Body.reflex('n',negateCoord);
}

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

