/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */


 /* -------------------------------------------------------------------- *\

                                 NetBehavior

     This sample behavior, derived from a stripped-down SpinDog, serves
     to show how to integrate network operations into behavior code. For
     an actual introduction to networking with EasyNet, please see the
     NetServer object in dogs/agent/NetServer/NetServer.cc

     This behavior starts out listening for a connection on port 5100,
     then simply keeps switching between states "A" and "B" every four
     seconds, sending out the new state if a connection is established.
      

 \* -------------------------------------------------------------------- */



// ----------------------------------------------------------------------
// ****    See NetBehavior.h for required additions to the header    ****
// ----------------------------------------------------------------------

#include "NetBehavior.h"

char const * const NetBehavior::beh_name = "NetBehavior";

char const * const NetBehavior::state_names[NetBehavior::NumStates] = {
  "NET_START,"
  "NET_SEND_A",
  "NET_SEND_B" 
};

static const ulong WAIT_TIME = 4000000;

NetBehavior::NetBehavior()
{
  // Initialize finite state machine to NET_START

  fsm.init(state_names,NumStates,NET_START,16,16);
  fsm.setState(NET_START,0,"Reset",0);

  fs_id = ~0;
  fs = NULL;
}

NetBehavior::~NetBehavior()
{
}

void
NetBehavior::reset(ulong time) {
  fsm.setState(NET_START,0,"Reset",time);
}




double
NetBehavior::operator()(FeatureSet *FS, MotionCommand *command) {

  bool done=false;
  fsm.startLoop(FS->current_time);

  while(!done && fsm.error==0){

    switch(fsm.getState()){

    case NET_START:

      // In the start state, we just start waiting for a connection on port 5100
      // Do NOT use blocking operations in behaviors, since they will stall the
      // whole MainObject.

      Connection = NetTCPListen(5100, &NetBehavior::Connected,
                                      NULL,  // No callback for Send
                                      NULL,  // No callback for Receive
                                      &NetBehavior::Closed);

      TRANS_CONT(fsm,NET_SEND_A,1,"Done");
      

    case NET_SEND_A:
      
      // If we've been here for WAIT_TIME, switch state.

      if(fsm.timeInState() > WAIT_TIME){

          // If we're connected, send out the new state

          if (Connection->Connected) {
              NetSend(Connection, "B...");
          }

          TRANS_CONT(fsm,NET_SEND_B,1,"Timeout");
      }
      
      done = true;
      break;
      
    case NET_SEND_B:

      // If we've been here for WAIT_TIME, switch directions.
      if(fsm.timeInState() > WAIT_TIME){

          // If we're connected, send out the new state

          if (Connection->Connected) {
              NetSend(Connection, "A...");
          }

          TRANS_CONT(fsm,NET_SEND_A,1,"Timeout");

      }

      done = true;
      break;
    }
  }
  
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  
  fsm.endLoop();
//  NetClose(Connection);
  
  return 1.0;
}



void NetBehavior::Connected(NetTCPConnection Conn, int Result)
{

    // Connnect callback. Will be called upon connection
    // Send a welcome string.

    NetSend(Conn, " Hello!\n Now switching to state: ");

}


void NetBehavior::Closed(NetTCPConnection Conn, int Result)
{

    // Close callback. The connection is now closed.
    // Let's wait for a new one.

    Connection = NetTCPListen(5100, &NetBehavior::Connected,
                                    NULL,  // No callback for Send
                                    NULL,  // No callback for Receive
                                    &NetBehavior::Closed);

}




// The rest is behavior stuff...


bool NetBehavior::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool NetBehavior::setupEventMgr(){
  return true;
};

bool NetBehavior::update(ulong time,const EventList *events)
{
  return true;
}

const MotionCommand *NetBehavior::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);

  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;
}

REGISTER_EVENT_PROCESSOR(NetBehavior,NetBehavior::beh_name,NetBehavior::create);