/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
 */

#ifndef PCL_WORLD_MODEL_IMPL_HPP_
#define PCL_WORLD_MODEL_IMPL_HPP_

#include <pcl/gpu/kinfu_large_scale/world_model.h>
#include <pcl/common/transforms.h> // for transformPointCloud

template <typename PointT>
void 
pcl::kinfuLS::WorldModel<PointT>::addSlice ( PointCloudPtr new_cloud)
{
  PCL_DEBUG("Adding new cloud. Current world contains %zu points.\n",
            static_cast<std::size_t>(world_->size()));

  PCL_DEBUG("New slice contains %zu points.\n",
            static_cast<std::size_t>(new_cloud->size()));

  *world_ += *new_cloud;

  PCL_DEBUG("World now contains  %zu points.\n",
            static_cast<std::size_t>(world_->size()));
}

template <typename PointT>
void 
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
  double newOriginX = previous_origin_x + offset_x; 
  double newOriginY = previous_origin_y + offset_y; 
  double newOriginZ = previous_origin_z + offset_z;
  double newLimitX = newOriginX + volume_x; 
  double newLimitY = newOriginY + volume_y; 
  double newLimitZ = newOriginZ + volume_z;
	
  // filter points in the space of the new cube
  PointCloudPtr newCube (new pcl::PointCloud<PointT>);
  // condition
  ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ))); 
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condremAND (true);
  condremAND.setCondition (range_condAND);
  condremAND.setInputCloud (world_);
  condremAND.setKeepOrganized (false);
  
  // apply filter
  condremAND.filter (*newCube);
	
  // filter points that belong to the new slice
  ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
  
  if(offset_x >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  previous_origin_x + volume_x - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  previous_origin_x )));
	
  if(offset_y >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  previous_origin_y + volume_y - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  previous_origin_y )));
	
  if(offset_z >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  previous_origin_z + volume_z - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  previous_origin_z )));
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condrem (true);
  condrem.setCondition (range_condOR);
  condrem.setInputCloud (newCube);
  condrem.setKeepOrganized (false);
  // apply filter
  condrem.filter (existing_slice);  
 
  if(!existing_slice.empty ())
  {
	//transform the slice in new cube coordinates
	Eigen::Affine3f transformation; 
	transformation.translation ()[0] = newOriginX;
	transformation.translation ()[1] = newOriginY;
	transformation.translation ()[2] = newOriginZ;
		
	transformation.linear ().setIdentity ();

	transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
	
  }
}


template <typename PointT>
void
pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
{
  
  if(world_->points.empty ())
  {
	PCL_INFO("The world is empty, returning nothing\n");
	return;
  }

  PCL_INFO("Getting world as cubes. World contains %zu points.\n",
           static_cast<std::size_t>(world_->size()));

  // remove nans from world cloud
  world_->is_dense = false;
  pcl::Indices indices;
  pcl::removeNaNFromPointCloud ( *world_, *world_, indices);

  PCL_INFO("World contains %zu points after nan removal.\n",
           static_cast<std::size_t>(world_->size()));

  // check cube size value
  double cubeSide = size;
  if (cubeSide <= 0.0f)
  {
	PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
	cubeSide = 512.0f;
  }

  std::cout << "cube size is set to " << cubeSide << std::endl;

  // check overlap value
  double step_increment = 1.0f - overlap;
  if (overlap < 0.0)
  {
	PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
	step_increment = 1.0f;
  }
  if (overlap > 1.0)
  {
	PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
	step_increment = 0.1f;
  }

  
  // get world's bounding values on XYZ
  PointT min, max;
  pcl::getMinMax3D(*world_, min, max);

  PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);

  PointT origin = min;
  
  // clear returned vectors
  cubes.clear();
  transforms.clear();

  // iterate with box filter
  while (origin.x < max.x)
  {
	origin.y = min.y;
	while (origin.y < max.y)
	{
	  origin.z = min.z;
	  while (origin.z < max.z)
	  {
		// extract cube here
		PCL_INFO ("Extracting cube at: [%f, %f, %f].\n",  origin.x,  origin.y,  origin.z);

		// pointcloud for current cube.
		PointCloudPtr box (new pcl::PointCloud<PointT>);


		// set conditional filter
		ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));

		// build the filter
		pcl::ConditionalRemoval<PointT> condrem;
		condrem.setCondition (range_cond);
		condrem.setInputCloud (world_);
		condrem.setKeepOrganized(false);
		// apply filter
		condrem.filter (*box);

		// also push transform along with points.
		if(!box->points.empty ())
		{
		  Eigen::Vector3f transform;
		  transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
		  transforms.push_back(transform);
		  cubes.push_back(box);        
		}
		else
		{
		  PCL_INFO ("Extracted cube was empty, skipping this one.\n");
		}
		origin.z += cubeSide * step_increment;
	  }
	  origin.y += cubeSide * step_increment;
	}
	origin.x += cubeSide * step_increment;
  }


 /* for(int c = 0 ; c < cubes.size() ; ++c)
  {
	std::stringstream name;
	name << "cloud" << c+1 << ".pcd";
	pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
	
  }*/

  std::cout << "returning " << cubes.size() << " cubes" << std::endl;
}

template <typename PointT>
inline void 
pcl::kinfuLS::WorldModel<PointT>::setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices)
{
  std::vector<pcl::PCLPointField> fields;
  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
  float my_nan = std::numeric_limits<float>::quiet_NaN ();
  
  for (int rii = 0; rii < static_cast<int> (indices->size ()); ++rii)  // rii = removed indices iterator
  {
    std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&(*cloud)[(*indices)[rii]]);
    for (const auto &field : fields)
      memcpy (pt_data + field.offset, &my_nan, sizeof (float));
  }
}


template <typename PointT>
void 
pcl::kinfuLS::WorldModel<PointT>::setSliceAsNans (const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
{ 
  // PCL_DEBUG ("IN SETSLICE AS NANS\n");
  
  PointCloudPtr slice (new pcl::PointCloud<PointT>);
  
  // prepare filter limits on all dimensions  
  double previous_origin_x = origin_x;
  double previous_limit_x = origin_x + size_x - 1;
  double new_origin_x = origin_x + offset_x;
  double new_limit_x = previous_limit_x + offset_x;

  double previous_origin_y = origin_y;
  double previous_limit_y = origin_y + size_y - 1;
  double new_origin_y = origin_y + offset_y;
  double new_limit_y = previous_limit_y + offset_y;  
 
  double previous_origin_z = origin_z;
  double previous_limit_z = origin_z + size_z - 1;
  double new_origin_z = origin_z + offset_z;
  double new_limit_z = previous_limit_z + offset_z; 
   
  // get points of slice on X (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
  double lower_limit_x, upper_limit_x;
  if(offset_x >=0)
  {
	lower_limit_x = previous_origin_x;
	upper_limit_x = new_origin_x;
  }
  else
  {
	lower_limit_x = new_limit_x;
	upper_limit_x = previous_limit_x;    
  }
  
  // PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
  
  ConditionOrPtr range_cond_OR_x (new pcl::ConditionOr<PointT> ());
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  upper_limit_x ))); // filtered dimension
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  lower_limit_x ))); // filtered dimension
	
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  previous_limit_y)));
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  previous_origin_y )));
	
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  previous_limit_z)));
  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  previous_origin_z )));

  pcl::ConditionalRemoval<PointT> condrem_x (true);
  condrem_x.setCondition (range_cond_OR_x);
  condrem_x.setInputCloud (world_);
  condrem_x.setKeepOrganized (false);
  // apply filter
  condrem_x.filter (*slice);  
  IndicesConstPtr indices_x = condrem_x.getRemovedIndices ();
  
  //set outliers (so our slice points) to nan
  setIndicesAsNans(world_, indices_x);
  
  // PCL_DEBUG("%d points set to nan on X\n", indices_x->size ());
  
  // get points of slice on Y (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
  double lower_limit_y, upper_limit_y;
  if(offset_y >=0)
  {
	lower_limit_y = previous_origin_y;
	upper_limit_y = new_origin_y;
  }
  else
  {
	lower_limit_y = new_limit_y;
	upper_limit_y = previous_limit_y;    
  }
  
  // PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
  
  ConditionOrPtr range_cond_OR_y (new pcl::ConditionOr<PointT> ());
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  previous_limit_x )));
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  previous_origin_x )));
	
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  upper_limit_y))); // filtered dimension
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  lower_limit_y ))); // filtered dimension
	
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  previous_limit_z)));
  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  previous_origin_z )));

  pcl::ConditionalRemoval<PointT> condrem_y (true);
  condrem_y.setCondition (range_cond_OR_y);
  condrem_y.setInputCloud (world_);
  condrem_y.setKeepOrganized (false);
  // apply filter
  condrem_y.filter (*slice);  
  IndicesConstPtr indices_y = condrem_y.getRemovedIndices ();
  
  //set outliers (so our slice points) to nan
  setIndicesAsNans(world_, indices_y);
  // PCL_DEBUG ("%d points set to nan on Y\n", indices_y->size ());
  
  // get points of slice on Z (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
  double lower_limit_z, upper_limit_z;
  if(offset_z >=0)
  {
	lower_limit_z = previous_origin_z;
	upper_limit_z = new_origin_z;
  }
  else
  {
	lower_limit_z = new_limit_z;
	upper_limit_z = previous_limit_z;    
  }
  
  // PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
  
  ConditionOrPtr range_cond_OR_z (new pcl::ConditionOr<PointT> ());
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  previous_limit_x )));
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  previous_origin_x )));
	
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  previous_limit_y)));
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  previous_origin_y )));
	
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  upper_limit_z))); // filtered dimension
  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  lower_limit_z ))); // filtered dimension

  pcl::ConditionalRemoval<PointT> condrem_z (true);
  condrem_z.setCondition (range_cond_OR_z);
  condrem_z.setInputCloud (world_);
  condrem_z.setKeepOrganized (false);
  // apply filter
  condrem_z.filter (*slice);  
  IndicesConstPtr indices_z = condrem_z.getRemovedIndices ();
  
  //set outliers (so our slice points) to nan
  setIndicesAsNans(world_, indices_z);
  // PCL_DEBUG("%d points set to nan on Z\n", indices_z->size ());
  
  
}

#define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;

#endif // PCL_WORLD_MODEL_IMPL_HPP_
