/* 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.
  ========================================================================= */

#include "PupPilot.h"
#include "PupPilotMsg.h"
#include "../headers/SystemUtility.h"

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

char const * const PupPilot::state_names[PupPilot::NumStates] = {
  "LISTENING",
  "CONNECTED"
};

//static const ulong PupPilotPauseTimeout = SecToTime(5.0);

PupPilot::PupPilot()
{
  fsm.init(state_names,NumStates,LISTENING,16,16);

  mzero(out_command);
}

PupPilot::~PupPilot()
{
}

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

double
PupPilot::operator()(MotionCommand *command)
{  
  bool done=false;
  
  fsm.startLoop(GetTime());
  while(!done && fsm.error==0){
    switch(fsm.getState()){
      case LISTENING:	
        // Create a motion command
        command->motion_cmd = MOTION_STAND_NEUTRAL;
        command->head_cmd = HEAD_LOOKAT;
        command->head_lookat.set(1000,0,50);
        command->led.cmd = 
          LED_UPPER_LEFT | LED_UPPER_RIGHT |
          LED_LOWER_LEFT | LED_LOWER_RIGHT; // indicate no connection
        
        // We did not transition to a new state, so set the done flag.
        done = true;
        break;
        
      case CONNECTED:
        // we're saying that a variable equals itself here.
        //      *command = out_command;
        
        // here are the only changes we want to make
        
        done = true;
        break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  
  fsm.endLoop();
  
  return 1.0;
}

// callback when connection is made
void PupPilot::connected(NetTCPConnection conn, int result)
{
  if(!result){
    pprintf(TextOutputStream, "Connection attempt failed.\n");
    listen();
  }else{
    fsm.setState(CONNECTED, 0, "from connected",GetTime());
  }
}

void PupPilot::closed(NetTCPConnection conn, int result)
{  
  fsm.setState(LISTENING, 0, "from closed", GetTime());
  listen();
}

void PupPilot::receive(NetTCPConnection conn, int result)
{  
  PupPilotMsg *buf;

  // Make sure something the size of an entire packet arrived.
  if(result % sizeof(PupPilotMsg) !=0)
    pprintf(TextOutputStream, "Discarding %d bytes\n", result);

  // skip to the most recent message.
  buf = (PupPilotMsg *)(conn->ReceiveBuffer + result - sizeof(PupPilotMsg));

  // Make sure the data at the start of the buffer contains a valid
  // control message.
  if(buf->header!=0xC0EDBEEF){
    pprintf(TextOutputStream, "Invalid packet header %X. Discarding %d bytes.\n", buf->header, result);
  }

  out_command = buf->cmd;

  // Deal with paused mode
  if(buf->pause)
    out_command.motion_cmd = MOTION_STAND_NEUTRAL;
}

void PupPilot::listen()
{
  connection = NetTCPListen(LISTEN_PORT, &PupPilot::connected,
			    NULL,                // No callback for Send
			    &PupPilot::receive,  // No callback for Receive
			    &PupPilot::closed);
}

bool PupPilot::initConnections()
{
  // Do this here so hopefully all of the network stuff
  // in Aperios is up and running.
  listen();

  return true;
}

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

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

const MotionCommand *PupPilot::get(ulong time)
{
  (*this)(&out_command);
  
  return &out_command;
}

// This bit is VERY important. You pass it your class name, a
// human-readable version of that name (to be placed in run.cfg
// to actually run your behavior), and a pointer to a function
// that creates an instance of your behavior class. It's not
// terribly important to worry about what this macro does.
// In short, it registers behaviors with the behavior system so
// that it knows they exist and can instantiate them if they
// are to be run.
REGISTER_EVENT_PROCESSOR(PupPilot,PupPilot::beh_name,PupPilot::create);