/*                                              -*- mode:C++; fill-column:100 -*-
  SFBNet.cpp - Simple 'functional gradient' network routing
  Copyright (C) 2010 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 <stdarg.h>

#include "SFBNet.h"
#include "SFBPrintf.h"          /* For facePrintf */ 
#include "SFBSerial.h"          /* For Faces */ 
#include "SFBRandom.h"          /* For random() */ 

/////////////START DEBUGGING CODE

#ifndef MAX_DEBUG_LEVEL
#define MAX_DEBUG_LEVEL 0
#endif

#if MAX_DEBUG_LEVEL > 0

static u8 debug_face IN_TALKRAM;
static u8 debug_mode IN_TALKRAM;

#define DEBUG0(level, f)                                                 \
 do {                                                                    \
  if ( MAX_DEBUG_LEVEL >= (level) )                                      \
    if ( debug_mode >= (level) && (debug_face < FACE_COUNT ) )           \
      facePrintf ( debug_face, f );                                      \
 while (0) 

#define DEBUG1(level, f, arg)                                            \
 do {                                                                    \
  if ( MAX_DEBUG_LEVEL >= (level) )                                      \
    if ( debug_mode >= (level) && (debug_face < FACE_COUNT ) )           \
      facePrintf ( debug_face, f,arg );                                  \
 while (0) 

#define DEBUG2(level, f, arg1, arg2)                                     \
 do {                                                                    \
  if ( MAX_DEBUG_LEVEL >= (level) )                                      \
    if ( debug_mode >= (level) && (debug_face < FACE_COUNT ) )           \
      facePrintf ( debug_face, f,arg1, arg2 );                           \
 while (0) 

#define DEBUG3(level, f, arg1, arg2, arg3)                               \
 do {                                                                    \
  if ( MAX_DEBUG_LEVEL >= (level) )                                      \
    if ( debug_mode >= (level) && (debug_face < FACE_COUNT ) )           \
      facePrintf ( debug_face, f,arg1, arg2, arg3 );                     \
 while (0) 

static void debug_packet_handler(u8 * packet) {
  char ch;
  u32 tmp_debug_mode;

  if (packetScanf(packet, "d%c", &ch) != 2) {
    facePrintf (packetSource(packet), "Ln Invalid d packet.\n");
    return;
  }

  switch (ch) {
  case 'd':
    if (packetScanf(packet,"%d",&tmp_debug_mode) != 1) {
      facePrintf(packetSource(packet), "Ln Invalid d packet.\n");
      return;
    }
    debug_mode = (u8) tmp_debug_mode;
    debug_face = packetSource(packet);
    pprintf( "Ln Set debug mode to %d, debug dest is %c\n", debug_mode, FACE_CODE(debug_face));
    break;

  case '?':
    //printDebugInfo ( packetSource(packet) );
    break;
  default:
    facePrintf(packetSource(packet), "Ln Unknown directive: %c\n", ch);
  }
}

static void debug_reflex() { 
  debug_face = FACE_COUNT;
  debug_mode = 0;
  Body.reflex('d',debug_packet_handler); 
}

#else

#define DEBUG0(level, f) do { } while (0)
#define DEBUG1(level, f, arg) do { } while (0)
#define DEBUG2(level, f, arg1, arg2) do { } while (0)
#define DEBUG3(level, f, arg1, arg2, arg3) do { } while (0)

static void debug_reflex() { /* nothing */ }

#endif

/////////////END DEBUGGING CODE

SFBNet Net IN_TALKRAM_BSS;

/* Static shims, for FacePrinters, reflexes, and alarms, that just dish to the singleton */
void SFBNet::NetFunctionPrint(u8 face, u8 byte) { Net.functionPrint(face, byte); }
void SFBNet::NetFunctionPrintln (u8 face) { Net.functionPrintln(face); }
void SFBNet::NetResponsePrint(u8 face, u8 byte) { Net.responsePrint(face, byte); }
void SFBNet::NetResponsePrintln (u8 face) { Net.responsePrintln(face); }
void SFBNet::NetPacketHandler(u8 * packet) { Net.packetHandler ( packet ); }
void SFBNet::NetBcastAlarm(u32 when){ Net.broadcastAlarm(when); }

const FacePrinter SFBNet::net_printer = { 
  SFBNet::NetFunctionPrint, SFBNet::NetFunctionPrintln, 0, 0 
};
void SFBNet::functionPrint(u8 face, u8 byte) { facePrint(packet_printer_face, byte, BYTE); }
void SFBNet::functionPrintln(u8 face) { facePrintln(packet_printer_face); ship(); }

const FacePrinter SFBNet::response_printer = { 
  SFBNet::NetResponsePrint, SFBNet::NetResponsePrintln, 0, 0 
};

void SFBNet::printResponseHeader() {
  for (int i = response_route_len-1; i >= 0; i--) {
    DEBUG1(4, "Ln printResponseHeader: Next link is %c\n", response_route[i]);
    facePrintf(response_src, "R%c", response_route[i]);
  }
  flags |= RESPONSE_SENT_HEADER;
}

void SFBNet::responsePrint(u8 face, u8 byte) { 
  printResponseHeaderIfNeeded();   
  faceGetPrinter(response_src)->print(response_src, byte);
}

void SFBNet::responsePrintln (u8 face) {
  printResponseHeaderIfNeeded();   
  facePrintln(response_src);
}

void SFBNet::setBroadcastPeriod(u32 millis) {
  u32 ticks = MsToTicks32(millis);
  if (ticks==0) ticks = 1;
  else if (ticks > 255) ticks = 255;
  bcastPeriodTicks = (u8) ticks;
}

void SFBNet::setHorizon(u32 distance) {
  API_ASSERT_MAX(distance,MAX_RETURN_ROUTE_LENGTH/2);
  defaultTTL = distance;
}

void SFBNet::begin() {

  defaultTTL = 6;
  bcastPeriodTicks = MsToTicks(5000);
  bcastGeneration = 0;
  ticksInGeneration = 0;

  last_route = 0;
  last_offered = 0;

  if (alarmNumber == 0)
    alarmNumber = Alarms.create(NetBcastAlarm);

  Alarms.set(alarmNumber, millis());

  if (!Body.reflexExists('N'))
    Body.reflexReflex('N',NetPacketHandler);
  else
    Body.allow('N',SFBReactor::TRIGGER_SPINE);

  debug_reflex();

  net_packet = makePacketPrinter(packet_printer_face, 
                                 packet_buffer,
                                 sizeof(packet_buffer), 
                                 NORTH); // TODO: What does NORTH mean in this context?

  if (!faceFindFreeFace(net_printer_face)) 
    BUG_CODE("net_printer face",E_API_RESOURCE_EXHAUSTED);

  faceSetPrinter(net_printer_face, &net_printer);

  if (!faceFindFreeFace(virtual_face))
    BUG_CODE("virtual_face",E_API_RESOURCE_EXHAUSTED);

  faceSetPrinter(virtual_face, &response_printer);

  flags = NET_ACTIVE;   // Clear all flags except active
}

void SFBNet::end() {
  if (alarmNumber != 0)         // If we've ever been begun
    Alarms.cancel(alarmNumber);  // Stop our ticker

  if (net_printer_face) {
    faceUnsetPrinter(net_printer_face); 
    net_printer_face = 0;
  }

  if (virtual_face) {
    faceUnsetPrinter(virtual_face); 
    virtual_face = 0;
  }

  if (Body.reflexExists('N'))
    Body.suppress('N',SFBReactor::TRIGGER_SPINE);

  flags = 0;                    // Clear all flags including active
}

void SFBNet::offer(u8 c) {
  if (!isOfferedByNode(c)) {    // Only one slot per service..
    API_ASSERT_MAX(last_offered,MAX_LOCAL_SVCS-1);
    offered[last_offered++] = c;
  }
}

bool SFBNet::withhold(u8 c) {
  u32 j = 0;
  bool ret = false;

  for (u32 i = 0; i < last_offered; i++) {
    if (offered[i] == c) {
      j++;
      last_offered--;
      ret = true;
      offered[i] = j;
    }
    
    j++;
  }
  return ret;
}

bool SFBNet::isOfferedByNode(u8 service) {

  for (u32 i = 0; i < last_offered; i++)
    if (offered[i] == service)
      return true;

  return false;
}

bool SFBNet::isOfferedByNet(u8 service) {
  return isOfferedByNode(service) || (lookup(service) != 0);
}

void SFBNet::openServiceStream(u8 service) {
  open_function = service;
}

void SFBNet::printf(const char * format, ...) {
  va_list ap;
  va_start(ap, format);

  vfacePrintf(getServiceStreamFace(), format, ap);
}

void SFBNet::printf(u8 func, const char * format, ... ) {
  va_list ap;
  va_start(ap, format);

  openServiceStream(func);
  vprintf(format, ap);
}

void SFBNet::vprintf(const char * format, va_list & ap) {
  vfacePrintf(getServiceStreamFace(), format, ap);
}

void SFBNet::broadcastAlarm(u32 when) {

  if ((ticks()&0x3)==0) {       // About once a second, reannounce to buddies
    for (u32 i = NORTH; i <= WEST; ++i) {
      if (neighborActive_(i) || random(10)==0) // Also randomly probe strangers about every 10 sceonds
        facePrintf(i,"Na\n");
    }
  }

  if (++ticksInGeneration >= bcastPeriodTicks) {
    ticksInGeneration = 0;

    if (last_offered > 0) {
      for (u32 f = NORTH; f <= WEST; f++) {
        if (neighborActive_(f)) {	
          facePrintf(f, "Nb%c%c", 1, defaultTTL);
          facePrint(f, offered, last_offered);
          facePrintln(f);
        }
      }
    }
    bcastGeneration++;
  }

  Alarms.set(Alarms.currentAlarmNumber(), when + ticksToMs(1));
}

void SFBNet::handleBroadcast(u8 * packet) {
  u8 hops, ttl;

  if (packetScanf(packet, "%c%c", &hops, &ttl) != 2)
    return;

  u8 source = packetSource(packet);
  u8 sendFaces[FACE_COUNT];
  u8 sendCount = 0;

  if (ttl > 1)
    for (u32 f = NORTH; f <= WEST; ++f) { 
      if (neighborActive_(f) && f != source) 
        sendFaces[sendCount++] = f;
    }

  for (u32 i = 0; i < sendCount; ++i) 
    facePrintf(sendFaces[i], "Nb%c%c", hops + 1, ttl - 1);

  u8 cur_func;
  while (packetScanf(packet, "%c", &cur_func) == 1) {
    Entry * e = lookup(cur_func,true); // G'teed non-null ret

    if (e->hops[source] == 0 || hops <= e->hops[source]) {
      e->hops[source] = hops;
      e->last_update[source] = bcastGeneration;
    }

    for (u32 i = 0; i < sendCount; ++i)
      facePrintf(sendFaces[i], "%c", cur_func);
  }

  for (u32 i = 0; i < sendCount; ++i )
    facePrintln(sendFaces[i]);
}

void SFBNet::handleInvoke(u8 * packet) {
  u32 face = 0;
  u8 func = 0;
  u8 hops = 0;
  u8 ttl = 0;
  u8 return_route_idx;

  DEBUG1(2, "Ln Got invoke: %P\n", packet);
  DEBUG1(2, "Ln invoke: source is %c\n", FACE_CODE(packetSource(packet)));

  if (packetScanf(packet, "%c%c%c", &func, &hops, &ttl ) != 3)
    return;

  if (ttl == 0 || hops > MAX_RETURN_ROUTE_LENGTH) // Stop bogosities.
    return;

  return_route_idx = packetCursor(packet); // Return route info starts here

  for (u32 i = 0; i < hops - 1u; i++) { // Do validity check and skip over return route
    u8 ignored;
    if (packetScanf(packet, "%F", &ignored) != 1) // Just want all face codes
      return;                                     // But something wasn't.
  }

  if (Net.isOfferedByNode(func)) {
    DEBUG0(2, "Ln Invoke: Parsed header triggering.\n");
    trigger(packet, &packet[return_route_idx], hops); 
    return;
  }

  face = Net.pickDirection(func, packetSource(packet));

  hops++;
  ttl--;

  if (ttl == 0)
    return;

  facePrintf(face, "Ni%c%c%c", func, hops, ttl);    // Start with the network header

  facePrint(face, &packet[return_route_idx], hops-2); // Plus the previous return route
  facePrintf(face, "%c", FACE_CODE(packetSource(packet))); // Plus us

  facePrintln(face, &packet[packetCursor(packet)], packetReadLength(packet)); // Plus payload
}

void SFBNet::packetHandler(u8 * packet) {
  u8 type;

  if (packetScanf(packet, "N%c", &type) != 2)
    return;

  switch (type) {
  case 'a':
    if (packetSource(packet) < FACE_COUNT)
      neighborActiveTicks[packetSource(packet)] = ticks();
    break;
  case 'b':
    handleBroadcast(packet);
    break;
  case 'i':
    handleInvoke(packet);
    break;
  default:
    return;
  }
  return;
}

SFBNet::Entry * SFBNet::lookup(u8 func, bool insert) {

  for (u32 i = 0; i < last_route; i++) {
    if (routes[i].func == func)
      return &routes[i];
  }

  Entry * e = 0;
  if (insert) {
    e = &routes[last_route];
    if (last_route < MAX_TOTAL_SVCS - 1) // Would like to die blinking, but can't:
      last_route++;                      // Outside world could then kill us.

    e->func = func;

    for (u32 i = NORTH; i <= WEST; ++i) {
      e->hops[i] = 0;
      e->last_update[i] = 0;
    }
  }
  return e;
}

u8 SFBNet::pickRandomFace(u8 origin) {
  u32 pick = origin;            // Return to sender if no other choice..
  u32 count = 0;

  for (u32 i = NORTH ; i <= WEST; i++) {
    if (neighborActive_(i) && i != origin)
      if (random(++count)==0) 
        pick = i;
  }
  
  return pick;
}

void SFBNet::dump(u8 face, const u8 * prefix) {
  facePrintf(face,"%sLn BG=%d; BPT=%d; dT=%d; Offered(%d):", 
             prefix?prefix:(const u8*)"",
             bcastGeneration, bcastPeriodTicks, defaultTTL,
             last_offered);
  for (u32 i = 0; i < last_offered; ++i)
    facePrintf(face,"%c",offered[i]);
  facePrintln(face);

  for (u32 i = 0; i < last_route; ++i) {
    Entry & e = routes[i];
    facePrintf(face,"%sLn f=%#c;", 
               prefix?prefix:(const u8*)"",
               e.func);
    for (u32 f = NORTH; f <= WEST; ++f) {
      if (neighborActive_(f))
        facePrintf(face," %c%d+%d",
                   FACE_CODE(f), e.last_update[f], e.hops[f]);
      else
        facePrintf(face," %c---", FACE_CODE(f));
    }
    facePrintln(face);
  }
}

bool SFBNet::neighborActive(u8 face) {
  API_ASSERT_VALID_FACE(face);
  return neighborActive_(face);
}

u8 SFBNet::pickDirection(u8 func, u8 origin) {
  Entry * e = lookup(func);

  if (e == 0)
    return pickRandomFace(origin);

  u32 minHops = 0;
  u32 bestFace = FACE_COUNT;
  u32 bestCount = 0;

  for (u32 i = NORTH; i <= WEST; i++ ) {
    if (!neighborActive_(i)) e->hops[i] = 0; // Blast dead routes
    else if (i != origin) {
      u8 diff = 0;
          
      if (bcastGeneration < e->last_update[i])
        diff = e->last_update[i] - bcastGeneration;
      else
        diff = bcastGeneration - e->last_update[i];
          
      if (diff >= 3) {          // XXX Magic number? 
        DEBUG2(1, "Ln diff[%d] is %d\n", i, diff);
        DEBUG3(1, "Ln e->last_update[%d] is %d, cur gen is %d\n", 
               i, e->last_update[i], bcastGeneration);
        e->hops[i] = 0;
      }	
          
      DEBUG2(1, "Ln e->hops[%d] = %d...\n", i, e->hops[i]);
          
      if (e->hops[i]) {
        if (minHops == 0 || e->hops[i] < minHops) {
          minHops = e->hops[i];
          bestFace = i;
          bestCount = 1;
        } else 
          if (e->hops[i] == minHops && !shouldTakeFirstTie()) {
            if (random(++bestCount) == 0)
              bestFace = i;
          }
      }
    }
  }

  DEBUG2(1, "Ln bestFace is apparently %d with minHops %d\n", bestFace, minHops);

  if (bestFace == FACE_COUNT)
    return pickRandomFace(origin);
  else
    return bestFace;
}

/* ************************************************************************** *
 * Trigger stuff.                                                             *
 * ************************************************************************** */

void SFBNet::trigger(u8 * packet, u8 * return_route, u8 hops) {
  u8 * subp = 0;

  DEBUG1(2, "Ln Made it to trigger. Pack is %P\n", packet);

  if (packetScanf(packet, "%P", &subp) != 1) 
    return;                     // Parse error

  DEBUG0(2, "Ln Parsed the subpacket.\n");
  DEBUG1(2, "Ln subpacket is %P.\n", subp);

  response_src = packetSource(packet);
  response_route_len = hops - 1;
  flags &= ~RESPONSE_SENT_HEADER; 

  for (int i = 0; i < response_route_len; i++)
    response_route[i] = return_route[i];

  DEBUG0(3, "Ln Setting up virtual_face\n");

  (*(PacketHeader *) (subp - 4)).f[PacketHeader::SOURCE] = virtual_face; // XXX USE API?

  DEBUG0(3, "Ln Set up virtual_face successfully. Triggering.\n");
  DEBUG1(3, "Ln trigger: %P\n", subp);

  Body.trigger(subp, 0);
}

void SFBNet::ship() {

  if (Net.isOfferedByNode(open_function)) { // If requesting this service from ourselves..
    PacketHeader & ph = packetHeaderInternalUnsafe(net_packet); 
    ph.f[PacketHeader::SOURCE] = WMEM;      // ..route replies back to our own working memory.

    //facePrintf(NORTH,"L local (%c), trigger: %#p\n",open_function, net_packet);
    Body.trigger(net_packet, SPINE); 
    return;
  }

  u8 face = Net.pickDirection(open_function, FACE_COUNT);

  if (face < FACE_COUNT) {
    facePrintf(face, "Ni%c%c%c", open_function, 1, defaultTTL - 1);
    facePrintPacket(face, net_packet);
    facePrintln(face);
  }
}

