/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2013-, Open Perception, 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 the copyright holder(s) 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.
 *
 * person_cluster.hpp
 * Created on: Nov 30, 2012
 * Author: Matteo Munaro
 */

#ifndef PCL_PEOPLE_PERSON_CLUSTER_HPP_
#define PCL_PEOPLE_PERSON_CLUSTER_HPP_

#include <pcl/people/person_cluster.h>

template <typename PointT>
pcl::people::PersonCluster<PointT>::PersonCluster (
    const PointCloudPtr& input_cloud,
    const pcl::PointIndices& indices,
    const Eigen::VectorXf& ground_coeffs,
    float sqrt_ground_coeffs,
    bool head_centroid,
    bool vertical)
    {
      init(input_cloud, indices, ground_coeffs, sqrt_ground_coeffs, head_centroid, vertical);
    }

template <typename PointT> void
pcl::people::PersonCluster<PointT>::init (
    const PointCloudPtr& input_cloud,
    const pcl::PointIndices& indices,
    const Eigen::VectorXf& ground_coeffs,
    float sqrt_ground_coeffs,
    bool head_centroid,
    bool vertical)
{

  vertical_ = vertical;
  head_centroid_ = head_centroid;
  person_confidence_ = std::numeric_limits<float>::quiet_NaN();

  min_x_ = 1000.0f;
  min_y_ = 1000.0f;
  min_z_ = 1000.0f;

  max_x_ = -1000.0f;
  max_y_ = -1000.0f;
  max_z_ = -1000.0f;

  sum_x_ = 0.0f;
  sum_y_ = 0.0f;
  sum_z_ = 0.0f;

  n_ = 0;

  points_indices_.indices = indices.indices;

  for (const auto& index : (points_indices_.indices))
  {
    PointT* p = &(*input_cloud)[index];

    min_x_ = std::min(p->x, min_x_);
    max_x_ = std::max(p->x, max_x_);
    sum_x_ += p->x;

    min_y_ = std::min(p->y, min_y_);
    max_y_ = std::max(p->y, max_y_);
    sum_y_ += p->y;

    min_z_ = std::min(p->z, min_z_);
    max_z_ = std::max(p->z, max_z_);
    sum_z_ += p->z;

    n_++;
  }

  c_x_ = sum_x_ / n_;
  c_y_ = sum_y_ / n_;
  c_z_ = sum_z_ / n_;


  Eigen::Vector4f height_point(c_x_, c_y_, c_z_, 1.0f);
  if(!vertical_)
  {
    height_point(1) = min_y_;
    distance_ = std::sqrt(c_x_ * c_x_ + c_z_ * c_z_);
  }
  else
  {
    height_point(0) = max_x_;
    distance_ = std::sqrt(c_y_ * c_y_ + c_z_ * c_z_);
  }

  float height = std::fabs(height_point.dot(ground_coeffs));
  height /= sqrt_ground_coeffs;
  height_ = height;

  if(head_centroid_)
  {
    float sum_x = 0.0f;
    float sum_y = 0.0f;
    float sum_z = 0.0f;
    int n = 0;

    float head_threshold_value;    // vertical coordinate of the lowest head point
    if (!vertical_)
    {
      head_threshold_value = min_y_ + height_ / 8.0f;    // head is suppose to be 1/8 of the human height
      for (const auto& index : (points_indices_.indices))
      {
        PointT* p = &(*input_cloud)[index];

        if(p->y < head_threshold_value)
        {
          sum_x += p->x;
          sum_y += p->y;
          sum_z += p->z;
          n++;
        }
      }
    }
    else
    {
      head_threshold_value = max_x_ - height_ / 8.0f;    // head is suppose to be 1/8 of the human height
      for (const auto& index : (points_indices_.indices))
      {
        PointT* p = &(*input_cloud)[index];

        if(p->x > head_threshold_value)
        {
          sum_x += p->x;
          sum_y += p->y;
          sum_z += p->z;
          n++;
        }
      }
    }

    c_x_ = sum_x / n;
    c_y_ = sum_y / n;
    c_z_ = sum_z / n;
  }

  if(!vertical_)
  {
    float min_x = c_x_;
    float min_z = c_z_;
    float max_x = c_x_;
    float max_z = c_z_;
    for (const auto& index : (points_indices_.indices))
    {
      PointT* p = &(*input_cloud)[index];

      min_x = std::min(p->x, min_x);
      max_x = std::max(p->x, max_x);
      min_z = std::min(p->z, min_z);
      max_z = std::max(p->z, max_z);
    }

    angle_ = std::atan2(c_z_, c_x_);
    angle_max_ = std::max(std::atan2(min_z, min_x), std::atan2(max_z, min_x));
    angle_min_ = std::min(std::atan2(min_z, max_x), std::atan2(max_z, max_x));

    Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f);
    float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2);
    float bottom_x = c_x_ - ground_coeffs(0) * t;
    float bottom_y = c_y_ - ground_coeffs(1) * t;
    float bottom_z = c_z_ - ground_coeffs(2) * t;

    tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
    Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;

    ttop_ = v * height / v.norm() + tbottom_;
    tcenter_ = v * height * 0.5 / v.norm() + tbottom_;
    top_ = Eigen::Vector3f(c_x_, min_y_, c_z_);
    bottom_ = Eigen::Vector3f(c_x_, max_y_, c_z_);
    center_ = Eigen::Vector3f(c_x_, c_y_, c_z_);

    min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);

    max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
  }
  else
  {
    float min_y = c_y_;
    float min_z = c_z_;
    float max_y = c_y_;
    float max_z = c_z_;
    for (const auto& index : (points_indices_.indices))
    {
      PointT* p = &(*input_cloud)[index];

      min_y = std::min(p->y, min_y);
      max_y = std::max(p->y, max_y);
      min_z = std::min(p->z, min_z);
      max_z = std::max(p->z, max_z);
    }

    angle_ = std::atan2(c_z_, c_y_);
    angle_max_ = std::max(std::atan2(min_z_, min_y_), std::atan2(max_z_, min_y_));
    angle_min_ = std::min(std::atan2(min_z_, max_y_), std::atan2(max_z_, max_y_));

    Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f);
    float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2);
    float bottom_x = c_x_ - ground_coeffs(0) * t;
    float bottom_y = c_y_ - ground_coeffs(1) * t;
    float bottom_z = c_z_ - ground_coeffs(2) * t;

    tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
    Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;

    ttop_ = v * height / v.norm() + tbottom_;
    tcenter_ = v * height * 0.5 / v.norm() + tbottom_;
    top_ = Eigen::Vector3f(max_x_, c_y_, c_z_);
    bottom_ = Eigen::Vector3f(min_x_, c_y_, c_z_);
    center_ = Eigen::Vector3f(c_x_, c_y_, c_z_);

    min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);

    max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
  }
}

template <typename PointT> pcl::PointIndices&
pcl::people::PersonCluster<PointT>::getIndices ()
{
  return (points_indices_);
}

template <typename PointT> float
pcl::people::PersonCluster<PointT>::getHeight () const
{
  return (height_);
}

template <typename PointT> float
pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs)
{
  float sqrt_ground_coeffs = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
  return (updateHeight(ground_coeffs, sqrt_ground_coeffs));
}

template <typename PointT> float
pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs, float sqrt_ground_coeffs)
{
  Eigen::Vector4f height_point;
  if (!vertical_)
    height_point << c_x_, min_y_, c_z_, 1.0f;
  else
    height_point << max_x_, c_y_, c_z_, 1.0f;

  float height = std::fabs(height_point.dot(ground_coeffs));
  height /= sqrt_ground_coeffs;
  height_ = height;
  return (height_);
}

template <typename PointT> float
pcl::people::PersonCluster<PointT>::getDistance () const
{
  return (distance_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getTTop ()
{
  return (ttop_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getTBottom ()
{
  return (tbottom_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getTCenter ()
{
  return (tcenter_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getTop ()
{
  return (top_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getBottom ()
{
  return (bottom_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getCenter ()
{
  return (center_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getMin ()
{
  return (min_);
}

template <typename PointT> Eigen::Vector3f&
pcl::people::PersonCluster<PointT>::getMax ()
{
  return (max_);
}

template <typename PointT> float
pcl::people::PersonCluster<PointT>::getAngle () const
{
  return (angle_);
}

template <typename PointT>
float pcl::people::PersonCluster<PointT>::getAngleMax () const
{
  return (angle_max_);
}

template <typename PointT>
float pcl::people::PersonCluster<PointT>::getAngleMin () const
{
  return (angle_min_);
}

template <typename PointT>
int pcl::people::PersonCluster<PointT>::getNumberPoints () const
{
  return (n_);
}

template <typename PointT>
float pcl::people::PersonCluster<PointT>::getPersonConfidence () const
{
  return (person_confidence_);
}

template <typename PointT>
void pcl::people::PersonCluster<PointT>::setPersonConfidence (float confidence)
{
  person_confidence_ = confidence;
}

template <typename PointT>
void pcl::people::PersonCluster<PointT>::setHeight (float height)
{
  height_ = height;
}

template <typename PointT>
void pcl::people::PersonCluster<PointT>::drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number)
{
  // draw theoretical person bounding box in the PCL viewer:
  pcl::ModelCoefficients coeffs;
  // translation
  coeffs.values.push_back (tcenter_[0]);
  coeffs.values.push_back (tcenter_[1]);
  coeffs.values.push_back (tcenter_[2]);
  // rotation
  coeffs.values.push_back (0.0);
  coeffs.values.push_back (0.0);
  coeffs.values.push_back (0.0);
  coeffs.values.push_back (1.0);
  // size
  if (vertical_)
  {
    coeffs.values.push_back (height_);
    coeffs.values.push_back (0.5);
    coeffs.values.push_back (0.5);
  }
  else
  {
    coeffs.values.push_back (0.5);
    coeffs.values.push_back (height_);
    coeffs.values.push_back (0.5);
  }

  const std::string bbox_name = "bbox_person_" + std::to_string(person_number);
  viewer.removeShape (bbox_name);
  viewer.addCube (coeffs, bbox_name);
  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name);
  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name);
}

template <typename PointT>
pcl::people::PersonCluster<PointT>::~PersonCluster () = default;
#endif /* PCL_PEOPLE_PERSON_CLUSTER_HPP_ */