/*            
  SFBReflex.cpp - Main reflex-level operational control
  Copyright (C) 2008 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
*/

#include "SFBReflex.h"
#include "SFBTypes.h"
#include "SFBErrors.h"
#include "SFBAlarm.h"
#include "SFBAssert.h"
#include "SFBSerial.h"          /* For Faces */
#include "SFBMemory.h"          /* For Memory */
#include "SFBWatchdog.h"        /* For watchdogFeed */

static bool reflexIsDispatching IN_BODYRAM_BSS;

u32 reflexLibraryFlags IN_BODYRAM_BSS;

/* We'd like to process all the packets we can, but that might just
   take too long too.  So we impose a per-dispatch cap on packets
   processed per face.
*/

#define MAX_PACKETS_PER_FACE_PER_DISPATCH 4

static bool doPacketDispatch(u32 deadline) {
  static u8 dispatchFace IN_BODYRAM = 0;
  bool ret = false;
  for (u8 i = 0; i < FACE_COUNT && IS_EARLIER_OR_EQUAL(millis(), deadline); ++i) {
    if (++dispatchFace > WEST) dispatchFace = NORTH;
    if (Faces[dispatchFace].isReflexMode())
      ret |= Faces[dispatchFace].dispatch(0, MAX_PACKETS_PER_FACE_PER_DISPATCH);
  }
  return ret;
}


#define MAX_MEMORY_PACKETS_PER_DISPATCH 1

static bool doMemoryDispatch(u32 deadline) {
  bool ret = false;
  ret |= Memory.dispatch(MAX_MEMORY_PACKETS_PER_DISPATCH,deadline);
  return ret;
}

/* If alarm code runs too long, we don't want to totally starve
   everything else, so we impose a max on how many alarms we'll run
   before going back around the loop.  Too small a value increases
   overhead; too large a value starves packet processing and whatever
   else happens during the background body processing  */

#define MAX_ALARMS_PER_DISPATCH 5

static bool doGlobalEvents(u32 deadline) {
  bool ret = false;
  ret |= Alarms.runDueEvents(MAX_ALARMS_PER_DISPATCH,deadline); 
  return ret;
}

int reflex_maybe_dispatch(u32 deadline) 
{
  if (reflexIsDispatching) return false;
  reflex_dispatch(deadline);
  return true;
}

void reflex_startup_initialization() {
  reflexIsDispatching = false;
}

#define MIN_MEMORY_RUN_MS 20

int reflex_dispatch(u32 deadline) 
{

  // The library doesn't start the watchdog timer on its own, because
  // that has a lot of implications, but if the sketch chooses to do
  // so, then..

  if (!TEST_REFLEX_FLAGS(RF_DONT_FEED_DOG)) // Unless we're told not to..
    watchdogFeed();                         // Alive enough to dispatch is alive enough

  BUG_IF_CODE(E_BUG_REFLEX_RECURSION, reflexIsDispatching);
  reflexIsDispatching = true;
  
  static u8 phase IN_BODYRAM_BSS;
  bool dispatched = false;
  static u32 nextMemRun IN_BODYRAM_BSS;

  u8 startPhase = phase;
  do {
    switch (phase) {
    case 0:
      dispatched |= doPacketDispatch(deadline);
      break;
    case 1:
      dispatched |= doGlobalEvents(deadline);
      break;
    case 2:
      if (IS_EARLIER_OR_EQUAL(nextMemRun,millis())) {
        dispatched |= doMemoryDispatch(deadline);
        nextMemRun = millis()+MIN_MEMORY_RUN_MS;
      }
      break;
    case 3: 
      break;
    }
    phase = (phase+1)&3;
  } while (IS_EARLIER(millis(),deadline) && phase != startPhase);

  reflexIsDispatching = false;
  return dispatched;
}
