/*                                              -*- mode:C++; fill-column:100 -*-
  SFBSerial.cpp - Low-level serial support
  Copyright (C) 2008-2009 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "SFBSerial.h"
#include "SFBRandom.h"          /* For random(u32) */
#include "SFBCodeFlow.h"        /* For programServerDispatcher, programServerInit */
#include "SFBAssert.h"
#include "SFBPrintf.h"          /* For packetScanf */ 
#include "SFBReflex.h"

const u32 standardBaudRates[] = {
         300, // 0
        1200, // 1
        9600, // 2 (Initial, default rate)
       57600, // 3
      115200, // 4 (Standard interSFB rate)
      230400, // 5
      500000, // 6
     1000000, // 7
     2000000, // 8
     3000000  // 9
};


void SFBSerial::setBaudRate(u32 baud, bool isOddParity) {
  if (getBaud() != baud || getOddParity() != isOddParity) {

    this->baud = baud;
    hwSerial.start(baud,isOddParity);

    //    hwSerial.setBaudRate(baud,isOddParity);
    setOddParity(isOddParity);

    //    if (face != NORTH)
    //      facePrintf(NORTH,"Lsbr %s %d %d\n",FACE_NAME(face),baud,isOddParity);
  }
}

void SFBSerial::setPreferredBaudCode(u8 code, bool renegotiateNow) {
  API_ASSERT_MAX(code,sizeof(standardBaudRates)/sizeof(standardBaudRates[0]));
  API_ASSERT(code >= hwSerial.getLowBaudCode() && code <= hwSerial.getHighBaudCode(),E_API_BAD_BAUD);
  if (preferredCode != code) {
    preferredCode = code;
    if (renegotiateNow)
      lowPrintOurBRO();
  }
}

void SFBSerial::lowPrintOurBRO() {
  if (backgroundProcessingActive()) {
    lowPrint("SB");
    lowPutc('0'+preferredCode);
    lowPutc('0'+hwSerial.getLowBaudCode());
    lowPutc('0'+hwSerial.getHighBaudCode());
    if (hwSerial.getPreferOddParity())
      lowPutc('o');
    lowTerpri();
  }
}

#if 0
#define BLOCKING_WRITE_BYTE(baseAddress,byte)                             \
  do {                                                                    \
     while (!CAN_WRITE(baseAddress)) { /* block */ }                      \
     WRITE_BYTE(baseAddress,byte);     /* write */                        \
  } while (0)

#define BLOCKING_READ_BYTE(baseAddress,byte)                              \
  do {                                                                    \
     while (!CAN_READ(baseAddress)) { /* block */ }                       \
     (byte) = READ_BYTE(baseAddress);                                     \
  } while (0)

#define NON_BLOCKING_READ_BYTE(baseAddress) \
 (CAN_READ(baseAddress)?READ_BYTE(baseAddress):-1)
#endif

u8 * SFBSerial::readPacket() {
  API_ASSERT((flags&(FLAG_RUNNING|FLAG_CONSCIOUS))==(FLAG_RUNNING|FLAG_CONSCIOUS), E_API_ILLEGAL_STATE);
  return hwSerial.getFrame().pollForRxPacket(); 
}

bool SFBSerial::available() {
  API_ASSERT(flags&FLAG_BLOCKING_BYTES, E_API_ILLEGAL_STATE);

  if (haveBufferedByte)
    return true;

  u8 * packet = hwSerial.getFrame().pollForRxPacket(); 
  if (!packet)
    return false;                  // Nothing yet

  if (packetFlags(packet) != 0 || packetLength(packet) != 1)
    return false;

  bufferedByte = packet[0];
  return haveBufferedByte = true;
}

void SFBSerial::flush() {
  haveBufferedByte = false;
  hwSerial.getFrame().resetRx();
}

int SFBSerial::peek() {
  API_ASSERT(flags&FLAG_BLOCKING_BYTES, E_API_ILLEGAL_STATE);

  if (!available())
    return -1;

  return (int) bufferedByte;
}

int SFBSerial::read() {
  API_ASSERT(flags&FLAG_BLOCKING_BYTES, E_API_ILLEGAL_STATE);

  if (!available())
    return -1;

  haveBufferedByte = false;

  return (int) bufferedByte;
}

bool SFBSerial::ready() {
  return (flags&(FLAG_CONSCIOUS|FLAG_BLOCKING_BYTES|FLAG_BRN_DONE))!=0;
}

bool SFBSerial::isInUse() { return flags&(FLAG_RUNNING|FLAG_GPIOS); }

bool SFBSerial::isManualMode() {
  return (flags&(FLAG_RUNNING|FLAG_BLOCKING_BYTES))==(FLAG_RUNNING|FLAG_BLOCKING_BYTES); 
}

bool SFBSerial::isReflexMode() {
  return (flags&(FLAG_RUNNING|FLAG_CONSCIOUS|FLAG_BLOCKING_BYTES))==FLAG_RUNNING;
}

bool SFBSerial::isConsciousMode() {
  return (flags&(FLAG_RUNNING|FLAG_CONSCIOUS|FLAG_BLOCKING_BYTES))==(FLAG_RUNNING|FLAG_CONSCIOUS);
}

bool SFBSerial::backgroundProcessingActive() {
  return isReflexMode();
}

void SFBSerial::lowPrint(const char * str) { 
  u8 ch;
  while ((ch = *str++)) 
    lowPutc(ch);
}
void SFBSerial::lowPrintln(const char * str) { 
  lowPrint(str);
  lowTerpri();
}
void SFBSerial::maybeBlockForOutput() {
  while (flags&FLAG_BLOCKING_BYTES && !hwSerial.getFrame().txCanAddByte())
    delay(1);
}
void SFBSerial::lowPutc(const u8 byte) { 
  maybeBlockForOutput();
  hwSerial.getFrame().txDataByte(byte);
}

void SFBSerial::lowTerpri() { 
  maybeBlockForOutput();
  hwSerial.getFrame().txEndPacket(); 
  hwSerial.checkTx(); 
}

void SFBSerial::putcInterrupt(const u8 byte) { if (ready()) lowPutc(byte); }

void SFBSerial::terpriInterrupt() { if (ready()) lowTerpri(); }

void SFBSerial::putcBlocking(const u8 byte) { hwSerial.putcBlocking(byte); }

void SFBSerial::terpriBlocking() { putcBlocking('\n'); }

SFBSerial::SFBSerial(int face) :
  hwSerial(face),
  baud(0),                      // Anything != SFBAUD_RENDEZVOUS is safe here, I think
  nextTimeout(0),
  alarmNumber(0),
  face(face),
  flags(0),
  state(STATE_INIT),
  preferredCode(hwSerial.getPreferredBaudCode()),
  selectedCode(0),
  selectedOdd(false),
  haveBufferedByte(false),
  bufferedByte(0)
{ }

/**
   Reserve the RX and TX pins of this face for general-purpose I/Os, instead of serial
   communications.  This method is one of the four ways of using the RX and TX pins on an SFB face;
   the other three are:
   
   - Packet communication under "full reflex control" with interrupt-based I/O, via #begin().

   - Packet communication under "conscious control" with interrupt-based I/O, via #begin(u32,bool)
     with the second argument explicitly set to \c false.

   - Byte-by-byte communication under "manual control" with no packet dispatching, via
     #begin(u32,bool) with the second argument omitted or set to \c true;

   Although there is no explicit 'end()' method to bookend a 'begin()', calling \c reserve() has
   that effect.  After calling \c reserve(), the #FACE_RX_PIN and #FACE_TX_PIN can be configured and
   used like any other GPIOs.

  \sideeffect The serial communications (UART) hardware in the #SFBHWSerial supporting this object
              is disconnected and powered down, and internal flags are set to record that the pins
              are reserved for GPIO usage.

  \usage
  \code
    void setup() {
      WestFace.reserve();              // Don't do serial on the west face (say we need those pins)
      pinMode(WEST_RX_PIN, OUTPUT);    // Make WEST RX an output (even though it's normally an input)..
      pinMode(WEST_TX_PIN, INPUT);     // ..and vv for WEST TX -- just to show we can.
    }
    void loop() {
      digitalWrite(WEST_RX_PIN, digitalRead(WEST_TX_PIN));  // Do whatever with our digital pins..
      println("Are we there yet?");    // Will go out all faces _except_ WestFace, which
      delay(1000);                     // we reserved in setup()..
    }
  \endcode

 */
void SFBSerial::reserve() {
  stop();
  flags = FLAG_GPIOS;           // Clear all flags except GPIOS
}

/**
   Start packet communication, operating under "full reflex control" with interrupt-based I/O.  This
   method is one of the four ways of using the RX and TX pins on an SFB face; the other three are:
   
   - Packet communication under "conscious control" with interrupt-based I/O, via #begin(u32, bool)
     with the second argument explicitly set to \c false.

   - Byte-by-byte communication under "manual control" with no packet dispatching, via
     #begin(u32,bool) with the second argument omitted or set to \c true;

   - RX and TX used as GPIO pins rather than a serial port, via #reserve().

   "Full reflex control" as initiated by this method is the default mode of operation for the four
   faces.  During initialization, \e after the sketch \c setup() routine has returned, this method
   is called automatically on any of the four SFB faces (#NorthFace, #SouthFace, #EastFace,
   #WestFace) that were \e not initialized in any of the other three ways.  

  \sideeffect The serial communications hardware in the #SFBHWSerial supporting this object is
              configured appropriately, and its transmit and receive buffers are
              SFBPacketIO::reset().  The internal state machine is initialized, starting the baud
              rate negotiation protocol, which commences by setting the baud to the
              #SFBAUD_RENDEZVOUS rate, and then (asynchronously, after this method has returned)
              transmitting "probe packets" and listening for any responses.

  \usage
  \code
    void setup() {
      NorthFace.begin();                   // Start a face explicitly now

      while (NorthFace.ready() == false)   // Wait until some SFB-aware entity has
        delay(1);                          // successfully negotiated with us

      NorthFace.println("foo");            // Then say something to them
    }

    ...
  \endcode

 */
void SFBSerial::begin() {      
  start(standardBaudRates[SFBAUD_RENDEZVOUS],0);
}

/**
   Start packet communication, operating under "conscious" control or in "manual" byte-at-a-time
   mode.  Conscious control is selected if the \a manual argument is specified as \c false; manual
   mode is selected if \a manual is omitted or specified as \c true.

   This method provides access to two of the four ways of using the RX and TX pins on an SFB face;
   the other two are:
   
   - Packet communication under "full reflex control" with interrupt-based I/O, via #begin().

   - RX and TX used as GPIO pins rather than a serial port, via #reserve().

   The differences between "full reflex control" as initiated by #begin(), and "conscious control"
   as can be initiated by this method, are:

   - Conscious control selects the baud rate explicitly, and so, unlike full reflex control, baud
     rate negotiation is not performed, and

   - Full reflex control triggers reflex handlers for received packets automatically (during
     ::delay() calls, and in between calls to the sketch \c loop() method), but conscious control
     does not.  In conscious control the sketch must deliberately check for and handle available
     packets (via #readPacket and/or #dispatch()).  If the sketch fails to call either method
     frequently enough to respond to arrived packets, the face's input buffer will fill and further
     packets will be discarded until room is available.

   Under manual control, normal line-oriented packet processing and dispatching is circumvented, and
   instead each byte sent or received is treated as a packet with a #packetLength() of 1.  Outgoing
   bytes are sent literally with no framing or escaping, and incoming single-byte 'packets' can be
   accessed by calling #read().  Note that all normal SFB packet communication is inherently
   line-buffered, and such manual control is the \e only mechanism (provided by the library) when
   byte-at-a-time round trip communications is required.

   \param baud The baud rate to configure the hardware.  The SFB is not limited to "standard baud
               rates"; on the board itself arbitrary baud rates in the range of perhaps 110 baud to
               230,400 baud (or possibly even higher, depending on total interrupt load) can be
               specified.

   \param manual If \c true, byte-at-a-time I/O is selected; if \c false, normal packet-oriented I/O
                 is selected.

   \sideeffect The serial communications hardware in the #SFBHWSerial supporting this object is
                configured appropriately, its transmit and receive buffers are SFBFrame::reset(),
                and the baud rate is set as indicated.  The internal state machine for baud rate
                negotiation is shut down.

  \usage
  \code
    void setup() {
      EastFace.begin(9600,false);          // Start east face at 9600 baud, conscious control
    }
    void MyDispatcher(u8 * packet, u8 face) {            // A special routine to 'dispatch' east packets
      if (packetLength(packet) > 0 && packet[0] == 'x')  // Check for our Packets Of Interest 
        println("Received 'x' packet from east");        // Announce arrival to everybody
    }
    void loop() {
      EastFace.println("status");          // Talk to whatever's out there
      EastFace.dispatch(MyDispatcher, 10); // Call MyDispatcher up to 10 times, if packets are available
      delay(1000);                         // Wait a second
    }
  \endcode
  \code
    void setup() {
      SouthFace.begin(300);                // Start South at 300 baud, under byte-at-a-time manual control
    }
    void loop() {
      int ch;
      ch = SouthFace.read();               // See if a byte is available
      if (ch >= 0) {                       // We got something!  Do work on it
        if (ch >= 'A' && ch <= 'A')        // If upper case..
          ch += 'a'-'A';                   // ..convert it to lower case
        SouthFace.print((u8) ch, BYTE);    // and echo it back to them
      }
    }
  \endcode
 */
void SFBSerial::begin(u32 baud, bool manual) {
  u8 startFlags = FLAG_CONSCIOUS;
  if (manual)
    startFlags |= FLAG_BLOCKING_BYTES;
  start(baud,startFlags);
}

void SFBSerial::setState(u8 newState, u32 aboutHowManyMs) {
  state = newState;
  nextTimeout = millis() + random(aboutHowManyMs) + (aboutHowManyMs>>1);
  Alarms.set(alarmNumber, nextTimeout);
}

static void facePrintSerialInterrupt(u8 face, u8 byte) {
  SFBSerial & f = SFBHWSerial::getFace(face);
  f.putcInterrupt(byte);
}
static void facePrintlnSerialInterrupt(u8 face) {
  SFBSerial & f = SFBHWSerial::getFace(face);
  f.terpriInterrupt();
}
static void facePrintFaceCodeSerialInterrupt(u8 face) {
  facePrint(face,FACE_CODE(face));
}
static const char * faceGetFaceNameSerialInterrupt(u8 face) {
  return FACE_NAME(face);
}
static const FacePrinter serialInterruptPrinter = {
  facePrintSerialInterrupt,
  facePrintlnSerialInterrupt,
  facePrintFaceCodeSerialInterrupt,
  faceGetFaceNameSerialInterrupt
};


static void facePrintSerialBlocking(u8 face, u8 byte) {
  SFBSerial & f = SFBHWSerial::getFace(face);
  f.putcBlocking(byte);
}
static void facePrintlnSerialBlocking(u8 face) {
  SFBSerial & f = SFBHWSerial::getFace(face);
  f.terpriBlocking();
}
static void facePrintFaceCodeSerialBlocking(u8 face) {
  facePrint(face,FACE_CODE(face));
}
static const char * faceGetFaceNameSerialBlocking(u8 face) {
  return FACE_NAME(face);
}
static const FacePrinter serialBlockingPrinter = {
  facePrintSerialBlocking,
  facePrintlnSerialBlocking,
  facePrintFaceCodeSerialBlocking,
  faceGetFaceNameSerialBlocking
};
void SFBSerial::switchToBlocking() {
  faceUnsetPrinter(face);
  faceSetPrinter(face,&serialBlockingPrinter);
  flags |= FLAG_BLOCKING_BYTES;
}

static void facePrintAllFacesInternal(u8, u8 byte) {
  for (u32 face = NORTH; face <= WEST; ++face) {
    SFBSerial & f = SFBHWSerial::getFace(face);
    if (!f.isManualMode())
      f.print(byte,BYTE);
  }
}
static void facePrintlnAllFacesInternal(u8) {
  for (u32 face = NORTH; face <= WEST; ++face) {
    SFBSerial & f = SFBHWSerial::getFace(face);
    if (!f.isManualMode())
      f.println();
  }
}
static void facePrintFaceCodeAllFacesInternal(u8) {
  for (u32 face = NORTH; face <= WEST; ++face) 
    facePrint(face,FACE_CODE(face));
}
static const char * faceGetFaceNameAllFacesInternal(u8) {
  return "ALL_FACES";
}
static const FacePrinter allFacesInternalPrinter = {
  facePrintAllFacesInternal,
  facePrintlnAllFacesInternal,
  facePrintFaceCodeAllFacesInternal,
  faceGetFaceNameAllFacesInternal
};

static u32 max(u32 a, u32 b) {
  return (a > b)?a:b;
}
static u32 min(u32 a, u32 b) {
  return (a < b)?a:b;
}

bool zPacketPrefix(u8 * p1, const char * to) {
  API_ASSERT_VALID_PACKET(p1);
  API_ASSERT(to, E_API_NULL_POINTER);
  while (*p1 && *to) {
    if (*p1++ != (u8) *to++)
      return false;
  }
  return *to == 0;
}

bool packetPrefix(u8 * p1, const u8 * to, const u32 length) {
  API_ASSERT_VALID_PACKET(p1);
  API_ASSERT(to, E_API_NULL_POINTER);
  u32 len = length;
  if (len > packetLength(p1)) return false;
  while (len--) {
    if (*p1++ != *to++)
      return false;
  }
  return true;
}

bool SFBSerial::stepInit(u32 , u8 * spacket) {
  if (spacket)                /* Ignore packets now, too early */
    return false;

  flags &= ~(FLAG_CONNECTED|FLAG_BRN_DONE); /* Clear packet-processing flags */

  //  if (face != NORTH)
  //    facePrintf(NORTH,"Linit %s b%d odd%d\n",FACE_NAME(face),baud,getOddParity());

  //hwSerial.start(standardBaudRates[SFBAUD_RENDEZVOUS],false,false);
  setBaudRate(standardBaudRates[SFBAUD_RENDEZVOUS],false); // Ensure we're at the guaranteed meet-up speed.. 9600-8N1

  setState(STATE_LISTEN,10);
  return false;
}

bool SFBSerial::stepListen(u32 , u8 * spacket) {
  if (!spacket) {             /* If we timed out, */

    lowPrintln("S!");           /* Announce ourselves, with an */
    lowPrintln("S!");           /* extra if the first got eaten for sync */
    setState(STATE_LISTEN,450); /* and wait a bit */

  } else {                    /* Analyze the spacket */

    //if (face != NORTH)
    //facePrintf(NORTH,"Ll %s '%s'\n",FACE_NAME(face),spacket);

    if (zPacketPrefix(spacket,"SR")) {

      // Eat resets here -- we're fresh ATM.
      setState(STATE_LISTEN,10);

    } else if (zPacketPrefix(spacket,"S!")) {

      // S! moves us forward.
      flags |= FLAG_CONNECTED;

      lowPrintOurBRO();

      setState(STATE_OFFER,250);

    } else setState(STATE_RESET,0);
  }
  return false;
}

bool SFBSerial::stepOffer(u32 , u8 * spacket) {

  if (!spacket || zPacketPrefix(spacket,"SR")) { /* If time out or reset request, reset  */

    setState(STATE_RESET,0);

  } else if (zPacketPrefix(spacket,"S!")) {

    // Eat S!s here
    setState(STATE_OFFER,250);

  } else if (zPacketPrefix(spacket,"SC")) {

    // Aww, they don't want to play.  Go to done and redispatch
    setState(STATE_DONE,0);
    return true;

  } else if (zPacketPrefix(spacket,"SB") && packetLength(spacket) >= 5) { /* Did they make us an offer? */
    u8 ourPref = preferredCode;
    u8 ourLo = hwSerial.getLowBaudCode();
    u8 ourHi = hwSerial.getHighBaudCode();
    u8 theirPref = spacket[2]-'0';
    u8 theirLo =   spacket[3]-'0';
    u8 theirHi =   spacket[4]-'0';
    u8 jointLo = max(ourLo, theirLo);
    u8 jointHi = min(ourHi, theirHi);
    u8 pmax = max(ourPref, theirPref);
    u8 pmin = min(ourPref, theirPref);

    bool ourPrefOdd = hwSerial.getPreferOddParity();
    bool theirPrefOdd = packetLength(spacket) >= 6 && (spacket[5] == 'o' || spacket[5] == 'O');
    selectedOdd = ourPrefOdd && theirPrefOdd;

    if (theirPref > 9 || theirLo > 9 || theirHi > 9
        || theirLo > 1 || theirHi < theirLo) {

      // Hmm, they sent us an illegal format, and after we'd been
      // talking so nicely.  We should really report an 'insane
      // neighbor' event, to think about trying to turn off our
      // outbound power to them.
      //
      // But for now:
      setState(STATE_RESET,0);
      return false;
    }

    if (ourPref==theirPref)        // (2) If ourPref==theirPref, select ourPref
      selectedCode = ourPref;
    else if (pmax <= jointHi && pmax >= jointLo)  //(4) If pmax <= jointHi && pmax >= jointLo, select pmax
      selectedCode = pmax;
    else if (pmin <= jointHi && pmin >= jointLo)
      selectedCode = pmin;
    else
      selectedCode = SFBAUD_RENDEZVOUS;

    lowPrint("SS");
    lowPutc('0'+selectedCode);
    if (selectedOdd)
      lowPutc('o');
    lowTerpri();

    setState(STATE_CONFIRM,250);

  } else {

    // Sorry, I don't swing that way (whatever it was you said).
    setState(STATE_RESET,0);
  }
  return false;
}

bool SFBSerial::stepConfirm(u32 , u8 * spacket) {
  if (!spacket) {             /* If we timed out, */

    setState(STATE_RESET,0);    /* Poop.  And we we so close. */
    return true;
  }

  //    if (face != NORTH)
  //      facePrintf(NORTH,"Lc %s '%s'\n",FACE_NAME(face),spacket);

  /* Do they concur? */
  if (zPacketPrefix(spacket,"SS")
      && packetLength(spacket) >= 3
      && spacket[2]-'0' == selectedCode
      && selectedOdd == (packetLength(spacket) >= 4)
      && ((packetLength(spacket) < 4) || spacket[3] == 'o' || spacket[3] == 'O')) {

    //    if (face != NORTH)
    //      facePrintf(NORTH,"Lconcur %s '%s'\n",FACE_NAME(face),spacket);

    // Yes, they concur.  Do we actually need to change speed?

    if (getBaud() != standardBaudRates[selectedCode] || getOddParity() != selectedOdd) {
      // Time to rock it out..

      delay(1);                 // ..after this brief pause to help ensure our SSP got out

      //if (face != NORTH)
      //facePrintf(NORTH,"Lsetting %s %d %d\n",FACE_NAME(face),standardBaudRates[selectedCode],selectedOdd);

      setBaudRate(standardBaudRates[selectedCode],selectedOdd);

    }

    setState(STATE_WAIT,1500);  /* Give them time to change bauds and confirm */
    return false;
  }

  setState(STATE_RESET,0);    /* Dammit, they spazzed out. */
  return true;
}

bool SFBSerial::stepWait(u32 , u8 * spacket) {
  lowPrintln("SC");
  lowPrintln("SC");
  setState(STATE_DONE,250);
  return spacket!=0;         /* If we got a packet, redispatch for it */
}

bool SFBSerial::stepDone(u32 , u8 * spacket) {
  flags |= FLAG_BRN_DONE;
  setState(STATE_SHOWTIME,2500);
  announcePhSketchInfoIfAny(face); // Hi neighbor, this is what _I'm_ running...
  return spacket != 0 && !zPacketPrefix(spacket,"SC");   /* If it's not a BRN done packet, redispatch it */
}

bool SFBSerial::stepShowtime(u32 , u8 * spacket) {
  if (!spacket) {  // Timeout.  Better check they're still around
    lowPrintln("S?");
    setState(STATE_CHECKIN,2500);
    return false;
  }
  if (zPacketPrefix(spacket,"S?")) {
    lowPrintln("S!");
    setState(STATE_SHOWTIME,2500);
    return false;
  }
  if (zPacketPrefix(spacket,"S!") || zPacketPrefix(spacket,"SC")) {
    setState(STATE_SHOWTIME,2000);
    return false;
  }
  if (zPacketPrefix(spacket,"SB")) {  // Do they want to renegotiate?
    lowPrintOurBRO();
    setState(STATE_OFFER,500);    // Yes..  So be it.
    return true;
  }

  // Any other S packet..(including SR)
  setState(STATE_RESET,0);
  return true;
}

bool SFBSerial::stepCheckin(u32 , u8 * spacket) {
  if (!spacket) {  // Timeout.  Damn, maybe they died
    setState(STATE_RESET,0);
    return true;
  }

  setState(STATE_SHOWTIME,2500); // We had activity.
  return true;                  // Go redispatch on it
}

void SFBSerial::step(u32 when, u8 * spacket) {
  if (!backgroundProcessingActive())
    return;

 again:
  switch (state) {

  case STATE_INIT:     if (stepInit(when,spacket)) goto again;     break;
  case STATE_LISTEN:   if (stepListen(when,spacket)) goto again;   break;
  case STATE_OFFER:    if (stepOffer(when,spacket)) goto again;    break;
  case STATE_CONFIRM:  if (stepConfirm(when,spacket)) goto again;  break;
  case STATE_WAIT:     if (stepWait(when,spacket)) goto again;     break;
  case STATE_DONE:     if (stepDone(when,spacket)) goto again;     break;
  case STATE_SHOWTIME: if (stepShowtime(when,spacket)) goto again; break;
  case STATE_CHECKIN:  if (stepCheckin(when,spacket)) goto again;  break;

  case STATE_RESET:
    lowPrintln("SR");
    setState(STATE_INIT,10);
    break;
  }
}

void SFBSerial::start(u32 baud, u8 startFlags) {

  stop();

  ////
  // Initialize state

  flags = startFlags|FLAG_RUNNING;

  ////
  // Remember the baud rate, set up the hardware
  this->baud = baud;
  hwSerial.start(baud,getOddParity());

  ////
  // Set up the packet framing and printer

  u32 framing = SFBFrame::STANDARD_PACKET;
  if (flags&FLAG_BLOCKING_BYTES)
    framing = SFBFrame::BYTE_PER_PACKET;

  hwSerial.getFrame().setRxFrameType(framing);

  faceUnsetPrinter(face);
  faceSetPrinter(face,&serialInterruptPrinter);

  ////
  // Start up the state machine, if we should
  if (backgroundProcessingActive())
    setState(STATE_INIT,0);
}

void SFBSerial::stop()
{
  /////
  // Stop the hardware
  hwSerial.stop();

  /////
  // Note the face is no longer active
  flags &= ~FLAG_RUNNING;

  /////
  // So it has no baud
  baud = 0;

  /////
  // Shut down our timer, if it was running
  Alarms.cancel(alarmNumber);

  /////
  // Clear our print handler
  faceUnsetPrinter(face);

  /////
  // Et Voila!  The face, she is reset
  state = STATE_RESET;
}

#if 0
static int readByteDispatchByte IN_BODYRAM_BSS;
static void readByteDispatch(u8 * packet, u8 face) {
  API_ASSERT_VALID_PACKET(packet);
  API_ASSERT(readByteDispatchByte == -1, E_BUG_INCONSISTENT_STATE);
  API_ASSERT(packetLength(packet)<=1,E_BUG_INCONSISTENT_STATE);

  if (packetLength(packet))
    readByteDispatchByte = packet[0];
}
int SFBSerial::readByte() {
  hwSerial.predispatch();
  
  SFBPacketIO & in = hwSerial.inputBuffer();

  if (!in.getRawInput())
    return -1;

  if (in.isEmptyOfPackets())
    return -1;

  readByteDispatchByte = -1;

  if (in.dispatchPacket(face, readByteDispatch))
    return readByteDispatchByte;

  return -1;
  
}
#endif

static void defaultDispatcher(u8 * packet, u8 face) {
  Body.trigger(packet,face);
}

bool SFBSerial::dispatch(DispatchHandler dispatcher, u32 max) {

  if (isManualMode())
    return false;               // No dispatching in manual mode

  if (dispatcher == 0) {
    dispatcher = defaultDispatcher;
  }

  u32 count = 0;

  hwSerial.predispatch();  /* Let the hw do its business, if any */

  u8 * packet;
  while (max-- && (packet = hwSerial.getFrame().pollForRxPacket())) {

    dispatcher(packet, face);
    ++count;
  }

  return count != 0;
}

void SFBSerial::spacketInit(u8 face) {
  Faces[face].setState(STATE_INIT, 2);
}

void SFBSerial::spacketDispatcher(u8 * packet) {
  SFBSerial & s = SFBHWSerial::getFace(Body.source());
  if (s.backgroundProcessingActive())
    s.step(millis(), packet);
  else
    Body.triggerUp();
}

static u32 stepRoute(u8 c, u32 from) {
  if (c >= 'A' && c <= 'Z') c += 'a'-'A';
  if (IS_FACE_CODE(c)) return FACE_NUMBER_FROM_CODE(c); // [NnSsEeWw]

  u32 out = NO_FACES;
  bool clockwise;
  switch (c) {
  case 'f': out = OPPOSITE_FACE(from); break;           // Forward
  case 'b': out = from; break;                          // Back
  case 'r': out = COUNTERCLOCKWISE_FACE(from); break;   // Right
  case 'l': out = CLOCKWISE_FACE(from); break;          // Left
  case 'c':                                             // Clockwise among active
    clockwise = true;
    goto firstActive;
  case 'a':
    clockwise = false;
    /* FALL THROUGH */
  firstActive:
    out = from;                 // If nothing better, back where you came from
    for (u32 i = from; (i = clockwise?CLOCKWISE_FACE(i):COUNTERCLOCKWISE_FACE(i)) != from; ) {
      if (SFBHWSerial::getFace(i).ready()) {
        out = i;
        break;
      }
    }
    break;

  case '*': out = ALL_FACES; break;                     // All
  case '?': out = random(WEST+1); break;                // Random (including nonactive)
  case '+': 
    {                                           // Random (among active)
      u32 activeCount = 0;
      for (u32 i = SFBHWSerial::getFirstFace(); SFBHWSerial::moreFaces(i); i = SFBHWSerial::nextFace(i)) {
        SFBSerial & face = SFBHWSerial::getFace(i);
        if (face.ready() && random(++activeCount)==0) {
          out = i;
        }
      }
    }
    break;
  }
  return out;
}

/* Boomerang routing example
 *
 *                         +-----+
 *                         |  S  |
 *                         |E 3 W|
 *                         |  N  |
 *                   (3)   +-----+
 *      (6)      R[sw]arL V3 |^
 * arL V3                    ||
 *    ^                      ||
 *    |  R[ne!(ar);]x?       ||
 *    |  |(1)                ||  (2)
 * (5)|  v     (1)           v|R[!(ar);sw]x?
 *  +-----+R[e!(ar);w]x?   +-----+
 *  |  W  |--------------->|  E  |
 *  |S 1 N|                |S 2 N|
 *  |  E  |<---------------|  W  |
 *  +-----+  R[w]arL V3    +-----+
 *                (4)
 *
 * (1) From W1: R[ne!(ar);]x? ->  Send N1:  R[e!(ar);w]x? (arrives at S2)
 * (2) From S2: R[e!(ar);w]x? ->  Send E2:  R[!(ar);sw]x? (arrives at N3)
 * (3) From N3: R[!(ar);sw]x? ->  Print 'R[;sw]ar' to N3 and dispatch 'x?' from N3
 *              result is like 'R[;sw]arL V3 ..etc..' to N3
 * (4) From E2: R[;sw]arL V3 ..etc.. -> Send S2: R[w]arL V3 ..etc..
 * (5) From N1: R[w]arL V3 ..etc.. ->  Send W1: arL V3 ..etc..
 * (6) From W1: arL V3 ..etc..  arrives at original emitter
 */

#if 0
static void dumpRest(u32 to, u8 * packet) {
  facePrintln(to,&packet[packetCursor(packet)],packetReadLength(packet));
}

void boomerPacket(u8 * packet, u32 from) {
  API_ASSERT_VALID_PACKET(packet);

  packetReread(packet);

  u8 result;
  if (packetScanf(packet,"R[%c",&result) != 3)
    return;

  if (result == ']') {          // boomerend - just dump post packet
    dumpRest(from,packet);
    return;
  }

  u32 step = stepRoute(result,from);
  if (step != NO_FACES) {       // Routing onward.  Do so, scanning for a boomer
    facePrint(step,packet,packetCursor(packet)-1); // Print prefix

    while (packetScanf(packet,"%c",&result) == 1) {
      facePrint(step,result,BYTE); // Copy onward
      if (result==']')             // No boomer, dump rest
        break;
      if (result==';') {           // If found first boomer..
        facePrint(step,FACE_CODE(from),BYTE); // ..push source code on front
        break;                                // and go dump rest
      }
    }
    dumpRest(step, packet);     // Send on rest of packet
    return;                     // Route onward complete!
  }

  if (result==';') {            // Found commence boomerang
    if (packetScanf(packet,"%c",&result) != 1) // Hmm?
      return;
    if (result==']') {          // Nowhere to go, dump post backwards
      dumpRest(from,packet);
      return;
    }
    // It really should be a face code, then..
    u32 boomStep = stepRoute(result,from);
    if (boomStep == NO_FACES)
      return;                   // But it wasn't
    
    facePrint(step,packet,packetCursor(packet)-2); // Onward the prefix, stopping before ';' and face code
    while (packetScanf(packet,"%c",&result) == 1) {
      facePrint(step,result,BYTE); // Copy onward
      if (result==';') {           // If found first boomer..
        facePrint(step,FACE_CODE(from),BYTE); // ..push source code on front
        break;
      }
    }
    dumpRest(step,packet);                                       // Onward the rest..
    //facePrintln(step,&packet[pos-1],packetLength(packet)-pos-1); // Dump rest of packet
    return;                      // Boomer route complete!
  }

  if (result=='!') {            // We have an execute order 
    u32 preIndex = packetCursor(packet)-1;
    u32 startPrefix;
    u32 endPrefix;
    startPrefix = endPrefix = 0;
    if (packetScanf(packet,"%c",&result) != 1) 
      return;
    if (result=='(') {
      startPrefix = packetCursor(packet);
      bool sawEnd = false;
      while (packetScanf(packet,"%c",&result) == 1) {
        if (result==')') {
          endPrefix = packetCursor(packet)-1;
          sawEnd = true;
          break;
        }
      }
      if (!sawEnd)
        return;
    } else packetReread(packet,packetCursor(packet)-1); // Push back the non-prefix ch

    facePrint(from,packet,preIndex); // Print prefix, stopping before '!'

    while (packetScanf(packet,"%c",&result) == 1) { // Scan for boomerend
      facePrint(from,result,BYTE); // Copy onward
      if (result==']') {           // If found boomend
        facePrint(from,&packet[startPrefix],endPrefix-startPrefix); // Insert return prefix

        u8 * subPacket;
        if (packetReadPacket(packet, subPacket)) {
          // successfully extracted a nested packet!  Dispatch!
          logVerbose("Boomsub: %#p fr %d\n",subPacket, from);         

          Body.trigger(subPacket, from); // XXX TODO: Should be wrapper face, not from?
        }
        return;
      }
    }
    
  }
}
#endif

void SFBSerial::rpacketDispatcher(u8 * packet) {
  u32 from = Body.source();
  SFBSerial & s = SFBHWSerial::getFace(from);
  if (s.backgroundProcessingActive()) {
    if (packetLength(packet) >= 2) {
      u8 c = packet[1];

#if 0  /* not ready for prime time */
      if (c=='[') {
        boomerPacket(packet,from);
        return;
      }
#endif
      u8 out = stepRoute(c,from); // If out==NO_FACES, packet is eaten
      facePrintln(out,&packet[2],packetLength(packet)-2);
    }
  } else
    Body.triggerUp();
}

void SFBSerial::stateTimeout(u32 when, void * arg) {
  ((SFBSerial*) arg)->step(when, 0);
}

const char SFBSerial::backgroundProcessorTypes[] = {
  /*S*/  'S',
  /*P*/  'P',
  /*R*/  'R',
};

const PacketHandler SFBSerial::backgroundProcessorHandlers[] = {
  /*S*/  SFBSerial::spacketDispatcher,
  /*P*/  programServerDispatcher,
  /*R*/  SFBSerial::rpacketDispatcher,
};
const SFBSerial::InitHandler SFBSerial::backgroundProcessorInits[] = {
  /*S*/  spacketInit,
  /*P*/  0,
  /*R*/  0,
};

extern "C" void flashAll(int);
void SFBSerial::backgroundProcessingInit() {
  u32 reflexCount =
    sizeof(SFBSerial::backgroundProcessorHandlers)/sizeof(SFBSerial::backgroundProcessorHandlers[0]);

  for (u32 i = 0; i < reflexCount; ++i) {
    u8 type = backgroundProcessorTypes[i];
    PacketHandler ph = backgroundProcessorHandlers[i];
    void (*inith)(u8 face) = backgroundProcessorInits[i];

    if (!Body.reflexExists(type,SFBReactor::TRIGGER_BRAIN)) {

      Body.reflexReflex(type,ph, SFBReactor::TRIGGER_BRAIN); /* Handle background packets at the Brain, so easy to override */
      if (inith)
        inith(face);
    }
  }
}

void SFBSerial::suppress(const char type) { // Suppress existing reflex type 'type' on this face only
  Body.suppress((const u8) type,Body.faceToFlag(face));
}
void SFBSerial::allow(const char type) { // Allow existing reflex type 'type' on this face only
  Body.allow(type,Body.faceToFlag(face));
}

void SFBSerial::reflex(const char type, PacketHandler pfunc) {
  Body.reflex(type,pfunc,Body.faceToFlag(face));
}

void SFBSerial::otherwise(PacketHandler pfunc) {
  Body.otherwise(pfunc,Body.faceToFlag(face));
}

void SFBSerial::startup_initialization() {

  const int reflexCount =
    sizeof(SFBSerial::backgroundProcessorHandlers)/sizeof(SFBSerial::backgroundProcessorHandlers[0]);
  Body.reflexes(SFBSerial::backgroundProcessorHandlers,reflexCount);

  for (u32 i = SFBHWSerial::getFirstFace(); SFBHWSerial::moreFaces(i); i = SFBHWSerial::nextFace(i)) {
    SFBSerial & face = SFBHWSerial::getFace(i);
    face.alarmNumber = Alarms.create(stateTimeout,(void*) &face);  /* Get an alarm timer for each face */
    face.backgroundProcessingInit();
  }

  faceSetPrinter(ALL_FACES,&allFacesInternalPrinter);
}

