/* 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 "VarLighting.h"
#include "BehaviorPacketEncoder.h"

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

char const * const VarLighting::state_names[VarLighting::NumStates] = {
  "PRIMARY_ATTACKER",
  "SUPPORTING_ATTACKER"};


VarLighting::VarLighting()
{
  fsm.init(state_names,NumStates,PRIMARY_ATTACKER,16,16);

  primary_attacker = new BePrimaryAttacker();
  //need to setupEventMgr since we're using the ()operator
  primary_attacker->setupEventMgr();

  supporting_attacker = new BeSupportingAttacker();

  fs_id = ~0;
  fs = NULL;
}

VarLighting::~VarLighting()
{
  if(primary_attacker!=NULL){
    delete primary_attacker;
    primary_attacker = NULL;
  }
  
  if(supporting_attacker!=NULL){
    delete supporting_attacker;
    supporting_attacker = NULL;
  }

}

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

double
VarLighting::operator()(FeatureSet *features,
		     MotionCommand *command)
{
  double active_activation = 0; // *shrug*

  bool done = false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    // Reset the motion structure to something sane.
    resetCommand(command);
    
    // Try to run our active behavior
    switch(fsm.getState()){
      case PRIMARY_ATTACKER:
        active_activation = (*primary_attacker)(features, command);
        if(fsm.timeInState() > SecToTime(5.0)){
          primary_attacker->reset(features->current_time);
          primary_attacker->sleep();
          TRANS_CONT(fsm,SUPPORTING_ATTACKER,5,"LowActivation");
        }

        done = true;
        break;
        
      case SUPPORTING_ATTACKER:
        
	if(fsm.isNewState()) {
	  RobotMain::GetObject().getVision()->startThreshTest();
	}

	command->motion_cmd = MOTION_STAND_NEUTRAL;
	command->head_cmd = HEAD_SCAN_MARKERS;
        
        if(fsm.timeInState() > SecToTime(5.0)){

	  RobotMain::GetObject().getVision()->endThreshTest();

          supporting_attacker->reset(features->current_time);
          supporting_attacker->sleep();
          TRANS_CONT(fsm,PRIMARY_ATTACKER,5,"LowActivation");
        }
        done=true;
        break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  
  return 1.0; // we never yield.
}

void VarLighting::resetCommand(MotionCommand *command)
{
  mzero(*command);
  
  command->motion_cmd = MOTION_STAND_NEUTRAL;
  command->head_cmd = HEAD_LOOKAT;
  command->tail_cmd = TAIL_NO_CMD;
  command->head_lookat.x = 200;
  command->head_lookat.y = 0;
  command->head_lookat.z = 50;
  command->led.cmd = 0; // turn off all LEDs
}

uchar *VarLighting::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,beh_id,beh_name,NumStates,state_names,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,2);
  bufp = primary_attacker   ->encodeAllNames(time,bufp,buf_size - (bufp - buf));
  bufp = supporting_attacker->encodeAllNames(time,bufp,buf_size - (bufp - buf));

  return bufp;
}

uchar *VarLighting::encodeTrace(uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeTrace::encodeTrace<FSM,State>(buf,beh_id,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,2);
  bufp = primary_attacker   ->encodeTrace(bufp,buf_size - (bufp - buf));
  bufp = supporting_attacker->encodeTrace(bufp,buf_size - (bufp - buf));

  return bufp;
}



bool VarLighting::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 VarLighting::setupEventMgr(){
  return true;
};


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

// The behavior system will call this function to find out
// what motions the robot should execute this frame.
const MotionCommand *VarLighting::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(VarLighting,VarLighting::beh_name,VarLighting::create);