//----------------------------------------------------------------------------
void Aggressor::threeManDefense(RobotIndex ID,
                             BasePlay* play,
                             const VisionData& field,
                             RobocupStrategyData* sd)
{
  //-------------------
  //Display message
  //-------------------
  sd->setMessage(ID, "AGGRESSOR Three Man Defense");

  //-------------------
  //get a handle on skillset for this robot
  //-------------------
SkillSet* skills = sd->getStrategyModule().getSkillSet(ID);

  //-------------------
  //get a handle on Agressor Recover Ball skill
  //-------------------
//  Skill* skillHandle = skills->getSkill(AcquirePossessionSkill::skillNum);
  Skill* skillHandle = skills->getSkill(SupplementThreeManSkill::skillNum);
  //-------------------
  //initialize skill if it is not initialized
  //-------------------
  if(!skillHandle->isInitialized())
	  skillHandle->initialize();

  //-------------------
  //run skill
  //-------------------
  skillHandle->run();
}
//----------------------------------------------------------------------------
void Defender::looseDefense(RobotIndex ID,
                            BasePlay* play,
                            const VisionData& field,
                            RobocupStrategyData* sd)
{
  //===================
  //===================
  //Handle the Defender
  //===================
  //===================

  //-------------------
  //Display message
  //-------------------
  sd->setMessage(ID, "DEFENDER Loose Defense");

  //-------------------
  //get a handle on skillset for this robot
  //-------------------
  SkillSet* skills = sd->getStrategyModule().getSkillSet(ID);

  //-------------------
  //get a handle on loose defense defender skill
  //-------------------
  TandemDefenderSkill* skillHandle = 
  (TandemDefenderSkill*)skills->getSkill(TandemDefenderSkill::skillNum);

  //-------------------
  //initialize skill if it is not initialized
  //-------------------
  if(!skillHandle->isInitialized())
	  skillHandle->initialize();

  //-------------------
  //run skill
  //-------------------
  skillHandle->run();


  //===================
  //===================
  //Handle the Special Op Defender if he exists
  //===================
  //===================
  RobotIndex SpecialOpID = sd->getRobotByPosition(SPECIAL_OP_DEFENDER);
  if(SpecialOpID != NO_ROBOT)
  {
    Pair specialOpDefenderLocation=getLocation(SpecialOpID,field,sd->getSystemParams());
    Pair defenderLocation=getLocation(ID,field,sd->getSystemParams());

    if(
       (specialOpDefenderLocation.getY() > getBallLocation(field).getY() && 
          defenderLocation.getY() > sd->getSystemParams().field.SPLIT_LINE) ||
       (specialOpDefenderLocation.getY() < getBallLocation(field).getY() && 
          defenderLocation.getY() < sd->getSystemParams().field.SPLIT_LINE)
      )
    {
      sd->setMessage(SpecialOpID, "SPECIAL OP DEFENDER Taking over for Defender");
      sd->getDestination(SpecialOpID)->setPos(sd->getDestination(ID)->getPos());
    }

    //-------------------
    //Display message
    //-------------------
    sd->setMessage(SpecialOpID, "SPECIAL OP DEFENDER Loose Defense");


    //-------------------
    //get a handle on skillset for this robot
    //-------------------
    SkillSet* skills = sd->getStrategyModule().getSkillSet(SpecialOpID);

    //-------------------
    //get a handle on loose defense special Op skill
    //-------------------
    Skill* soSkillHandle;

    if(sd->getSystemParams().strategy2002.ENABLE_FOLLOW_DEFNDER_SKILL)
    {
      soSkillHandle = skills->getSkill(FollowDefenderSkill::skillNum);
    }
    else
    {
      soSkillHandle = skills->getSkill(LooseDSpecialOpSkill::skillNum);
    }

    //-------------------
    //initialize skill if it is not initialized
    //-------------------
    if(!soSkillHandle->isInitialized())
  	  soSkillHandle->initialize();

    //-------------------
    //run skill
    //-------------------
    soSkillHandle->run();
  }
}
//===============================================================================
//Execute the skill.  This is the main part of the skill, where you tell the
//robot how to perform the skill.
void SupplementThreeManSkill::execute()
{    
  ///If not initialized, dont do anything!
  if(!initialized) 
  {
    return;  
  }

  //if the ball is close enough to defenisve players, move next to 
  //the defensive player that is closer to the ball
  //since he'll become the aggresor and move upfield. 
  //this way we can slide in and form the defensive wall immediately.
  Pair ballLoc = getBallLocation(*currentVisionData);
  Pair robotLoc = getLocation(robotID, *currentVisionData, *sp);
  RobotIndex closeDefensiveID = NO_ROBOT;
  float closestDistance;

  RobotIndex tempID;
  float tempDistance;

  tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER);
  if(tempID != NO_ROBOT)
  {
    closestDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
    closeDefensiveID = tempID;
  }

  tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
  if(tempID != NO_ROBOT)
  {
    tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
    if(tempDistance < closestDistance)
    {
      closestDistance = tempDistance;
      closeDefensiveID = tempID;
    }
    if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
      closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
  }

  tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
  if(tempID != NO_ROBOT)
  {
    tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
    if(tempDistance < closestDistance)
    {
      closestDistance = tempDistance;
      closeDefensiveID = tempID;
    }
    if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
      closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
  }
  
  //if closest defensive player 
  //a.) exists
  //b.) within tolerance, then go to side of him
  if(closestDistance < 3.0f*sp->general.PLAYER_RADIUS)
  {
    Pair defensivePlayer = getLocation(closeDefensiveID, *currentVisionData, *sp);
    if(defensivePlayer.getY() > sp->field.SPLIT_LINE)
    {
      command->setYPos(defensivePlayer.getY() + sp->general.PLAYER_RADIUS + 0.02f);
    }
    else
    {
      command->setYPos(defensivePlayer.getY() - sp->general.PLAYER_RADIUS - 0.02f);
    }
    command->setXPos(defensivePlayer.getX());
    command->setRotation(angleBetween(robotLoc, ballLoc));

  }
  //else acquire the ball
  else
  {
    Skill* skillHandle = skillSet->getSkill(AcquirePossessionSkill::skillNum);
    if(!skillHandle->isInitialized())
      skillHandle->initialize();
    skillHandle->run();
  }
}