/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Alexandru-Eugen Ichim
 *                      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: pyramid_feature_matching.hpp 7153 2012-09-16 22:24:29Z aichim $
 *
 */

#ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
#define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_

#include <pcl/pcl_macros.h>

#include <pcl/registration/pyramid_feature_matching.h>

/** \brief Helper function to calculate the binary logarithm
 * \param n_arg: some value
 * \return binary logarithm (log2) of argument n_arg
 */
__inline float Log2(float n_arg) { return std::log(n_arg) / float(M_LN2); }

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
float pcl::PyramidFeatureHistogram<PointFeature>::
    comparePyramidFeatureHistograms(
        const PyramidFeatureHistogramPtr &pyramid_a,
        const PyramidFeatureHistogramPtr &pyramid_b) {
    // do a few consistency checks before and during the computation
    if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) {
        PCL_ERROR("[pcl::PyramidFeatureMatching::"
                  "comparePyramidFeatureHistograms] The two given pyramids "
                  "have different numbers of dimensions: %u vs %u\n",
                  pyramid_a->nr_dimensions, pyramid_b->nr_dimensions);
        return -1;
    }
    if (pyramid_a->nr_levels != pyramid_b->nr_levels) {
        PCL_ERROR("[pcl::PyramidFeatureMatching::"
                  "comparePyramidFeatureHistograms] The two given pyramids "
                  "have different numbers of levels: %u vs %u\n",
                  pyramid_a->nr_levels, pyramid_b->nr_levels);
        return -1;
    }

    // calculate for level 0 first
    if (pyramid_a->hist_levels[0].hist.size() !=
        pyramid_b->hist_levels[0].hist.size()) {
        PCL_ERROR("[pcl::PyramidFeatureMatching::"
                  "comparePyramidFeatureHistograms] The two given pyramids "
                  "have different numbers of bins on level 0: %u vs %u\n",
                  pyramid_a->hist_levels[0].hist.size(),
                  pyramid_b->hist_levels[0].hist.size());
        return -1;
    }
    float match_count_level = 0.0f, match_count_prev_level = 0.0f;
    for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size();
         ++bin_i) {
        if (pyramid_a->hist_levels[0].hist[bin_i] <
            pyramid_b->hist_levels[0].hist[bin_i])
            match_count_level +=
                static_cast<float>(pyramid_a->hist_levels[0].hist[bin_i]);
        else
            match_count_level +=
                static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
    }

    float match_count = match_count_level;
    for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) {
        if (pyramid_a->hist_levels[level_i].hist.size() !=
            pyramid_b->hist_levels[level_i].hist.size()) {
            PCL_ERROR("[pcl::PyramidFeatureMatching::"
                      "comparePyramidFeatureHistograms] The two given pyramids "
                      "have different numbers of bins on level %u: %u vs %u\n",
                      level_i, pyramid_a->hist_levels[level_i].hist.size(),
                      pyramid_b->hist_levels[level_i].hist.size());
            return -1;
        }

        match_count_prev_level = match_count_level;
        match_count_level = 0.0f;
        for (size_t bin_i = 0;
             bin_i < pyramid_a->hist_levels[level_i].hist.size(); ++bin_i) {
            if (pyramid_a->hist_levels[level_i].hist[bin_i] <
                pyramid_b->hist_levels[level_i].hist[bin_i])
                match_count_level += static_cast<float>(
                    pyramid_a->hist_levels[level_i].hist[bin_i]);
            else
                match_count_level += static_cast<float>(
                    pyramid_b->hist_levels[level_i].hist[bin_i]);
        }

        float level_normalization_factor =
            powf(2.0f, static_cast<float>(level_i));
        match_count += (match_count_level - match_count_prev_level) /
                       level_normalization_factor;
    }

    // include self-similarity factors
    float self_similarity_a = static_cast<float>(pyramid_a->nr_features),
          self_similarity_b = static_cast<float>(pyramid_b->nr_features);
    PCL_DEBUG("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] "
              "Self similarity measures: %f, %f\n",
              self_similarity_a, self_similarity_b);
    match_count /= sqrtf(self_similarity_a * self_similarity_b);

    return match_count;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram()
    : nr_dimensions(0), nr_levels(0), nr_features(0), dimension_range_input_(),
      dimension_range_target_(),
      feature_representation_(new DefaultPointRepresentation<PointFeature>),
      is_computed_(false), hist_levels() {}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
void pcl::PyramidFeatureHistogram<
    PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel() {
    size_t total_vector_size = 1;
    for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin();
         dim_it != bins_per_dimension.end(); ++dim_it)
        total_vector_size *= *dim_it;

    hist.resize(total_vector_size, 0);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
bool pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram() {
    // a few consistency checks before starting the computations
    if (!PCLBase<PointFeature>::initCompute()) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase "
                  "initCompute failed\n");
        return false;
    }

    if (dimension_range_input_.size() == 0) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input "
                  "dimension range was not set\n");
        return false;
    }

    if (dimension_range_target_.size() == 0) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target "
                  "dimension range was not set\n");
        return false;
    }

    if (dimension_range_input_.size() != dimension_range_target_.size()) {
        PCL_ERROR(
            "[pcl::PyramidFeatureHistogram::initializeHistogram] Input and "
            "target dimension ranges do not agree in size: %u vs %u\n",
            dimension_range_input_.size(), dimension_range_target_.size());
        return false;
    }

    nr_dimensions = dimension_range_target_.size();
    nr_features = input_->points.size();
    float D = 0.0f;
    for (std::vector<std::pair<float, float>>::iterator range_it =
             dimension_range_target_.begin();
         range_it != dimension_range_target_.end(); ++range_it) {
        float aux = range_it->first - range_it->second;
        D += aux * aux;
    }
    D = sqrtf(D);
    nr_levels = static_cast<size_t>(ceilf(Log2(D)));
    PCL_DEBUG(
        "[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have "
        "%u levels with a hyper-parallelepiped diagonal size of %f\n",
        nr_levels, D);

    hist_levels.resize(nr_levels);
    for (size_t level_i = 0; level_i < nr_levels; ++level_i) {
        std::vector<size_t> bins_per_dimension(nr_dimensions);
        std::vector<float> bin_step(nr_dimensions);
        for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
            bins_per_dimension[dim_i] = static_cast<size_t>(
                ceilf((dimension_range_target_[dim_i].second -
                       dimension_range_target_[dim_i].first) /
                      (powf(2.0f, static_cast<float>(level_i)) *
                       sqrtf(static_cast<float>(nr_dimensions)))));
            bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
                              sqrtf(static_cast<float>(nr_dimensions));
        }
        hist_levels[level_i] =
            PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);

        PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created "
                  "vector of size %u at level %u\nwith #bins per dimension:",
                  hist_levels.back().hist.size(), level_i);
        for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
            PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
        PCL_DEBUG("\n");
    }

    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
unsigned int &
pcl::PyramidFeatureHistogram<PointFeature>::at(std::vector<size_t> &access,
                                               size_t &level) {
    if (access.size() != nr_dimensions) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::at] Cannot access histogram "
                  "position because the access point does not have the right "
                  "number of dimensions\n");
        return hist_levels.front().hist.front();
    }
    if (level >= hist_levels.size()) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::at] Trying to access a too "
                  "large level\n");
        return hist_levels.front().hist.front();
    }

    size_t vector_position = 0;
    size_t dim_accumulator = 1;

    for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
        vector_position += access[i] * dim_accumulator;
        dim_accumulator *= hist_levels[level].bins_per_dimension[i];
    }

    return hist_levels[level].hist[vector_position];
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
unsigned int &
pcl::PyramidFeatureHistogram<PointFeature>::at(std::vector<float> &feature,
                                               size_t &level) {
    if (feature.size() != nr_dimensions) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector "
                  "does not match the feature dimensions of the pyramid "
                  "histogram: %u vs %u\n",
                  feature.size(), nr_dimensions);
        return hist_levels.front().hist.front();
    }
    if (level >= hist_levels.size()) {
        PCL_ERROR("[pcl::PyramidFeatureHistogram::at] Trying to access a too "
                  "large level\n");
        return hist_levels.front().hist.front();
    }

    std::vector<size_t> access;
    for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
        access.push_back(static_cast<size_t>(
            floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
                  hist_levels[level].bin_step[dim_i])));

    return at(access, level);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
void pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
    const PointFeature &feature, std::vector<float> &feature_vector) {
    // convert feature to vector representation
    feature_vector.resize(feature_representation_->getNumberOfDimensions());
    feature_representation_->vectorize(feature, feature_vector);

    // adapt the values from the input range to the target range
    for (size_t i = 0; i < feature_vector.size(); ++i)
        feature_vector[i] =
            (feature_vector[i] - dimension_range_input_[i].first) /
                (dimension_range_input_[i].second -
                 dimension_range_input_[i].first) *
                (dimension_range_target_[i].second -
                 dimension_range_target_[i].first) +
            dimension_range_target_[i].first;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
void pcl::PyramidFeatureHistogram<PointFeature>::compute() {
    if (!initializeHistogram())
        return;

    for (size_t feature_i = 0; feature_i < input_->points.size(); ++feature_i) {
        std::vector<float> feature_vector;
        convertFeatureToVector(input_->points[feature_i], feature_vector);
        addFeature(feature_vector);
    }

    is_computed_ = true;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
void pcl::PyramidFeatureHistogram<PointFeature>::addFeature(
    std::vector<float> &feature) {
    for (size_t level_i = 0; level_i < nr_levels; ++level_i)
        at(feature, level_i)++;
}

#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature)                  \
    template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;

#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
