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

#include "Attacker.h"
#include "BehaviorPacketEncoder.h"
#include "../headers/Reporting.h"

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

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


Attacker::Attacker()
{
  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;
}

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

}

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

double
Attacker::operator()(FeatureSet *features,
		     MotionCommand *command)
{
  static EventTimeReporter reporter("attacker (toplevel)",
				    SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  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: {
        vector2d ball;
        features->getBestBallPosnPrimary(ball);
        features->world_model->sendWMDebug(ball.x, ball.y, 0, 0xB5, 0x94, 0xCD);
        active_activation = (*primary_attacker)(features, command);
        if(active_activation <= 0.0){
          primary_attacker->reset(features->current_time);
          primary_attacker->sleep();
          TRANS_CONT(fsm,SUPPORTING_ATTACKER,5,"LowActivation");
        }

        done = true;
        break;
    }

    case SUPPORTING_ATTACKER:
      {
        active_activation = (*supporting_attacker)(features, command);
        
        if(active_activation <= 0.0){
          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();

  // For debugging we'll play a little beep if we sent a role
  // message.
  switch(features->team_msg_mgr->last_role_message) {
    case TeamMsgMgr::MSG_TOKEN_REQ:
      command->sound_cmd = SOUND_NOTE;
      command->sound_frequency = 880;
      command->sound_duration = SecToTime(.2);
      break;
    case TeamMsgMgr::MSG_TOKEN_PASS:
      command->sound_cmd = SOUND_NOTE;
      command->sound_frequency = 1760;
      command->sound_duration = SecToTime(.2);
      break;
    case TeamMsgMgr::MSG_NO_TOKEN:
      command->sound_cmd = SOUND_NOTE;
      command->sound_frequency = 440;
      command->sound_duration = SecToTime(.2);
      break;
  }

  features->team_msg_mgr->last_role_message = 
    TeamMsgMgr::MSG_NONE;
  
  return 1.0; // we never yield.
}

void Attacker::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 *Attacker::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 *Attacker::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 Attacker::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 Attacker::setupEventMgr(){
  return true;
};


bool Attacker::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 *Attacker::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(Attacker,Attacker::beh_name,Attacker::create);