/* 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 "ObeyGameMgr.h"
#include "../Main/RobotMain.h"


// button must be down for at least this many frames
// before we transition between states.
const double trans_press_time = 1;

/*============================================================*/

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

ObeyGameMgr::ObeyGameMgr(){

  gotopoint = new BeGotoPointGlobal();
  navtopoint = new BeNavToPointGlobal();
  track_obj = new BeTrackObjects();

  event_mgr = NULL;
  fs = NULL;

  trans_button_down_cnt = 0;
  color_button_down_cnt = 0;
  kick_button_down_cnt = 0;
};

ObeyGameMgr::~ObeyGameMgr(){
  if(gotopoint!=NULL){
    delete gotopoint;
    gotopoint = NULL;
  }
  if(navtopoint!=NULL){
    delete navtopoint;
    navtopoint = NULL;
  }
  if(track_obj!=NULL) {
    delete track_obj;
    track_obj = NULL;
  }
}

double ObeyGameMgr::operator()(FeatureSet *features, MotionCommand *command) {

  RobotMain &r_main = RobotMain::GetObject();
  int game_state = r_main.robot_state;

  static bool penalty_resume_play = false;
  static bool penalty_state_reset = false;

  /* Button variable declarations */
  int transition_button;
  int team_color_button;
  int kickoff_button;
  
  // Figure out which model we are and use buttons accordingly
  if(strcmp(model,"ERS7")==0){
    if(game_state == ROBOCUP_STATE_PLAYING){
      transition_button = BackButton;
    } else {
      transition_button = HeadFrontButton;
    }

    kickoff_button = BackButton;
    if(features->world_model->teamColor == TEAM_COLOR_BLUE){
      team_color_button = BackRearButton;
    } else {
      team_color_button = BackFrontButton;
    }
  } else {
    transition_button = BackButton;
    kickoff_button    = HeadFrontButton;
    team_color_button = HeadBackButton;
  }

  // We need to track how long they hold down buttons for
  // and reset our counts when each button is released.
  if(features->sensors->button & transition_button) {
    // When we transition we set the button count to a negative
    // number to indicate that they need to release it before
    // we'll consider it as a new button press
    if(trans_button_down_cnt >= 0)
      trans_button_down_cnt++;
    
  } else {
    if(trans_button_down_cnt < 0)
      trans_button_down_cnt++;
    else
      trans_button_down_cnt = 0;
  }

  if(features->sensors->button & team_color_button) {
    // When we transition we set the button count to a negative
    // number to indicate that they need to release it before
    // we'll consider it as a new button press
    if(color_button_down_cnt >= 0)
      color_button_down_cnt++;
  } else {
    if(color_button_down_cnt < 0)
      color_button_down_cnt++;
    else
      color_button_down_cnt = 0;
  }

  if(features->sensors->button & kickoff_button) {
    // When we transition we set the button count to a negative
    // number to indicate that they need to release it before
    // we'll consider it as a new button press
    if(kick_button_down_cnt >= 0)
      kick_button_down_cnt++;
    
  } else {
    if(kick_button_down_cnt < 0)
      kick_button_down_cnt++;
    else
      kick_button_down_cnt = 0;
  }

  // we'll use these when we move into position
  double side = sign(features->defend_goal_const.x);
  double angle_to_center = features->score_goal_const.angle();

  switch(game_state) {
    
    case ROBOCUP_STATE_INITIAL:
      
      // Handle button presses
      if(trans_button_down_cnt >= trans_press_time) {
	r_main.robot_state = ROBOCUP_STATE_READY;
	trans_button_down_cnt = -10;
      } else if(color_button_down_cnt > 5) {
	if(features->world_model->teamColor == TEAM_COLOR_BLUE) {
	  features->world_model->teamColor = TEAM_COLOR_RED;
	  features->world_model->goalColor = DEFEND_YELLOW_GOAL;
	} else {
	  features->world_model->teamColor = TEAM_COLOR_BLUE;
	  features->world_model->goalColor = DEFEND_CYAN_GOAL;
	}
	color_button_down_cnt = -10;
      } else if(kick_button_down_cnt > 5) {
	r_main.have_kickoff = !r_main.have_kickoff;
	kick_button_down_cnt = -10;
      }

      // The robot is not allowed to move
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->head_cmd = HEAD_LOOKAT;			
      command->head_lookat.set(350.0,0.0,0.0);

      command->setBinLEDs(LED_EARS_GREEN);

      /* Set tail/back LED to show team color */
      if(features->world_model->teamColor == TEAM_COLOR_BLUE){
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_FRONT_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_BLUE);
	}
      }
      else{
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_REAR_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_RED);
	}
      }

      // Display whether or not we have the kickoff.
      if(r_main.have_kickoff){
	if(strcmp(model,"ERS7")==0) {
	  command->setBackLEDs(LED_BACK_MIDDLE_COLOR,LED_BRIGHT);
	} else {
	  command->addBinLEDs(LED_TOP);
	}
      }
      penalty_resume_play = false;
      penalty_state_reset = false;
      break;

  case ROBOCUP_STATE_READY:
    // Move into position for kickoff
    command->setBinLEDs(LED_EARS_CYAN);

    if(features->team_msg_mgr->i_am_goalie){
      target.set(features->defend_goal_const.x*0.95, 
		 features->defend_goal_const.y, angle_to_center);
    }
    else if(features->kickoff){ 
      switch(RobotMain::GetObject().getRobotName()){
      case RobotMain::MOON:
      case RobotMain::MIST:
	//case RobotMain::MIST:
	target.set(side*halfLength*0.6, halfWidth*0.5, angle_to_center);
	break;
	//case RobotMain::CLOUD:
	//case RobotMain::SNOW:
      case RobotMain::FROST:
	target.set(side*250.0, 0.0, angle_to_center);
	break;
	//case RobotMain::SILVER:
      case RobotMain::COTTON: 
	target.set(side*250.0, -halfWidth*0.7, angle_to_center);
	break;
      default:
	target.set(side*halfLength*0.6, 0, angle_to_center);
	break;
      }
    }else{
      switch(RobotMain::GetObject().getRobotName()){
      case RobotMain::MOON:
      case RobotMain::MIST:
	target.set(side*halfLength*0.6, halfWidth*0.6, angle_to_center);
	break;
      case RobotMain::FROST:
	target.set(side*halfLength*0.6, 0.0, angle_to_center);
	break;
      case RobotMain::COTTON: 
	target.set(side*halfLength*0.6, -halfWidth*0.6, angle_to_center);
	break;
      default:
	target.set(side*halfLength*0.6, 0, angle_to_center);
	break;
      }
    }
	
    navtopoint->setCheckObstParams(200,200,4);
    navtopoint->setTargetGlobal(target.x, target.y);
    gotopoint->setTarget(target.x, target.y, target.z);

    if(sign(features->my_position.position.mean.x) != side &&
       gotopoint->getTarget().length() > 900.0){
      command->head_cmd = HEAD_SCAN_OBSTACLES;
      (*navtopoint)(features, command,true);
    }else{
      (*track_obj)(features, command, true, false);
      (*gotopoint)(features, command);
    }
    
      if(fabs(features->position.x - target.x) < 150.0 &&
	 fabs(features->position.y - target.y) < 150.0 &&
	 fabs(features->my_angle - target.z) < 0.2){
	command->motion_cmd = MOTION_STAND_NEUTRAL;
      }
      
      // Handle transitions
      if(trans_button_down_cnt >= trans_press_time) {
	r_main.robot_state = ROBOCUP_STATE_SET;
	trans_button_down_cnt = -10;
      }
      penalty_resume_play = false;
      penalty_state_reset = false;

      /* Set tail/back LED to show team color */
      if(features->world_model->teamColor == TEAM_COLOR_BLUE){
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_FRONT_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_BLUE);
	}
      }
      else{
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_REAR_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_RED);
	}
      }

      // Display whether or not we have the kickoff.
      if(r_main.have_kickoff){
	if(strcmp(model,"ERS7")==0) {
	  command->setBackLEDs(LED_BACK_MIDDLE_COLOR,LED_BRIGHT);
	} else {
	  command->addBinLEDs(LED_TOP);
	}
      }
      break;
      
    case ROBOCUP_STATE_SET:
      // We *are* allowed to use our heads for active localization,
      // but we can't move our feet
      command->setBinLEDs(LED_EARS_PURPLE);
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      
      (*track_obj)(features, command, true, true);
      
      if(trans_button_down_cnt >= trans_press_time) {
	r_main.robot_state = ROBOCUP_STATE_PLAYING;
	trans_button_down_cnt = -10;
      }
      penalty_resume_play = false;
      penalty_state_reset = false;
      
      /* Set tail/back LED to show team color */
      if(features->world_model->teamColor == TEAM_COLOR_BLUE){
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_FRONT_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_BLUE);
	}
      }
      else{
	if(strcmp(model,"ERS7")==0){
	  command->setBackLEDs(LED_BACK_REAR_COLOR,LED_BRIGHT);
	}else{
	  command->addBinLEDs(LED_TAIL_RED);
	}
      }

      // Display whether or not we have the kickoff.
      if(r_main.have_kickoff){
	if(strcmp(model,"ERS7")==0) {
	  command->setBackLEDs(LED_BACK_MIDDLE_COLOR,LED_BRIGHT);
	} else {
	  command->addBinLEDs(LED_TOP);
	}
      }
      break;

    case ROBOCUP_STATE_PLAYING:
      // do absolutely nothing except transition
      if(trans_button_down_cnt > 30) {
	r_main.robot_state = ROBOCUP_STATE_PENALIZED;
	trans_button_down_cnt = -10;
      }
      penalty_resume_play = false;
      penalty_state_reset = false;
      break;

    case ROBOCUP_STATE_PENALIZED:
      // We are not allowed to move when penalized
      command->setBinLEDs(LED_EARS_RED);
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->head_cmd = HEAD_LOOKAT;			
      command->head_lookat.set(350.0,0.0,0.0);
      
      if(trans_button_down_cnt >= trans_press_time)
	penalty_resume_play = true;

      if(trans_button_down_cnt > 90) {
	//	penalty_state_reset = true;
	r_main.robot_state = ROBOCUP_STATE_INITIAL;
	trans_button_down_cnt = -10;
	penalty_resume_play = false;
	penalty_state_reset = false;
      }

      if(trans_button_down_cnt==0 &&
	 penalty_state_reset) {
	r_main.robot_state = ROBOCUP_STATE_INITIAL;
	trans_button_down_cnt = -10;
	penalty_resume_play = false;
	penalty_state_reset = false;
      } else if(trans_button_down_cnt==0 &&
		penalty_resume_play) {
	r_main.robot_state = ROBOCUP_STATE_PLAYING;
	trans_button_down_cnt = -10;
	penalty_resume_play = false;
	penalty_state_reset = false;
      }

      break;

    case ROBOCUP_STATE_FINISHED:

      break;
    
  }

  return 1.0;
}

double ObeyGameMgr::status(FeatureSet *features){
  
  if(fabs(features->position.x - target.x) < 100.0 &&
     fabs(features->position.y - target.y) < 100.0 &&
     fabs(features->my_angle - target.z) < 0.2 &&
     features->current_time - start_time > timeout){
    return 1.0;
  }
  return 0.0;

}

void ObeyGameMgr::reset(ulong time){
  if(event_mgr == NULL)
    setupEventMgr();

  start_time = time;

  gotopoint->reset(time);
}

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

const MotionCommand *ObeyGameMgr::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(ObeyGameMgr,ObeyGameMgr::beh_name,ObeyGameMgr::create);