/*
 * map_grid_test.cpp
 *
 *  Created on: May 2, 2012
 *      Author: tkruse
 */
#include <queue>

#include <gtest/gtest.h>

#include <base_local_planner/map_grid.h>
#include <base_local_planner/map_cell.h>

#include "wavefront_map_accessor.h"

namespace base_local_planner {

TEST(MapGridTest, initNull){
  MapGrid map_grid;
  EXPECT_EQ(0, map_grid.size_x_);
  EXPECT_EQ(0, map_grid.size_y_);
}

TEST(MapGridTest, operatorBrackets){
  MapGrid map_grid(10, 10);
  map_grid(3, 5).target_dist = 5;
  EXPECT_EQ(5, map_grid.getCell(3, 5).target_dist);
}

TEST(MapGridTest, copyConstructor){
  MapGrid map_grid(10, 10);
  map_grid(3, 5).target_dist = 5;
  MapGrid map_grid2;
  map_grid2 = map_grid;
  EXPECT_EQ(5, map_grid(3, 5).target_dist);
}

TEST(MapGridTest, getIndex){
  MapGrid map_grid(10, 10);
  EXPECT_EQ(53, map_grid.getIndex(3, 5));
}

TEST(MapGridTest, reset){
  MapGrid map_grid(10, 10);
  map_grid(0, 0).target_dist = 1;
  map_grid(0, 0).target_mark = true;
  map_grid(0, 0).within_robot = true;
  map_grid(3, 5).target_dist = 1;
  map_grid(3, 5).target_mark = true;
  map_grid(3, 5).within_robot = true;
  map_grid(9, 9).target_dist = 1;
  map_grid(9, 9).target_mark = true;
  map_grid(9, 9).within_robot = true;
  EXPECT_EQ(1, map_grid(0, 0).target_dist);
  EXPECT_EQ(true, map_grid(0, 0).target_mark);
  EXPECT_EQ(true, map_grid(0, 0).within_robot);
  EXPECT_EQ(1, map_grid(3, 5).target_dist);
  EXPECT_EQ(true, map_grid(3, 5).target_mark);
  EXPECT_EQ(true, map_grid(3, 5).within_robot);
  EXPECT_EQ(1, map_grid(9, 9).target_dist);
  EXPECT_EQ(true, map_grid(9, 9).target_mark);
  EXPECT_EQ(true, map_grid(9, 9).within_robot);

  map_grid.resetPathDist();

  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(9, 9).target_dist);
  EXPECT_EQ(false, map_grid(9, 9).target_mark);
  EXPECT_EQ(false, map_grid(9, 9).within_robot);
  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(3, 5).target_dist);
  EXPECT_EQ(false, map_grid(3, 5).target_mark);
  EXPECT_EQ(false, map_grid(3, 5).within_robot);
  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(0, 0).target_dist);
  EXPECT_EQ(false, map_grid(0, 0).target_mark);
  EXPECT_EQ(false, map_grid(0, 0).within_robot);
}


TEST(MapGridTest, properGridConstruction){
  MapGrid mg(10, 10);
  MapCell mc;

  for(int i = 0; i < 10; ++i){
    for(int j = 0; j < 10; ++j){
      EXPECT_FLOAT_EQ(mg(i, j).cx, i);
      EXPECT_FLOAT_EQ(mg(i, j).cy, j);
    }
  }
}

TEST(MapGridTest, sizeCheck){
  MapGrid mg(10, 10);
  MapCell mc;

  mg.sizeCheck(20, 25);

  for(int i = 0; i < 20; ++i){
    for(int j = 0; j < 25; ++j){
      EXPECT_FLOAT_EQ(mg(i, j).cx, i);
      EXPECT_FLOAT_EQ(mg(i, j).cy, j);
    }
  }
}

TEST(MapGridTest, adjustPlanEmpty){
  MapGrid mg(10, 10);
  const std::vector<geometry_msgs::PoseStamped> global_plan_in;
  std::vector<geometry_msgs::PoseStamped> global_plan_out;
  double resolution = 0;
  mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
  EXPECT_EQ(0, global_plan_out.size());
}

TEST(MapGridTest, adjustPlan){
  MapGrid mg(10, 10);
  std::vector<geometry_msgs::PoseStamped> global_plan_in;
  std::vector<geometry_msgs::PoseStamped> global_plan_out;
  double resolution = 1;
  geometry_msgs::PoseStamped start;
  start.pose.position.x = 1;
  start.pose.position.y = 1;
  geometry_msgs::PoseStamped end;
  end.pose.position.x = 5;
  end.pose.position.y = 5;
  global_plan_in.push_back(start);
  global_plan_in.push_back(end);
  mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
  
  EXPECT_EQ(1, global_plan_out[0].pose.position.x);
  EXPECT_EQ(1, global_plan_out[0].pose.position.y);
  EXPECT_EQ(5, global_plan_out.back().pose.position.x);
  EXPECT_EQ(5, global_plan_out.back().pose.position.y);

  for (unsigned int i = 1; i < global_plan_out.size(); ++i)
  {
    geometry_msgs::Point& p0 = global_plan_out[i - 1].pose.position;
    geometry_msgs::Point& p1 = global_plan_out[i].pose.position;
    double d = hypot(p0.x - p1.x, p0.y - p1.y);
    EXPECT_LT(d, resolution);
  }
}

TEST(MapGridTest, adjustPlan2){
  std::vector<geometry_msgs::PoseStamped> base_plan, result;

  // Push two points, at (0,0) and (0,1). Gap is 1 meter
  base_plan.push_back(geometry_msgs::PoseStamped());
  base_plan.push_back(geometry_msgs::PoseStamped());
  base_plan.back().pose.position.y = 1.0;

  // resolution >= 1, path won't change
  MapGrid::adjustPlanResolution(base_plan, result, 2.0);
  EXPECT_EQ(2, result.size());
  result.clear();
  MapGrid::adjustPlanResolution(base_plan, result, 1.0);
  EXPECT_EQ(2, result.size());
  result.clear();

  // 0.5 <= resolution < 1.0, one point should be added in the middle
  MapGrid::adjustPlanResolution(base_plan, result, 0.8);
  EXPECT_EQ(3, result.size());
  result.clear();
  MapGrid::adjustPlanResolution(base_plan, result, 0.5);
  EXPECT_EQ(3, result.size());
  result.clear();

  // 0.333 <= resolution < 0.5, two points should be added in the middle
  MapGrid::adjustPlanResolution(base_plan, result, 0.34);
  EXPECT_EQ(4, result.size());
  result.clear();

  // 0.25 <= resolution < 0.333, three points should be added in the middle
  MapGrid::adjustPlanResolution(base_plan, result, 0.32);
  EXPECT_EQ(5, result.size());
  result.clear();

  MapGrid::adjustPlanResolution(base_plan, result, 0.1);
  EXPECT_EQ(11, result.size());
  result.clear();
}

TEST(MapGridTest, distancePropagation){
  MapGrid mg(10, 10);

  WavefrontMapAccessor* wa = new WavefrontMapAccessor(&mg, .25);
  std::queue<MapCell*> dist_queue;
  mg.computeTargetDistance(dist_queue, *wa);
  EXPECT_EQ(false, mg(0, 0).target_mark);

  MapCell& mc = mg.getCell(0, 0);
  mc.target_dist = 0.0;
  mc.target_mark = true;
  dist_queue.push(&mc);
  mg.computeTargetDistance(dist_queue, *wa);
  EXPECT_EQ(true, mg(0, 0).target_mark);
  EXPECT_EQ(0.0,  mg(0, 0).target_dist);
  EXPECT_EQ(true, mg(1, 1).target_mark);
  EXPECT_EQ(2.0,  mg(1, 1).target_dist);
  EXPECT_EQ(true, mg(0, 4).target_mark);
  EXPECT_EQ(4.0,  mg(0, 4).target_dist);
  EXPECT_EQ(true, mg(4, 0).target_mark);
  EXPECT_EQ(4.0,  mg(4, 0).target_dist);
  EXPECT_EQ(true, mg(9, 9).target_mark);
  EXPECT_EQ(18.0, mg(9, 9).target_dist);
}

}