/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, 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.
 *
 * $Id:$
 *
 */

#ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_
#define PCL_RECOGNITION_HOUGH_3D_IMPL_H_

#include <pcl/common/io.h> // for copyPointCloud
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
//#include <pcl/sample_consensus/ransac.h>
//#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/board.h>


template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT>
template<typename PointType, typename PointRfType> void
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::computeRf (const typename pcl::PointCloud<PointType>::ConstPtr &input, pcl::PointCloud<PointRfType> &rf)
{
  if (local_rf_search_radius_ == 0)
  {
    PCL_WARN ("[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n");
    local_rf_search_radius_ = static_cast<float> (hough_bin_size_);
  }
  pcl::PointCloud<Normal>::Ptr normal_cloud (new pcl::PointCloud<Normal> ());
  NormalEstimation<PointType, Normal> norm_est;
  norm_est.setInputCloud (input);
  if (local_rf_normals_search_radius_ <= 0.0f)
  {
    norm_est.setKSearch (15);
  }
  else
  {
    norm_est.setRadiusSearch (local_rf_normals_search_radius_);
  }  
  norm_est.compute (*normal_cloud);

  BOARDLocalReferenceFrameEstimation<PointType, Normal, PointRfType> rf_est;
  rf_est.setInputCloud (input);
  rf_est.setInputNormals (normal_cloud);
  rf_est.setFindHoles (true);
  rf_est.setRadiusSearch (local_rf_search_radius_);
  rf_est.compute (rf);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::train ()
{
  if (!input_)
  {
    PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n");
    return (false);
  }

  if (!input_rf_)
  {
    ModelRfCloudPtr new_input_rf (new ModelRfCloud ());
    computeRf<PointModelT, PointModelRfT> (input_, *new_input_rf);
    input_rf_ = new_input_rf;
    //PCL_ERROR(
    //  "[pcl::Hough3DGrouping::train()] Error! Input reference frame not set.\n");
    //return (false);
  }

  if (input_->size () != input_rf_->size ())
  {
    PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n");
    return (false);
  }

  model_votes_.clear ();
  model_votes_.resize (input_->size ());

  // compute model centroid
  Eigen::Vector3f centroid (0, 0, 0);
  for (std::size_t i = 0; i < input_->size (); ++i)
  {
    centroid += input_->at (i).getVector3fMap ();
  }
  centroid /= static_cast<float> (input_->size ());

  // compute model votes
  for (std::size_t i = 0; i < input_->size (); ++i)
  {
    Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]);
    Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]);
    Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]);

    model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ());
    model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ());
    model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ());
  }

  needs_training_ = false;
  return (true);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::houghVoting ()
{
  if (needs_training_)
  {
    if (!train ())//checks input and input_rf
      return (false);
  }

  //if (!scene_)
  //{
  //  PCL_ERROR(
  //    "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud not set.\n");
  //  return (false);
  //}

  if (!scene_rf_)
  {
    ModelRfCloudPtr new_scene_rf (new ModelRfCloud ());
    computeRf<PointSceneT, PointSceneRfT> (scene_, *new_scene_rf);
    scene_rf_ = new_scene_rf;
    //PCL_ERROR(
    //  "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene reference frame not set.\n");
    //return (false);
  }

  if (scene_->size () != scene_rf_->size ())
  {
    PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n");
    return (false);
  }

  if (!model_scene_corrs_)
  {
    PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n");
    return (false);
  }

  int n_matches = static_cast<int> (model_scene_corrs_->size ());
  if (n_matches == 0)
  {
    return (false);
  }

  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
  Eigen::Vector3d d_min, d_max, bin_size;

  d_min.setConstant (std::numeric_limits<double>::max ());
  d_max.setConstant (-std::numeric_limits<double>::max ());
  bin_size.setConstant (hough_bin_size_);

  float max_distance = -std::numeric_limits<float>::max ();

  // Calculating 3D Hough space dimensions and vote position for each match
  for (int i=0; i< n_matches; ++i)
  {
    int scene_index = model_scene_corrs_->at (i).index_match;
    int model_index = model_scene_corrs_->at (i).index_query;

    const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap ();
    const PointSceneRfT&   scene_point_rf = scene_rf_->at (scene_index);
    
    Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]);
    Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]);
    Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]);

    //const Eigen::Vector3f& model_point = input_->at (model_index).getVector3fMap ();
    const Eigen::Vector3f& model_point_vote = model_votes_[model_index];

    scene_votes[i].x () = scene_point_rf_x[0] * model_point_vote.x () + scene_point_rf_y[0] * model_point_vote.y () + scene_point_rf_z[0] * model_point_vote.z () + scene_point.x ();
    scene_votes[i].y () = scene_point_rf_x[1] * model_point_vote.x () + scene_point_rf_y[1] * model_point_vote.y () + scene_point_rf_z[1] * model_point_vote.z () + scene_point.y ();
    scene_votes[i].z () = scene_point_rf_x[2] * model_point_vote.x () + scene_point_rf_y[2] * model_point_vote.y () + scene_point_rf_z[2] * model_point_vote.z () + scene_point.z ();

    if (scene_votes[i].x () < d_min.x ()) 
      d_min.x () = scene_votes[i].x (); 
    if (scene_votes[i].x () > d_max.x ()) 
      d_max.x () = scene_votes[i].x (); 

    if (scene_votes[i].y () < d_min.y ()) 
      d_min.y () = scene_votes[i].y (); 
    if (scene_votes[i].y () > d_max.y ()) 
      d_max.y () = scene_votes[i].y (); 

    if (scene_votes[i].z () < d_min.z ()) 
      d_min.z () = scene_votes[i].z (); 
    if (scene_votes[i].z () > d_max.z ()) 
      d_max.z () = scene_votes[i].z ();

    // Calculate max distance for interpolated votes
    if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).distance)
    {
      max_distance = model_scene_corrs_->at (i).distance;
    }
  }

  // Hough Voting
  hough_space_.reset (new pcl::recognition::HoughSpace3D (d_min, bin_size, d_max));

  for (int i = 0; i < n_matches; ++i)
  {
    double weight = 1.0;
    if (use_distance_weight_ && max_distance != 0)
    {
      weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance);
    }
    if (use_interpolation_)
    {
      hough_space_->voteInt (scene_votes[i], weight, i);
    } 
    else
    {
      hough_space_->vote (scene_votes[i], weight, i);
    }
  }

  hough_space_initialized_ = true;

  return (true);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> void
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::clusterCorrespondences (std::vector<Correspondences> &model_instances)
{
  model_instances.clear ();
  found_transformations_.clear ();

  if (!hough_space_initialized_ && !houghVoting ())
  {
    return;
  }

  // Finding max bins and voters
  std::vector<double> max_values;
  std::vector<std::vector<int> > max_ids;

  hough_space_->findMaxima (hough_threshold_, max_values, max_ids);

  // Insert maximas into result vector, after Ransac correspondence rejection
  // Temp copy of scene cloud with the type cast to ModelT in order to use Ransac
  PointCloudPtr temp_scene_cloud_ptr (new PointCloud);
  pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);

  pcl::registration::CorrespondenceRejectorSampleConsensus<PointModelT> corr_rejector;
  corr_rejector.setMaximumIterations (10000);
  corr_rejector.setInlierThreshold (hough_bin_size_);
  corr_rejector.setInputSource (input_);
  corr_rejector.setInputTarget (temp_scene_cloud_ptr);

  for (std::size_t j = 0; j < max_values.size (); ++j)
  {
    Correspondences temp_corrs, filtered_corrs;
    for (const int &i : max_ids[j])
    {
      temp_corrs.push_back (model_scene_corrs_->at (i));
    }
    // RANSAC filtering
    corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
    // Save transformations for recognize
    found_transformations_.push_back (corr_rejector.getBestTransformation ());

    model_instances.push_back (filtered_corrs);
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
//pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform)
//{
//  std::vector<int> model_indices;
//  std::vector<int> scene_indices;
//  pcl::registration::getQueryIndices (corrs, model_indices);
//  pcl::registration::getMatchIndices (corrs, scene_indices);
//
//  typename pcl::SampleConsensusModelRegistration<PointModelT>::Ptr model (new pcl::SampleConsensusModelRegistration<PointModelT> (input_, model_indices));
//  model->setInputTarget (scene_cloud, scene_indices);
//
//  pcl::RandomSampleConsensus<PointModelT> ransac (model);
//  ransac.setDistanceThreshold (hough_bin_size_);
//  ransac.setMaxIterations (10000);
//  if (!ransac.computeModel ())
//    return (false);
//
//  // Transform model coefficients from vectorXf to matrix4f
//  Eigen::VectorXf coeffs;
//  ransac.getModelCoefficients (coeffs);
//
//  transform.row (0) = coeffs.segment<4> (0);
//  transform.row (1) = coeffs.segment<4> (4);
//  transform.row (2) = coeffs.segment<4> (8);
//  transform.row (3) = coeffs.segment<4> (12);
//
//  return (true);
//}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::recognize (
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
{
  std::vector<pcl::Correspondences> model_instances;
  return (this->recognize (transformations, model_instances));
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::recognize (
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
{
  transformations.clear ();
  if (!this->initCompute ())
  {
    PCL_ERROR ("[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
    return (false);
  }

  clusterCorrespondences (clustered_corrs);

  transformations = found_transformations_;

  //// Temp copy of scene cloud with the type cast to ModelT in order to use Ransac
  //PointCloudPtr temp_scene_cloud_ptr (new PointCloud);
  //pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);

  //for (std::size_t i = 0; i < model_instances.size (); ++i)
  //{
  //  Eigen::Matrix4f curr_transf;
  //  if (getTransformMatrix (temp_scene_cloud_ptr, model_instances[i], curr_transf))
  //    transformations.push_back (curr_transf);
  //}

  this->deinitCompute ();
  return (transformations.size ());
}


#define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;

#endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_