#pragma once
//===----------------------------------------------------------------------===//
#include "bah_patterns.hpp"
#include "bah_types.hpp"

#include <dtl/bitmap/util/plain_bitmap.hpp>
#include <dtl/dtl.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <iomanip>
#include <type_traits>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Implementation of the Byte Aligned Hybrid bitmap compression technique as
/// described in the paper 'BAH: A Bitmap Index Compression Algorithm for Fast
/// Data Retrieval' by Li et al.
class bah {
  static constexpr std::size_t word_bitlength = sizeof(bah_word_t) * 8;

  /// The encoded bitmap.
  std::vector<bah_byte_t> main_;
  std::vector<bah_word_t> data_;
  std::vector<bah_byte_t> index_;
  std::vector<bah_word_t> counter_;
  /// The length of the encoded bitmap.
  std::size_t encoded_bitmap_length_ = 0;

  /// The different word kinds.
  enum class word_kind {
    ZERO,
    ENCODABLE,
    LITERAL
  };

  /// Determines the word kind for a single word of the plain bitmap.
  static word_kind
  get_word_kind(bah_word_t word) {
    if (word == 0) {
      return word_kind::ZERO;
    }
    if (get_oep_code(word) != dtl::bah_code_not_found) {
      return word_kind::ENCODABLE;
    }
    if (get_tep_code(word) != dtl::bah_code_not_found) {
      return word_kind::ENCODABLE;
    }
    return word_kind::LITERAL;
  }

public:
  bah() = default;

  explicit bah(const boost::dynamic_bitset<$u32>& in) {
    auto* bitmap_begin = in.m_bits.data();
    auto* bitmap_end = in.m_bits.data() + in.m_bits.size();
    const auto word_cnt = bitmap_end - bitmap_begin; // TODO handle the case when the bitmap size is not a multiple of the word size

    // The plain bitmap is segmented into words of the same kind.
    // Keep track of the first and last word of the current segment.
    std::size_t seg_begin = 0;
    std::size_t seg_end = 0;

    // Encode the bitmap segment by segment.
    while (seg_begin < word_cnt) {
      auto& w = bitmap_begin[seg_begin];
      auto seg_kind = get_word_kind(w);
      seg_end = seg_begin + 1;
      // Find the end of the current 'segment'.
      while (seg_end < word_cnt
          && get_word_kind(bitmap_begin[seg_end]) == seg_kind) {
        auto seg_len = seg_end - seg_begin;
        if ((seg_kind == word_kind::LITERAL && seg_len == 63)
            || (seg_kind == word_kind::ENCODABLE)) {
          break;
        }
        ++seg_end;
      }
      // Encode the segment.
      if (seg_kind == word_kind::ZERO) {
        auto len = seg_end - seg_begin;
        if (len > 252) {
          main_.push_back(bah_byte_t(0));
          counter_.push_back(len);
        }
        else {
          while (len > 0) {
            auto l = std::min(len, 63ul);
            main_.push_back(static_cast<bah_byte_t>(l));
            len -= l;
          }
        }
      }
      else if (seg_kind == word_kind::LITERAL) {
        auto len = seg_end - seg_begin;
        main_.push_back(static_cast<bah_byte_t>(len) | (bah_byte_t(0b01) << 6));
        for (std::size_t i = seg_begin; i < seg_end; ++i) {
          data_.push_back(bitmap_begin[i]);
        }
      }
      else { /* ENCODABLE */
        auto code = get_oep_code(w);
        if (code != dtl::bah_code_not_found) {
          main_.push_back(
              static_cast<bah_byte_t>(code) | (bah_byte_t(0b10) << 6));
        }
        else {
          code = get_tep_code(w);
          bah_byte_t code_lo = code & 0b111111;
          bah_byte_t code_hi = code >> 6;
          main_.push_back(code_lo | (bah_byte_t(0b11) << 6));
          index_.push_back(code_hi);
        }
      }

      // Next.
      seg_begin = seg_end;
    }
    encoded_bitmap_length_ = in.size();

    // Try to reduce the memory consumption.
    shrink();
  }

  ~bah() = default;
  bah(const bah& other) = default;
  bah(bah&& other) noexcept = default;
  bah& operator=(const bah& other) = default;
  bah& operator=(bah&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t __forceinline__
  size_in_bytes() const {
    return main_.size() * sizeof(bah_byte_t)
        + data_.size() * sizeof(bah_word_t)
        + index_.size() * sizeof(bah_byte_t)
        + counter_.size() * sizeof(bah_word_t)
        + 3 * sizeof(void*) /* pointers/offsets to the three other arrays */
        + sizeof(encoded_bitmap_length_); /* bit-length of the original bitmap */
  }

  /// Returns the size of the bitmap.
  std::size_t __forceinline__
  size() const {
    return encoded_bitmap_length_;
  }

  /// Conversion to an plain bitmap. // TODO remove
  dtl::plain_bitmap<$u64>
  to_plain_bitmap() {
    dtl::plain_bitmap<$u64> ret(encoded_bitmap_length_, false);
    // The write position.
    std::size_t i = 0;

    // The read position(s).
    std::size_t data_idx = 0;
    std::size_t index_idx = 0;
    std::size_t counter_idx = 0;
    for (std::size_t main_idx = 0; main_idx < main_.size(); ++main_idx) {
      const auto& w = main_[main_idx];
      const auto type = w >> 6;
      const auto n = w & 0b111111;
      switch (type) {
        case 0b00: { /* ZERO */
          std::size_t count = 0;
          if (n == 0) {
            count = counter_[counter_idx++];
          }
          else {
            count = n;
          }
          auto l = count * word_bitlength;
          ret.clear(i, std::min(i + l, encoded_bitmap_length_));
          i += l;
          break;
        }
        case 0b01: { /* LITERAL */
          for (std::size_t j = 0; j < n; ++j) {
            auto lit = data_[data_idx++];
            ret.store_bits(i,
                std::min(i + word_bitlength, encoded_bitmap_length_), lit);
            i += word_bitlength;
          }
          break;
        }
        case 0b10: { /* ENCODABLE (one byte) */
          auto pattern = get_oep(n);
          ret.store_bits(i,
              std::min(i + word_bitlength, encoded_bitmap_length_), pattern);
          i += word_bitlength;
          break;
        }
        case 0b11: { /* ENCODABLE (two bytes) */
          auto code = bah_word_t(n) | (bah_word_t(index_[index_idx++]) << 6);
          auto pattern = get_tep(code);
          ret.store_bits(i,
              std::min(i + word_bitlength, encoded_bitmap_length_), pattern);
          i += word_bitlength;
          break;
        }
      }
    }
    if (i < encoded_bitmap_length_) {
      ret.clear(i, encoded_bitmap_length_);
    }
    return std::move(ret);
  }

  static std::string
  name() {
    return "bah";
  }

  /// Returns the value of the bit at the position pos.
  u1 __forceinline__
  test(const std::size_t pos) const {
    // The current range spanned by the current encoded word.
    std::size_t b = 0;
    std::size_t e = 0;

    // The read position(s).
    std::size_t data_idx = 0;
    std::size_t index_idx = 0;
    std::size_t counter_idx = 0;
    for (std::size_t main_idx = 0; main_idx < main_.size(); ++main_idx) {
      const auto& w = main_[main_idx];
      const auto type = w >> 6;
      const auto n = w & 0b111111;
      switch (type) {
        case 0b00: { /* ZERO */
          std::size_t count = 0;
          if (n == 0) {
            count = counter_[counter_idx++];
          }
          else {
            count = n;
          }
          auto l = count * word_bitlength;
          e = std::min(b + l, encoded_bitmap_length_);
          if (b <= pos && pos < e) {
            return false;
          }
          break;
        }
        case 0b01: { /* LITERAL */
          e = std::min(b + n * word_bitlength, encoded_bitmap_length_);
          if (b <= pos && pos < e) {
            auto offset = pos - b;
            auto lit_word = data_[data_idx + (offset / word_bitlength)];
            return dtl::bits::bit_test(lit_word, offset % word_bitlength);
          }
          data_idx += n;
          break;
        }
        case 0b10: { /* ENCODABLE (one byte) */
          e = std::min(b + word_bitlength, encoded_bitmap_length_);
          if (b <= pos && pos < e) {
            auto pattern = get_oep(n);
            return dtl::bits::bit_test(pattern, pos % word_bitlength);
          }
          break;
        }
        case 0b11: { /* ENCODABLE (two bytes) */
          e = std::min(b + word_bitlength, encoded_bitmap_length_);
          auto code = bah_word_t(n) | (bah_word_t(index_[index_idx++]) << 6);
          if (b <= pos && pos < e) {
            auto pattern = get_tep(code);
            return dtl::bits::bit_test(pattern, pos % word_bitlength);
          }
          break;
        }
      }
      // Next.
      b = e;
    }
    return false;
  }

  /// Try to reduce the memory consumption. This function is supposed to be
  /// called after the bitmap has been modified.
  __forceinline__ void
  shrink() {
    main_.shrink_to_fit();
    data_.shrink_to_fit();
    index_.shrink_to_fit();
    counter_.shrink_to_fit();
  }

  //===--------------------------------------------------------------------===//
  /// 1-run iterator
  class iter {
    const bah& outer_;

    /// The read positions.
    std::size_t main_idx_;
    std::size_t data_idx_;
    std::size_t index_idx_;
    std::size_t counter_idx_;

    //    word_kind segment_kind_;
    bah_word_t literal_;
    std::size_t literal_words_remaining_;
    std::size_t literal_in_word_idx_;

    /// Points to the beginning of a 1-run.
    $u64 pos_;
    /// The length of the current 1-run.
    $u64 length_;

  public:
    explicit __forceinline__
    iter(const bah& outer)
        : outer_(outer),
          main_idx_(0),
          data_idx_(0),
          index_idx_(0),
          counter_idx_(0),
          literal_(0),
          literal_words_remaining_(0),
          literal_in_word_idx_(0),
          pos_(0), length_(0) {
      // Find the first 1-run.
      const auto main_cnt = outer_.main_.size();
      while (main_idx_ < main_cnt) {
        pos_ += length_;
        length_ = 0;

        const auto& w = outer_.main_[main_idx_++];
        const auto type = w >> 6;
        const auto n = w & 0b111111;

        switch (type) {
          case 0b00: { /* ZERO */
            std::size_t count = 0;
            if (n == 0) {
              count = outer_.counter_[counter_idx_++];
            }
            else {
              count = n;
            }
            auto l = count * word_bitlength;
            length_ = l;
            break; // Skip over zeros.
          }
          case 0b01: { /* LITERAL */
            literal_ = outer_.data_[data_idx_++];
            assert(literal_ != 0);
            literal_words_remaining_ = n - 1;
            assert(literal_words_remaining_ < n);
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) {
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
          case 0b10: { /* ENCODABLE (one byte) */
            literal_ = get_oep(n);
            assert(literal_ != 0);
            literal_words_remaining_ = 0;
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) {
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
          case 0b11: { /* ENCODABLE (two bytes) */
            auto code =
                bah_word_t(n) | (bah_word_t(outer_.index_[index_idx_++]) << 6);
            literal_ = get_tep(code);
            assert(literal_ != 0);
            literal_words_remaining_ = 0;
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) {
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
        }
      }
      pos_ = outer_.encoded_bitmap_length_;
      length_ = 0;
    }

    /// Forward the iterator to the next 1-run.
    void __forceinline__
    next() {
      const auto main_cnt = outer_.main_.size();
      while (main_idx_ < main_cnt || literal_ != 0) {
        pos_ += length_;
        length_ = 0;

        // Check, whether we are currently in a literal segment.
        if (literal_ != 0) {
          if (literal_in_word_idx_ < word_bitlength) {
            // Find the next set bit.
            std::size_t b = literal_in_word_idx_;
            for (; b < word_bitlength; b++) { // TODO optimize
              u1 is_set = dtl::bits::bit_test(literal_, b);
              if (is_set) break;
            }
            if (b < word_bitlength) {
              // Determine the length of the current 1-run and return.
              std::size_t e = b + 1;
              for (; e < word_bitlength; ++e) { // TODO optimize
                u1 is_set = dtl::bits::bit_test(literal_, e);
                if (!is_set) break;
              }
              pos_ += b - literal_in_word_idx_;
              length_ = e - b;
              literal_in_word_idx_ = e;
              return;
            }
            pos_ += b - literal_in_word_idx_;
          }
          // Fetch the next literal word, if any.
          if (literal_words_remaining_ > 0) {
            literal_ = outer_.data_[data_idx_++];
            --literal_words_remaining_;
            literal_in_word_idx_ = 0;
            continue;
          }
          else {
            // Reached the end of the literal segment.
            literal_ = 0;
            literal_in_word_idx_ = 0;
          }
        }

        assert(literal_ == 0);

        if (main_idx_ >= main_cnt) break;

        const auto& w = outer_.main_[main_idx_++];
        const auto type = w >> 6;
        const auto n = w & 0b111111;
        switch (type) {
          case 0b00: { /* ZERO */
            std::size_t count = 0;
            if (n == 0) {
              count = outer_.counter_[counter_idx_++];
            }
            else {
              count = n;
            }
            auto l = count * word_bitlength;
            length_ = l;
            break; // Skip over zeros.
          }
          case 0b01: { /* LITERAL */
            literal_ = outer_.data_[data_idx_++];
            assert(literal_ != 0);
            literal_words_remaining_ = n - 1;
            assert(literal_words_remaining_ < n);
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) {
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
          case 0b10: { /* ENCODABLE (one byte) */
            literal_ = get_oep(n);
            assert(literal_ != 0);
            literal_words_remaining_ = 0;
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) { // TODO optimize
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
          case 0b11: { /* ENCODABLE (two bytes) */
            auto code =
                bah_word_t(n) | (bah_word_t(outer_.index_[index_idx_++]) << 6);
            literal_ = get_tep(code);
            assert(literal_ != 0);
            literal_words_remaining_ = 0;
            const std::size_t b = dtl::bits::tz_count(literal_);
            std::size_t e = b + 1;
            for (; e < word_bitlength; ++e) { // TODO optimize
              u1 is_set = dtl::bits::bit_test(literal_, e);
              if (!is_set) break;
            }
            literal_in_word_idx_ = e;
            pos_ += b;
            length_ = e - b;
            return;
          }
        }
      }
      pos_ = outer_.encoded_bitmap_length_;
      length_ = 0;
    }

    /// Forward the iterator to the desired position.
    void __forceinline__
    skip_to(const std::size_t to_pos) {
      assert(pos_ <= to_pos);
      if (to_pos >= outer_.encoded_bitmap_length_) {
        pos_ = outer_.encoded_bitmap_length_;
        length_ = 0;
        return;
      }
      // Call next until the desired position has been reached.
      while (!end() && pos() + length() <= to_pos) {
        next();
      }
      // Adjust the current position and run length.
      if (!end() && pos() < to_pos) {
        length_ -= to_pos - pos_;
        pos_ = to_pos;
      }
    }

    u1 __forceinline__
    end() const noexcept {
      return length_ == 0;
    }

    u64 __forceinline__
    pos() const noexcept {
      return pos_;
    }

    u64 __forceinline__
    length() const noexcept {
      return length_;
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  /// Returns a 1-run iterator.
  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  /// Returns a 1-run iterator.
  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(encoded_bitmap_length_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + ",\"word_size\":" + std::to_string(sizeof(bah_word_t))
        + "}";
  }

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "main idx | word type | content" << std::endl;
    os << "---------|-----------|---------------------------------" << std::endl;
    // The read position(s).
    std::size_t data_idx = 0;
    std::size_t index_idx = 0;
    std::size_t counter_idx = 0;
    for (std::size_t main_idx = 0; main_idx < main_.size(); ++main_idx) {
      os << std::setw(8) << main_idx << " | ";
      auto& w = main_[main_idx];
      const auto type = w >> 6;
      const auto n = w & 0b111111;
      switch (type) {
        case 0b00: { /* ZERO */
          std::size_t count = 0;
          if (n == 0) {
            count = counter_[counter_idx++];
          }
          else {
            count = n;
          }
          os << count << " x ZERO";
          break;
        }
        case 0b01: { /* LITERAL */
          os << n << " x LITERAL";
          for (std::size_t i = 0; i < n; ++i) {
            os << std::endl;
            os << "         |           | "
               << std::bitset<word_bitlength>(data_[data_idx + i]);
          }
          data_idx += n;
          break;
        }
        case 0b10: { /* ENCODABLE (one byte) */
          os << "1"
             << " x ENCODABLE (one byte)";
          os << std::endl;
          os << "         |           | "
              << std::bitset<word_bitlength>(get_oep(n));
          break;
        }
        case 0b11: { /* ENCODABLE (two bytes) */
          os << "1"
             << " x ENCODABLE (two bytes)";
          auto code = bah_word_t(n) | (bah_word_t(index_[index_idx++]) << 6);
          os << std::endl;
          os << "         |           | "
              << std::bitset<word_bitlength>(get_tep(code));
          break;
        }
      }
      os << std::endl;
    }
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


/*
 * KMajority.cpp
 *
 *  Created on: Aug 28, 2013
 *      Author: andresf
 */

#include <KMajority.h>
#include <CentersChooser.h>

#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filtering_stream.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/flann/random.h>
#include <opencv2/flann/dist.h>

#include <iostream>
#include <bitset>
#include <fstream>
#include <functional>

namespace vlr {

KMajority::KMajority(vlr::Mat& data, const cvflann::IndexParams& params,
		const cvflann::IndexParams& nnIndexParams) :
		m_dataset(data), m_dim(data.cols), m_nnIndex(NULL), m_nnIndexParams(
				nnIndexParams) {

	// Attributes initialization
	m_numClusters = cvflann::get_param<int>(params, "num.clusters");
	m_maxIterations = cvflann::get_param<int>(params, "max.iterations");
	m_centersInitMethod = cvflann::get_param<cvflann::flann_centers_init_t>(
			params, "centers.init.method");
	m_nnType = cvflann::get_param<vlr::indexType>(params, "nn.type");
	m_numDatapoints = m_dataset.rows;

	// Initially all transactions belong to any cluster
	m_belongsTo.clear();
	m_belongsTo.resize(m_dataset.rows, m_numClusters);

	// Initially all transactions are at the farthest possible distance
	// i.e. m_dim*8 the max Hamming distance
	m_distanceTo.clear();
	m_distanceTo.resize(m_dataset.rows, data.cols * 8);

	// Initially no transaction is assigned to any cluster
	m_clusterCounts.clear();
	m_clusterCounts.resize(m_numClusters, 0);

}

// --------------------------------------------------------------------------

KMajority::~KMajority() {
	delete m_nnIndex;
}

// --------------------------------------------------------------------------

void KMajority::build() {

	if (m_dataset.type() != CV_8U) {
		throw std::runtime_error(
				"[KMajority::build] Descriptors matrix is not binary");
	}

	if (m_dataset.empty()) {
		throw std::runtime_error("[KMajority::build] Descriptors is empty");
	}

	// Trivial case: less data than clusters, assign one data point per cluster
	if (m_numDatapoints <= m_numClusters) {
		m_centroids.create(m_numClusters, m_dim, m_dataset.type());
		for (int i = 0; i < m_numDatapoints; ++i) {
			m_dataset.row(i).copyTo(
					m_centroids(cv::Range(i, i + 1), cv::Range(0, m_dim)));
			m_belongsTo[i] = i;
		}
		return;
	}

#if KMAJVERBOSE
	printf("-- Bootstrapping clustering process\n");
#endif

	// Randomly generate clusters
#if KMAJVERBOSE
	printf("   Initializing clusters centers.\n");
#endif
	initCentroids();

	// Update nearest neighbors index upon new centers
#if KMAJVERBOSE
	printf("   Updating nearest neighbors index.\n");
#endif
	updateIndex();

	// Assign data to clusters
#if KMAJVERBOSE
	printf("   Quantizing data into clusters.\n");
#endif
	quantize();

	bool converged = false;
	int iteration = 0;

	while (converged == false && iteration < m_maxIterations) {

		++iteration;

#if KMAJVERBOSE
		printf("-- Iteration=[%d]\n", iteration);
		fflush(stdout);
#endif

		// Compute the new clusters centers
#if KMAJVERBOSE
		printf("   Computing new clusters centers.\n");
#endif
		computeCentroids();

		// Update nearest neighbors index upon new centers
#if KMAJVERBOSE
		printf("   Updating nearest neighbors index.\n");
#endif
		updateIndex();

		// Reassign data to clusters
#if KMAJVERBOSE
		printf("   Quantizing data into clusters.\n");
#endif
		converged = quantize();

		// Handle empty clusters case
#if KMAJVERBOSE
		printf("   Handling empty clusters case.\n");
#endif
		handleEmptyClusters();
	}

}

// --------------------------------------------------------------------------

void KMajority::initCentroids() {

	// Initializing variables useful for obtaining indexes of random chosen center
	std::vector<int> centers_idx(m_numClusters);
	int centers_length;

	// Array of indices indicating data points involved in the clustering process
	int* indices = new int[m_dataset.rows];
	for (int i = 0; i < m_dataset.rows; ++i) {
		indices[i] = i;
	}

	// Randomly chose centers
	CentersChooser<Distance::ElementType, cv::Hamming>::create(
			m_centersInitMethod)->chooseCenters(m_numClusters, indices,
			m_numDatapoints, centers_idx, centers_length, m_dataset);
	CV_Assert(centers_length == m_numClusters);

	std::sort(centers_idx.begin(), centers_idx.end());

	delete[] indices;

	// Assign centers based on the chosen indexes
	m_centroids.create(centers_length, m_dim, m_dataset.type());
	for (int i = 0; i < centers_length; ++i) {
		m_dataset.row(centers_idx[i]).copyTo(
				m_centroids(cv::Range(i, i + 1), cv::Range(0, m_dim)));
	}

}

// --------------------------------------------------------------------------

bool KMajority::quantize() {

	bool converged = true;

	// Number of nearest neighbors
	int knn = 1;

	// The indices of the nearest neighbors found (numQueries X numNeighbors)
	cvflann::Matrix<int> indices(new int[1 * knn], 1, knn);

	// Distances to the nearest neighbors found (numQueries X numNeighbors)
	cvflann::Matrix<DistanceType> distances(new DistanceType[1 * knn], 1, knn);

	for (int i = 0; i < m_numDatapoints; ++i) {
		std::fill(indices.data, indices.data + indices.rows * indices.cols, 0);
		std::fill(distances.data,
				distances.data + distances.rows * distances.cols, 0.0f);

		cvflann::Matrix<Distance::ElementType> descriptor(
				(Distance::ElementType*) m_dataset.row(i).data, 1,
				m_dataset.cols);

		/* Get new cluster it belongs to */
		m_nnIndex->knnSearch(descriptor, indices, distances, knn,
				cvflann::SearchParams());

		/* Check if cluster assignment changed */
		// If it did then algorithm hasn't converged yet
		if (m_belongsTo[i] != indices[0][0]) {
			converged = false;
		}

		/* Update cluster assignment and cluster counts */
		// Decrease cluster count in case it was assigned to some valid cluster before.
		// Recall that initially all transaction are assigned to kth cluster which
		// is not valid since valid clusters run from 0 to k-1 both inclusive.
		if (m_belongsTo[i] != m_numClusters) {
			--m_clusterCounts[m_belongsTo[i]];
		}
		m_belongsTo[i] = indices[0][0];
		++m_clusterCounts[indices[0][0]];
		m_distanceTo[i] = distances[0][0];
	}

	delete[] indices.data;
	delete[] distances.data;

	return converged;
}

// --------------------------------------------------------------------------

void KMajority::computeCentroids() {

	// Warning: using matrix of integers, there might be an overflow when summing too much descriptors
	cv::Mat bitwiseCount(m_numClusters, m_dim * 8, cv::DataType<int>::type);
	// Zeroing matrix of cumulative bits
	bitwiseCount = cv::Scalar::all(0);
	// Zeroing all cluster centers dimensions
	m_centroids = cv::Scalar::all(0);

	// Bitwise summing the data into each center
	for (int i = 0; i < m_numDatapoints; ++i) {
		cv::Mat b = bitwiseCount.row(m_belongsTo[i]);
		KMajority::cumBitSum(m_dataset.row(i), b);
	}

	// Bitwise majority voting
	for (int j = 0; j < m_numClusters; j++) {
		cv::Mat centroid = m_centroids.row(j);
		KMajority::majorityVoting(bitwiseCount.row(j), centroid,
				m_clusterCounts[j]);
	}
}

// --------------------------------------------------------------------------

void KMajority::save(const std::string& filename) const {

	if (m_centroids.empty()) {
		throw std::runtime_error("[KMajority::save] Tree is empty");
	}

	cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);

	if (fs.isOpened() == false) {
		throw std::runtime_error("[KMajority::save] "
				"Unable to open file [" + filename + "] for writing");
	}

	fs << "type" << "AKMAJ";
	fs << "Centers" << m_centroids;

	fs.release();

}

// --------------------------------------------------------------------------

void KMajority::load(const std::string& filename) {

	enum nodeFields {
		header, start, rows, cols, dt, data
	};
	std::string nodeFieldsNames[] = { "YAML", "Centers", "rows:", "cols:",
			"dt:", "data:" };

	std::ifstream inputZippedFileStream;
	boost::iostreams::filtering_istream inputFileStream;

	std::string line, field;
	std::stringstream ss;

	// Open file
	inputZippedFileStream.open(filename.c_str(),
			std::fstream::in | std::fstream::binary);

	// Check file
	if (inputZippedFileStream.good() == false) {
		throw std::runtime_error("[KMajority::load] "
				"Unable to open file [" + filename + "] for reading");
	}

	int _rows = -1;
	int _cols = -1;
	std::string _type;
	int elemIdx = -1;
	unsigned int elem;

	try {
		inputFileStream.push(boost::iostreams::gzip_decompressor());
		inputFileStream.push(inputZippedFileStream);

		while (getline(inputFileStream, line)) {
			ss.clear();
			ss.str(line);
			ss >> field;
			if (field.compare(nodeFieldsNames[header]) == 0) {
				continue;
			} else if (field.compare(nodeFieldsNames[start]) == 0) {
				continue;
			} else if (field.compare(nodeFieldsNames[rows]) == 0) {
				ss >> _rows;
			} else if (field.compare(nodeFieldsNames[cols]) == 0) {
				ss >> _cols;
			} else if (field.compare(nodeFieldsNames[dt]) == 0) {
				ss >> _type;
			} else {
				if (field.compare(nodeFieldsNames[data]) == 0) {
					m_centroids = cv::Mat::zeros(_rows, _cols,
							_type.compare("f") == 0 ? CV_32F : CV_8U);
					line.replace(line.find(nodeFieldsNames[data]), 5, " ");
				}

				std::replace(line.begin(), line.end(), '[', ' ');
				std::replace(line.begin(), line.end(), ',', ' ');
				std::replace(line.begin(), line.end(), ']', ' ');

				ss.clear();
				ss.str(line);

				while ((ss >> elem).fail() == false) {
					++elemIdx;
					int row = floor(elemIdx / _cols);
					int col = elemIdx % _cols;
					m_centroids.at<uchar>(row, col) = elem;
				}

			}
		}

	} catch (const boost::iostreams::gzip_error& e) {
		throw std::runtime_error("[KMajority::load] "
				"Got error while parsing file [" + std::string(e.what()) + "]");
	}

	// Close file
	inputZippedFileStream.close();

}

// --------------------------------------------------------------------------

void KMajority::cumBitSum(const cv::Mat& data, cv::Mat& accVector) {

	// cumResult and data must be row vectors
	if (data.rows != 1 || accVector.rows != 1) {
		throw std::runtime_error(
				"[KMajority::cumBitSum] data and cumResult parameters must be row vectors\n");
	}
	// cumResult and data must be same length
	if (data.cols * 8 != accVector.cols) {
		throw std::runtime_error(
				"[KMajority::cumBitSum] number of columns in cumResult must be that of data times 8\n");
	}

	uchar byte = 0;
	for (int l = 0; l < accVector.cols; l++) {
		// bit: 7-(l%8) col: (int)l/8 descriptor: i
		// Load byte every 8 bits
		if ((l % 8) == 0) {
			byte = *(data.col((int) l / 8).data);
		}
		// Note: ignore maybe-uninitialized warning because loop starts with l=0 that means byte gets a value as soon as the loop start
		// bit at ith position is mod(bitleftshift(byte,i),2) where ith position is 7-mod(l,8) i.e 7, 6, 5, 4, 3, 2, 1, 0
		accVector.at<int>(0, l) += ((int) ((byte >> (7 - (l % 8))) % 2));
	}

}

// --------------------------------------------------------------------------

void KMajority::majorityVoting(const cv::Mat& accVector, cv::Mat& result,
		const int& threshold) {

	// cumResult and data must be a row vectors
	if (accVector.rows != 1 || result.rows != 1) {
		throw std::runtime_error(
				"[KMajority::majorityVoting] 'accVector' and 'result' parameters must be row vectors\n");
	}

	// cumResult and data must be same length
	if (result.cols * 8 != accVector.cols) {
		throw std::runtime_error(
				"[KMajority::majorityVoting] number of columns in 'accVector' must be that of 'result' times 8\n");
	}

	// In this point I already have stored in bitwiseCount the bitwise sum of all data assigned to jth cluster
	for (int l = 0; l < accVector.cols; ++l) {
		// If the bitcount for jth cluster at dimension l is greater than half of the data assigned to it
		// then set lth centroid bit to 1 otherwise set it to 0 (break ties randomly)
		bool bit;
		// There is a tie if the number of data assigned to jth cluster is even
		// AND the number of bits set to 1 in lth dimension is the half of the data assigned to jth cluster
		if (threshold % 2 == 1
				&& 2 * accVector.at<int>(0, l) == (int) threshold) {
			bit = rand() % 2;
		} else {
			bit = 2 * accVector.at<int>(0, l) > (int) (threshold);
		}
		// Stores the majority voting result from the LSB to the MSB
		result.at<unsigned char>(0, (int) (accVector.cols - 1 - l) / 8) += (bit)
				<< ((accVector.cols - 1 - l) % 8);
	}
}

// --------------------------------------------------------------------------

void KMajority::handleEmptyClusters() {

	// If some cluster appeared to be empty then:
	// 1. Find the biggest cluster.
	// 2. Find farthest point in the biggest cluster
	// 3. Exclude the farthest point from the biggest cluster and form a new 1-point cluster.

	for (int k = 0; k < m_numClusters; ++k) {
		if (m_clusterCounts[k] != 0) {
			continue;
		}

		// 1. Find the biggest cluster
		int max_k = 0;
		for (int k1 = 1; k1 < m_numClusters; ++k1) {
			if (m_clusterCounts[max_k] < m_clusterCounts[k1])
				max_k = k1;
		}

		// 2. Find farthest point in the biggest cluster
		DistanceType maxDist(-1);
		int idxFarthestPt = -1;
		for (int i = 0; i < m_numDatapoints; ++i) {
			if (m_belongsTo[i] == max_k) {
				if (maxDist < m_distanceTo[i]) {
					maxDist = m_distanceTo[i];
					idxFarthestPt = i;
				}
			}
		}

		// 3. Exclude the farthest point from the biggest cluster and form a new 1-point cluster
		--m_clusterCounts[max_k];
		++m_clusterCounts[k];
		m_belongsTo[idxFarthestPt] = k;
	}
}

// --------------------------------------------------------------------------

void KMajority::updateIndex() {

	m_nnIndex = vlr::createIndexByType(
			cvflann::Matrix<Distance::ElementType>(
					(Distance::ElementType*) m_centroids.data, m_centroids.rows,
					m_centroids.cols), m_nnType, m_nnIndexParams);

	double mytime = cv::getTickCount();
        m_nnIndex->buildIndex();
	mytime = ((double) cv::getTickCount() - mytime) / cv::getTickFrequency() * 1000;

	printf("   Index built in [%lf] ms\n", mytime);

}

// --------------------------------------------------------------------------

const cv::Mat& KMajority::getCentroids() const {
	return m_centroids;
}

// --------------------------------------------------------------------------

const std::vector<int>& KMajority::getClusterCounts() const {
	return m_clusterCounts;
}

// --------------------------------------------------------------------------

const std::vector<int>& KMajority::getClusterAssignments() const {
	return m_belongsTo;
}

// --------------------------------------------------------------------------

cvflann::NNIndex<Distance>* createIndexByType(
		const cvflann::Matrix<typename Distance::ElementType>& dataset,
		vlr::indexType type, const cvflann::IndexParams& userDefParams) {

	cvflann::IndexParams::const_iterator it;

	cvflann::IndexParams params;
	cvflann::NNIndex<Distance>* nnIndex;

	double mytime = cv::getTickCount();

	switch (type) {
	case vlr::LINEAR:
		printf("-- Creating [Linear] index\n");
		params = cvflann::LinearIndexParams();
		// Do not copy any parameters, linear index doesn't need any
		nnIndex = new cvflann::LinearIndex<Distance>(dataset, params,
				Distance());
		break;
	case vlr::HIERARCHICAL:
		printf("-- Creating [HierarchicalClustering] index\n");
		params = cvflann::HierarchicalClusteringIndexParams();
		for (it = userDefParams.begin(); it != userDefParams.end(); ++it) {
			int value = it->second.cast<int>();
			params[it->first] = value;
		}
		nnIndex = new cvflann::HierarchicalClusteringIndex<Distance>(dataset,
				params, Distance());
		break;
	default:
		throw std::runtime_error("Unknown index type");
	}
	mytime = ((double) cv::getTickCount() - mytime) / cv::getTickFrequency()
			* 1000;

	printf("   Index created in [%lf] ms\n", mytime);

	return nnIndex;
}

} /* namespace vlr */


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * 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 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 HOLDER 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.
 *
 * ****************************************************************************
 * Gaussian distribution.
 * ****************************************************************************
 */
#ifndef VISIONCORE_TYPES_GAUSSIAN_HPP
#define VISIONCORE_TYPES_GAUSSIAN_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Dense>

namespace vc
{
namespace types
{
    template<typename T> class Gaussian;
    template<typename _Scalar, int _Dimension = 1> class MultivariateGaussian;
}
}

namespace Eigen
{
    namespace internal
    {
        // Gaussian of scalars or Eigens
        template<typename T>
        struct traits< vc::types::Gaussian<T> >
        {
            static constexpr int Rows = 1;
            static constexpr int Cols = 1;
            static constexpr int Dimension = Rows * Cols;
            typedef T Scalar;
            typedef Scalar MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols>
        struct traits<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Rows,_Cols> MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols, int _Options>
        struct traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> >, _Options> > :
            traits<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Rows,_Cols>, _Options> MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols, int _Options>
        struct traits<Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> >, _Options> > :
            traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Rows,_Cols>, _Options> MeanType;
        };

        // Multivariate
        template<typename _Scalar, int _Dimension>
        struct traits<vc::types::MultivariateGaussian<_Scalar,_Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Dimension,1> MeanType;
            typedef Matrix<Scalar,_Dimension,_Dimension> CovarianceType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > :
            traits<vc::types::MultivariateGaussian<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Dimension,1>, _Options> MeanType;
            typedef Map<Matrix<Scalar,_Dimension,_Dimension>, _Options> CovarianceType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > :
            traits<const vc::types::MultivariateGaussian<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Dimension,1>, _Options> MeanType;
            typedef Map<const Matrix<Scalar,_Dimension,_Dimension>, _Options> CovarianceType;
        };

    }
}

namespace vc
{

namespace types
{

/**
 * Type agnostic Gaussian operations.
 */
template<typename Derived>
class GaussianBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    EIGEN_DEVICE_FUNC MeanType& mean()
    {
        return static_cast<Derived*>(this)->mean_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& mean() const
    {
        return static_cast<const Derived*>(this)->mean_const();
    }

    EIGEN_DEVICE_FUNC MeanType& variance()
    {
        return static_cast<Derived*>(this)->variance_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& variance() const
    {
        return static_cast<const Derived*>(this)->variance_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived>& operator=(const GaussianBase<OtherDerived>& other)
    {
        mean() = other.mean();
        variance() = other.variance();
        return *this;
    }

#if 0
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator+(const GaussianBase<Derived>& other) const
    {
    // TBD
    }

    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator-(const GaussianBase<Derived>& other) const
    {
    // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator*(const OtherScalar& other) const
    {
    // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator/(const OtherScalar& other) const
    {
    // TBD
    }
#endif

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Variance", variance()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Variance", variance()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Eigen operations for Gaussian.
 */
template<typename Derived, int _Dimension>
class GaussianDispatchingBase : public GaussianBase<Derived>
{
    typedef GaussianBase<Derived> Base;
    friend class GaussianBase<Derived>;
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    using Base::operator=;

    template<typename _NewScalarType>
    EIGEN_DEVICE_FUNC inline Gaussian<Eigen::Matrix<_NewScalarType, Rows, Cols> >  cast() const
    {
        return Gaussian<Eigen::Matrix<_NewScalarType, Rows, Cols>>(Base::mean().template cast<_NewScalarType>(),
                                                                   Base::variance().template cast<_NewScalarType>());
    }

    EIGEN_DEVICE_FUNC inline MeanType evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::exp;
        using Eigen::numext::pow;

        const MeanType inv_sqrt_2pi_s2 = Scalar(1.0) / sqrt(Scalar(2.0 * M_PI) * Base::variance().array());
        const MeanType a = (x - Base::mean());
        return inv_sqrt_2pi_s2.array() * exp(-(pow(a.array(), 2))/(Scalar(2.0) * Base::variance().array()));
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        Base::mean() = MeanType::Zero();
        Base::variance() = MeanType::Zero();
    }

    EIGEN_DEVICE_FUNC MeanType stddev() const
    {
        return sqrt(Base::variance().array());
    }
};

/**
 * Scalar operations for Gaussian.
 */
template<typename Derived>
class GaussianDispatchingBase<Derived, 1> : public GaussianBase<Derived>
{
    typedef GaussianBase<Derived> Base;
    friend class GaussianBase<Derived>;
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    using Base::operator=;

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline Gaussian<NewScalarType> cast() const
    {
        return Gaussian<NewScalarType>((NewScalarType)Base::mean(), (NewScalarType)Base::variance());
    }

    EIGEN_DEVICE_FUNC inline MeanType evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::exp;

        const Scalar inv_sqrt_2pi_s2 = Scalar(1.0) / sqrt(Scalar(2.0 * M_PI) * Base::variance());
        const Scalar a = (x - Base::mean());
        return inv_sqrt_2pi_s2 * exp(-(a * a)/(Scalar(2.0) * Base::variance()));
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        Base::mean() = Scalar(0.0);
        Base::variance() = Scalar(0.0);
    }

    EIGEN_DEVICE_FUNC MeanType stddev() const
    {
        using Eigen::numext::sqrt;
        return sqrt(Base::variance());
    }
};

/**
 * Multivariate Gaussian Base.
 */
template<typename Derived>
class MultivariateGaussianBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Derived>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Derived>::CovarianceType ConstCovarianceType;

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline MultivariateGaussian<NewScalarType,Dimension> cast() const
    {
        return MultivariateGaussian<NewScalarType,Dimension>(mean().template cast<NewScalarType>(),
                                                             covariance().template cast<NewScalarType>());
    }

    EIGEN_DEVICE_FUNC MeanType& mean()
    {
        return static_cast<Derived*>(this)->mean_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& mean() const
    {
        return static_cast<Derived*>(this)->mean_const();
    }

    EIGEN_DEVICE_FUNC CovarianceType& covariance()
    {
        return static_cast<Derived*>(this)->covariance_nonconst();
    }

    EIGEN_DEVICE_FUNC const CovarianceType& covariance() const
    {
        return static_cast<Derived*>(this)->covariance_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived>& operator=(const MultivariateGaussianBase<OtherDerived>& other)
    {
        mean() = other.mean();
        covariance() = other.covariance();
        return *this;
    }

#if 0
EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator+(const MultivariateGaussianBase<Derived>& other) const
    {
        // TBD
    }

    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator-(const MultivariateGaussianBase<Derived>& other) const
    {
        // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator*(const OtherScalar& other) const
    {
        // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator/(const OtherScalar& other) const
    {
        // TBD
    }
#endif

    EIGEN_DEVICE_FUNC inline Scalar evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::pow;
        using Eigen::numext::exp;

        auto term1 = x - mean();
        auto det = covariance().determinant();
        auto part1 = (Scalar(1.0) / ( sqrt(pow(Scalar(2.0 * M_PI), Scalar(Dimension)) * det) ) );
        auto part2 = exp(Scalar(-0.5) * term1.transpose() * covariance().inverse() * term1);

        return part1 * part2;
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        mean().setZero();
        covariance().setZero();
    }

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Covariance", covariance()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Covariance", covariance()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Gaussian distribution.
 */
template<typename T>
class Gaussian : public GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension>
{
    typedef GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Gaussian>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Gaussian>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Gaussian>::Cols;
    typedef typename Eigen::internal::traits<Gaussian>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Gaussian>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Gaussian>::MeanType ConstMeanType;

    friend class GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension>;
    friend class GaussianBase<Gaussian<T> >;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base::operator=;
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Gaussian()
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline Gaussian(const GaussianDispatchingBase<GaussianBase<OtherDerived>,
                                      Eigen::internal::traits<Gaussian>::Dimension>& other)
        : mean_(other.mean()), variance_(other.variance())
    {
    }

    EIGEN_DEVICE_FUNC inline Gaussian(ConstMeanType& mean, ConstMeanType& variance)
        : mean_(mean), variance_(variance)
    {
    }

    EIGEN_DEVICE_FUNC inline ~Gaussian()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }
    EIGEN_DEVICE_FUNC inline MeanType& variance_nonconst() { return variance_; }

    MeanType mean_;
    MeanType variance_;
};

template<typename T>
inline std::ostream& operator<<(std::ostream& os, const Gaussian<T>& p)
{
    os << "N(" << p.mean() << "," << p.stddev() <<  ")";
    return os;
}

/**
 * Multivariate Gaussian distribution.
 */
template<typename _Scalar, int _Dimension>
class MultivariateGaussian : public MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>>
{
    typedef MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<MultivariateGaussian>::Dimension;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::Scalar Scalar;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<MultivariateGaussian>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<MultivariateGaussian>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base::operator=;
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline MultivariateGaussian()
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline MultivariateGaussian(const MultivariateGaussianBase<OtherDerived>& other)
        : mean_(other.mean()), covariance_(other.covariance())
    {
    }

    EIGEN_DEVICE_FUNC inline MultivariateGaussian(ConstMeanType& mean, ConstCovarianceType& covariance)
        : mean_(mean), covariance_(covariance)
    {
    }

    EIGEN_DEVICE_FUNC inline ~MultivariateGaussian()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }
    EIGEN_DEVICE_FUNC inline CovarianceType& covariance_nonconst() { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

template<typename _Scalar, int _Dimension>
inline std::ostream& operator<<(std::ostream& os, const MultivariateGaussian<_Scalar,_Dimension>& p)
{
    os << "MultivariateGaussian(" << p.mean() << "," << p.covariance() <<  ")";
    return os;
}

}

}

namespace Eigen
{
/**
 * Specialisation of Eigen::Map for Gaussian.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>
    : public vc::types::GaussianDispatchingBase<
        Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
            Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension>
{
    typedef vc::types::GaussianDispatchingBase<
        Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
            Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;

    friend class vc::types::GaussianDispatchingBase<
                Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
                Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension>;
    friend class vc::types::GaussianBase<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : mean_(coeffs), variance_(coeffs + Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }
    EIGEN_DEVICE_FUNC inline MeanType& variance_nonconst() { return variance_; }

    MeanType mean_;
    MeanType variance_;
};

/**
 * Specialisation of Eigen::Map for const Gaussian.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>
    : public vc::types::GaussianDispatchingBase<
        Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
          Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>
{
    typedef vc::types::GaussianDispatchingBase<
      Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
      Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>  Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;

    friend class vc::types::GaussianDispatchingBase<
      Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
      Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>;
    friend class vc::types::GaussianBase<Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : mean_(coeffs), variance_(coeffs + Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }

    ConstMeanType mean_;
    ConstMeanType variance_;
};

/**
 * Specialisation of Eigen::Map for Multivariate Gaussian.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>
  : public vc::types::MultivariateGaussianBase<
    Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::MultivariateGaussianBase<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>> Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Map>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Map>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : mean_(coeffs), covariance_(coeffs + _Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }
    EIGEN_DEVICE_FUNC inline CovarianceType& covariance_nonconst() { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

/**
 * Specialisation of Eigen::Map for const Multivariate Gaussian.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>
  : public vc::types::MultivariateGaussianBase<
    Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::MultivariateGaussianBase<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Map>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Map>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : mean_(coeffs), covariance_(coeffs + _Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

}

#endif // VISIONCORE_TYPES_GAUSSIAN_HPP


// -------------------------------------------
//  @description: 根据彩色图像的像素产生点云代码 产生三维骨架点云 计算外积 计算方差 方差保存供svm训练
//  @author: hts
//  @data: 2020-04-20
//  @version: wpdwp
// -------------------------------------------
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cmath>
#include <mutex>
#include <thread>
#include <chrono>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <kinect2_bridge/kinect2_definitions.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
#define pi 3.14159265359

class Posepoint
{
private:
  std::mutex lock;
  cv::Mat color, depth;
  cv::Mat cameraMatrixColor, cameraMatrixDepth;
  cv::Mat lookupX, lookupY;
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
  pcl::PCDWriter writer;
  bool visualize_stop;
  double pick_points[24];
  // double pick_points2[18];
  Vector3f Lshoulder, Rshoulder, Pelv;
  Vector3f Head,Neck,Thrx,Lhip,Lknee,Lankle,Rhip,Rknee,Rankle;
  std::string   pic_class, person_class, pic_name, depth_name, color_path, depth_path;
  int sp;
  int op;
  bool right;
public:
  Posepoint():visualize_stop(false),sp(0),op(0)
  {
    cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F);
    // cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
    // static double my_points[6] = {255.67636108398438,292.16888427734375,320.4430236816406,266.26220703125,330.15802001953125,298.64556884765625};
    // static double my_points2[18] = {191.4891815185547, 215.0364227294922, 218.52777099609375, 226.16993713378906, 248.7473602294922, 234.1224822998047, 263.0619201660156, 219.8079376220703, 231.25181579589844, 221.3984375, 228.07080078125, 207.08389282226562, 259.88092041015625, 227.7604522705078, 296.4625244140625, 262.7515869140625, 301.23406982421875, 264.34210205078125, 315.548583984375, 283.42816162109375};
    for (int i=1; i<24; i++)
    {
      // fscanf(fq,"%f\n",&pick_points[i]);//%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
      // cout<<pick_points[i]<<endl;
      pick_points[i] = 1;
    }
  }
  ~Posepoint()
  {
  }
public:
    void start()
    {
      FILE  *fq;
      fq=fopen("/home/hts/Desktop/kinect_keypoints/a/test_keypoints.txt" ,"rt+");//"rt+"是打开一个文本文件，可以读写。
      while(!feof(fq))
      {
        if(fscanf(fq,"%lf\n",&pick_points[0])==1)
        {
          // cout<<pick_points[0]<<endl;
            for (int i=1; i<24; i++)
          {
            fscanf(fq,"%lf\n",&pick_points[i]);//%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
            // cout<<pick_points[i]<<endl;
          }
          char * a,*b,*c,*d;
          a= new char [3];
          b= new char [1];
          c= new char [14];
          d = new char [14];
          fscanf(fq,"%s", a);
          fscanf(fq,"%s", b);
          fscanf(fq,"%s", c);
          pic_class = a;
          person_class = b;
          pic_name = c;
          for(int i=0; i<5;i++)
          {
            d[i] =c[i];
          }
          d[5]='d';d[6]='e';d[7]='p';d[8]='t';d[9]='h';d[10]='.';d[11]='p';d[12]='n';d[13]='g';
          depth_name = d;
          // depth_name = depth_name+"depth.png";
          // depth_name = depth_name.substring(0, depth_name.length() -1);
          color_path = "/home/hts/Desktop/kinect_pic/"+person_class+'/'+ pic_class+'/'+pic_name;
          depth_path = "/home/hts/Desktop/kinect_depth/"+person_class+'/'+pic_class+'/'+depth_name;
          // cout<<color_path<<endl;
          // cout<<depth_path<<endl;
          begin(color_path,depth_path);
          // ThreeCross();
          Transformation();
          calculate();
          // if(!right)
          // {
          //   cout<<color_path<<endl;
          // }
        }
      }
      // cout<<"爬识别正确："<<sp<<"  爬识别错误："<<op<<endl;
    }
    void begin(std::string color_path, std::string depth_path)
  {
    // cout<<"开始!"<<endl;
    color = cv::imread(color_path);
    depth = cv::imread(depth_path,2);
    // cv::imshow("color", color);
    // cout<<"!!!"<<endl;
    // cv::imshow("depth", depth);
    // cv::waitKey(0);
    cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
    cloud->height = color.rows;
    cloud->width = color.cols;
    cloud->is_dense = false;
    cloud->points.resize(cloud->height * cloud->width);
    readCameraInfo();
    createLookup(this->color.cols, this->color.rows); //像极坐标系中的值保存在lookupX　lookupY里面
    cloudViewer();
  }
  // 3x3内参矩阵
//   367.933   0         254.169
//     0     367.933     204.267
//     0       0            1
    void readCameraInfo()
  {
    double cameraInfoK[9]={367.933 , 0 , 254.169,
                           0 , 367.933 , 204.267,
                          0 , 0 , 1             };
    double *itC = cameraMatrixColor.ptr<double>(0, 0);
    for(size_t i = 0; i < 9; ++i, ++itC)
    {
      *itC = cameraInfoK[i];
      // cout<<*itC<<endl;
    }
    // cout<<cameraMatrixColor.at<double>(0, 0)<<endl;
  }
    // 求在相机坐标系中的坐标
    void createLookup(size_t width, size_t height)
  {
    // width是列->x　height是行->y
    // 得到相机的内参数
    //  成像模型
// [u       [ fx　0 cx   [x
//  v  = 1/z  0  fy cy    y
//  1]        0   0  1]   z]
// 求逆矩阵　得到像极坐标系中的坐标
    const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
    const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
    const float cx = cameraMatrixColor.at<double>(0, 2);
    const float cy = cameraMatrixColor.at<double>(1, 2);
    float *it;
    // cout<<"相机的内参:"<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<" "<<endl;
    lookupY = cv::Mat(1, height, CV_32F);
    it = lookupY.ptr<float>();
    for(size_t r = 0; r < height; ++r, ++it)
    {
      *it = (r - cy) * fy;
      // cout<<*it<<endl;
    }

    lookupX = cv::Mat(1, width, CV_32F);
    it = lookupX.ptr<float>();
    for(size_t c = 0; c < width; ++c, ++it)
    {
      *it = (c - cx) * fx;
      // cout<<*it<<endl;
    }
  }
    void cloudViewer()
  {
    cv::Mat color, depth;
    // pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
    const std::string cloudName = "rendered";

    lock.lock();
    color = this->color;
    depth = this->depth;
    // updateCloud = false;
    lock.unlock();

    createCloud(depth, color, cloud);
    // 将点云数据添加到视窗中，并为其定义一个唯一的字符串作为ID号，利用此ID号保证其他成员方法也能表示该点云。
    // 多次调用addPointCloud()可以实现多个点云的叠加，每调用一次就创建一个新的ID号。如果想要更新一个已经
    // 显示的点云，用户必须先调用removePointCloud()，并提供新的ID号。（在PCL1.1版本之后直接调用updatePointCloud()
    //  就可以了，不必手动调用removePointCloud()就可实现点云更新）

    // visualizer->addPointCloud(cloud, cloudName);
    // // 修改现实点云的尺寸。用户可通过该方法控制点云在视窗中的显示方式
    // visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName);
    // // 设置XYZ三个坐标轴的大小和长度，该值也可以缺省
    // // 查看复杂的点云图像会让用户没有方向感，为了让用户保持正确的方向判断，需要显示坐标轴。三个坐标轴X（R，红色）
    // // Y（G，绿色）Z（B，蓝色）分别用三种不同颜色的圆柱体代替
    // visualizer->addCoordinateSystem(1.0);
    // // 通过设置相机参数是用户从默认的角度和方向观察点
    // visualizer->initCameraParameters();
    // // 设置窗口viewer的背景颜色
    // visualizer->setBackgroundColor(0, 0, 0);
    // // visualizer->setPosition(mode == BOTH ? color.cols : 0, 0);
    // visualizer->setPosition(0, 0);
    // visualizer->setSize(color.cols, color.rows);
    // visualizer->setShowFPS(true);
    // visualizer->setCameraPosition(0, 0, 0, 0, -1, 0);
    // visualizer->registerKeyboardCallback(&Posepoint::keyboardEvent, *this);
    // saveCloud(cloud);
    // // visualizer->spinOnce(300);
    // while (!visualize_stop) {
    //     visualizer->spinOnce(100);
    //     // boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    // }

    // visualizer->close();

  }
    void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
  {
    int body_part;
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    // #pragma omp parallel for
    for(int r = 0; r < depth.rows; ++r)
    {
      // 创建点云row行，每一行有col列
      pcl::PointXYZRGBA *itP = &cloud->points[r * depth.cols];
      // ptr函数访问任意一行像素的首地址
      const uint16_t *itD = depth.ptr<uint16_t>(r);
      const cv::Vec3b *itC = color.ptr<cv::Vec3b>(r);
      const float y = lookupY.at<float>(0, r);
      const float *itX = lookupX.ptr<float>();

        for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX)
      {
        register const float depthValue = *itD / 1000.0f;
        // Check for invalid measurements
        if(*itD == 0)
        {
          // not valid
          itP->x = itP->y = itP->z = badPoint;
          itP->rgba = 0;
          continue;
        }
        itP->z = depthValue;
        itP->x = *itX * depthValue;
        itP->y = y * depthValue;
        itP->b = itC->val[0];
        itP->g = itC->val[1];
        itP->r = itC->val[2];
        itP->a = 255;
          bool picked = false;
          if(size_t(pick_points[0]) == c && int(pick_points[1]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 0;
        }
          if(size_t(pick_points[2]) == c && int(pick_points[3]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 1;
        }
          if(size_t(pick_points[4]) == c && int(pick_points[5]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 2;
        }
          if(size_t(pick_points[6]) == c && int(pick_points[7]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 3;
        }
          if(size_t(pick_points[8]) == c && int(pick_points[9]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 4;
        }
          if(size_t(pick_points[10]) == c && int(pick_points[11]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 5;
        }
          if(size_t(pick_points[12]) == c && int(pick_points[13]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 6;
        }
          if(size_t(pick_points[14]) == c && int(pick_points[15]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 7;
        }
          if(size_t(pick_points[16]) == c && int(pick_points[17]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 8;
        }
          if(size_t(pick_points[18]) == c && int(pick_points[19]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 9;
        }
          if(size_t(pick_points[20]) == c && int(pick_points[21]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 12;
        }
          if(size_t(pick_points[22]) == c && int(pick_points[23]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 13;
        }
        if (picked)
        {
          register const float depthValue = *itD / 1000.0f;
          switch(body_part)
          {
          case 0:;
                Rankle(0) = itP->x; Rankle(1) = itP->y; Rankle(2) = itP->z;
                break;
          case 1:
                Rknee(0) = itP->x; Rknee(1) = itP->y; Rknee(2) = itP->z;
                break;
          case 2:
                Rhip(0) = itP->x; Rhip(1) = itP->y; Rhip(2) = itP->z;
                break;
          case 3:
                Lhip(0) = itP->x; Lhip(1) = itP->y; Lhip(2) = itP->z;
                break;
          case 4:
                Lknee(0) = itP->x; Lknee(1) = itP->y; Lknee(2) = itP->z;
                break;
          case 5:
                Lankle(0) = itP->x; Lankle(1) = itP->y; Lankle(2) = itP->z;
                break;
          case 6:
                Pelv(0) = itP->x; Pelv(1) = itP->y; Pelv(2) = itP->z;
                break;
          case 7:
                Thrx(0) = itP->x; Thrx(1) = itP->y; Thrx(2) = itP->z;
                break;
          case 8:
                Neck(0) = itP->x; Neck(1) = itP->y; Neck(2) = itP->z;
                break;
          case 9:
                Head(0) = itP->x; Head(1) = itP->y; Head(2) = itP->z;
                break;
          case 12:
                Rshoulder(0) = itP->x; Rshoulder(1) = itP->y; Rshoulder(2) = itP->z;
                break;
          case 13:
                Lshoulder(0) = itP->x; Lshoulder(1) = itP->y; Lshoulder(2) = itP->z;
                break;
          }
        }
      }
    }
  }
    void saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud)
  {
    std::ostringstream oss;
    oss.str("");
    oss << "/home/hts/Desktop/kinect_cloud/a/aaa/" << std::setfill('0') << std::setw(4);
    const std::string baseName = oss.str();
    const std::string cloudName = baseName + "_cloud.pcd";
    OUT_INFO("saving cloud: " << cloudName);
     // writer是该类的pcl::PCDWriter类型的成员变量
    writer.writeBinary(cloudName, *cloud);
  }
    void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)
  {
    if(event.keyUp())
    {
      switch(event.getKeyCode())
      {
      case 27:
      case 'q':
        visualize_stop = true;
        break;
      }
    }
  }
    //叉积
  Vector3f ThreeCross()
  {
    Vector3f a, b,result;
    a=Lshoulder-Pelv;
    b=Rshoulder-Pelv;
    // cout<<Lshoulder<<endl;cout<<Rshoulder<<endl;cout<<Pelv<<endl;
    // cout<<"左肩向量:\n"<<a<<"   右肩向量:\n"<<b<<endl;
    result = a.cross(b);
    // cout<<"躯干法向量:\n"<<result<<endl;
    // Eigen::Vector3d v3(0, 0, 0);
	  // v3.x() = 1;
	  // v3[2] = 1;
	  AngleAxisd angle_axis3(pi *25/ 18, Eigen::Vector3d(1, 0, 0));//1系绕x轴逆时针旋转250得到2系
    // angle_axis3.matrix().cast<float>()
	  Vector3f rotated_result = angle_axis3.matrix().cast<float>()*result;
	  // cout << "绕x轴顺时针旋转250°(Rcw):" << endl << angle_axis3.matrix() << endl;
	  // cout << "躯干法向量旋转后:" << endl << rotated_result.transpose() << endl;
    if (rotated_result(2)<0)
    {
      sp+=1;
      right = true;
    }
    if (rotated_result(2)>0)
    {
      op+=1;
      right = false;
    }
    return rotated_result;
  }
  //转换到世界坐标系
  void  Transformation()
  {
    // Vector3f result = ori;
    // // 旋转矩阵就是3x3的矩阵
    // Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量(方向与旋转轴[1,0,0]相同,模为旋转的角度250°)
    Eigen::AngleAxisd rotation_vector (pi *25/ 18,Eigen::Vector3d(1,0,0));
    cout.precision(3);//指定输出的精度
    cout<<"Rwc旋转矩阵为：\n"<<rotation_vector.matrix()<<endl;
    // 三维的齐次变换矩阵是4x4的矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    // 根据旋转向量进行旋转（注意旋转向量包含了旋转轴和旋转了的角度）
    T.rotate(rotation_vector);
    cout<<"旋转后的变换矩阵为：\n"<<T.matrix()<<endl;
    T.pretranslate(Eigen::Vector3d(0,0,1.35)); // 把第四列的平移向量设置为(1,3,4)
    cout<<"设置平移量后的矩阵为：\n"<<T.matrix()<<endl;
    // result = T*ori;
    Head = T.cast<float>()*Head; Neck = T.cast<float>()*Neck; Thrx = T.cast<float>()*Thrx; Pelv = T.cast<float>()*Pelv; Lhip = T.cast<float>()*Lhip; Lknee = T.cast<float>()*Lknee; Lankle = T.cast<float>()*Lankle; Rhip = T.cast<float>()*Rhip; Rknee = T.cast<float>()*Rknee; Rankle = T.cast<float>()*Rankle; Lshoulder = T.cast<float>()*Lshoulder; Rshoulder = T.cast<float>()*Rshoulder;
    cout<<Head<<endl;
    cout<<Neck<<endl;
    cout<<Thrx<<endl;
    cout<<Pelv<<endl;
    cout<<Lhip<<endl;
    cout<<Lknee<<endl;
    cout<<Lankle<<endl;
    cout<<Rhip<<endl;
    cout<<Rknee<<endl;
    cout<<Rankle<<endl;
  }
  double calculate()
  {
    double sum =0;
    sum = Head(2) + Neck(2) + Thrx(2) + Pelv(2) + Lhip(2) + Rhip(2)+Rshoulder(2)+Lshoulder(2);
    cout<<"和"<<sum<<endl;
    double mean = sum/10;
    cout<<"均值:"<<mean<<endl;
    double stdvar = sqrt(((Head(2)-mean)*(Head(2)-mean)+(Neck(2)-mean)*(Neck(2)-mean)+(Thrx(2)-mean)*(Thrx(2)-mean)+(Pelv(2)-mean)*(Pelv(2)-mean)+(Lhip(2)-mean)*(Lhip(2)-mean)+(Rhip(2)-mean)*(Rhip(2)-mean)+(Rshoulder(2)-mean)*(Rshoulder(2)-mean)+(Lshoulder(2)-mean)*(Lshoulder(2)-mean))/10);
    cout<<"标准差为:"<<stdvar<<endl;
    // if(stdvar<0.2)
    // {
    //   right = true;
    //   sp+=1;
    // }
    // else
    // {
    //   right =false;
    //   op+=1;
    // }
    FILE *fp = NULL;
    fp = fopen("/home/hts/catkin_ws/var.txt", "a+");
    fprintf(fp, "%lf\n%lf\n", mean,stdvar);
    fclose(fp);
    return stdvar;
  }

};

int main(int argc, char**argv)
{
  // std::string color_path = argv[1];
  // std::string depth_path = argv[2];
  Posepoint posepoint;
  posepoint.start();
  // cout<<"!!";
  // posepoint.ThreeCross();
  // posepoint.Transformation();
  // posepoint.calculate();
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_WHIncoherentAdjointPhotonScatteringDistribution_def.hpp
//! \author Alex Robinson
//! \brief  The Waller-Hartree adjoint incoherent photon scattering dist. def.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP
#define MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "Utility_InverseLengthConversionPolicy.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

// Constructor
template<typename ScatteringFunctionArgUnitConversionPolicy>
WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::WHIncoherentAdjointPhotonScatteringDistribution(
     const double max_energy,
     const Teuchos::RCP<const Utility::OneDDistribution>& scattering_function )
  : IncoherentAdjointPhotonScatteringDistribution( max_energy ),
    d_scattering_function( scattering_function )
{
  // Make sure the unit conversion policy is valid
  testStaticPrecondition( (boost::is_same<typename ScatteringFunctionArgUnitConversionPolicy::Dimension,Utility::InverseLengthDimension>::value) );
  // Make sure the scattering function is valid
  testPrecondition( !scattering_function.is_null() );
}

// Evaluate the distribution
template<typename ScatteringFunctionArgUnitConversionPolicy>
double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluate(
				   const double incoming_energy,
				   const double max_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       max_energy ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  const double scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy,
				      max_energy,
				      scattering_angle_cosine );

  const double diff_kn_cross_section =
    this->evaluateAdjointKleinNishinaDist( incoming_energy,
					   max_energy,
					   scattering_angle_cosine );

  return diff_kn_cross_section*scattering_function_value;
}

// Evaluate the integrated cross section (b)
template<typename ScatteringFunctionArgUnitConversionPolicy>
double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateIntegratedCrossSection(
						 const double incoming_energy,
						 const double max_energy,
						 const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );

  // Evaluate the integrated cross section
  boost::function<double (double x)> diff_cs_wrapper =
    boost::bind<double>( &WHIncoherentAdjointPhotonScatteringDistribution::evaluate,
			 boost::cref( *this ),
			 incoming_energy,
			 max_energy,
			 _1 );

  double abs_error, integrated_cs;

  Utility::GaussKronrodIntegrator quadrature_gkq_int( precision );

  const double min_scattering_angle_cosine =
    calculateMinScatteringAngleCosine( incoming_energy, max_energy );

  quadrature_gkq_int.integrateAdaptively<15>( diff_cs_wrapper,
					     min_scattering_angle_cosine,
					     1.0,
					     integrated_cs,
					     abs_error );

  // Make sure the integrated cross section is valid
  testPostcondition( integrated_cs >= 0.0 );

  return integrated_cs;
}

// Sample an outgoing energy and direction from the distribution
/*! \details This function will only sample an adjoint Compton line energy (no
 * Doppler broadening).
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::sample(
					const double incoming_energy,
					double& outgoing_energy,
					double& scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );

  unsigned trial_dummy;

  return this->sampleAndRecordTrials( incoming_energy,
				      outgoing_energy,
				      scattering_angle_cosine,
				      trial_dummy );
}

// Sample an outgoing energy and direction and record the number of trials
/*! \details This function will only sample an adjoint Compton line energy (no
 * Doppler broadening).
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::sampleAndRecordTrials(
					       const double incoming_energy,
					       double& outgoing_energy,
					       double& scattering_angle_cosine,
					       unsigned& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );

  // Evaluate the maximum scattering function value
  const double min_scattering_angle_cosine =
    calculateMinScatteringAngleCosine( incoming_energy, this->getMaxEnergy() );

  const double max_scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy,
				      min_scattering_angle_cosine );

  while( true )
  {
    this->sampleAndRecordTrialsAdjointKleinNishina( incoming_energy,
						    outgoing_energy,
						    scattering_angle_cosine,
						    trials );

    const double scattering_function_value =
      this->evaluateScatteringFunction( incoming_energy,
					scattering_angle_cosine );

    const double scaled_random_number = max_scattering_function_value*
      Utility::RandomNumberGenerator::getRandomNumber<double>();

    if( scaled_random_number <= scattering_function_value )
      break;
  }

  // Make sure the scattering angle cosine is valid
  testPostcondition( scattering_angle_cosine >= min_scattering_angle_cosine );
  testPostcondition( scattering_angle_cosine <= 1.0 );
  // Make sure the adjoint Compton line energy is valid
  testPostcondition( outgoing_energy >= incoming_energy );
}

// Randomly scatter the photon and return the shell that was interacted with
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::scatterAdjointPhoton(
				     AdjointPhotonState& adjoint_photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  // Make sure the adjoint photon energy is valid
  testPrecondition( adjoint_photon.getEnergy() <= this->getMaxEnergy() );

  // Generate probe particles
  this->createProbeParticles( adjoint_photon, bank );

  // Scattering the adjoint photon
  double outgoing_energy, scattering_angle_cosine;

  this->sample( adjoint_photon.getEnergy(),
		outgoing_energy,
		scattering_angle_cosine );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  adjoint_photon.setEnergy( outgoing_energy );

  adjoint_photon.rotateDirection( scattering_angle_cosine,
				  this->sampleAzimuthalAngle() );
}

// Check if an energy is above the scattering window
/*! \details Becuase of the scattering function evaluating to zero when
 * the scattering angle cosine is 1.0, the scattering window upper bound must
 * exclude the energy of interest.
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
bool WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::isEnergyAboveScatteringWindow(
				            const double energy_of_interest,
				            const double initial_energy ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( initial_energy > 0.0 );
  // Make sure the energy of interest is valid
  testPrecondition( energy_of_interest >= 0.0 );

  return initial_energy >= energy_of_interest;
}

// Evaluate the scattering function
template<typename ScatteringFunctionArgUnitConversionPolicy>
inline double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateScatteringFunction(
				   const double incoming_energy,
				   const double max_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       max_energy ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  // Calculate the outgoing energy
  const double outgoing_energy =
    calculateAdjointComptonLineEnergy( incoming_energy,
				       scattering_angle_cosine );

  // Calculate the inverse wavelength of the outgoing photon (1/cm)
  const double inverse_wavelength = outgoing_energy/
    (Utility::PhysicalConstants::planck_constant*
     Utility::PhysicalConstants::speed_of_light);

  // The scattering function argument (1/cm)
  double scattering_function_arg =
    sqrt( (1.0 - scattering_angle_cosine)/2.0 )*inverse_wavelength;

  ScatteringFunctionArgUnitConversionPolicy::convertFromNativeUnits(
						     scattering_function_arg );

  if( scattering_function_arg >=
      d_scattering_function->getUpperBoundOfIndepVar() )
    scattering_function_arg = d_scattering_function->getUpperBoundOfIndepVar();

  // Make sure the scattering function arg is valid
  testPostcondition( scattering_function_arg >=
		     d_scattering_function->getLowerBoundOfIndepVar() );
  testPostcondition( scattering_function_arg <=
		     d_scattering_function->getUpperBoundOfIndepVar() );

  return d_scattering_function->evaluate( scattering_function_arg );
}

// Evaluate the scattering function
template<typename ScatteringFunctionArgUnitConversionPolicy>
inline double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateScatteringFunction(
				   const double incoming_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       this->getMaxEnergy() ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  return this->evaluateScatteringFunction( incoming_energy,
					   this->getMaxEnergy(),
					   scattering_angle_cosine );
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_WHIncoherentAdjointPhotonScatteringDistribution.cpp
//---------------------------------------------------------------------------//