/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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.
 *
 */

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/projection.hpp>

// libmv headers
#include "libmv/multiview/projection.h"

#include <iostream>

namespace cv
{
namespace sfm
{

template<typename T>
void
homogeneousToEuclidean(const Mat & _X, Mat & _x)
{
  int d = _X.rows - 1;

  const Mat_<T> & X_rows = _X.rowRange(0,d);
  const Mat_<T> h = _X.row(d);

  const T * h_ptr = h[0], *h_ptr_end = h_ptr + h.cols;
  const T * X_ptr = X_rows[0];
  T * x_ptr = _x.ptr<T>(0);
  for (; h_ptr != h_ptr_end; ++h_ptr, ++X_ptr, ++x_ptr)
  {
    const T * X_col_ptr = X_ptr;
    T * x_col_ptr = x_ptr, *x_col_ptr_end = x_col_ptr + d * _x.step1();
    for (; x_col_ptr != x_col_ptr_end; X_col_ptr+=X_rows.step1(), x_col_ptr+=_x.step1() )
      *x_col_ptr = (*X_col_ptr) / (*h_ptr);
  }
}

void
homogeneousToEuclidean(const InputArray _X, OutputArray _x)
{
  // src
  const Mat X = _X.getMat();

  // dst
   _x.create(X.rows-1, X.cols, X.type());
  Mat x = _x.getMat();

  // type
  if( X.depth() == CV_32F )
  {
    homogeneousToEuclidean<float>(X,x);
  }
  else
  {
    homogeneousToEuclidean<double>(X,x);
  }
}

void
euclideanToHomogeneous(const InputArray _x, OutputArray _X)
{
  const Mat x = _x.getMat();
  const Mat last_row = Mat::ones(1, x.cols, x.type());
  vconcat(x, last_row, _X);
}

template<typename T>
void
projectionFromKRt(const Mat_<T> &K, const Mat_<T> &R, const Mat_<T> &t, Mat_<T> P)
{
  hconcat( K*R, K*t, P );
}

void
projectionFromKRt(InputArray _K, InputArray _R, InputArray _t, OutputArray _P)
{
  const Mat K = _K.getMat(), R = _R.getMat(), t = _t.getMat();
  const int depth = K.depth();
  CV_Assert((K.cols == 3 && K.rows == 3) && (t.cols == 1 && t.rows == 3) && (K.size() == R.size()));
  CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R.depth() && depth == t.depth());

  _P.create(3, 4, depth);

  Mat P = _P.getMat();

  // type
  if( depth == CV_32F )
  {
    projectionFromKRt<float>(K, R, t, P);
  }
  else
  {
    projectionFromKRt<double>(K, R, t, P);
  }

}

template<typename T>
void
KRtFromProjection( const Mat_<T> &_P, Mat_<T> _K, Mat_<T> _R, Mat_<T> _t )
{
  libmv::Mat34 P;
  libmv::Mat3 K, R;
  libmv::Vec3 t;

  cv2eigen( _P, P );

  libmv::KRt_From_P( P, &K, &R, &t );

  eigen2cv( K, _K );
  eigen2cv( R, _R );
  eigen2cv( t, _t );
}

void
KRtFromProjection( InputArray _P, OutputArray _K, OutputArray _R, OutputArray _t )
{
  const Mat P = _P.getMat();
  const int depth = P.depth();
  CV_Assert((P.cols == 4 && P.rows == 3) && (depth == CV_32F || depth == CV_64F));

  _K.create(3, 3, depth);
  _R.create(3, 3, depth);
  _t.create(3, 1, depth);

  Mat K = _K.getMat(), R = _R.getMat(), t = _t.getMat();

  // type
  if( depth == CV_32F )
  {
    KRtFromProjection<float>(P, K, R, t);
  }
  else
  {
    KRtFromProjection<double>(P, K, R, t);
  }
}

template<typename T>
T
depthValue( const Mat_<T> &_R, const Mat_<T> &_t, const Mat_<T> &_X )
{
  Matx<T,3,3> R(_R);
  Vec<T,3> t(_t);

  if ( _X.rows == 3)
  {
    Vec<T,3> X(_X);
    return (R*X)(2) + t(2);
  }
  else
  {
    Vec<T,4> X(_X);
    Vec<T,3> Xe;
    homogeneousToEuclidean(X,Xe);
    return depthValue<T>( Mat(R), Mat(t), Mat(Xe) );
  }
}

double
depth( InputArray _R, InputArray _t, InputArray _X)
{
  const Mat R = _R.getMat(), t = _t.getMat(), X = _X.getMat();
  const int depth = R.depth();
  CV_Assert( R.rows == 3 && R.cols == 3 && t.rows == 3 && t.cols == 1 );
  CV_Assert( (X.rows == 3 && X.cols == 1) || (X.rows == 4 && X.cols == 1) );
  CV_Assert( depth == CV_32F || depth == CV_64F );

  double depth_value = 0.0;

  if ( depth == CV_32F )
  {
    depth_value = static_cast<double>(depthValue<float>(R, t, X));
  }
  else
  {
    depth_value = depthValue<double>(R, t, X);
  }

  return depth_value;
}

} /* namespace sfm */
} /* namespace cv */


#include <atomic>
#include <chrono>
#include <string>
#include "QPanda.h"
#include <algorithm>
#include "gtest/gtest.h"

#include "ThirdParty/rapidjson/writer.h"
#include "ThirdParty/rapidjson/document.h"
#include "ThirdParty/rapidjson/stringbuffer.h"

#include "ThirdParty/Eigen/Dense"
#include "ThirdParty/Eigen/Sparse"
#include <EigenUnsupported/Eigen/KroneckerProduct>

#define CHECK_RUN_TIME_BEGIN                  auto start_time = chrono::system_clock::now()\

#define CHECK_RUN_TIME_END_AND_COUT_SECONDS(argv)   auto final_time = chrono::system_clock::now();\
                                                    auto duration = chrono::duration_cast<chrono::microseconds>(final_time - start_time);\
                                                    std::cout << argv << " -> run_time counts :" << double(duration.count()) * chrono::microseconds::period::num / chrono::microseconds::period::den << std::endl\

using namespace std;
using namespace rapidjson;

constexpr size_t SHOTS = 100;

QCircuit qft_prog(const QVec& qvec)
{
    QCircuit qft = CreateEmptyCircuit();
    for (auto i = 0; i < qvec.size(); i++)
    {
        qft << H(qvec[qvec.size() - 1 - i]);
        for (auto j = i + 1; j < qvec.size(); j++)
        {
            qft << CR(qvec[qvec.size() - 1 - j],
                qvec[qvec.size() - 1 - i], 2 * PI / (1 << (j - i + 1)));
        }
    }
    return qft;
}

QProg ghz_prog(const QVec& q)
{
    auto prog = QProg();
    prog << H(q[0]);

    for (auto i = 0; i < q.size() - 1; ++i)
    {
        prog << CNOT(q[i], q[i + 1]);
    }

    return prog;
}

static std::string build_chain_typed_quantum_chip_config_data(size_t qubit_num)
{
    rapidjson::Document arch_doc;
    arch_doc.SetObject();

    rapidjson::Document::AllocatorType &arch_alloc = arch_doc.GetAllocator();

    rapidjson::Value adj_object(rapidjson::kObjectType);
    for (SizeType qubit_addr = 0; qubit_addr < qubit_num; qubit_addr++)
    {
        rapidjson::Value qubit_array(rapidjson::kArrayType);

        if (qubit_addr != 0)
        {
            rapidjson::Value qubit_value(rapidjson::kObjectType);

            qubit_value.AddMember("v", qubit_addr - 1, arch_alloc);
            qubit_value.AddMember("w", 1.0, arch_alloc);

            qubit_array.PushBack(qubit_value, arch_alloc);
        }

        if (qubit_addr != (qubit_num - 1))
        {
            rapidjson::Value qubit_value(rapidjson::kObjectType);

            qubit_value.AddMember("v", qubit_addr + 1, arch_alloc);
            qubit_value.AddMember("w", 1.0, arch_alloc);

            qubit_array.PushBack(qubit_value, arch_alloc);
        }

        std::string qubit_addr_str = to_string(qubit_addr);

        rapidjson::Value qubit_value(kStringType);
        qubit_value.SetString(qubit_addr_str.c_str(), (rapidjson::SizeType)qubit_addr_str.size(), arch_alloc);

        adj_object.AddMember(qubit_value, qubit_array, arch_alloc);
    }

    arch_doc.AddMember("adj", adj_object, arch_alloc);
    arch_doc.AddMember("QubitCount", (SizeType)qubit_num, arch_alloc);

    //construct final json
    rapidjson::Document doc;
    doc.SetObject();

    rapidjson::Document::AllocatorType &alloc = doc.GetAllocator();

    doc.AddMember("QuantumChipArch", arch_doc, alloc);

    rapidjson::StringBuffer buffer;
    rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);

    return buffer.GetString();
}
#if 0
void test_real_chip()
{
    QCloudMachine QCM;;

    QCM.init("A0D08FA558CE45B2AF4F1DC122CF2589", true);

    QCM.set_qcloud_api("http://www.72bit.com");
    auto q = QCM.allocateQubits(6);
    auto c = QCM.allocateCBits(6);

    //构建量子程序
    auto measure_prog = QProg();
    measure_prog << HadamardQCircuit(q)
        << RX(q[1], PI / 4)
        << RX(q[2], PI / 4)
        << RX(q[1], PI / 4)
        << CZ(q[0], q[1])
        << CZ(q[1], q[2])
        << Measure(q[0], c[0])
        << Measure(q[1], c[1]);

    auto result = QCM.real_chip_measure(measure_prog, 1000, RealChipType::ORIGIN_WUYUAN_D4,true, true);
    for (auto val : result)
    {
        std::cout << val.first << " : " << val.second << std::endl;
    }

    auto result2 = QCM.get_state_tomography_density(measure_prog, 1000, RealChipType::ORIGIN_WUYUAN_D4);
    for (auto val : result2)
    {
        cout << val << endl;
    }

    auto result3 = QCM.get_state_fidelity(measure_prog, 1000, RealChipType::ORIGIN_WUYUAN_D4);
    cout << result3 << endl;

    QCM.finalize();
    return;
}

void test_partial()
{
    MPSQVM QCM;;

    QCM.init();
    auto q = QCM.allocateQubits(6);
    auto c = QCM.allocateCBits(6);

    auto measure_prog = QProg();
    auto measure_prog1 = QProg();
    measure_prog1 << X(q[0])
    << CZ(q[1], q[2])
    << CZ(q[0], q[1])
    << CZ(q[1], q[2])
    << MeasureAll(q,c);

    QCM.set_noise_model(BIT_PHASE_FLIP_OPRATOR, GateType::CZ_GATE, 0.799);
    auto result = QCM.runWithConfiguration(measure_prog1, c, 10000);

    QCM.finalize();
    return;
}

void test_qcloud()
{
    try
    {
        QCloudMachine QCM;
        QCM.init("A0D08FA558CE45B2AF4F1DC122CF2589", true);

        QCM.set_qcloud_api("http://www.72bit.com");
        //QCM.set_qcloud_api("http://10.10.10.39:8060");

        auto q = QCM.allocateQubits(4);
        auto c = QCM.allocateCBits(4);

        std::vector<QProg> measure_prog_array(3);
        measure_prog_array[0] << H(q[0]) << MeasureAll(q, c);
        measure_prog_array[1] << H(q[0]) << CNOT(q[0], q[1]) << MeasureAll(q, c);
        measure_prog_array[2] << H(q[0]) << H(q[1]) << H(q[2]) << MeasureAll(q, c);

        std::vector<QProg> pmeasure_prog_array(3);
        pmeasure_prog_array[0] << H(q[0]) << H(q[1]);
        pmeasure_prog_array[1] << H(q[0]) << CNOT(q[0], q[1]);
        pmeasure_prog_array[2] << H(q[0]) << H(q[1]) << H(q[2]);

        //auto measure_result = QCM.full_amplitude_measure_batch(measure_prog_array, 10000);
        //TaskStatus status;
        //auto taskids = QCM.real_chip_measure_batch_commit(measure_prog_array, 10000, status);
        auto real_result = QCM.real_chip_measure_batch(measure_prog_array, 10000);
        //auto a = QCM.real_chip_measure_batch_query(taskids);
        //auto pmeasure_result = QCM.full_amplitude_pmeasure_batch(pmeasure_prog_array, { 0,1,2,3,4,5 });
        //auto partial_pmeasure_result = QCM.partial_amplitude_pmeasure_batch(pmeasure_prog_array, { "0","1","2" });
        //auto single_pmeasure_result = QCM.single_amplitude_pmeasure_batch(pmeasure_prog_array, "0");

        //for (auto item : measure_result)
        //{
            //for (auto val : item)
            //{
                //cout << val.first << " : " << val.second << endl;
            //}
        //}

        //QCM.set_noise_model(NOISE_MODEL::BITFLIP_KRAUS_OPERATOR, { 0.9 }, { 0.9 });
        //auto noise_measure_result = QCM.noise_measure(measure_prog_array[0], 10000);

        //for (auto item : measure_result)
        //{
            //for (auto val : item)
            //{
                //cout << val.first << " : " << val.second << endl;
            //}
        //}

        cout << "--------------" << endl;

        //for (auto item : real_measure_result)
        //{
            //cout << "--------------" << endl;
            //for (auto val : item)
            //{
            //    cout << val.first << " : " << val.second << endl;
            //}
        //}

        cout << "--------------" << endl;

        //for (auto item : pmeasure_result)
        //{
            //cout << "--------------" << endl;
            //for (auto val : item)
            //{
                //cout << val.first << " : " << val.second << endl;
            //}
        //}

        cout << "--------------" << endl;

        cout << "--------------" << endl;

        //for (auto item : single_pmeasure_result)
        //{
        //    cout << item << endl;
        //}

        cout << "--------------" << endl;

        QCM.finalize();
    }
    catch (const std::exception e)
    {
        cout << e.what() << endl;
    }

    return;
}

TEST(MPS, test)
{
    //test_partial();
    cout << 1222 << endl;

    test_partial();

    MPSQVM qvm;
    qvm.init();

    auto q = qvm.qAllocMany(2);
    auto c = qvm.cAllocMany(2);

    qvm.add_single_noise_model(NOISE_MODEL::PHASE_DAMPING_OPRATOR, GateType::PAULI_X_GATE, 0.8);
    qvm.add_single_noise_model(NOISE_MODEL::BITFLIP_KRAUS_OPERATOR, GateType::PAULI_X_GATE, 0.8);
    //qvm.add_single_noise_model(NOISE_MODEL::DEPOLARIZING_KRAUS_OPERATOR, GateType::PAULI_X_GATE, 0.99);
    //qvm.add_single_noise_model(NOISE_MODEL::DEPHASING_KRAUS_OPERATOR, GateType::PAULI_X_GATE, 0.49);
    //qvm.add_single_noise_model(NOISE_MODEL::DECOHERENCE_KRAUS_OPERATOR, GateType::PAULI_X_GATE, 5, 5, 0.9);
    //qvm.set_noise_model(NOISE_MODEL::BITFLIP_KRAUS_OPERATOR, GateType::CNOT_GATE, 0.1);
    //QVec qv0 = { q[0], q[1] };
    //qvm.set_noise_model(NOISE_MODEL::DEPHASING_KRAUS_OPERATOR, GateType::HADAMARD_GATE, 0.1, qv0);
    //std::vector<QVec> qves = { {q[0], q[1]}, {q[1], q[2]} };
    //qvm.set_noise_model(NOISE_MODEL::DAMPING_KRAUS_OPERATOR, GateType::CNOT_GATE, 0.1, qves);

    QProg prog;
    //prog << X(q[0]) << MeasureAll(q, c);
    prog << H(q[0]) << X(q[1]) << H(q[1]) << CNOT(q[0],q[1]) << H(q[0]) << Measure(q[0], c[0]);

    //auto noise_ptr = (NoiseQVM*)(&qvm);
    //auto noise_ptr = dynamic_cast<NoiseQVM*>(&qvm);
    //single_qubit_rb(&qvm, q[0], {6}, 1, 100);

    auto result = qvm.runWithConfiguration(prog, c, 10000);
    for (auto &item : result)
    {
        std::cout << item.first << " : " << item.second << std::endl;
    }

    return;
}

#endif


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cfloat>

// Eigen
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

// Boost
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

// OpenMP
#include <omp.h>

#include "triangle_point/poitri.h"

/** \brief Compute triangle point distance and corresponding closest point.
 * \param[in] point point
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \param[out] ray corresponding closest point
 * \return distance
 */
float triangle_point_distance(const Eigen::Vector3f point, const Eigen::Vector3f v1, const Eigen::Vector3f v2, const Eigen::Vector3f v3,
    Eigen::Vector3f &closest_point) {

  Vec3f x0(point.data());
  Vec3f x1(v1.data());
  Vec3f x2(v2.data());
  Vec3f x3(v3.data());

  Vec3f r(0);
  float distance = point_triangle_distance(x0, x1, x2, x3, r);

  for (int d = 0; d < 3; d++) {
    closest_point(d) = r[d];
  }

  return distance;
}

/** \brief Point cloud class forward declaration. */
class PointCloud;

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
  /** \brief Empty constructor. */
  Mesh() {

  }

  /** \brief Reading an off file and returning the vertices x, y, z coordinates and the
   * face indices.
   * \param[in] filepath path to the OFF file
   * \param[out] mesh read mesh with vertices and faces
   * \return success
   */
  static bool from_off(const std::string filepath, Mesh& mesh) {

    std::ifstream* file = new std::ifstream(filepath.c_str());
    std::string line;
    std::stringstream ss;
    int line_nb = 0;

    std::getline(*file, line);
    ++line_nb;

    if (line != "off" && line != "OFF") {
      std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
      return false;
    }

    size_t n_edges;
    std::getline(*file, line);
    ++line_nb;

    int n_vertices;
    int n_faces;
    ss << line;
    ss >> n_vertices;
    ss >> n_faces;
    ss >> n_edges;

    for (size_t v = 0; v < n_vertices; ++v) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      Eigen::Vector3f vertex;
      ss << line;
      ss >> vertex(0);
      ss >> vertex(1);
      ss >> vertex(2);

      mesh.add_vertex(vertex);
    }

    size_t n;
    for (size_t f = 0; f < n_faces; ++f) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      size_t n;
      ss << line;
      ss >> n;

      if(n != 3) {
        std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
        return false;
      }

      Eigen::Vector3i face;
      ss >> face(0);
      ss >> face(1);
      ss >> face(2);

      mesh.add_face(face);
    }

    if (n_vertices != mesh.num_vertices()) {
      std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
      return false;
    }

    if (n_faces != mesh.num_faces()) {
      std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
      return false;
    }

    file->close();
    delete file;

    return true;
  }

  /** \brief Write mesh to OFF file.
   * \param[in] filepath path to OFF file to write
   * \return success
   */
  bool to_off(const std::string filepath) const {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(out)) {
      return false;
    }

    (*out) << "OFF" << std::endl;
    (*out) << this->num_vertices() << " " << this->num_faces() << " 0" << std::endl;

    for (unsigned int v = 0; v < this->num_vertices(); v++) {
      (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
    }

    for (unsigned int f = 0; f < this->num_faces(); f++) {
      (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Add a vertex.
   * \param[in] vertex vertex to add
   */
  void add_vertex(Eigen::Vector3f& vertex) {
    this->vertices.push_back(vertex);
  }

  /** \brief Get the number of vertices.
   * \return number of vertices
   */
  int num_vertices() const {
    return static_cast<int>(this->vertices.size());
  }

  /** \brief Get a vertex.
   * \param[in] v vertex index
   * \return vertex
   */
  Eigen::Vector3f vertex(int v) const {
    assert(v >= 0 && v < this->num_vertices());
    return this->vertices[v];
  }

  /** \brief Add a face.
   * \param[in] face face to add
   */
  void add_face(Eigen::Vector3i& face) {
    this->faces.push_back(face);
  }

  /** \brief Get the number of faces.
   * \return number of faces
   */
  int num_faces() const {
    return static_cast<int>(this->faces.size());
  }

  /** \brief Get a face.
   * \param[in] f face index
   * \return face
   */
  Eigen::Vector3i face(int f) const {
    assert(f >= 0 && f < this->num_faces());
    return this->faces[f];
  }

  /** \brief Sample points from the mesh
   * \param[in] mesh mesh to sample from
   * \param[in] n batch index in points
   * \param[in] points pre-initialized tensor holding points
   */
  bool sample(const int N, PointCloud &point_cloud) const;

private:

  /** \brief Vertices as (x,y,z)-vectors. */
  std::vector<Eigen::Vector3f> vertices;

  /** \brief Faces as list of vertex indices. */
  std::vector<Eigen::Vector3i> faces;
};

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
  /** \brief Constructor. */
  PointCloud() {

  }

  /** \brief Copy constructor.
   * \param[in] point_cloud point cloud to copy
   */
  PointCloud(const PointCloud &point_cloud) {
    this->points.clear();

    for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
      this->points.push_back(point_cloud.points[i]);
    }
  }

  /** \brief Destructor. */
  ~PointCloud() {

  }

  /** \brief Read point cloud from txt file.
   * \param[in] filepath path to file to read
   * \param[out] point_cloud
   * \return success
   */
  static bool from_txt(const std::string &filepath, PointCloud &point_cloud) {
    std::ifstream file(filepath.c_str());
    std::string line;
    std::stringstream ss;

    std::getline(file, line);
    ss << line;

    int n_points = 0;
    ss >> n_points;

    if (n_points < 0) {
      return false;
    }

    for (int i = 0; i < n_points; i++) {
      std::getline(file, line);

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

      Eigen::Vector3f point(0, 0, 0);
      ss >> point(0);
      ss >> point(1);
      ss >> point(2);

      point_cloud.add_point(point);
    }

    return true;
  }

  /** \brief Add a point to the point cloud.
   * \param[in] point point to add
   */
  void add_point(const Eigen::Vector3f &point) {
    this->points.push_back(point);
  }

  /** \brief Get number of points.
   * \return number of points
   */
  unsigned int num_points() const {
    return this->points.size();
  }

  /** \brief Compute distance to mesh.
    * \param[in] mesh
    * \param[out] distances per point distances
    * \param[out] distance
    * \return success
    */
  bool compute_distance(const Mesh &mesh, float &_distance) {
    _distance = 0;

    if (this->num_points() <= 0) {
      std::cout << "[Error] no points in this point clouds" << std::endl;
      return false;
    }

    if (mesh.num_faces() <= 0) {
      std::cout << "[Error] no faces in given mesh" << std::endl;
      return false;
    }

    #pragma omp parallel
    {
      #pragma omp for
      for (unsigned int i = 0; i < this->points.size(); i++) {

        float min_distance = FLT_MAX;
        for (int f = 0; f < mesh.num_faces(); f++) {
          Eigen::Vector3f closest_point;
          Eigen::Vector3f v1 = mesh.vertex(mesh.face(f)(0));
          Eigen::Vector3f v2 = mesh.vertex(mesh.face(f)(1));
          Eigen::Vector3f v3 = mesh.vertex(mesh.face(f)(2));

          triangle_point_distance(this->points[i], v1, v2, v3, closest_point);
          float distance = (this->points[i] - closest_point).norm();

          if (distance < min_distance) {
            min_distance = distance;
          }
        }

        #pragma omp atomic
        _distance += min_distance;
      }
    }

    _distance /= this->num_points();
    return true;
  }

private:
  /** \brief The points of the point cloud. */
  std::vector<Eigen::Vector3f> points;

};

/** \brief Sample points from the mesh
 * \param[in] mesh mesh to sample from
 * \param[in] n batch index in points
 * \param[in] points pre-initialized tensor holding points
 */
bool Mesh::sample(const int N, PointCloud &point_cloud) const {

  // Stores the areas of faces.
  std::vector<float> areas(this->num_faces());
  float sum = 0;

  // Build a probability distribution over faces.
  for (int f = 0; f < this->num_faces(); f++) {
    Eigen::Vector3f a = this->vertices[this->faces[f][0]];
    Eigen::Vector3f b = this->vertices[this->faces[f][1]];
    Eigen::Vector3f c = this->vertices[this->faces[f][2]];

    // Angle between a->b and a->c.
    Eigen::Vector3f ab = b - a;
    Eigen::Vector3f ac = c - a;
    float cos_angle = ab.dot(ac)/(ab.norm()*ac.norm());
    float angle = std::acos(cos_angle);

    // Compute triangle area.
    float area = std::max(0., 0.5*ab.norm()*ac.norm()*std::sin(angle));
    //std::cout << area << " " << std::pow(area, 1./4.) << " " << angle << " " << ab.norm() << " " << ac.norm() << " " << std::sin(angle) << std::endl;

    // Accumulate.
    //area = std::sqrt(area);
    areas[f] = area;
    sum += area;
    //areas.push_back(1);
    //sum += 1;
  }

  //std::cout << sum << std::endl;
  if (sum < 1e-6) {
    std::cout << "[Error] face area sum of " << sum << std::endl;
    return false;
  }

  for (int f = 0; f < this->num_faces(); f++) {
    //std::cout << areas[f] << " ";
    areas[f] /= sum;
    //std::cout << areas[f] << std::endl;
  }

  std::vector<float> cum_areas(areas.size());
  cum_areas[0] = areas[0];

  for (int f = 1; f < this->num_faces(); f++) {
    cum_areas[f] = areas[f] + cum_areas[f - 1];
  }

  for (int f = 0; f < this->num_faces(); f++) {
    int n = std::max(static_cast<int>(areas[f]*N), 1);

    for (int i = 0; i < n; i++) {
      float r1 = 0;
      float r2 = 0;
      do {
        r1 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
        r2 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
      }
      while (r1 + r2 > 1.f);

      int s = std::rand()%3;
      //std::cout << face << " " << areas[face] << std::endl;

      Eigen::Vector3f a = this->vertices[this->faces[f](s)];
      Eigen::Vector3f b = this->vertices[this->faces[f]((s + 1)%3)];
      Eigen::Vector3f c = this->vertices[this->faces[f]((s + 2)%3)];

      Eigen::Vector3f ab = b - a;
      Eigen::Vector3f ac = c - a;

      Eigen::Vector3f point = a + r1*ab + r2*ac;
      point_cloud.add_point(point);
    }
  }

  return true;
}

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const boost::filesystem::path directory, std::map<int, boost::filesystem::path>& files, const std::vector<std::string> &extensions) {

  files.clear();
  boost::filesystem::directory_iterator end;

  for (boost::filesystem::directory_iterator it(directory); it != end; ++it) {
    bool filtered = true;
    for (unsigned int i = 0; i < extensions.size(); i++) {
      if (it->path().extension().string() == extensions[i]) {
        filtered = false;
      }
    }

    if (!filtered) {
      int number = std::stoi(it->path().filename().string());
      files.insert(std::pair<int, boost::filesystem::path>(number, it->path()));
    }
  }
}

/** \brief Main entrance point of the script.
 * Expects one parameter, the path to the corresponding config file in config/.
 */
int main(int argc, char** argv) {
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
      ("input",  boost::program_options::value<std::string>(), "input, either single OFF file or directory containing OFF files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero")
      ("reference", boost::program_options::value<std::string>(), "reference, either single OFF or TXT file or directory containing OFF or TXT files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero (the file names need to correspond to those found in the input directory); for TXT files, accuracy cannot be computed")
      ("output", boost::program_options::value<std::string>(), "output file, a TXT file containing accuracy and completeness for each input-reference pair as well as overall averages")
      ("n_points", boost::program_options::value<int>()->default_value(10000), "number points to sample from meshes in order to compute distances");

  boost::program_options::positional_options_description positionals;
  positionals.add("input", 1);
  positionals.add("reference", 1);
  positionals.add("output", 1);

  boost::program_options::variables_map parameters;
  boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(positionals).run(), parameters);
  boost::program_options::notify(parameters);

  if (parameters.find("help") != parameters.end()) {
    std::cout << desc << std::endl;
    return 1;
  }

  boost::filesystem::path input(parameters["input"].as<std::string>());
  if (!boost::filesystem::is_directory(input) && !boost::filesystem::is_regular_file(input)) {
    std::cout << "Input is neither directory nor file." << std::endl;
    return 1;
  }

  boost::filesystem::path reference(parameters["reference"].as<std::string>());
  if (!boost::filesystem::is_directory(reference) && !boost::filesystem::is_regular_file(reference)) {
    std::cout << "Reference is neither directory nor file." << std::endl;
    return 1;
  }

  boost::filesystem::path output(parameters["output"].as<std::string>());
  if (boost::filesystem::is_regular_file(output)) {
    std::cout << "Output file already exists; overwriting." << std::endl;
  }

  int N_points = parameters["n_points"].as<int>();
  std::cout << "Using " << N_points << " points." << std::endl;

  std::map<int, boost::filesystem::path> input_files;
  std::map<int, boost::filesystem::path> reference_files;

  if (boost::filesystem::is_regular_file(input)) {
    if (input.extension().string() != ".off") {
      std::cout << "Only OFF files supported as input." << std::endl;
      return 1;
    }

    input_files.insert(std::pair<int, boost::filesystem::path>(0, input));
  }
  else {
    read_directory(input, input_files, {".off"});

    if (input_files.size() <= 0) {
      std::cout << "Could not find any OFF files in input directory." << std::endl;
      return 1;
    }

    std::cout << "Read " << input_files.size() << " input files." << std::endl;
  }

  if (boost::filesystem::is_regular_file(reference)) {
    if (reference.extension().string() != ".off" && reference.extension().string() != ".txt") {
      std::cout << "Only OFF or TXT files supported as reference." << std::endl;
      return 1;
    }

    reference_files.insert(std::pair<int, boost::filesystem::path>(0, reference));
  }
  else {
    read_directory(reference, reference_files, {".off", ".txt"});

    if (input_files.size() <= 0) {
      std::cout << "Could not find any OFF or TXT files in reference directory." << std::endl;
      return 1;
    }

    std::cout << "Read " << reference_files.size() << " reference files." << std::endl;
  }

  std::map<int, float> accuracies;
  std::map<int, float> completenesses;

  for (std::map<int, boost::filesystem::path>::iterator it = input_files.begin(); it != input_files.end(); it++) {

    int n = it->first;
    if (reference_files.find(n) == reference_files.end()) {
      std::cout << "Could not find the reference file corresponding to " << input_files[n] << "." << std::endl;
      return 1;
    }

    boost::filesystem::path input_file = input_files[n];
    boost::filesystem::path reference_file = reference_files[n];

    Mesh input_mesh;
    bool success = Mesh::from_off(input_file.string(), input_mesh);

    if (!success) {
      std::cout << "Could not read " << input_file << "." << std::endl;
      return 1;
    }

    if (reference_file.extension().string() == ".off") {
      Mesh reference_mesh;
      success = Mesh::from_off(reference_file.string(), reference_mesh);

      if (!success) {
        std::cout << "Could not read " << reference_file << "." << std::endl;
        return 1;
      }

      PointCloud input_point_cloud;
      success = input_mesh.sample(N_points, input_point_cloud);

      if (success) {
        float accuracy = 0;
        success = input_point_cloud.compute_distance(reference_mesh, accuracy);

        if (success) {
          accuracies[n] = accuracy;
          std::cout << "Computed accuracy for " << input_file << "." << std::endl;
        }
        else {
          std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
        }
      }
      else {
        std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
      }

      PointCloud reference_point_cloud;
      reference_mesh.sample(N_points, reference_point_cloud);

      if (success) {
        float completeness = 0;
        success = reference_point_cloud.compute_distance(input_mesh, completeness);

        if (success) {
          completenesses[n] = completeness;
          std::cout << "Computed completeness for " << input_file << "." << std::endl;
        }
        else {
          std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
        }
      }
      else {
        std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
      }
    }
    else if (reference_file.extension().string() == ".txt") {
      PointCloud reference_point_cloud;
      success = PointCloud::from_txt(reference_file.string(), reference_point_cloud);

      if (!success) {
        std::cout << "Could not read " << reference_file << "." << std::endl;
        return 1;
      }

      float completeness = 0;
      success = reference_point_cloud.compute_distance(input_mesh, completeness);

      if (success) {
        completenesses[n] = completeness;
        std::cout << "Computed completeness for " << input_file << "." << std::endl;
      }
      else {
        std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
      }
    }
    else {
      std::cout << "Reference file " << reference_file << " has invalid extension." << std::endl;
    }
  }

  std::ofstream* out = new std::ofstream(output.string(), std::ofstream::out);
  if (!static_cast<bool>(*out)) {
    std::cout << "Could not open " << output << std::endl;
    exit(1);
  }

  float accuracy = 0;
  float completeness = 0;

  for (std::map<int, boost::filesystem::path>::iterator it = input_files.begin(); it != input_files.end(); it++) {
    int n = it->first;

    (*out) << n << " ";
    if (accuracies.find(n) != accuracies.end()) {
      (*out) << accuracies[n];
      accuracy += accuracies[n];
    }
    else {
      (*out) << "-1";
    }

    (*out) << " ";
    if (completenesses.find(n) != completenesses.end()) {
      (*out) << completenesses[n];
      completeness += completenesses[n];
    }
    else {
      (*out) << "-1";
    }

    (*out) << std::endl;
  }

  if (accuracies.size() > 0) {
    accuracy /= accuracies.size();
    (*out) << accuracy;
    std::cout << "Accuracy (input to reference): " << accuracy << std::endl;
  }
  else {
    (*out) << "-1";
    std::cout << "Could not compute accuracy." << std::endl;
  }

  (*out) << " ";
  if (completenesses.size() > 0) {
    completeness /= completenesses.size();
    (*out) << completeness;
  std::cout << "Completeness (reference to input): " << completeness << std::endl;
  }
  else {
    (*out) << "-1";
    std::cout << "Could not compute completeness." << std::endl;
  }

  out->close();
  delete out;
  std::cout << "Wrote " << output << "." << std::endl;

  exit(0);
}


#include "drake/systems/framework/basic_vector.h"

#include <cmath>
#include <sstream>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/autodiff.h"
#include "drake/common/eigen_types.h"
#include "drake/common/symbolic.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_throws_message.h"
#include "drake/systems/framework/test_utilities/my_vector.h"

namespace drake {
namespace systems {
namespace {

// Tests the initializer_list functionality.
GTEST_TEST(BasicVectorTest, InitializerList) {
  const BasicVector<double> empty1;  // Default constructor.
  const BasicVector<double> empty2{};  // Initializer list.
  EXPECT_EQ(0, empty1.size());
  EXPECT_EQ(0, empty2.size());

  const BasicVector<double> pair{1.0, 2.0};
  ASSERT_EQ(2, pair.size());
  EXPECT_EQ(1.0, pair[0]);
  EXPECT_EQ(2.0, pair[1]);

  const BasicVector<double> from_floats{3.0f, 4.0f, 5.0f};
  ASSERT_EQ(3, from_floats.size());
  EXPECT_EQ(3.0, from_floats[0]);
  EXPECT_EQ(4.0, from_floats[1]);
  EXPECT_EQ(5.0, from_floats[2]);

  const BasicVector<AutoDiffXd> autodiff{22.0};
  ASSERT_EQ(1, autodiff.size());
  EXPECT_EQ(22.0, autodiff[0].value());
}

GTEST_TEST(BasicVectorTest, Access) {
  const BasicVector<double> pair{1.0, 2.0};
  EXPECT_EQ(1.0, pair.GetAtIndex(0));
  EXPECT_EQ(2.0, pair.GetAtIndex(1));
}

GTEST_TEST(BasicVectorTest, OutOfRange) {
  BasicVector<double> pair{1.0, 2.0};
  EXPECT_THROW(pair.GetAtIndex(-1), std::exception);
  EXPECT_THROW(pair.GetAtIndex(10), std::exception);
  EXPECT_THROW(pair.SetAtIndex(-1, 0.0), std::exception);
  EXPECT_THROW(pair.SetAtIndex(10, 0.0), std::exception);
}

// Tests SetZero functionality.
GTEST_TEST(BasicVectorTest, SetZero) {
  BasicVector<double> vec{1.0, 2.0, 3.0};
  EXPECT_EQ(Eigen::Vector3d(1.0, 2.0, 3.0), vec.get_value());
  vec.SetZero();
  EXPECT_EQ(Eigen::Vector3d(0, 0, 0), vec.get_value());
}

// Tests that the BasicVector<double> is initialized to NaN.
GTEST_TEST(BasicVectorTest, DoubleInitiallyNaN) {
  BasicVector<double> vec(3);
  Eigen::Vector3d expected(NAN, NAN, NAN);
  EXPECT_TRUE(CompareMatrices(expected, vec.get_value(),
                              Eigen::NumTraits<double>::epsilon(),
                              MatrixCompareType::absolute));
}

// Tests that the BasicVector<AutoDiffXd> is initialized to NaN.
GTEST_TEST(BasicVectorTest, AutodiffInitiallyNaN) {
  BasicVector<AutoDiffXd> vec(3);
  EXPECT_TRUE(std::isnan(vec[0].value()));
  EXPECT_TRUE(std::isnan(vec[1].value()));
  EXPECT_TRUE(std::isnan(vec[2].value()));
}

// Tests that the BasicVector<symbolic::Expression> is initialized to NaN.
GTEST_TEST(BasicVectorTest, SymbolicInitiallyNaN) {
  BasicVector<symbolic::Expression> vec(1);
  EXPECT_TRUE(symbolic::is_nan(vec.get_value()[0]));
}

// Tests BasicVector<T>::Make.
GTEST_TEST(BasicVectorTest, Make) {
  auto dut1 = BasicVector<double>::Make(1.0, 2.0);
  auto dut2 = BasicVector<double>::Make({3.0, 4.0});
  EXPECT_TRUE(CompareMatrices(dut1->get_value(), Eigen::Vector2d(1.0, 2.0)));
  EXPECT_TRUE(CompareMatrices(dut2->get_value(), Eigen::Vector2d(3.0, 4.0)));
}

// Tests that BasicVector<symbolic::Expression>::Make does what it says on
// the tin.
GTEST_TEST(BasicVectorTest, MakeSymbolic) {
  auto vec = BasicVector<symbolic::Expression>::Make(
      symbolic::Variable("x"),
      2.0,
      symbolic::Variable("y") + 2.0);
  EXPECT_EQ("x", vec->GetAtIndex(0).to_string());
  EXPECT_EQ("2", vec->GetAtIndex(1).to_string());
  EXPECT_EQ("(2 + y)", vec->GetAtIndex(2).to_string());
}

// Tests that the BasicVector has a size as soon as it is constructed.
GTEST_TEST(BasicVectorTest, Size) {
  BasicVector<double> vec(5);
  EXPECT_EQ(5, vec.size());
}

// Tests that the BasicVector can be mutated in-place.
GTEST_TEST(BasicVectorTest, Mutate) {
  BasicVector<double> vec(2);
  vec.get_mutable_value() << 1, 2;
  Eigen::Vector2d expected;
  expected << 1, 2;
  EXPECT_EQ(expected, vec.get_value());
}

// Tests that the BasicVector can be addressed as an array.
GTEST_TEST(BasicVectorTest, ArrayOperator) {
  BasicVector<double> vec(2);
  vec[0] = 76;
  vec[1] = 42;

  Eigen::Vector2d expected;
  expected << 76, 42;
  EXPECT_EQ(expected, vec.get_value());

  const auto& const_vec = vec;
  EXPECT_EQ(76, const_vec[0]);
  EXPECT_EQ(42, const_vec[1]);
}

// Tests that the BasicVector can be set from another vector.
GTEST_TEST(BasicVectorTest, SetWholeVector) {
  BasicVector<double> vec(2);
  vec.get_mutable_value() << 1, 2;

  Eigen::Vector2d next_value;
  next_value << 3, 4;
  vec.set_value(next_value);
  EXPECT_EQ(next_value, vec.get_value());

  Eigen::Vector2d another_value;
  another_value << 5, 6;
  vec.SetFromVector(another_value);
  EXPECT_EQ(another_value, vec.get_value());

  EXPECT_THROW(vec.SetFromVector(Eigen::Vector3d::Zero()), std::exception);
}

// Tests that the BasicVector can be set from another vector base.
GTEST_TEST(BasicVectorTest, SetFrom) {
  BasicVector<double> vec(2);
  vec.get_mutable_value() << 1, 2;
  BasicVector<double> next_vec(2);
  next_vec.get_mutable_value() << 3, 4;
  vec.SetFrom(next_vec);
  EXPECT_EQ(next_vec.get_value(), vec.get_value());
  EXPECT_THROW(vec.SetFrom(BasicVector<double>(3)), std::exception);
}

// Tests that when BasicVector is cloned, its data is preserved.
GTEST_TEST(BasicVectorTest, Clone) {
  BasicVector<double> vec(2);
  vec.get_mutable_value() << 1, 2;

  std::unique_ptr<BasicVector<double>> clone = vec.Clone();

  Eigen::Vector2d expected;
  expected << 1, 2;
  EXPECT_EQ(expected, clone->get_value());
}

// Tests that an error is thrown when the BasicVector is set from a vector
// of a different size.
GTEST_TEST(BasicVectorTest, ReinitializeInvalid) {
  BasicVector<double> vec(2);
  Eigen::Vector3d next_value;
  next_value << 3, 4, 5;
  EXPECT_THROW(vec.set_value(next_value), std::exception);
}

// Tests all += * operations for BasicVector.
GTEST_TEST(BasicVectorTest, PlusEqScaled) {
  BasicVector<double> ogvec(2), vec1(2), vec2(2), vec3(2), vec4(2), vec5(2);
  Eigen::Vector2d ans1, ans2, ans3, ans4, ans5;
  ogvec.SetZero();
  vec1.get_mutable_value() << 1, 2;
  vec2.get_mutable_value() << 3, 5;
  vec3.get_mutable_value() << 7, 11;
  vec4.get_mutable_value() << 13, 17;
  vec5.get_mutable_value() << 19, 23;
  VectorBase<double>& v1 = vec1;
  VectorBase<double>& v2 = vec2;
  VectorBase<double>& v3 = vec3;
  VectorBase<double>& v4 = vec4;
  VectorBase<double>& v5 = vec5;
  ogvec.PlusEqScaled(2, v1);
  ans1 << 2, 4;
  EXPECT_EQ(ans1, ogvec.get_value());

  ogvec.SetZero();
  ogvec.PlusEqScaled({{2, v1}, {3, v2}});
  ans2 << 11, 19;
  EXPECT_EQ(ans2, ogvec.get_value());

  ogvec.SetZero();
  ogvec.PlusEqScaled({{2, v1}, {3, v2}, {5, v3}});
  ans3 << 46, 74;
  EXPECT_EQ(ans3, ogvec.get_value());

  ogvec.SetZero();
  ogvec.PlusEqScaled({{2, v1}, {3, v2}, {5, v3}, {7, v4}});
  ans4 << 137, 193;
  EXPECT_EQ(ans4, ogvec.get_value());

  ogvec.SetZero();
  ogvec.PlusEqScaled({{2, v1}, {3, v2}, {5, v3}, {7, v4}, {11, v5}});
  ans5 << 346, 446;
  EXPECT_EQ(ans5, ogvec.get_value());
}

template <typename T>
class TypedBasicVectorTest : public ::testing::Test {};

using DefaultScalars =
    ::testing::Types<double, AutoDiffXd, symbolic::Expression>;
TYPED_TEST_SUITE(TypedBasicVectorTest, DefaultScalars);

// Tests ability to stream a BasicVector into a string.
TYPED_TEST(TypedBasicVectorTest, StringStream) {
  using T = TypeParam;
  BasicVector<T> vec(3);
  vec.get_mutable_value() << 1.0, 2.2, 3.3;
  std::stringstream s;
  s << "hello " << vec << " world";
  std::stringstream s_expected;
  s_expected << "hello " << vec.get_value().transpose() << " world";
  EXPECT_EQ(s.str(), s_expected.str());
}

// Tests ability to stream a BasicVector of size zero into a string.
TYPED_TEST(TypedBasicVectorTest, ZeroLengthStringStream) {
  using T = TypeParam;
  BasicVector<T> vec(0);
  std::stringstream s;
  s << "foo [" << vec << "] bar";
  std::stringstream s_expected;
  s_expected << "foo [" << vec.get_value().transpose() << "] bar";
  EXPECT_EQ(s.str(), s_expected.str());
}

// Tests the default set of bounds (empty).
GTEST_TEST(BasicVectorTest, DefaultCalcInequalityConstraint) {
  VectorX<double> value = VectorX<double>::Ones(22);
  BasicVector<double> vec(1);
  Eigen::VectorXd lower, upper;
  // Deliberately set lower/upper to size 2, to check if GetElementBounds will
  // resize the bounds to empty size.
  lower.resize(2);
  upper.resize(2);
  vec.GetElementBounds(&lower, &upper);
  EXPECT_EQ(lower.size(), 0);
  EXPECT_EQ(upper.size(), 0);
}

// Tests the protected `::values()` methods.
GTEST_TEST(BasicVectorTest, ValuesAccess) {
  MyVector2d dut;
  dut[0] = 11.0;
  dut[1] = 22.0;

  // Values are as expected.
  ASSERT_EQ(dut.values().size(), 2);
  EXPECT_EQ(dut.values()[0], 11.0);
  EXPECT_EQ(dut.values()[1], 22.0);
  dut.values()[0] = 33.0;

  // The const overload is the same.
  const auto& const_dut = dut;
  EXPECT_EQ(&dut.values(), &const_dut.values());
  EXPECT_EQ(const_dut.values()[0], 33.0);
}

// Tests for reasonable exception message text; because the formatting is
// reused from VectorBase, this effectively tests the formatting for all
// subclasses of VectorBase.
GTEST_TEST(BasicVectorTest, ExceptionMessages) {
  BasicVector<double> pair{1.0, 2.0};
  DRAKE_EXPECT_THROWS_MESSAGE(
      pair.GetAtIndex(-1), std::exception,
      "Index -1 is not within \\[0, 2\\) while accessing .*BasicVector.*");
  DRAKE_EXPECT_THROWS_MESSAGE(
      pair.GetAtIndex(10), std::exception,
      "Index 10 is not within \\[0, 2\\) while accessing .*BasicVector.*");
  DRAKE_EXPECT_THROWS_MESSAGE(
      pair.SetFromVector(Eigen::Vector3d::Zero()), std::exception,
      "Operand vector size 3 does not match this .*BasicVector.* size 2");
}

}  // namespace
}  // namespace systems
}  // namespace drake


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/overloads.hpp>

#include <mmtbx/scaling/outlier.h>

namespace mmtbx { namespace scaling { namespace outlier {

namespace{

  struct likelihood_ratio_outlier_test_wrapper
  {
    typedef likelihood_ratio_outlier_test<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("likelihood_ratio_outlier_test", no_init)
        .def(init<
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< bool >  const&,
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< double >  const&>
             ((arg("f_obs"),
               arg("sigma_obs"),
               arg("f_calc"),
               arg("epsilon"),
               arg("centric"),
               arg("alpha"),
               arg("beta")
               )))
        .def("log_likelihood", &w_t::log_likelihood)
        .def("posterior_mode_log_likelihood", &w_t::posterior_mode_log_likelihood)
        .def("posterior_mode", &w_t::posterior_mode)
        .def("posterior_mode_snd_der", &w_t::posterior_mode_snd_der)
        .def("flag_potential_outliers", &w_t::flag_potential_outliers)
        .def("f_obs_fst_der", &w_t::f_obs_fst_der)
        .def("f_obs_snd_der", &w_t::f_obs_snd_der)
        .def("mean_fobs", &w_t::mean_fobs)
        .def("std_fobs", &w_t::std_fobs)
        .def("standardized_likelihood", &w_t::standardized_likelihood)
        ;
    }

  };

  struct sigmaa_estimator_wrapper
  {
    typedef sigmaa_estimator<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("sigmaa_estimator", no_init)
        .def(init<
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< double >  const&,
             scitbx::af::const_ref< bool >    const&,
             scitbx::af::const_ref< double >  const&,
             double                           const&>
             ((arg("e_obs"),
               arg("e_calc"),
               arg("centric"),
               arg("d_star_cubed"),
               arg("width")
               )))
        .def("sum_weights", &w_t::sum_weights,
             ( arg("d_star_cubed")
               )
             )
        .def("target", &w_t::target,
             (( arg("d_star_cubed"),
                arg("sigmaa")
                ))
             )
        .def("dtarget", &w_t::dtarget,
             (( arg("d_star_cubed"),
                arg("sigmaa")
                ))
             )
        .def("target_and_gradient", &w_t::target_and_gradient,
             (( arg("d_star_cubed"),
                arg("sigmaa")
                ))
             )
        ;
    }

  };

}} // namespace <anonymous>

namespace boost_python{

  void wrap_outlier()
  {
    mmtbx::scaling::outlier::likelihood_ratio_outlier_test_wrapper::wrap();
    mmtbx::scaling::outlier::sigmaa_estimator_wrapper::wrap();
  }

}

}}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE ssp_test

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cstdio>
#include <cstring>
#include <vector>

#include <nil/crypto3/zk/snark/reductions/uscs_to_ssp.hpp>
#include <nil/crypto3/zk/snark/arithmetization/constraint_satisfaction_problems/uscs.hpp>

#include <nil/crypto3/algebra/random_element.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/fields/mnt4/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt4/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>
#include <nil/crypto3/algebra/fields/mnt6/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt6/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt6.hpp>

#include "uscs_examples.hpp"

using namespace nil::crypto3::zk::snark;
using namespace nil::crypto3::algebra;

template<typename FieldType>
void test_ssp(const std::size_t num_constraints, const std::size_t num_inputs, const bool binary_input) {
    uscs_example<FieldType> example;
    if (binary_input) {
        example = generate_uscs_example_with_binary_input<FieldType>(num_constraints, num_inputs);
    } else {
        example = generate_uscs_example_with_field_input<FieldType>(num_constraints, num_inputs);
    }

    BOOST_CHECK(example.constraint_system.is_satisfied(example.primary_input, example.auxiliary_input));

    const typename FieldType::value_type t = random_element<FieldType>(),
                                         d = random_element<FieldType>();

    ssp_instance<FieldType> ssp_inst_1 = reductions::uscs_to_ssp<FieldType>::instance_map(example.constraint_system);

    ssp_instance_evaluation<FieldType> ssp_inst_2 =
        reductions::uscs_to_ssp<FieldType>::instance_map_with_evaluation(example.constraint_system, t);

    ssp_witness<FieldType> ssp_wit =
        reductions::uscs_to_ssp<FieldType>::witness_map(example.constraint_system, example.primary_input, example.auxiliary_input, d);

    BOOST_CHECK(ssp_inst_1.is_satisfied(ssp_wit));
    BOOST_CHECK(ssp_inst_2.is_satisfied(ssp_wit));
}

BOOST_AUTO_TEST_SUITE(ssp_test_suite)

BOOST_AUTO_TEST_CASE(ssp_test) {
    const std::size_t num_inputs = 10;

    using basic_curve_type = curves::mnt6<298>;

    const std::size_t basic_domain_size = 1ul << fields::arithmetic_params<basic_curve_type::scalar_field_type>::s;
    const std::size_t step_domain_size = (1ul << 10) + (1ul << 8);
    const std::size_t extended_domain_size = 1ul << (fields::arithmetic_params<basic_curve_type::scalar_field_type>::s + 1);
    const std::size_t extended_domain_size_special = extended_domain_size - 1;

    test_ssp<typename basic_curve_type::scalar_field_type>(basic_domain_size, num_inputs, true);
    test_ssp<typename basic_curve_type::scalar_field_type>(step_domain_size, num_inputs, true);
    test_ssp<typename basic_curve_type::scalar_field_type>(extended_domain_size, num_inputs, true);
    test_ssp<typename basic_curve_type::scalar_field_type>(extended_domain_size_special, num_inputs, true);

    test_ssp<typename basic_curve_type::scalar_field_type>(basic_domain_size, num_inputs, false);
    test_ssp<typename basic_curve_type::scalar_field_type>(step_domain_size, num_inputs, false);
    test_ssp<typename basic_curve_type::scalar_field_type>(extended_domain_size, num_inputs, false);
    test_ssp<typename basic_curve_type::scalar_field_type>(extended_domain_size_special, num_inputs, false);
}

BOOST_AUTO_TEST_SUITE_END()


// CHAP - The Channel Annotation Package
//
// Copyright (c) 2016 - 2018 Gianni Klesse, Shanlin Rao, Mark S. P. Sansom, and
// Stephen J. Tucker
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <numeric>

#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/tools/roots.hpp>

#include "statistics/amise_optimal_bandwidth_estimator.hpp"
#include "statistics/summary_statistics.hpp"

/*!
 * Estimates the AMISE-optimal bandwidth for kernel density estimation on a
 * given sample. Requires there to be at least two distinct sample points.
 */
real
AmiseOptimalBandWidthEstimator::estimate(
        const std::vector<real> &sampleIn)
{
    // make copy of sample:
    // (not so memory efficient, but avoids rescaling the data back)
    auto sample = sampleIn;

    // sanity checks:
    if( sample.size() < 2 )
    {
        // one angstrom returned in this case as default:
        return 0.1;
    }

    // shift and scale data:
    auto ss = gdd_.getShiftAndScaleParams(sample, sample);
    gdd_.shiftAndScale(sample, ss.first, ss.second);

    // estimate standard deviation of the sample:
    SummaryStatistics sumStats;
    for(auto s : sample)
    {
        sumStats.update(s);
    }
    real sigma = sumStats.sd();

    // estimate functionals six and eight by assuming Gaussianity:
    real phi6 = functionalPhi6(sigma);
    real phi8 = functionalPhi8(sigma);

    // calculate prototype bandwidth optimal wrt asymptotic MSE:
    real g1 = std::pow(-6.0 / ( SQRT2PI_*phi6*sample.size() ), 1.0/7.0);
    real g2 = std::pow(30.0 / ( SQRT2PI_*phi8*sample.size() ), 1.0/9.0);

    // estimate functionals via KDE with bandwidth from optimal MSE:
    real phi4 = functionalPhiFast(sample, g1, 4);
    phi6 = functionalPhiFast(sample, g2, 6);

    // calculate constant prefactor in gamma expression:
    gammaFactor_ = gammaFactor(phi4, phi6);

    // parameters for boost root finder:
    // initial guess is Silverman's rule of thumb:
    real guess = 1.06*sigma/std::pow(sample.size(), 1.0/5.0);
    real factor = 2.0;
    boost::uintmax_t it = 20;
    boost::math::tools::eps_tolerance<real> tol(std::numeric_limits<real>::digits - 4);

    // objective function for root finding:
    std::function<real(real)> objectiveFunction = std::bind(
            &AmiseOptimalBandWidthEstimator::optimalBandwidthEquation,
            this,
            std::placeholders::_1,
            sample);

    // find root:
    std::pair<real, real> root = bracket_and_solve_root(
        objectiveFunction,
        guess,
        factor,
        true,
        tol,
        it);

    // return AMISE-optimal bandwidth (scaled back to original interval):
    return root.first / ss.second;
}

/*!
 * Calculates the eighth order density derivative functional:
 *
 * \f[
 *      \phi_6 = \frac{-15}{16 \sqrt{\pi} \sigma^7}
 * \f]
 *
 * This is based on an approximation assuming a Gaussian probability density.
 */
real
AmiseOptimalBandWidthEstimator::functionalPhi6(real sigma)
{
    return -15.0 / ( 16.0 * std::pow(sigma, 7) * SQRTPI_ );
}

/*!
 * Calculates the eighth order density derivative functional:
 *
 * \f[
 *      \phi_8 = \frac{105}{32 \sqrt{\pi} \sigma^9}
 * \f]
 *
 * This is based on an approximation assuming a Gaussian probability density.
 */
real
AmiseOptimalBandWidthEstimator::functionalPhi8(real sigma)
{
    return 105.0 / ( 32.0 * std::pow(sigma, 9) * SQRTPI_ );
}

/*!
 * Evaluation the functional
 *
 * \f[
 *      \Phi_r = \frac{1}{N}\sum_{i=1}^N p^{(r)}(s_i)
 * \f]
 *
 * i.e. the sum over the density derivative evaluated at each sample point. The
 * derivative itself is evaluated using the GaussianDensityDerivative class,
 * which uses an approximate method to evaluate this expression in linear
 * complexity with the number of sample points.
 */
real
AmiseOptimalBandWidthEstimator::functionalPhiFast(
        const std::vector<real> &sample,
        real bw,
        int deriv)
{
    // set density derivative estimation parameters:
    gdd_.setErrorBound(0.01);
    gdd_.setBandWidth(bw);
    gdd_.setDerivOrder(deriv);

    // obtain derivative estimate at each sample point:
    std::vector<real> d = gdd_.estimateApprox(sample, sample);

    // average of derivatives is estimate for phi:
    real phi = std::accumulate(d.begin(), d.end(), 0.0);
    phi /= sample.size();

    // return phi parameter:
    return phi;
}

/*!
 * Computes the constant prefactor in gamma(), which needs to be assigned to
 * gammaFactor_ prior to calling gamma().
 */
real
AmiseOptimalBandWidthEstimator::gammaFactor(
        const real phi4,
        const real phi6)
{
    return std::pow(-6.0*std::sqrt(2.0)*phi4 / phi6 , 1.0/7.0);
}

/*!
 * Returns the bandwidth, \f$ \gamma \f$, used to estimate the density
 * derivative functional entering the optimalBandwidthEquation():
 *
 * \f[
 *      \gamma = \left[ \frac{-6\sqrt{2}\Phi_4(g_1)}{\Phi_6(g_2)} \right]^{\frac{1}{7}} h^{\frac{5}{7}}
 * \f]
 *
 * Note that as the prefactor on square brackets is independent of \f$ h \f$,
 * it is computed only once using gammaFactor() and stored in a member variable
 * of AmiseOptimalBandWidthEstimator (this precompution must be carried out
 * manually!).
 */
real
AmiseOptimalBandWidthEstimator::gamma(real bw)
{
    return gammaFactor_ * std::pow(bw, 5.0/7.0);
}

/*!
 * Returns the value for the implicit expression for the AMISE-optimal
 * bandwidth:
 *
 * \f[
 *      h - \left[ \frac{1.0}{2\sqrt{\pi}\Phi_4\big(\gamma (h)\big) N} \right]^{\frac{1}{5}}
 * \f]
 *
 * This is solved iteratively to obtain \f$ h \f$, which can then used to
 * obtain a kernel density estimate via the KernelDensityEstimator class.
 */
real
AmiseOptimalBandWidthEstimator::optimalBandwidthEquation(
        const real bw,
        const std::vector<real> &samples)
{

    // estimate density derivative functional:
    real phi4 = functionalPhiFast(samples, gamma(bw), 4);

    // evaluate optimal bandwidth equation:
    return bw - std::pow(1.0/(2.0*SQRTPI_*phi4*samples.size()) , 1.0/5.0);
}


#ifndef PYLC_LIB_HPP
#define PYLC_LIB_HPP

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <stdexcept>
#include <opencv2/core/core.hpp>
#include <Eigen/Dense>
#include <eigen3/Eigen/src/Core/Matrix.h>
#include <pybind11/eigen.h>
#include <opencv2/opencv.hpp>
#include <opencv/cxeigen.hpp>
#include <converters.hpp>
#include <sim_class.hpp>
#include <chrono>
#include <spline_class.hpp>

namespace py = pybind11;

struct Sorted{
    std::string camera_name, laser_name;
    int input_index;
};

void processPointsJoint(std::shared_ptr<DatumProcessor>& datumProcessor, std::vector<std::shared_ptr<Input>>& inputs, py::dict& input_names, std::vector<py::dict>& soi, std::shared_ptr<Output>& output, bool get_cloud = false){

    // Resize
    output->clouds.resize(soi.size());
    output->images_multi.resize(soi.size());

    // Resort
    std::vector<Sorted> sortedVec;
    sortedVec.resize(soi.size());
    for(int i=0; i<soi.size(); i++){
        std::string camera_name = std::string(py::str(soi[i]["C"]));
        std::string laser_name = std::string(py::str(soi[i]["L"]));
        int input_index = py::int_(input_names[camera_name.c_str()]);
        sortedVec[input_index].camera_name = camera_name;
        sortedVec[input_index].laser_name = laser_name;
        sortedVec[input_index].input_index = input_index;
    }

    // Compute
    //#pragma omp parallel for shared(inputs, output, sortedVec)
    for(int i=0; i<sortedVec.size(); i++){
        Sorted& sorted = sortedVec[i];
        std::shared_ptr<Input>& input = inputs[i];

        // Get Depth Map
        cv::Mat depth_img = input->depth_image;

        // Resize local
        output->images_multi[i].resize(input->design_pts_multi.size());

        // Process
        std::vector<PointXYZI> combined_cloud;
        // pcl::PointCloud<pcl::PointXYZRGB> combined_cloud;
        for(int j=0; j<input->design_pts_multi.size(); j++){
            const Eigen::MatrixXf& m = input->design_pts_multi[j];

            cv::Mat& image = output->images_multi[i][j];
            std::vector<PointXYZI> cloud;
            // pcl::PointCloud<pcl::PointXYZRGB> cloud;
            datumProcessor->processPointsT(m, depth_img, sorted.camera_name, sorted.laser_name, image, cloud, get_cloud);
            // combined_cloud += cloud;
            combined_cloud.insert(combined_cloud.end(), cloud.begin(), cloud.end());
        }

        // Convert pcl::PointCloud to Eigen::MatrixXf.
        output->clouds[i] = Eigen::Matrix<float, Eigen::Dynamic, 4> (combined_cloud.size(), 4);
        for (int pt_index = 0; pt_index < combined_cloud.size(); pt_index++) {
            PointXYZI point = combined_cloud[pt_index];
            output->clouds[i](pt_index, 0) = point[0];
            output->clouds[i](pt_index, 1) = point[1];
            output->clouds[i](pt_index, 2) = point[2];
            output->clouds[i](pt_index, 3) = point[3];
        }
        // output->clouds[i] = combined_cloud.getMatrixXfMap(6, 8, 0);

        // Write Cloud
        // pcl::toROSMsg(combined_cloud, output->clouds[i]);
    }
}

PYBIND11_MODULE(pylc_lib, m) {

    // Datum Object
    py::class_<Datum, std::shared_ptr<Datum>>(m, "Datum")
    .def(py::init<>())
            .def_readwrite("type", &Datum::type)
            .def_readwrite("camera_name", &Datum::camera_name)
            .def_readwrite("rgb_matrix", &Datum::rgb_matrix)
            .def_readwrite("depth_matrix", &Datum::depth_matrix)
            .def_readwrite("world_to_rgb", &Datum::world_to_rgb)
            .def_readwrite("world_to_depth", &Datum::world_to_depth)
            .def_readwrite("cam_to_laser", &Datum::cam_to_laser)
            .def_readwrite("cam_to_world", &Datum::cam_to_world)
            .def_readwrite("fov", &Datum::fov)
            .def_readwrite("laser_name", &Datum::laser_name)
            .def_readwrite("distortion", &Datum::distortion)
            .def_readwrite("imgh", &Datum::imgh)
            .def_readwrite("imgw", &Datum::imgw)
            .def_readwrite("limit", &Datum::limit)
            .def_readwrite("galvo_m", &Datum::galvo_m)
            .def_readwrite("galvo_b", &Datum::galvo_b)
            .def_readwrite("maxADC", &Datum::maxADC)
            .def_readwrite("thickness", &Datum::thickness)
            .def_readwrite("divergence", &Datum::divergence)
            .def_readwrite("laser_limit", &Datum::laser_limit)
            .def_readwrite("laser_timestep", &Datum::laser_timestep)
    ;

    // Input Object
    py::class_<Input, std::shared_ptr<Input>>(m, "Input")
            .def(py::init<>())
            .def_readwrite("camera_name", &Input::camera_name)
            .def_readwrite("rgb_image", &Input::rgb_image)
            .def_readwrite("depth_image", &Input::depth_image)
            .def_readwrite("design_pts", &Input::design_pts)
            .def_readwrite("design_pts_multi", &Input::design_pts_multi)
            .def_readwrite("surface_pts", &Input::surface_pts)
            .def_readwrite("design_pts_conv", &Input::design_pts_conv)
            ;

    // Output Object
    py::class_<Output, std::shared_ptr<Output>>(m, "Output")
            .def(py::init<>())
            .def_readwrite("clouds", &Output::clouds)
            .def_readwrite("images_multi", &Output::images_multi)
            .def_readwrite("output_pts", &Output::output_pts)
            .def_readwrite("laser_rays", &Output::laser_rays)
            .def_readwrite("angles", &Output::angles)
            .def_readwrite("velocities", &Output::velocities)
            .def_readwrite("accels", &Output::accels)
            .def_readwrite("spline", &Output::spline)
            .def_readwrite("output_pts_set", &Output::output_pts_set)
            .def_readwrite("spline_set", &Output::spline_set)
            ;

    // DatumProcessor
    py::class_<DatumProcessor, std::shared_ptr<DatumProcessor>>(m, "DatumProcessor")
            .def(py::init<>())
            .def("setSensors", &DatumProcessor::setSensors)
            .def("processTest", &DatumProcessor::processTest)
            .def("evalPaths", &DatumProcessor::evalPaths)
            .def("evalPath", &DatumProcessor::evalPath)
            ;

    m.def("makeRandom", &makeRandom, "makeRandom");
    m.def("fitBSpline", &fitBSpline, "fitBSpline");
    m.def("convolve", &convolve, "convolve");
    m.def("processPointsJoint", &processPointsJoint, "processPointsJoint");

    #ifdef VERSION_INFO
        m.attr("__version__") = VERSION_INFO;
    #else
        m.attr("__version__") = "dev";
    #endif
}

#endif

//std::vector<int> strides(bufferdim.size());
//if (!strides.empty())
//{
//    strides.back() = sizeof(float);
//    for (auto i = (int)strides.size()-2 ; i > -1 ; i--)
//        strides[i] = strides[i+1] * bufferdim[i+1];
//}


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "bleach-transition.h"

// Local project headers:
#include "common/error-model.h"
#include "hmm/fit/error-model-fitter.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(bleach_transition_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    double q = 0.2;
    int channel = -1;  // can be ignored for this test.
    BleachTransition bt(q, channel);
    BOOST_TEST(bt.q == q);
    BOOST_TEST(bt.channel == channel);
}

BOOST_AUTO_TEST_CASE(improve_fit_basic_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    BleachTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    delete[] loc;
    int edmans = 0;
    double probability = 1.0;
    ErrorModelFitter emf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &emf);
    BOOST_TEST(emf.p_bleach_fit.get() == (0.71 * q * 0.33) / (0.71 * 0.72));
}

BOOST_AUTO_TEST_SUITE_END()  // bleach_transition_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <iostream>

using namespace boost::gil;

// Demonstrates Adaptive Thresholding

// threshold_adaptive works following either the mean or the gaussian method, and accepts also a direction
// The direction indicates whether the pixels are assigned the max value when their values are greater
// than the threshold (regular), or when they are less than the threshold (inverse)
// threshold_adaptive is defined in include/boost/gil/image_processing/threshold.hpp
// See also:
// threshold.cpp - Simple thresholding

int main()
{
    gray8_image_t img;
    read_image("test_adaptive.png", img, png_tag{});
    gray8_image_t img_out(img.dimensions());

    boost::gil::threshold_adaptive(const_view(img), view(img_out), 11, threshold_adaptive_method::mean, threshold_direction::regular, 2);
    write_view("out-threshold-adaptive-mean.png", view(img_out), png_tag{});

    boost::gil::threshold_adaptive(const_view(img), view(img_out), 11, threshold_adaptive_method::mean, threshold_direction::inverse, 2);
    write_view("out-threshold-adaptive-mean-inv.png", view(img_out), png_tag{});

    boost::gil::threshold_adaptive(const_view(img), view(img_out), 7, threshold_adaptive_method::gaussian, threshold_direction::regular, 2);
    write_view("out-threshold-adaptive-gaussian.png", view(img_out), png_tag{});

    boost::gil::threshold_adaptive(const_view(img), view(img_out), 11, threshold_adaptive_method::gaussian, threshold_direction::inverse, 2);
    write_view("out-threshold-adaptive-gaussian-inv.png", view(img_out), png_tag{});

    return 0;
}


#include <boost/test/unit_test.hpp>
#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE( Net_Height_Gain_N0683858 )

const bool isFileName = true;

/* Test case checks that the net height gain is calcualted correctly when all the vales are negative,
 * to see if the subtraction is done properly*/
BOOST_AUTO_TEST_CASE( AllNegKLM )
{
   Route route = Route(LogFiles::GPXRoutesDir + "All_Neg_KLM_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(), 100  );
}

/*Test case checks that the net height gain is caculated correctly when all the values are positive,
 * makes sure the subtraction is done correctly*/
BOOST_AUTO_TEST_CASE( AllPosMNO )
{
   Route route = Route(LogFiles::GPXRoutesDir + "All_Pos_MNO_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(), 0 );
}
/*Test case checks the subtraction is done properly when the elevation is negative to poisitive*/

BOOST_AUTO_TEST_CASE( NegToPosKLM )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Neg_to_Pos_KLM_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(), 240 );
}
/*Test case checks if the net height gain is calculated correctly when the values are increasing (positive),
 * checks to see if the subtraction is done peroperly with the evlevation increasing*/
BOOST_AUTO_TEST_CASE( PosIncreasingWRM )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Pos_Increasing_WRM_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(), 268 );
}

/*Test case checks the subtraction is done properly when the elevation is poisitive to negative*/
BOOST_AUTO_TEST_CASE( PosToNegMGA )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Pos_to_Neg_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(), 0 );
}
/*Test case checks if the net height gain is calculated correctly when the elevation values are the same,
 * checks to see if the subtraction is done peroperly with the elevation same*/
BOOST_AUTO_TEST_CASE( SameElevationABC )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Same_Elevation_ABC_N0683858.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.netHeightGain(),  0);
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

//          Copyright Rein Halbersma 2010-2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <dctl/core.hpp>
#include <dctl/egdb/index.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <algorithm>
#include <array>
#include <cassert>
#include <cstddef>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <numeric>

namespace dctl::egdb {

template<class Position>
class dependencies
{
        using subdivision_type = subdivision<Position>;
        std::vector<subdivision_type> m_subdivisions;
        using Graph = boost::adjacency_list<>;
        Graph m_graph;
        using edge_type = std::pair<int, int>;
        using vertex_map = boost::property_map<Graph, boost::vertex_index_t>::type ;
        vertex_map m_vertex_map;
public:
        explicit dependencies(int n)
        {
                for (auto in = 2; in <= n; ++in) {
                        subdivide(in);
                }
                create_graph();
        }

        dependencies(int b, int w)
        {
                auto const n = b + w;
                for (auto in = 2; in <= n; ++in) {
                        for (auto ib = std::max(1, in - w); ib <= std::min(b, in - 1); ++ib) {
                                auto const iw = in - ib;
                                assert(std::max(1, in - b) <= iw); assert(iw <= std::min(w, in - 1));
                                subdivide(ib, iw);
                        }
                }
                create_graph();
        }

        dependencies(int b, int w, int bp, int wp)
        {
                assert(0 <= bp); assert(bp <= b);
                assert(0 <= wp); assert(wp <= w);
                auto const n = b + w;
                for (auto in = 2; in <= n; ++in) {
                        for (auto ib = std::max(1, in - w); ib <= std::min(b, in - 1); ++ib) {
                                auto const iw = in - ib;
                                assert(std::max(1, in - b) <= iw); assert(iw <= std::min(w, in - 1));
                                for (auto ibp = 0; ibp <= std::min(bp, ib); ++ibp) {
                                        for (auto iwp = 0; iwp <= std::min(wp, iw); ++iwp) {
                                                subdivide(ib, iw, ibp, iwp);
                                        }
                                }
                        }
                }
                create_graph();
        }

        auto print_vertices() const
        {
                std::cout << "---------\n";

                std::cout << "vertices(g) = " << boost::num_vertices(m_graph) << " = ";
                for (auto vp = vertices(m_graph); vp.first != vp.second; ++vp.first) {
                        std::cout << m_vertex_map[*vp.first] <<  " ";
                }
                std::cout << "\n";
        }

        auto print_edges() const
        {
                std::cout << "---------\n";

                std::cout << "edges(g) = " << boost::num_edges(m_graph) << " = ";
                boost::graph_traits<Graph>::edge_iterator ei, ei_end;
                for (boost::tie(ei, ei_end) = boost::edges(m_graph); ei != ei_end; ++ei) {
                        std::cout << "(" << m_vertex_map[boost::source(*ei, m_graph)]
                                  << "," << m_vertex_map[boost::target(*ei, m_graph)] << ") ";
                }
                std::cout << "\n";
        }

        auto sort()
        {
                std::vector<unsigned> ordered;
                boost::topological_sort(m_graph, std::back_inserter(ordered));
                std::cout << "Topologically sorted = ";
                std::cout << std::boolalpha << std::is_sorted(ordered.begin(), ordered.end()) << "\n";
        }
private:
        void subdivide(int n)
        {
                assert(2 <= n);
                for (auto b = 1; b < n; ++b) {
                        auto const w = n - b;
                        assert(1 <= w); assert(w < n);
                        subdivide(b, w);
                }
        }

        void subdivide(int b, int w)
        {
                assert(1 <= b); assert(1 <= w);
                for (auto bp = 0; bp <= b; ++bp) {
                        for (auto wp = 0; wp <= w; ++wp) {
                                subdivide(b, w, bp, wp);
                        }
                }
        }

        void subdivide(int b, int w, int bp, int wp)
        {
                assert(0 <= bp); assert(bp <= b);
                assert(0 <= wp); assert(wp <= w);
                m_subdivisions.emplace_back(b, w, bp, wp);
                auto const& s = m_subdivisions.back();
                std::cout << "Adding: " << s.to_string() << ", ";
                std::cout << "size = " << static_cast<double>(s.size()) / static_cast<double>(1ULL<<32) << " GiB, ";
                std::cout << "legal = " << s.legal() << "\n";
        }

        void create_graph()
        {
                auto const sum = std::accumulate(m_subdivisions.begin(), m_subdivisions.end(), int64_t{0}, [](auto res, auto const& s) {
                        return res + s.size();
                });

                auto const leg = std::accumulate(m_subdivisions.begin(), m_subdivisions.end(), int64_t{0}, [](auto res, auto const& s) {
                        return res + s.legal();
                });

                auto const max = (*std::max_element(m_subdivisions.begin(), m_subdivisions.end(), [](auto const& lhs, auto const& rhs) {
                        return lhs.size() < rhs.size();
                })).size();

                std::sort(m_subdivisions.begin(), m_subdivisions.end(), [](auto const& lhs, auto const& rhs) {
                        return lhs.size() < rhs.size();
                });

                for (auto const& s : m_subdivisions) {
                        std::cout << s.to_string() << ", size = ";
                        std::cout << s.size() << ", ";
                        std::cout << "legal = " << s.legal() << ", ";
                        std::cout << "overhead = " << static_cast<double>(s.size()) / static_cast<double>(s.legal()) << "\n";
                }

                std::cout << "Total files = " << m_subdivisions.size() << "\n";
                std::cout << "Total size = " << sum << "\n";
                std::cout << "Total positions = " << leg << "\n";
                std::cout << "Max size = " << static_cast<double>(max) / static_cast<double>(1ULL<<32) << " GiB\n";
/*
                for (auto i = 0u; i < m_subdivisions.size(); ++i) {
                        for (auto j = i + 1; j < m_subdivisions.size(); ++j) {
                                if (depends_on{}(m_subdivisions[j], m_subdivisions[i])) {
                                        boost::add_edge(j, i, m_graph);
                                }
                                assert(!depends_on{}(m_subdivisions[i], m_subdivisions[j]));
                        }
                }
                m_vertex_map = boost::get(boost::vertex_index, m_graph);*/
        }

};

}       // namespace dctl::egdb


#pragma once

#include <polymesh/pm.hh>
#include <typed-geometry/tg.hh>

#include <Eigen/Dense>

namespace LayoutEmbedding {

struct IGLMesh
{
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
};

IGLMesh to_igl_mesh(const pm::vertex_attribute<tg::pos3>& _pos);

void to_polymesh(const IGLMesh& _igl, pm::Mesh& _m, pm::vertex_attribute<tg::dpos3>& _pos);
void to_polymesh_param(const Eigen::MatrixXd& _W, const pm::Mesh& _m, pm::vertex_attribute<tg::dpos2>& _pos);

}


/*
 * @Description:  5刚性物体的鲁棒姿态估计  http://robot.czxy.com/docs/pcl/chapter03/registration/#_9
 * https://blog.csdn.net/zfjBIT/article/details/94566603
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-22 19:29:32
 * @LastEditTime: 2020-10-23 10:36:17
 * @FilePath: /pcl-learning/14registration配准/5刚性物体的鲁棒姿态估计/alignment_prerejective.cpp
 */

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

// 首先从定义类型开始，以免使代码混乱
// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // 然后，我们实例化必要的数据容器，检查输入参数并加载对象和场景点云。
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  FeatureCloudT::Ptr scene_features (new FeatureCloudT);

  // Get input object and scene
  if (argc != 3)
  {
    pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
    return (1);
  }

  // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");
  if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||  pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) // 判断加载点云文件失败
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }

  // 1 Downsample 下采样：使用0.005提速分辨率对目标物体和场景点云进行空间下采样
  // 为了加快处理速度，我们使用PCL的：pcl::VoxelGrid类将对象和场景点云的采样率下采样至5 mm。
  pcl::console::print_highlight ("Downsampling...\n");
  pcl::VoxelGrid<PointNT> grid;
  const float leaf = 0.005f;
  grid.setLeafSize (leaf, leaf, leaf);
  grid.setInputCloud (object);  // 对象下采样
  grid.filter (*object);
  grid.setInputCloud (scene); // 场景下采样
  grid.filter (*scene);

  // 2 估计场景法线 Estimate normals for scene
  pcl::console::print_highlight ("Estimating scene normals...\n");
  pcl::NormalEstimationOMP<PointNT,PointNT> nest;
  nest.setRadiusSearch (0.01);
  nest.setInputCloud (scene);
  nest.compute (*scene);

  // 3 特征估计 Estimate features
  // 对于下采样点云中的每个点，我们现在使用PCL的pcl::FPFHEstimationOMP<>类来计算用于对齐过程中用于匹配的快速点特征直方图（FPFH）描述符。
  pcl::console::print_highlight ("Estimating features...\n");
  FeatureEstimationT fest;
  fest.setRadiusSearch (0.025);
  fest.setInputCloud (object);
  fest.setInputNormals (object);
  fest.compute (*object_features);
  fest.setInputCloud (scene);
  fest.setInputNormals (scene);
  fest.compute (*scene_features);

  // 4 对齐配准对象创建与配置  实施配准Perform alignment
  // SampleConsensusPrerejective 实现了有效的RANSAC姿势估计循环
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align; // //基于采样一致性的位姿估计
  align.setInputSource (object); // 对象
  align.setSourceFeatures (object_features); // 对象特征
  align.setInputTarget (scene);  // 场景
  align.setTargetFeatures (scene_features); //  场景特征
  align.setMaximumIterations (50000); // 采样一致性迭代次数 Number of RANSAC iterations
  align.setNumberOfSamples (3); // 在对象和场景之间进行采样的点对应数。 至少需要3个点才能计算姿势。Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (5); //  使用的临近特征点的数目 Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值 Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.5f * leaf); // 判断是否为内点的距离阈值 Inlier threshold
  align.setInlierFraction (0.25f); //  接受位姿假设所需的内点比例 Required inlier fraction for accepting a pose hypothesis
  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned); // 对齐的对象存储在点云object_aligned中。
  }

  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); // Inliers: 1384/3432

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment-刚性物体的鲁棒姿态估计");
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
    return (1);
  }

  return (0);
}


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main(int argc, char** argv)
{
  // Loading first scan of room.加载房间的第一次扫描点云数据作为目标
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

  // Loading second scan of room from new perspective.加载从新视角得到的房间的第二次扫描点云数据作为源点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

  //以上的代码加载了两个PCD文件得到共享指针，后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵
   // 将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度，只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理
   //因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  //将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from room_scan2.pcd" << std::endl;

  // Initializing Normal Distributions Transform (NDT).
  //初始化正态分布变换（NDT）对象
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // Setting scale dependent NDT parameters根据输入数据的尺度设置NDT相关参数
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.05);//为终止条件设置最小转换差异
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.05);//为More-Thuente线搜索设置最大步长
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);//设置NDT网格结构的分辨率（VoxelGridCovariance）
  //以上参数在使用房间尺寸比例下运算比较好，但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体，需要对参数进行很大程度的缩小

  // Setting max number of registration iterations.设置匹配迭代的最大次数
  //设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
   //添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  ndt.setMaximumIterations (35);

  // Setting point cloud to be aligned.设置源点云
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.设置目标点云
  ndt.setInputTarget (target_cloud);

  // Set initial alignment estimate found using robot odometry.
  //设置使用机器人测距法得到的粗略初始变换矩阵结果
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
//  Eigen::AngleAxisf init_rotation (Eigen::Matrix3f::Identity());
//  Eigen::Translation3f init_translation (0, 0, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  std::cout << "init_guess: " << "\n"
            << init_guess << std::endl;

  // Calculating required rigid transform to align the input cloud to the target cloud.
  //计算需要的刚体变换以便将输入的源点云匹配到目标点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess);
  //此处output_cloud不能作为最终的源点云变换，因为上面对源点云进行了滤波处理
  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // Transforming unfiltered, input cloud using found transform.
  //使用创建的变换对未过滤的输入点云进行变换
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
  std::cout << "transformation is: "  << "\n"
            << ndt.getFinalTransformation() << std::endl;

  // Saving transformed input cloud.
  //保存转换后的源点云作为最终的变换输出
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // Initializing point cloud visualizer
  // 初始化点云可视化对象
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);

  // Coloring and visualizing target cloud (red).
  //对目标点云着色（红色）并可视化
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color (target_cloud, 255, 0, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "target cloud");

  // Coloring and visualizing transformed input cloud (green).
  //对转换后的源点云着色（绿色）并可视化
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color (output_cloud, 0, 255, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "output cloud");

  // Starting visualizer启动可视化
  viewer_final->addCoordinateSystem (1.0, "global");//显示xyz指示轴
  viewer_final->initCameraParameters();         //初始化摄像头参数等

  // Wait until visualizer window is closed.
  //等待直到可视化窗口关闭。
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {
    //! [heterogeneous]
    BOOST_HANA_CONSTEXPR_LAMBDA auto safe_make_unsigned = [](auto t) {
        return eval_if(trait<std::is_integral>(t),
            [=](auto id) { return id(template_<std::make_unsigned_t>)(t); },
            always(t)
        );
    };

    BOOST_HANA_CONSTANT_ASSERT(safe_make_unsigned(type<void>) == type<void>);
    BOOST_HANA_CONSTANT_ASSERT(safe_make_unsigned(type<int>) == type<unsigned int>);
    //! [heterogeneous]
    (void)safe_make_unsigned;

    //! [homogeneous]
    BOOST_HANA_CONSTEXPR_LAMBDA auto safe_divide = [](auto x, auto y) {
        return eval_if(y == 0,
            [=](auto) { return 0; },
            [=](auto id) { return id(x) / y; }
        );
    };

    BOOST_HANA_CONSTEXPR_ASSERT(safe_divide(6, 3) == 2);
    BOOST_HANA_CONSTEXPR_ASSERT(safe_divide(6, 0) == 0);
    //! [homogeneous]
}


/// @file
///
/// Day 14: Disk Defragmentation
///

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <fmt/format.h>

#include "advent/days/10/knot_hash.hh"
#include "advent/days/14/disk_defragmentation.hh"
#include "advent/utils/misc.hh"

namespace advent {

namespace {

inline int char_to_value(char c) {
	return (c > '9') ? (c &~ 0x20) - 'A' + 10: (c - '0');
}

}

// Override.
void Day14::solve_part_one() {
	std::string input = "ugkiagan";
	std::cout << part_one()
			<< solve_part_one_for_input(input)
		<< std::endl;
}

// Override.
void Day14::solve_part_two() {
	std::string input = "ugkiagan";
	std::cout << part_two()
			<< solve_part_two_for_input(input)
		<< std::endl;
}

// Override.
std::string Day14::part_one() const {
	return __COMPACT_PRETTY_FUNCTION__;
}

// Override.
std::string Day14::part_two() const {
	return __COMPACT_PRETTY_FUNCTION__;
}

///
/// Solves first part of the puzzle.
///
int Day14::solve_part_one_for_input(const std::string& input) {
	int bits = 0;
	for (int i = 0; i < 128; i++) {
		auto hash = Day10::compute_hash_for_input(fmt::format("{}-{}", input, i));
		for (const auto& ch : hash) {
			bits += std::bitset<8>(char_to_value(ch)).count();
		}
	}
	return bits;
}

///
/// Solves second part of the puzzle using graps.
/// Using: http://www.boost.org/doc/libs/1_65_1/libs/graph/example/connected_components.cpp
///
int Day14::solve_part_two_for_input(const std::string& input) {
	using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;

	Graph graph;
	int id = 0;
	std::vector<std::vector<int>> bit_array(128);
	for (auto& ba : bit_array) {
		ba.resize(128, -1);
	}

	for (int i = 0; i < 128; i++) {
		auto hash = Day10::compute_hash_for_input(fmt::format("{}-{}", input, i));
		for (std::size_t j = 0; j < hash.length(); j++) {
			std::bitset<4> bits(char_to_value(hash[j]));
			for (int b = 0; b < 4; b++) {
				if (bits[3 - b]) {
					bit_array[i][j*4 + b] = id;
					if (i != 0 && bit_array[i - 1][j*4 + b] != -1) {
						add_edge(id, bit_array[i - 1][j*4 + b], graph);
					}
					if ((j != 0 || b != 0) && bit_array[i][j*4 + b - 1] != -1) {
						add_edge(id, bit_array[i][j*4 + b - 1], graph);
					}
					id++;
				}
			}
		}
	}

	std::vector<int> components(boost::num_vertices(graph));
	return connected_components(graph, &components[0]);
}

}


#ifndef RCHORD_MEANSHIFT_HPP
#define RCHORD_MEANSHIFT_HPP

#include <vector>
#include <Eigen/Dense>

unsigned int  MultiGuessRChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
					const std::vector<float> &votesWeight,
					const std::vector<Eigen::Vector3f> guesses,
					std::vector<Eigen::Vector3f> &modes,
					std::vector<int> &guessModeID,
					float b, float stepSize, float stepThr,
					unsigned int nIterThr, float sameModeThr);

void RChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
		     const std::vector<float> &votesWeight,
		     const Eigen::Vector3f &guess, Eigen::Vector3f &mode,
		     float b, float stepSize, float stepThr, unsigned int nIterThr);

#endif // RMEANSHIFT_HPP