/*
 * footprint_helper_test.cpp
 *
 *  Created on: May 2, 2012
 *      Author: tkruse
 */

#include <gtest/gtest.h>

#include <vector>

#include <base_local_planner/footprint_helper.h>

#include <base_local_planner/map_grid.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/costmap_2d.h>

#include "wavefront_map_accessor.h"

namespace base_local_planner {


class FootprintHelperTest : public testing::Test {
public:
  FootprintHelper fh;

  FootprintHelperTest() {
  }

  virtual void TestBody(){}

  void correctLineCells() {
    std::vector<base_local_planner::Position2DInt> footprint;
    fh.getLineCells(0, 10, 0, 10, footprint);
    EXPECT_EQ(11, footprint.size());
    EXPECT_EQ(footprint[0].x, 0);
    EXPECT_EQ(footprint[0].y, 0);
    EXPECT_EQ(footprint[5].x, 5);
    EXPECT_EQ(footprint[5].y, 5);
    EXPECT_EQ(footprint[10].x, 10);
    EXPECT_EQ(footprint[10].y, 10);
  }

  void correctFootprint(){
    MapGrid* mg = new MapGrid (10, 10);
    WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
    const costmap_2d::Costmap2D& map = *wa;

    std::vector<geometry_msgs::Point> footprint_spec;
    geometry_msgs::Point pt;
    //create a square footprint
    pt.x = 2;
    pt.y = 2;
    footprint_spec.push_back(pt);
    pt.x = 2;
    pt.y = -2;
    footprint_spec.push_back(pt);
    pt.x = -2;
    pt.y = -2;
    footprint_spec.push_back(pt);
    pt.x = -2;
    pt.y = 2;
    footprint_spec.push_back(pt);

    Eigen::Vector3f pos(4.5, 4.5, 0);
    //just create a basic footprint
    std::vector<base_local_planner::Position2DInt> footprint = fh.getFootprintCells(pos, footprint_spec, map, false);

    EXPECT_EQ(20, footprint.size());
    //we expect the front line to be first
    EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
    EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
    EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
    EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
    EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);

    //next the right line
    EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
    EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
    EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
    EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
    EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);

    //next the back line
    EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
    EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
    EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
    EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
    EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);

    //finally the left line
    EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
    EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
    EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
    EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
    EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);


    pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
    //check that rotation of the footprint works
    footprint = fh.getFootprintCells(pos, footprint_spec, map, false);

    //first the left line
    EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
    EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
    EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
    EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
    EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);

    //next the front line
    EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
    EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
    EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
    EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
    EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);

    //next the right line
    EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
    EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
    EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
    EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
    EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);

    //next the back line
    EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
    EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
    EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
    EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
    EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
  }

};


TEST(FootprintHelperTest, correctFootprint){
  FootprintHelperTest tct;
  tct.correctFootprint();
}

TEST(FootprintHelperTest, correctLineCells){
  FootprintHelperTest tct;
  tct.correctLineCells();
}

}