/* 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 "LEDs.h"

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

LEDs::LEDs()
{
  fs_id = ~0;
  fs = NULL;

  sensor_id = 0;
  sensor_data = NULL;
}

LEDs::~LEDs()
{
}


double
LEDs::operator()(FeatureSet *FS, SensorData *s_data,
		     MotionCommand *command) {

  const SensorDataFrame *frame = s_data->getFrame(0);

  if(strcmp(model,"ERS7")==0){
    if(FS->see_ball_high)
      command->addBinLEDs(LED_EARS_CYAN);
    else if(FS->see_ball_low)
      command->addBinLEDs(LED_EARS_BLUE);

    
    if(FS->world_model->ball_on_chest_prob > .5)
      command->setBinLEDs(LED_EARS_GREEN);

    if(frame->wlan)
      command->addBinLEDs(LED_TINY);

    if(!FS->team_msg_mgr->networkOkay())
      command->addBinLEDs(LED_TOP_ORANGE);


    if(FS->team_msg_mgr->isPrimaryAttacker()) {
      // primary attacker
      command->setFaceLEDs(LED_G|LED_K|LED_H, LED_BRIGHT,MODE_A);
    } else if(FS->team_msg_mgr->isOffensiveSupporter()) {
      // offensive supporter
      command->setFaceLEDs(LED_G|LED_H, LED_BRIGHT,MODE_A);
    } else {
      // defensive supported
      command->setFaceLEDs(LED_K, LED_BRIGHT,MODE_A);
    }
    
    int num_teammates = FS->team_msg_mgr->countActiveTeammates();
    if(num_teammates == 1) {
      command->setFaceLEDs(LED_L, LED_BRIGHT,MODE_A);
    } else if(num_teammates==2) {
      command->setFaceLEDs(LED_C|LED_D, LED_BRIGHT,MODE_A);
    } else if(num_teammates == 3) {
      command->setFaceLEDs(LED_C|LED_L|LED_D, LED_BRIGHT,MODE_A);
    } else if(num_teammates==4) {
      command->setFaceLEDs(LED_C|LED_E|LED_L|LED_D, LED_BRIGHT,MODE_A);
    }

    if(FS->defend_goal_color == DEFEND_YELLOW_GOAL){
      command->setBackLEDs(LED_BACK_REAR_COLOR, LED_BRIGHT);
    }else{
      command->setBackLEDs(LED_BACK_FRONT_COLOR, LED_BRIGHT);
    }

  }else{
    if(FS->see_ball_low)
      command->addBinLEDs(LED_MIDDLE_RIGHT);
    if(FS->see_ball_med)
      command->addBinLEDs(LED_MIDDLE_LEFT);

    
    if(FS->team_msg_mgr->isPrimaryAttacker()) {
      // primary attacker
      command->setBinLEDs(LED_UPPER_RIGHT|LED_UPPER_LEFT);
    } else if(FS->team_msg_mgr->isOffensiveSupporter()) {
      // offensive supporter
      
    } else {
      // defensive supported
    }
  }



  return 1.0;
}

bool LEDs::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *ep_ptr;

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

  sensor_id = event_mgr->getEventProcessorId("SensorData");
  ep_ptr = event_mgr->listenEventProcessor(beh_name, sensor_id);
  sensor_data = (SensorData *)(ep_ptr);

  return true;
}


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

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


const MotionCommand *LEDs::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set
  SensorData *lsd = NULL;

  mzero(out_command);
  mzero(out_command.led);

  if(fs!=NULL &&sensor_data!=NULL){
    lfs = fs->get(time);    
    lsd = sensor_data->get(time);

    (*this)(lfs,lsd, &out_command);
  }
  
  return &out_command;
}


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