#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/cost_values.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>

namespace costmap_2d {

class CostmapTester : public testing::Test {
  public:
    CostmapTester(tf3::BufferCore& tf);
    void checkConsistentCosts();
    void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
    void compareCells(costmap_2d::Costmap2D& costmap, 
        unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
    virtual void TestBody(){}

  private:
    costmap_2d::Costmap2DROBOT costmap_ros_;
};

CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_global_params", tf){}

void CostmapTester::checkConsistentCosts(){
  costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();

  //get a copy of the costmap contained by our ros wrapper
  costmap->saveMap("costmap_test.pgm");

  //loop through the costmap and check for any unexpected drop-offs in costs
  for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
    for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
      compareCellToNeighbors(*costmap, i, j);
    }
  }
}

void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
  //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
  for(int offset_x = -1; offset_x <= 1; ++offset_x){
    for(int offset_y = -1; offset_y <= 1; ++offset_y){
      int nx = x + offset_x;
      int ny = y + offset_y;

      //check to make sure that the neighbor cell is a legal one
      if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){
        compareCells(costmap, x, y, nx, ny);
      }
    }
  }
}

//for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
  double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));

  unsigned char cell_cost = costmap.getCost(x, y);
  unsigned char neighbor_cost = costmap.getCost(nx, ny);

  if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
    //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
    unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
  }
  else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
    //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
    double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
    unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
    if(neighbor_cost < expected_lowest_cost){
      printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
          x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
      printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
      costmap.saveMap("failing_costmap.pgm");
    }
    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE));
  }
}
};

costmap_2d::CostmapTester* map_tester = NULL;


int main(int argc, char** argv){

  tf3::BufferCore tf(tf3::Duration(10));

  map_tester = new costmap_2d::CostmapTester(tf);


  return(0);
}