bool ChooseAccessibleBalls::canMove(float x, float y){
	return true;

//	  ROS_INFO("enter canMove");
	  // Get a copy of the current costmap to test. (threadsafe)
	  costmap_2d::Costmap2D costmap;
//	  ROS_INFO("aaa");
	  if(costmap_ros != NULL)
		  costmap_ros->getCostmapCopy( costmap );

//	  ROS_INFO("bbb");
	  // Coordinate transform.
	  unsigned int cell_x, cell_y;
	  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	    return false;
	  }

	  double cost = double( costmap.getCost( cell_x, cell_y ));
	  if(cost <= 127){
		  return true;
	  }
	  else{
		  return false;
	  }
}

void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;
  unsigned char* master_array = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();

  for (int j = min_j; j < max_j; j++)
  {
    unsigned int it = j * span + min_i;
    for (int i = min_i; i < max_i; i++)
    {
      if (costmap_[it] == NO_INFORMATION){
        it++;
        continue;
      }

      unsigned char old_cost = master_array[it];
      if (old_cost == NO_INFORMATION)
        master_array[it] = costmap_[it];
      else
      {
        int sum = old_cost + costmap_[it];
        if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
            master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
        else
            master_array[it] = sum;
      }
      it++;
    }
  }
}

void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();

  for (int j = min_j; j < max_j; j++)
  {
    unsigned int it = j * span + min_i;
    for (int i = min_i; i < max_i; i++)
    {
      if (costmap_[it] == NO_INFORMATION){
        it++;
        continue;
      }

      unsigned char old_cost = master_array[it];
      if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
        master_array[it] = costmap_[it];
      it++;
    }
  }
}

void upTo(costmap_2d::Costmap2D &gradient_){

	for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
		for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
			//if(gradient_.getCost(varX, varY) == 0)
				gradient_.setCost(varX, varY, DEF);
		}
	}
}

bool CSUROGlobalPlanner::isCoordInMap(const costmap_2d::Costmap2D &costmap, const float& x, const float& y)
{
	int cellI, cellJ;

	original_costmap_.worldToMapEnforceBounds(x, y, cellI, cellJ);

	if(cellI < costmap.getSizeInCellsX() && cellI >= 0 && cellJ < costmap.getSizeInCellsY() && cellJ >= 0)
		return true;
	return false;
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

bool inMap(costmap_2d::Costmap2D &gradient_, int i, int j)
{
	if(i < gradient_.getSizeInCellsX() && i >= 0 && j < gradient_.getSizeInCellsY() && j >= 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!map_received_)
    return;

  if (!layered_costmap_->isRolling())
  {
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_)
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
  }
  else
  {
    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame
    tf::StampedTransform transform;
    try
    {
      tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    for (unsigned int i = min_i; i < max_i; ++i)
    {
      for (unsigned int j = min_j; j < max_j; ++j)
      {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf::Point p(wx, wy, 0);
        p = transform(p);
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my))
        {
          if (!use_maximum_)
            master_grid.setCost(i, j, getCost(mx, my));
          else
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
        }
      }
    }
  }
}

//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells (const costmap_2d::Costmap2D& costmap,
                              const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
    sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    bool started_path = false;

    queue<MapCell*> path_dist_queue;

    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());

    if (adjusted_global_plan.size() != global_plan.size())
    {
        ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
    }

    unsigned int i;

    // put global path points into local map until we reach the border of the local map
    for (i = 0; i < adjusted_global_plan.size(); ++i)
    {
        double g_x = adjusted_global_plan[i].pose.position.x;
        double g_y = adjusted_global_plan[i].pose.position.y;
        unsigned int map_x, map_y;

        if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
        {
            MapCell& current = getCell (map_x, map_y);
            current.target_dist = 0.0;
            current.target_mark = true;
            path_dist_queue.push (&current);
            started_path = true;
        }
        else if (started_path)
        {
            break;
        }
    }

    if (!started_path)
    {
        ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
                   i, adjusted_global_plan.size(), global_plan.size());
        return;
    }

    computeTargetDistance (path_dist_queue, costmap);
}

double getCost(float x, float y){
  // Get a copy of the current costmap to test. (threadsafe)
  costmap_2d::Costmap2D costmap;
  costmap_ros->getCostmapCopy( costmap );

  // Coordinate transform.
  unsigned int cell_x, cell_y;
  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	  return -1.0;
  }

  double cost = double( costmap.getCost( cell_x, cell_y ));
//  ROS_INFO(" world pose = (%f, %f)   map pose = (%d, %d)  cost =%f", x, y, cell_x, cell_y,  cost);
  return cost;
}

inline bool MapGrid::updatePathCell (MapCell* current_cell, MapCell* check_cell,
                                     const costmap_2d::Costmap2D& costmap)
{

    //if the cell is an obstacle set the max path distance
    unsigned char cost = costmap.getCost (check_cell->cx, check_cell->cy);

    if (! getCell (check_cell->cx, check_cell->cy).within_robot &&
            (cost == costmap_2d::LETHAL_OBSTACLE ||
             cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
             cost == costmap_2d::NO_INFORMATION))
    {
        check_cell->target_dist = obstacleCosts();
        return false;
    }

    double new_target_dist = current_cell->target_dist + 1;

    if (new_target_dist < check_cell->target_dist)
    {
        check_cell->target_dist = new_target_dist;
    }

    return true;
}

void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;
  unsigned char* master = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();

  for (int j = min_j; j < max_j; j++)
  {
    unsigned int it = span*j+min_i;
    for (int i = min_i; i < max_i; i++)
    {
      if (costmap_[it] != NO_INFORMATION)
        master[it] = costmap_[it];
      it++;
    }
  }
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(pode_marcar){
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
      for(int i = -1; i < 1; i++)
    master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE);
  }

  pode_marcar = false;
  }

 // ROS_INFO("Hi!");
}

  void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
  {
    if (!enabled_)
      return;

    const unsigned char* master_array = master_grid.getCharMap();

    for (int j = min_j; j < max_j; j++)
    {
      for (int i = min_i; i < max_i; i++)
      {

        if (clear_flag)
        {
          master_grid.setCost(i, j, FREE_SPACE);
        }
        else
        {
          int index = getIndex(i, j);

          if (costmap_[index] == NO_INFORMATION)
            continue;
        
          unsigned char old_cost = master_array[index];

          if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
            master_grid.setCost(i, j, costmap_[index]);
	 }
       }
     }

     for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
       costmap_[i] == NO_INFORMATION;

     if (!obstacle_buffer.empty())
     {
       ROS_INFO("Costmap updated with ball obstacles.");
       obstacle_buffer.clear();
       std_srvs::Empty emptysrv;
       confirm_update_client.call(emptysrv);
     }

  }

bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost)
{
	if(inMap(gradient_, i, j) == false){
		//ROS_INFO("salgo por no estar en mapa");
		return false;
	}

	if(original_.getCost( i, j) != 0 ){
		//ROS_INFO("salgo por ser obstaculo");
		return false;
	}

	if(gradient_.getCost( i, j) <= cost){
		//ROS_INFO("salgo por ser igual o menor");
		return false;
	}

	return true;
}

bool canMove(float x, float y){
	  // Get a copy of the current costmap to test. (threadsafe)
	  costmap_2d::Costmap2D costmap;
	  costmap_ros->getCostmapCopy( costmap );

	  // Coordinate transform.
	  unsigned int cell_x, cell_y;
	  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	 //   res.cost = -1.0;
	    return false;
	  }

	  double cost = double( costmap.getCost( cell_x, cell_y ));
	  ROS_INFO("cost = %f", cost);
	  if(cost <= 1){
		  return true;
	  }
	  else{
		  return false;
	  }
	 //ROS_INFO(" world pose = (%f, %f)   map pose = (%d, %d)  cost =%f", x, y, cell_x, cell_y,  cost);
}

void VirtualWallLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_)
        return;

    for (int j = min_j; j < max_j; j++)
    {
        for (int i = min_i; i < max_i; i++)
        {
            int index = getIndex(i, j);
            if (costmap_[index] == NO_INFORMATION)
                continue;
            master_grid.setCost(i, j, costmap_[index]);
        }
    }
}

//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
                            const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
    sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    int local_goal_x = -1;
    int local_goal_y = -1;
    bool started_path = false;

    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());

    // skip global path points until we reach the border of the local map
    for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
    {
        double g_x = adjusted_global_plan[i].pose.position.x;
        double g_y = adjusted_global_plan[i].pose.position.y;
        unsigned int map_x, map_y;

        if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
        {
            local_goal_x = map_x;
            local_goal_y = map_y;
            started_path = true;
        }
        else
        {
            if (started_path)
            {
                break;
            }// else we might have a non pruned path, so we just continue
        }
    }

    if (!started_path)
    {
        ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
        return;
    }

    queue<MapCell*> path_dist_queue;

    if (local_goal_x >= 0 && local_goal_y >= 0)
    {
        MapCell& current = getCell (local_goal_x, local_goal_y);
        costmap.mapToWorld (local_goal_x, local_goal_y, goal_x_, goal_y_);
        current.target_dist = 0.0;
        current.target_mark = true;
        path_dist_queue.push (&current);
    }

    computeTargetDistance (path_dist_queue, costmap);
}

void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ)
{

	gradient_.setCost(i, j, value);

	//ROS_INFO("(%d, %d) <- %d", i, j, value);

	value = value + 5;
	if(value>254)
		value=254;

	if(go(gradient_, original_, i-1,j, value))
		spread(gradient_, original_, value, i-1, j, startCellI, startCellJ);
	if(go(gradient_, original_, i,j-1, value))
		spread(gradient_, original_, value, i, j-1, startCellI, startCellJ);
	if(go(gradient_, original_, i+1,j, value))
		spread(gradient_, original_, value, i+1, j, startCellI, startCellJ);
	if(go(gradient_, original_, i,j+1, value))
		spread(gradient_, original_, value, i, j+1, startCellI, startCellJ);


}

void fillPlan(costmap_2d::Costmap2D gradient_, std::vector<geometry_msgs::PoseStamped>& plan, const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, int startI, int startJ, int goalI, int goalJ)
{


	int iPoint = startI;
	int jPoint = startJ;

	plan.push_back(start);

	double x,y;
	int counter = 0;
	geometry_msgs::PoseStamped before = start;
	geometry_msgs::PoseStamped point;
	tf::Quaternion point_quat;

	while(isEnd(gradient_, iPoint, jPoint, goalI, goalJ) == false)
	{
		counter++;
		getMin(gradient_, iPoint, jPoint);
		gradient_.mapToWorld (iPoint, jPoint, x, y);
		point.pose.position.x = x;
		point.pose.position.y = y;
		point_quat = tf::createQuaternionFromYaw(atan2 (y, x));
		point.pose.orientation.x = point_quat.x();
		point.pose.orientation.y = point_quat.y();
		point.pose.orientation.z = point_quat.z();
		point.pose.orientation.w = point_quat.w();
		if(counter%2 == 0)
			plan.push_back(point);
		before = point;
	}

	plan.push_back(goal);



}