/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <voxel_grid/voxel_grid.h>
#include <gtest/gtest.h>

TEST(voxel_grid, basicMarkingAndClearing){
  int size_x = 50, size_y = 10, size_z = 16;
  voxel_grid::VoxelGrid vg(size_x, size_y, size_z);

  //Put a "tabletop" into the scene.  A flat rectangle of set voxels at z = 12.
  int table_z = 12;
  int table_x_min = 5, table_x_max = 15;
  int table_y_min = 0, table_y_max = 3;
  for(int x = table_x_min; x <= table_x_max; x++){
    vg.markVoxelLine(x, table_y_min, table_z, x, table_y_max, table_z);
  }

  for(int i = table_x_min; i <= table_x_max; ++i){
    for(int j = table_y_min; j <= table_y_max; ++j){
      //check that each cell of the table is marked
      ASSERT_EQ(voxel_grid::MARKED, vg.getVoxel(i, j, table_z));
    }
  }

  int mark_count = 0;
  unsigned int unknown_count = 0;
  //go through each cell in the voxel grid and make sure that only 44 are filled in
  for(unsigned int i = 0; i < vg.sizeX(); ++i){
    for(unsigned int j = 0; j < vg.sizeY(); ++j){
      for(unsigned int k = 0; k < vg.sizeZ(); ++k){
        if(vg.getVoxel(i, j, k) == voxel_grid::MARKED){
          mark_count++;
        }
        else if(vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN){
          unknown_count++;
        }
      }
    }
  }
  ASSERT_EQ(mark_count, 44);

  //the rest of the cells should be unknown
  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);

  //now, let's clear one of the rows of the table
  vg.clearVoxelLine(table_x_min, table_y_min, table_z, table_x_max, table_y_min, table_z);

  mark_count = 0;
  unknown_count = 0;
  int free_count = 0;
  //go through each cell in the voxel grid and make sure that only 33 are now filled in
  for(unsigned int i = 0; i < vg.sizeX(); ++i){
    for(unsigned int j = 0; j < vg.sizeY(); ++j){
      for(unsigned int k = 0; k < vg.sizeZ(); ++k){
        if(vg.getVoxel(i, j, k) == voxel_grid::MARKED){
          mark_count++;
        }
        else if(vg.getVoxel(i, j, k) == voxel_grid::FREE){
          free_count++;
        }
        else if(vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN){
          unknown_count++;
        }
      }
    }
  }

  //we've switched 11 cells from marked to free
  ASSERT_EQ(mark_count, 33);

  //we've just explicitly seen through 11 cells
  ASSERT_EQ(free_count, 11);

  //the rest of the cells should still be unknown
  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);

  //now let's put in a vertical column manually to test markVoxel
  for(unsigned int i = 0; i < vg.sizeZ(); ++i){
    vg.markVoxel(0, 0, i);
    ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::MARKED);
  }

  vg.printColumnGrid();
  vg.printVoxelGrid();

  //now, let's clear that line of voxels and make sure that they clear out OK
  vg.clearVoxelLine(0, 0, 0, 0, 0, vg.sizeZ() - 1);

  for(unsigned int i = 0; i < vg.sizeZ(); ++i){
    ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::FREE);
  }

  mark_count = 0;

  //Visualize the output
  /*
     v->printVoxelGrid();
     v->printColumnGrid();

     printf("CostMap:\n===========\n");
     for(int y = 0; y < size_y; y++){
     for(int x = 0; x < size_x; x++){
     printf((costMap[y * size_x + x] > 0 ? "#" : " "));
     }printf("|\n");
     }
     */
}

int main(int argc, char** argv){
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}