//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#pragma once

#include "InferenceTest.hpp"
#include "YoloDatabase.hpp"

#include <algorithm>
#include <array>
#include <utility>

#include <boost/assert.hpp>
#include <boost/multi_array.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

constexpr size_t YoloOutputSize = 1470;

template <typename Model>
class YoloTestCase : public InferenceModelTestCase<Model>
{
public:
    YoloTestCase(Model& model,
        unsigned int testCaseId,
        YoloTestCaseData& testCaseData)
     : InferenceModelTestCase<Model>(model, testCaseId, { std::move(testCaseData.m_InputImage) }, { YoloOutputSize })
     , m_FloatComparer(boost::math::fpc::percent_tolerance(1.0f))
     , m_TopObjectDetections(std::move(testCaseData.m_TopObjectDetections))
    {
    }

    virtual TestCaseResult ProcessResult(const InferenceTestOptions& options) override
    {
        using Boost3dArray = boost::multi_array<float, 3>;

        const std::vector<float>& output = boost::get<std::vector<float>>(this->GetOutputs()[0]);
        BOOST_ASSERT(output.size() == YoloOutputSize);

        constexpr Boost3dArray::index gridSize = 7;
        constexpr Boost3dArray::index numClasses = 20;
        constexpr Boost3dArray::index numScales = 2;

        const float* outputPtr =  output.data();

        // Range 0-980. Class probabilities. 7x7x20
        Boost3dArray classProbabilities(boost::extents[gridSize][gridSize][numClasses]);
        for (Boost3dArray::index y = 0; y < gridSize; ++y)
        {
            for (Boost3dArray::index x = 0; x < gridSize; ++x)
            {
                for (Boost3dArray::index c = 0; c < numClasses; ++c)
                {
                    classProbabilities[y][x][c] = *outputPtr++;
                }
            }
        }

        // Range 980-1078. Scales. 7x7x2
        Boost3dArray scales(boost::extents[gridSize][gridSize][numScales]);
        for (Boost3dArray::index y = 0; y < gridSize; ++y)
        {
            for (Boost3dArray::index x = 0; x < gridSize; ++x)
            {
                for (Boost3dArray::index s = 0; s < numScales; ++s)
                {
                    scales[y][x][s] = *outputPtr++;
                }
            }
        }

        // Range 1078-1469. Bounding boxes. 7x7x2x4
        constexpr float imageWidthAsFloat = static_cast<float>(YoloImageWidth);
        constexpr float imageHeightAsFloat = static_cast<float>(YoloImageHeight);

        boost::multi_array<float, 4> boxes(boost::extents[gridSize][gridSize][numScales][4]);
        for (Boost3dArray::index y = 0; y < gridSize; ++y)
        {
            for (Boost3dArray::index x = 0; x < gridSize; ++x)
            {
                for (Boost3dArray::index s = 0; s < numScales; ++s)
                {
                    float bx = *outputPtr++;
                    float by = *outputPtr++;
                    float bw = *outputPtr++;
                    float bh = *outputPtr++;

                    boxes[y][x][s][0] = ((bx + static_cast<float>(x)) / 7.0f) * imageWidthAsFloat;
                    boxes[y][x][s][1] = ((by + static_cast<float>(y)) / 7.0f) * imageHeightAsFloat;
                    boxes[y][x][s][2] = bw * bw * static_cast<float>(imageWidthAsFloat);
                    boxes[y][x][s][3] = bh * bh * static_cast<float>(imageHeightAsFloat);
                }
            }
        }
        BOOST_ASSERT(output.data() + YoloOutputSize == outputPtr);

        std::vector<YoloDetectedObject> detectedObjects;
        detectedObjects.reserve(gridSize * gridSize * numScales * numClasses);

        for (Boost3dArray::index y = 0; y < gridSize; ++y)
        {
            for (Boost3dArray::index x = 0; x < gridSize; ++x)
            {
                for (Boost3dArray::index s = 0; s < numScales; ++s)
                {
                    for (Boost3dArray::index c = 0; c < numClasses; ++c)
                    {
                        // Resolved confidence: class probabilities * scales.
                        const float confidence = classProbabilities[y][x][c] * scales[y][x][s];

                        // Resolves bounding box and stores.
                        YoloBoundingBox box;
                        box.m_X = boxes[y][x][s][0];
                        box.m_Y = boxes[y][x][s][1];
                        box.m_W = boxes[y][x][s][2];
                        box.m_H = boxes[y][x][s][3];

                        detectedObjects.emplace_back(c, box, confidence);
                    }
                }
            }
        }

        // Sorts detected objects by confidence.
        std::sort(detectedObjects.begin(), detectedObjects.end(),
            [](const YoloDetectedObject& a, const YoloDetectedObject& b)
            {
                // Sorts by largest confidence first, then by class.
                return a.m_Confidence > b.m_Confidence
                    || (a.m_Confidence == b.m_Confidence && a.m_Class > b.m_Class);
            });

        // Checks the top N detections.
        auto outputIt  = detectedObjects.begin();
        auto outputEnd = detectedObjects.end();

        for (const YoloDetectedObject& expectedDetection : m_TopObjectDetections)
        {
            if (outputIt == outputEnd)
            {
                // Somehow expected more things to check than detections found by the model.
                return TestCaseResult::Abort;
            }

            const YoloDetectedObject& detectedObject = *outputIt;
            if (detectedObject.m_Class != expectedDetection.m_Class)
            {
                BOOST_LOG_TRIVIAL(error) << "Prediction for test case " << this->GetTestCaseId() <<
                    " is incorrect: Expected (" << expectedDetection.m_Class << ")" <<
                    " but predicted (" << detectedObject.m_Class << ")";
                return TestCaseResult::Failed;
            }

            if (!m_FloatComparer(detectedObject.m_Box.m_X, expectedDetection.m_Box.m_X) ||
                !m_FloatComparer(detectedObject.m_Box.m_Y, expectedDetection.m_Box.m_Y) ||
                !m_FloatComparer(detectedObject.m_Box.m_W, expectedDetection.m_Box.m_W) ||
                !m_FloatComparer(detectedObject.m_Box.m_H, expectedDetection.m_Box.m_H) ||
                !m_FloatComparer(detectedObject.m_Confidence, expectedDetection.m_Confidence))
            {
                BOOST_LOG_TRIVIAL(error) << "Detected bounding box for test case " << this->GetTestCaseId() <<
                    " is incorrect";
                return TestCaseResult::Failed;
            }

            ++outputIt;
        }

        return TestCaseResult::Ok;
    }

private:
    boost::math::fpc::close_at_tolerance<float> m_FloatComparer;
    std::vector<YoloDetectedObject> m_TopObjectDetections;
};

template <typename Model>
class YoloTestCaseProvider : public IInferenceTestCaseProvider
{
public:
    template <typename TConstructModelCallable>
    explicit YoloTestCaseProvider(TConstructModelCallable constructModel)
        : m_ConstructModel(constructModel)
    {
    }

    virtual void AddCommandLineOptions(boost::program_options::options_description& options) override
    {
        namespace po = boost::program_options;

        options.add_options()
            ("data-dir,d", po::value<std::string>(&m_DataDir)->required(),
                "Path to directory containing test data");

        Model::AddCommandLineOptions(options, m_ModelCommandLineOptions);
    }

    virtual bool ProcessCommandLineOptions(const InferenceTestOptions &commonOptions) override
    {
        if (!ValidateDirectory(m_DataDir))
        {
            return false;
        }

        m_Model = m_ConstructModel(commonOptions, m_ModelCommandLineOptions);
        if (!m_Model)
        {
            return false;
        }

        m_Database = std::make_unique<YoloDatabase>(m_DataDir.c_str());
        if (!m_Database)
        {
            return false;
        }

        return true;
    }

    virtual std::unique_ptr<IInferenceTestCase> GetTestCase(unsigned int testCaseId) override
    {
        std::unique_ptr<YoloTestCaseData> testCaseData = m_Database->GetTestCaseData(testCaseId);
        if (!testCaseData)
        {
            return nullptr;
        }

        return std::make_unique<YoloTestCase<Model>>(*m_Model, testCaseId, *testCaseData);
    }

private:
    typename Model::CommandLineOptions m_ModelCommandLineOptions;
    std::function<std::unique_ptr<Model>(const InferenceTestOptions&,
                                         typename Model::CommandLineOptions)> m_ConstructModel;
    std::unique_ptr<Model> m_Model;

    std::string m_DataDir;
    std::unique_ptr<YoloDatabase> m_Database;
};


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::example::sampler.cpp                                 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 <vector>
#include <cmath>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/utility/result_of.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/ref.hpp>
#include <boost/foreach.hpp>

#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/make_map.hpp>

#include <boost/math/tools/precision.hpp>

#include <boost/statistics/detail/accumulator/statistics/proportion_less_than.hpp>
#include <boost/statistics/detail/distribution_common/distributions/reference/wrapper.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/generator.hpp>
#include <boost/statistics/detail/distribution_common/functor/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/ratio_pdf/include.hpp>

#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/check_convergence.hpp>

#include <boost/statistics/detail/fusion/at_key/functor.hpp>
#include <boost/statistics/detail/fusion/at_key/range.hpp>
#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/statistic.hpp>

#include <boost/statistics/detail/importance_sampling/weights/prepare_weights.hpp>
#include <boost/statistics/detail/importance_sampling/random/include.hpp>
#include <boost/statistics/detail/importance_sampling/statistics/percentage_effective_sample_size.hpp>

void example_sampler(std::ostream& os){
    os << "->example_sampler : \n";

    // Sample from N(x|mu,sigma^2)N(x|mu,sigma^2) = N(x|mu,sigma^2/2),
    // using SIR with N(x|mu+sigma,sigma^2) as proposal density.
    // The quality of the sample is assessed by a series of
    // kolmorov-distances along the the sample size of the targets.
    using namespace boost;
    namespace stat = boost::statistics::detail;
    namespace dist = stat::distribution;
    namespace tk = stat::distribution::toolkit;
    namespace is = stat::importance_sampling;

    typedef std::string                                 str_;
    typedef double                                      val_;
    typedef std::vector<val_>                           vals_;
    typedef range_iterator<vals_>::type                 vals_it_;
    typedef math::normal_distribution<val_>             dist_;
    typedef mt19937                                     urng_;
    typedef is::prepare_weights<val_>                   prepare_weights_;

    typedef stat::accumulator::tag::percentage_effective_sample_size tag_ess_;
    typedef stat::accumulator::tag::proportion_less_than tag_plt_;
    typedef boost::accumulators::stats<tag_ess_,tag_plt_> stats_;
    typedef boost::accumulators::accumulator_set<val_,stats_> acc_;
    typedef std::size_t                                 size_;

    typedef boost::mpl::int_<0> k0_;
    typedef boost::mpl::int_<1> k1_;
	typedef boost::fusion::result_of::make_map<k0_,k1_,val_,val_>::type data_;
    typedef std::vector<data_>                          vec_data_;
    typedef range_iterator<vec_data_>::type             vec_data_it_;

    typedef stat::fusion::at_key::meta_range<
        vec_data_it_,
        k0_
    >::type range1_;
    typedef is::sampler<range1_,val_>                   	is_sampler_;
    typedef boost::variate_generator<urng_&,is_sampler_> 	vg_;

    // Constants
    const unsigned n_p          = 5e4;
    const val_ max_log          = 100.0;
    const val_ mu               = 0.0;
    const val_ sigma            = 1.0;
    const val_ t_mu             = mu + sigma;
    const val_ t_sigma          = sigma/sqrt(2.0);
    const val_ eps = boost::math::tools::epsilon<val_>();

    const unsigned n_loops  = 7;
    const unsigned n1       = 1e1;
    const unsigned n2       = 1e1;

    prepare_weights_ prepare_weights( max_log );
    dist_ p_d( mu, sigma );       // proposal
    dist_ t_d( t_mu, t_sigma );   // target

    urng_ urng;

    vec_data_ proposals(n_p);
    {
        // Generate proposal values and their log_pdf
        // {(x,log_pdf(x)):i=1,...,n_p}
		BOOST_AUTO(vg,dist::make_random_generator(urng,p_d));
        BOOST_FOREACH(data_& data,proposals){
        	val_ x = vg();
			boost::fusion::at_key<k0_>(data) = x;
            boost::fusion::at_key<k1_>(data) = log_unnormalized_pdf(p_d,x);
		}
    }
    vals_ is_weights( n_p );
    {
        // is_weights <- log_pdf(t_d) - log_pdf(p_d)

        vals_it_ i = boost::begin( is_weights );
        BOOST_FOREACH(const data_& d, proposals)
        {
            *i = log_unnormalized_pdf( t_d, fusion::at_key<k0_>(d) )
                - fusion::at_key<k1_>(d);
            ++i;
        }

    }
    {
        //is_weights <- exp(is_weights + c )
        prepare_weights(
            boost::begin(is_weights),
            boost::end(is_weights)
        );
        os << "weights : " << std::endl
            << prepare_weights_::header << " = "
            << prepare_weights << std::endl;
    }
    {
        acc_ acc = std::for_each(
            boost::begin(is_weights),
            boost::end(is_weights),
            acc_(( stat::accumulator::keyword::threshold = eps ))
        );
        val_ ess = boost::accumulators::extract_result<tag_ess_>(acc);
        val_ plt_eps = boost::accumulators::extract_result<tag_plt_>(acc);
        const std::string str
        	= (boost::format("(ess,plt_eps) = (%1%,%2%)")% ess % plt_eps).str();
        os << str << std::endl;
    }
    {
        range1_ r1 = stat::fusion::at_key::make_range<k0_>(
            boost::begin(proposals),
            boost::end(proposals)
        );
        vg_ vg(
            urng,
            is_sampler_(
                is_weights,
                r1
            )
        );
        vec_data_ targets;
        {
            os << "proposal : " << description(p_d) << std::endl;
            os << "target : " 	<< description(t_d) << std::endl;
			namespace ks = boost::statistics::detail::kolmogorov_smirnov;
			typedef ks::check_convergence<val_> check_;
	        check_ check;
			check(n_loops,n1,n2,t_d,vg,os);

        }
    }

    os << "<-" << std::endl;

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_PC_ILU_0_INCLUDE
#define ITL_PC_ILU_0_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/invert_diagonal.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/matrix/strict_lower.hpp>
#include <boost/numeric/mtl/matrix/upper.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/pc/ilu.hpp>

namespace itl { namespace pc {

// Dummy type to perform factorization in initializer list not in
struct ilu_0_factorizer
{
    template <typename Matrix, typename L_type, typename U_type>
    ilu_0_factorizer(const Matrix &A, L_type& L, U_type& U)
    {   factorize(A, L, U, mtl::traits::is_sparse<Matrix>());  }

    template <typename Matrix, typename L_type, typename U_type>
    void factorize(const Matrix&, L_type&, U_type&, boost::mpl::false_)
    {  MTL_THROW_IF(true, mtl::logic_error("ILU is not intended for dense matrices")); }

    template <typename Matrix, typename L_type, typename U_type>
    void factorize(const Matrix& A, L_type& L, U_type& U, boost::mpl::true_)
    {
	using namespace mtl; using namespace mtl::tag;  using mtl::traits::range_generator;
	using math::reciprocal;
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	mtl::vampir_trace<5038> tracer;

	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef mtl::matrix::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
	typedef mtl::matrix::compressed2D<value_type, para>  LU_type;
	LU_type LU(A);

	typedef typename range_generator<row, LU_type>::type      cur_type;
	typedef typename range_generator<nz, cur_type>::type      icur_type;
	typename mtl::traits::col<LU_type>::type                  col(LU);
	typename mtl::traits::value<LU_type>::type                value(LU);
	mtl::vector::dense_vector<value_type, mtl::vector::parameters<> > inv_dia(num_rows(A));
	cur_type ic= begin<row>(LU), iend= end<row>(LU);
	for (size_type i= 0; ic != iend; ++ic, ++i) {

	    for (icur_type kc= begin<nz>(ic), kend= end<nz>(ic); kc != kend; ++kc) {
		size_type k= col(*kc);
		if (k == i) break;

		value_type aik= value(*kc) * inv_dia[k];
		value(*kc, aik);

		for (icur_type jc= kc + 1; jc != kend; ++jc)
		    value(*jc, value(*jc) - aik * LU[k][col(*jc)]);
		// std::cout << "LU after eliminating A[" << i << "][" << k << "] =\n" << LU;
	    }
	    inv_dia[i]= reciprocal(LU[i][i]);
	}
	invert_diagonal(LU);
	L= strict_lower(LU);
	U= upper(LU);
    }
};

template <typename Matrix, typename Value= typename mtl::Collection<Matrix>::value_type>
class ilu_0
  : public ilu<Matrix, ilu_0_factorizer, Value>
{
    typedef ilu<Matrix, ilu_0_factorizer, Value> base;
  public:
    ilu_0(const Matrix& A) : base(A) {}
};

// ic_0_evaluator not needed IC(0) and ILU(0) do the same at the upper triangle ;-)

}} // namespace itl::pc

#endif // ITL_PC_ILU_0_INCLUDE


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_NEXTAFTER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEXTAFTER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    Returns the next representable value of @c x in the direction of @c y

    @par Header <boost/simd/function/nextafter.hpp>

    @par Semantic:

    @code
    auto r = nextafter(x, y);
    @endcode

    is similar to:

    @code
    if (y >  x)  r = next(x);
    else if (y == x)  r = x;
    else if (y <  x)  r = prev(x);
    @endcode

    @see next, prev, successor, predecessor

    @par Note
    C++11 standard library also defines @c nexttoward, but as (for SIMD considerations)
    @c x and @c y must be here of same type, the two function would coincide.
    So @c nexttoward is not defined in boost.simd.

    @par Decorators

     - std_ calls std::nextafter

    @par Example:

      @snippet nextafter.cpp nextafter

    @par Possible output:

      @snippet nextafter.txt nextafter

  **/
  Value nextafter(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/nextafter.hpp>
#include <boost/simd/function/simd/nextafter.hpp>

#endif


/**
 * This file is part of S-PTAM.
 *
 * Copyright (C) 2013-2017 Taihú Pire
 * Copyright (C) 2014-2017 Thomas Fischer
 * Copyright (C) 2016-2017 Gastón Castro
 * Copyright (C) 2017 Matias Nitsche
 * For more information see <https://github.com/lrse/sptam>
 *
 * S-PTAM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * S-PTAM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with S-PTAM. If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors:  Taihú Pire
 *           Thomas Fischer
 *           Gastón Castro
 *           Matías Nitsche
 *
 * Laboratory of Robotics and Embedded Systems
 * Department of Computer Science
 * Faculty of Exact and Natural Sciences
 * University of Buenos Aires
 */

#include "SmoothEstimatePropagator.hpp"

#include <Eigen/Eigen>

// OpenCV
//#include <opencv2/core/core.hpp>
//#include <opencv2/core/eigen.hpp>

using namespace std;
//using namespace cv;
namespace g2o {
SmoothEstimatePropagator::SmoothEstimatePropagator(g2o::SparseOptimizer* g,
                         const double& maxDistance,
                         const double& maxEdgeCost)
  : g2o::EstimatePropagator(g), _smoothAction(&_adjacencyMap,maxDistance), _treeCost(g),
    _maxDistance(maxDistance), _maxEdgeCost(maxEdgeCost) {}

void SmoothEstimatePropagator::propagate(g2o::OptimizableGraph::Vertex* v)
{ g2o::EstimatePropagator::propagate(v, _treeCost, _smoothAction, _maxDistance, _maxEdgeCost); }

SmoothEstimatePropagator::SmoothPropagateAction::
  SmoothPropagateAction(g2o::EstimatePropagator::AdjacencyMap* adj, const double& max_distance)
  : adjacency(adj), maxDistance(max_distance){}

void SmoothEstimatePropagator::SmoothPropagateAction::
  operator()(g2o::OptimizableGraph::Edge* e_, const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* to_) const
{
  if (to_->fixed())
    return;

  // Static cast to SE3, this must be ensure beforehand using the propagator.
  g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(e_->vertex(0));
  g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(e_->vertex(1));
  g2o::EdgeSE3* e = static_cast<g2o::EdgeSE3*>(e_);

  if (from_.count(from) > 0){
    auto entry = adjacency->find(to);
    double distance = entry == adjacency->end() ? 0 : entry->second.distance(); // this shouldnt happen! "to" must be on the adjacency map
    to->setEstimate(exponencialInterpolation(to->estimate(), from->estimate() * e->measurement(), distance));
  }else{
    auto entry = adjacency->find(from);
    double distance = entry == adjacency->end() ? 0 : entry->second.distance(); // this shouldnt happen! "to" must be on the adjacency map
    from->setEstimate(exponencialInterpolation(from->estimate(), to->estimate() * e->measurement().inverse(), distance));
  }
}

Eigen::Isometry3d SmoothEstimatePropagator::SmoothPropagateAction::
  exponencialInterpolation(const Eigen::Isometry3d& from, const Eigen::Isometry3d& to, double step) const
{
  Eigen::Isometry3d res;

  double maxdist = maxDistance-2;

  // step goes from 1 to maxDistance, we need x from 0 to 1 in a linear way.
  double x = 1 - ((maxdist - (step-1))/maxdist);
  // alpha in [0, inf) describes the explonential ramp "steepness"
  double alpha = 50;
  // exponential ramp from 0 to 1
  double exp_ramp = 1 - (x/(1+alpha*(1-x)));

  // using quaternion representation and slerp for interpolate between from and to isometry transformations
  res.linear() = (Eigen::Quaterniond(from.rotation()).slerp(exp_ramp, Eigen::Quaterniond(to.rotation()))).toRotationMatrix();
  res.translation() = (1 - exp_ramp) * from.translation() + exp_ramp * to.translation();

  return res;
}
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *
 *
 */

#include <cmath>

#include <Eigen/Geometry>

#include "Tudat/Mathematics/BasicMathematics/rotationAboutArbitraryAxis.h"

namespace tudat
{
namespace basic_mathematics
{

//! Compute rotation of point about arbitrary axis
Eigen::Vector3d computeRotationOfPointAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfPoint )
{

    //Declare and initialize rotation matrix
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Zero( );

    // Compute rotation matrix using AngleAxis object.
    rotationMatrix = Eigen::AngleAxisd( angleOfRotation, axisOfRotation.normalized( ) );

    // Compute initial of position of point with respect to origin of rotation.
    const Eigen::Vector3d initialPositionOfPointWithRespectToOriginOfRotation =
            initialPositionOfPoint - originOfRotation;

    // Compute rotation of point about axis of rotation with respect to origin of rotation.
    const Eigen::Vector3d rotatedPositionWithRespectToOriginOfRotation =
            rotationMatrix * initialPositionOfPointWithRespectToOriginOfRotation;

    //Return position with respect to the chosen arbitrary origin after rotation about
    //arbitrary axis.
    return rotatedPositionWithRespectToOriginOfRotation + originOfRotation;

}

//! Compute rotation of vector about arbitrary axis
Eigen::Vector3d computeRotationOfVectorAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfVectorTail,
        const Eigen::Vector3d& initialVector )
{

    // Compute rotation of the tail of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorTail =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation, initialPositionOfVectorTail );

    // Compute rotation of the head of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorHead =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation,
                                                      initialPositionOfVectorTail + initialVector );

    // Return rotated vector with respect to the chosen arbitrary origin
    return rotatedPositionOfVectorHead - rotatedPositionOfVectorTail;

}

} // namespace basic_mathematics
} // namespace tudat


#include <boost/python.hpp>
#include <corsika/Constants.h>

using namespace boost::python;
using namespace corsika;

void register_MathConstants()
{
  namespace bp = boost::python;
  // map the Unit namespace to a sub-module
  // make "from mypackage.Unit import <whatever>" work
  bp::object mathModule(bp::handle<>(bp::borrowed(PyImport_AddModule("corsika.math"))));
  bp::scope().attr("math") = mathModule;
  bp::scope unit_scope = mathModule;

  bp::scope().attr("kPi")      = kPi;
  bp::scope().attr("kPiOnTwo") = kPiOnTwo;
  bp::scope().attr("kTwoPi")   = kTwoPi;
  bp::scope().attr("kE")       = kE;
  bp::scope().attr("kLn10")    = kLn10;
  bp::scope().attr("kSqrt2")   = kSqrt2;
  bp::scope().attr("kSqrt3")   = kSqrt3;
  bp::scope().attr("kSqrt2Pi") = kSqrt2Pi;
  bp::scope().attr("kSqrtPi")  = kSqrtPi;

  // Euler-Mascheroni Constant
  bp::scope().attr("kEulerGamma") = kEulerGamma;

}


#ifndef SHIFTINVERT_SOLVER_H_
#define SHIFTINVERT_SOLVER_H_

#include <Eigen/Dense>
#include <complex>

class ShiftinvertSolver {
public:
  ShiftinvertSolver(const Eigen::Ref<const Eigen::MatrixXd> &matA,
                    const Eigen::Ref<const Eigen::MatrixXd> &matB, int maxiter,
                    double tol);
  std::complex<double> compute(std::complex<double> sigma);

private:
  const int ndim_;
  Eigen::MatrixXd matA_;
  Eigen::MatrixXd matB_;
  const int maxiter_;
  const double tol_;
};
#endif


#include <string>
#include <boost/mpi.hpp>
#include <cstdlib>

#define SKYLARK_NO_ANY
#include <skylark.hpp>
#include "io.hpp"

int main (int argc, char** argv) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);

    if (argc!=5)
    {
    	std::cout << "convert2hdf5 inputfile hdf5file mode[0:dense,1:sparse] min_d" << std::endl;
    	exit(1);
    }

    std::string inputfile = argv[1];
    std::string hdf5file = argv[2];
    int mode = atoi(argv[3]);
    int min_d  = atoi(argv[4]);

    boost::mpi::environment env (argc, argv);

    // get communicator
    boost::mpi::communicator comm;
    int rank = comm.rank();

    El::Initialize (argc, argv);

    if (rank == 0)
        std::cout << "input: " << inputfile << " hdf5file:" << hdf5file << " mode:" <<  mode << " min_d:" << min_d << std::endl;

    if (mode==0) { // dense
        El::Matrix<double> X;
        El::Matrix<double> Y;
    	read_libsvm(comm, inputfile, X, Y, min_d);
        write_hdf5(comm, hdf5file, X,Y);
    } else {
        skylark::base::sparse_matrix_t<double> X;
        El::Matrix<double> Y;
    	read_libsvm(comm, inputfile, X, Y, min_d);
    	if(rank==0)
    		write_hdf5(hdf5file, X,Y);
    }

}


/**
* Project: ECGR 3090 Final
* Authors: Danny Murray, John Humphries
* Date: 12/10/2017
*/

#include <fstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>

using namespace std;

typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS> MyGraph;
typedef boost::graph_traits<MyGraph>::vertex_descriptor MyVertex;
typedef boost::graph_traits <MyGraph>::adjacency_iterator adjacency_iterator;
typedef boost::property_map<MyGraph, boost::vertex_index_t>::type IndexMap;
typedef boost::graph_traits<MyGraph>::vertex_iterator VertexItr;

/**
	vector of all members of the graph
*/
vector<int> everyone;

/**
	@param id the identifier for vertex
	@param mutualFriends the number of friends shared
*/
struct connection_t{
    int id;
    int mutualFriends = 0;
};

/**
	@param id the identifier for vertex
	@param numOccurences the number of times friend is mutual
*/
struct friend_t {
    int id;
    int numOccurences = 0;
};

/**
	Comparator for friend_t.
 	@param a vector to compare
	@param b vector to compare
	@return true if a.numOccurences is greater than b.numOccurences
*/
bool compareNumOccurences(const friend_t &a, const friend_t &b) {
    return a.numOccurences > b.numOccurences;
}

/**
	Comparator for connection_t.
	@param a vector to compare
	@param b vector to compare
	@return true if a.mutualFriends is greater than b.mutualFriends
*/
bool compareMutualFriends(const connection_t &a, const connection_t &b) {
    return a.mutualFriends > b.mutualFriends;
}

/**
	Finds a vertex in the graph.
	@param g graph to search
	@param k vertex to find
	@return true if k exists in g
*/
bool findvertex(const MyGraph& g, int k){
    VertexItr vi, vi_end;
    boost::tie(vi, vi_end) = boost::vertices(g);
    for (; vi != vi_end; vi++) {
        if (*vi == k) {
            return true;
        }
    }
    return false;
}

/**
	Populate the graph with vertices and edges.
 	@param g graph to populate
	@param ifs file to extract data
*/
void populateNetwork(MyGraph& g, ifstream& ifs) {
    string line;
    string first;
    string second;
    int vs;
    int vt;
    int pos;
    string SPACE_DELIM = " ";
    if(ifs.is_open()) {
        while(getline(ifs, line)) {
            pos = line.find(SPACE_DELIM);
            first = line.substr(0, line.find(SPACE_DELIM));
            line.erase(0, pos + SPACE_DELIM.length());
            second = line;
            vs = atoi(first.c_str());
            vt = atoi(second.c_str());
            everyone.emplace_back(vs);
            everyone.emplace_back(vt);
            boost::add_edge(vs, vt, g);
        }
        ifs.close();
    } else {
        cout << "File couldn't be opened." << endl;
    }

    sort(everyone.begin(), everyone.end());
    vector<int>::iterator it;
    it = unique(everyone.begin(), everyone.end());
    everyone.resize(distance(everyone.begin(), it));

}

/**
	Find all vertices that share an edge with a given vertex.
	@param g graph to search
	@param k root vertex
	@return vector of verticies that share an edge with k
*/
vector<int> findFriends(const MyGraph& g, const int& k) {
    vector<int> friends;

    IndexMap index = get(boost::vertex_index, g);
    pair<adjacency_iterator, adjacency_iterator> neighbors =
    boost::adjacent_vertices(boost::vertex(k, g), g);
    for(; neighbors.first != neighbors.second; ++neighbors.first) {
        friends.emplace_back(index[*neighbors.first]);
    }

    sort(friends.begin(), friends.end());
    return friends;
}

/**
	Find all verticies that share an edge with vertices in a vector.
	@param g graph to search
	@param v vertex of vertices
	@return vector of vectors that contain verticies with shared edges
*/
vector<vector<int>> friendsOfFriends(const MyGraph& g, vector<int>& v) {
	vector<vector<int>> fOfF;

    for (vector<int>::iterator it = v.begin(); it != v.end(); ++it) {
        fOfF.push_back(findFriends(g, *it));
    }

    return fOfF;
}

/**
	Search sorted vector for a value. O(logn).
	@param key value to search for
	@param b vector to search
	@retrun true if key exists in b
*/
bool binarySearch(int key, const vector<int>& b) {
    int low = 0;
    int high = b.size() - 1;
    while(high >= low)
    {
        int middle = (low + high) / 2;
        if(b.at(middle) == key)
        {
            return true;
        }
        if(b.at(middle) < key)
        {
            low = middle + 1;
        }
        if(b.at(middle) > key)
        {
            high = middle - 1;
        }
    }
    return false;
}

/**
	Search sorted vector of type friend_t for a value.
	@param key value to search for
	@param b vector to search
	@retrun true if key exists in b
*/
bool linearSearch(int key, const vector<friend_t>& b) {
    for (int i = 0; i < b.size(); i++) {
        if (key == b.at(i).id) {
            return true;
        }
    }
    return false;
}

/**
	Count the number of shared values between two vectors.
	@param a vector to compare
	@param b vector to compare
	@return size of vector containing shared values
*/
int countIntersection(const vector<int>&a, const vector<int>& b) {
    vector<int> result;
    for (int i = 0; i < a.size(); i++)
    {
        if (binarySearch(a.at(i), b))
        {
            result.emplace_back(a.at(i));
        }
    }

    vector<int>::iterator it;
    it = unique(result.begin(), result.end());
    result.resize(distance(result.begin(), it));

    return result.size();
}

/**
	Find shared values between two vectors.
	@param a vector to compare
	@param b vector to compare
	@return vector containing shared values
*/
vector<int> intersection(const vector<int>&a, const vector<int>& b) {
    vector<int> result;
    for (int i = 0; i < a.size(); i++)
    {
        if (binarySearch(a.at(i), b))
        {
            result.emplace_back(a.at(i));
        }
    }

    vector<int>::iterator it;
    it = unique(result.begin(), result.end());
    result.resize(distance(result.begin(), it));

    return result;
}

/**
	Sort vector of verticies by number of edges in common.
	@param KsFriends vector of verticies to sort
	@param friendsFriends vector of vectors of vertices to check for common edges
	@return sorted vector of type connection_t
*/
vector<connection_t> sortedFriends(const vector<int>& KsFriends, const vector<vector<int>>& friendsFriends){
    vector<connection_t> KsNetwork;
    KsNetwork.resize(KsFriends.size());
    for (int i = 0; i < KsFriends.size(); ++i) {
        KsNetwork.at(i).id = KsFriends.at(i);
        KsNetwork.at(i).mutualFriends = countIntersection(friendsFriends.at(i), KsFriends);
    }

    sort(KsNetwork.begin(), KsNetwork.end(), compareMutualFriends);

    return KsNetwork;

}

/**
	Find the top N friends of a vertex.
	@param g graph to search
	@param k root vertex
	@param N number of top friends to identify
*/
void topNFriends(MyGraph& g, int k, int N) {
    if (N < 0) {
        cout << "N must be a positive integer" << endl;
        exit(EXIT_SUCCESS);
    }
    vector<int> kFriends = findFriends(g, k);
    vector<vector<int>> kFriendsOfFriends = friendsOfFriends(g, kFriends);
    vector<connection_t> ksortedFriends = sortedFriends(kFriends, kFriendsOfFriends);

    if (N > ksortedFriends.size()) {
        if (ksortedFriends.size() == 1) {
            cout << k << " only has " << ksortedFriends.size() << " friend." << endl;
            cout << "(S)He is:" << endl;
        } else {
            cout << k << " only has " << ksortedFriends.size() << " friends." << endl;
            cout << k << "'s top " << ksortedFriends.size() << " friends are:" << endl;
        }
        N = ksortedFriends.size();
    } else {
        if (N == 1) {
            cout << k << "'s top friend is:" << endl;
        } else {
            cout << k << "'s top " << N << " friends are:" << endl;
        }
    }

    for (int i = 0; i < N; ++i) {
	    cout << ksortedFriends.at(i).id << endl;
    }
}

/**
	Find all verticies that are not friends of a vertex.
	@param vector of friends
	@return vector of verticies that do not share an edge with root vertex
*/
vector<int> notFriends(const vector<int>& friends) {
    vector<int> result;
    for (int i = 0; i < everyone.size(); i++)
    {
        if (!binarySearch(everyone.at(i), friends))
        {
            result.emplace_back(everyone.at(i));
        }
    }

    vector<int>::iterator it;
    it = unique(result.begin(), result.end());
    result.resize(distance(result.begin(), it));

    return result;
}
/**
	Suggest M new friends of a given vertex.
	@param g graph to search
	@param k root vertex
	@param N number of friends to suggest
*/
void TopMutualFriends(MyGraph& g, int k, int N) {
    if (N < 0) {
        cout << "N must be a positive integer" << endl;
        exit(EXIT_SUCCESS);
    }
    vector<int> kFriends = findFriends(g, k);
    vector<vector<int>> kFriendsOfFriends = friendsOfFriends(g, kFriends);
    vector<int> friendNot = notFriends(kFriends);
    friendNot.erase(find(friendNot.begin(), friendNot.end(), k));
    vector<vector<int>> resultSet;
	resultSet.resize(kFriendsOfFriends.size());
    vector<int> listOfSuggestedFriends;

	for(int i = 0; i < kFriendsOfFriends.size(); i++) {
		resultSet.at(i) = intersection(kFriendsOfFriends.at(i), friendNot);
	}

    for (int i = 0; i < resultSet.size(); i++) {
        listOfSuggestedFriends.insert(listOfSuggestedFriends.end(), resultSet.at(i).begin(), resultSet.at(i).end());
    }

	sort(listOfSuggestedFriends.begin(), listOfSuggestedFriends.end());

	vector<friend_t> suggestedConnections;
    suggestedConnections.resize(listOfSuggestedFriends.size());

    int i = 0;
    int j = 0;
    int l = 0;

    while (i < listOfSuggestedFriends.size()) {
		if (!linearSearch(listOfSuggestedFriends.at(i), suggestedConnections)) {
			suggestedConnections.at(j).id = listOfSuggestedFriends.at(i);
			suggestedConnections.at(j).numOccurences++;
            l = j;
            j++;
		} else {
			suggestedConnections.at(l).numOccurences++;
		}
        i++;
    }

    int numZeros = suggestedConnections.size() - j - 1;

    suggestedConnections.resize(j);

    friend_t fixedZero;
    fixedZero.id = 0;
    fixedZero.numOccurences = numZeros;

    suggestedConnections.emplace_back(fixedZero);

    sort(suggestedConnections.begin(), suggestedConnections.end(), compareNumOccurences);

    if (N > suggestedConnections.size()) {
        if (suggestedConnections.size() == 1) {
            cout << k << " only has " << suggestedConnections.size() << " second tier connection." << endl;
            cout << "(S)He is:" << endl;
        } else {
            cout << k << " only has " << suggestedConnections.size() << " second tier connections." << endl;
            cout << k << "'s top " << suggestedConnections.size() << " second tier connections are:" << endl;
        }
        N = suggestedConnections.size();
    } else {
        if (N == 1) {
            cout << k << "'s recommended friend is:" << endl;
        } else {
            cout << k << "'s top " << N << " recommended friends are:" << endl;
        }
    }

    for (int i = 0; i < N; ++i) {
	    cout << suggestedConnections.at(i).id << endl;
    }
}

int main() {
    ifstream ifs("facebook_combined.txt");
    MyGraph g;
    populateNetwork(g, ifs);

    int k = 946;
    int M = 15;
	int N = 20;

    if (!findvertex(g, k)) {
        cout << k << " is not in the network." << endl;
        exit(EXIT_SUCCESS);
    }

    struct timeval start, end;

    gettimeofday(&start, 0);

    topNFriends(g, k, N);
    cout << endl;
    TopMutualFriends(g, k, M);

    gettimeofday(&end, 0);
    cout << endl;
    cout << "Total time elapsed:\t";
    cout << (end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec)<< " us" << endl;

}


//  (C) Copyright Raffi Enficiaud 2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE boost_test_container_lex
#include <boost/test/included/unit_test.hpp>
#include <vector>

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE( test_collections_vectors_lex )
{
  std::vector<int> a{1,2,3}, b{1,2,2}, c{1,2,3,4};

  BOOST_TEST(a < a, tt::lexicographic());
  BOOST_TEST(a < b, tt::lexicographic());
  BOOST_TEST(a < c, tt::lexicographic());
  BOOST_TEST(a >= c, tt::lexicographic());

  // does not compile
  //BOOST_TEST(a == c, tt::lexicographic());
  //BOOST_TEST(a != c, tt::lexicographic());
}
//]


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
// g++ -Wall -std=c++1z -I /usr/include/eigen3/ main.cpp -o test
#include <random>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
#include "vmf.hpp"
#include "vmfPrior.hpp"
#include "normal.hpp"
#include "sample.hpp"

int main() {

  std::mt19937 rnd(1);

  vMF<float,3> vmfA(Eigen::Vector3f(1,0,0), 500);
  vMF<float,3> vmfB(Eigen::Vector3f(cos(5*M_PI/180.),sin(5*M_PI/180.),0), 500);
  Eigen::Matrix3f SigmaO = 0.0001*Eigen::Matrix3f::Identity();
  Normal<float,3> gaussO(SigmaO);
  float tauO = 500.;

  size_t N=100;
  std::vector<std::vector<Eigen::Vector3f>> n; // normals
  std::vector<std::vector<Eigen::Vector3f>> xn; // normal observations
  std::vector<std::vector<Eigen::Vector3f>> x; // loc observations
  std::vector<std::vector<Eigen::Vector3f>> p; // plane location
  for (size_t i=0; i<N; ++i) {
    n.push_back(std::vector<Eigen::Vector3f>());
    xn.push_back(std::vector<Eigen::Vector3f>());
    x.push_back(std::vector<Eigen::Vector3f>());
    p.push_back(std::vector<Eigen::Vector3f>());
    for (size_t j=0; j<N; ++j) {
      if (i<N/2) {
        n[i].push_back(vmfA.sample(rnd));
        xn[i].push_back(vmfA.sample(rnd));
        x[i].push_back(Eigen::Vector3f(1.,(i-N*0.25)/float(0.25*N),(j-N*0.5)/float(0.5*N)));
      }
      if (i>=N/2) {
        n[i].push_back(vmfB.sample(rnd));
        xn[i].push_back(vmfB.sample(rnd));
        x[i].push_back(Eigen::Vector3f((N*0.75-i)/float(0.25*N),1.,(j-N*0.5)/float(0.5*N)));
      }
      p[i].push_back(x[i][j]);
      x[i][j] += gaussO.sample(rnd);
      p[i][j] += gaussO.sample(rnd);
//      std::cout << i << " " << j << ": "  <<  x[i][j][0] << "\t" << x[i][j][1] << std::endl;
    }
  }
  std::cout << "have " << n.size() << " input data" << std::endl;

  std::vector<Eigen::Vector3f> xSum(1, Eigen::Vector3f::Zero());
  std::vector<std::vector<uint32_t>> z(n.size());
  for (size_t i=0; i<n.size(); ++i)
    for (size_t j=0; j<n[i].size(); ++j)  {
      xSum[0] += n[i][j];
      z[i].push_back(0);
    }
  std::vector<float> counts(1, n.size()*n.size());
  std::vector<vMF<float,3>> vmfs;
  vMFprior<float> base(Eigen::Vector3f(0,0,1), .1, 0.0);
  float logAlpha = log(10.);
  float lambda = .10;

  vmfs.push_back(base.sample(rnd));
  for (size_t it=0; it<10000; ++it) {
    // sample labels | parameters
    size_t K = vmfs.size();
    for (size_t i=0; i<n.size(); ++i) {
      for (size_t j=0; j<n[i].size(); ++j) {
        Eigen::VectorXf logPdfs(K+1);
        Eigen::VectorXf pdfs(K+1);

        Eigen::VectorXf neighNs = Eigen::VectorXf::Zero(K);
//        if (i+1<N) neighNs[z[i+1][j]] += 1.f;
//        if (i>=1)  neighNs[z[i-1][j]] += 1.f;
//        if (j+1<N) neighNs[z[i][j+1]] += 1.f;
//        if (j>=1)  neighNs[z[i][j-1]] += 1.f;

        if (i+1<N) neighNs[z[i+1][j]] += n[i+1][j].dot(n[i][j]);
        if (i>=1)  neighNs[z[i-1][j]] += n[i-1][j].dot(n[i][j]);
        if (j+1<N) neighNs[z[i][j+1]] += n[i][j+1].dot(n[i][j]);
        if (j>=1)  neighNs[z[i][j-1]] += n[i][j-1].dot(n[i][j]);

//        if (i+1<N) neighNs[z[i+1][j]] += vmfs[z[i+1][j]].mu_.dot(n[i][j]);
//        if (i>=1)  neighNs[z[i-1][j]] += vmfs[z[i-1][j]].mu_.dot(n[i][j]);
//        if (j+1<N) neighNs[z[i][j+1]] += vmfs[z[i][j+1]].mu_.dot(n[i][j]);
//        if (j>=1)  neighNs[z[i][j-1]] += vmfs[z[i][j-1]].mu_.dot(n[i][j]);

//        if (i+1<N) neighNs[z[i+1][j]] += vmfs[z[i+1][j]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (i>=1)  neighNs[z[i-1][j]] += vmfs[z[i-1][j]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (j+1<N) neighNs[z[i][j+1]] += vmfs[z[i][j+1]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (j>=1)  neighNs[z[i][j-1]] += vmfs[z[i][j-1]].mu_.dot(vmfs[z[i][j]].mu_);

        for (size_t k=0; k<K; ++k) {
          logPdfs[k] = lambda*(neighNs[k]-4);
          if (z[i][j] == k) {
            // TODO what if last in cluster
            logPdfs[k] += log(counts[k]-1)+vmfs[k].logPdf(n[i][j]);
          } else {
            logPdfs[k] += log(counts[k])+vmfs[k].logPdf(n[i][j]);
          }
        }
        logPdfs[K] = logAlpha + base.logMarginal(n[i][j]);
        logPdfs = logPdfs.array() - logSumExp<float>(logPdfs);
        pdfs = logPdfs.array().exp();
        size_t zPrev = z[i][j];
        z[i][j] = sampleDisc(pdfs, rnd);
        //      std::cout << z[i] << " " << K << ": " << pdfs.transpose() << std::endl;
        if (z[i][j] == K) {
          vmfs.push_back(base.posterior(n[i][j],1).sample(rnd));
          counts.push_back(0);
          xSum.push_back(Eigen::Vector3f::Zero());
          K++;
        }
        if (zPrev != z[i][j]) {
          counts[zPrev] --;
          counts[z[i][j]] ++;
          xSum[zPrev] -= n[i][j];
          xSum[z[i][j]] += n[i][j];
        }
      }
    }
//    std::cout << "sample parameters" << std::endl;
    // sample parameters | labels
//    for (size_t i=0; i<n.size(); ++i) {
//      xSum[z[i]] += n[i]; // TODO: can fold in above as well
//    }
    for (size_t k=0; k<K; ++k) {
      if (counts[k] > 0) {
        vmfs[k] = base.posterior(xSum[k],counts[k]).sample(rnd);
      }
    }
    std::cout << "counts " << K << ": ";
    for (size_t k=0; k<K; ++k) if (counts[k] > 0) std::cout << counts[k] << " ";
    std::cout << "\ttaus: " ;
    for (size_t k=0; k<K; ++k) if (counts[k] > 0) std::cout << vmfs[k].tau_ << " ";
    std::cout << std::endl;
//    for (size_t k=0; k<K; ++k)
//      if (counts[k] > 0) {
//        std::cout << vmfs[k].mu_.transpose() << std::endl;
//      }

    // sample ns
    for (size_t k=0; k<K; ++k)
      xSum[k] = Eigen::Vector3f::Zero();
    for (size_t i=0; i<n.size(); ++i) {
      for (size_t j=0; j<n[i].size(); ++j) {
        Eigen::Vector3f mu = xn[i][j]*tauO + vmfs[z[i][j]].mu_*vmfs[z[i][j]].tau_;
        n[i][j] = vMF<float,3>(mu).sample(rnd);
        xSum[z[i][j]] += n[i][j];
      }
    }
//    std::cout << n[N/4][N/2].transpose() << "\t" << n[(3*N)/4][N/2].transpose() << std::endl;
//    std::cout << xn[N/4][N/2].transpose() << "\t" << xn[(3*N)/4][N/2].transpose() << std::endl;

    // sample locations
    for (size_t i=0; i<x.size(); ++i) {
      for (size_t j=0; j<x[i].size(); ++j) {
        Eigen::Matrix3f SigmaPl;
        Eigen::Matrix3f Info =  SigmaO.inverse();
        Eigen::Vector3f xi = SigmaO.ldlt().solve(x[i][j]);
        if (i+1<N && z[i][j] == z[i+1][j]) {
          SigmaPl = vmfs[z[i+1][j]].mu_*vmfs[z[i+1][j]].mu_.transpose();
          Info += SigmaPl;
          xi += SigmaPl*p[i+1][j];
        }
        if (i>=1 && z[i][j] == z[i-1][j])  {
          SigmaPl = vmfs[z[i-1][j]].mu_*vmfs[z[i-1][j]].mu_.transpose();
          Info += SigmaPl;
          xi += SigmaPl*p[i-1][j];
        }
        if (j+1<N && z[i][j] == z[i][j+1]) {
          SigmaPl = vmfs[z[i][j+1]].mu_*vmfs[z[i][j+1]].mu_.transpose();
          Info += SigmaPl;
          xi += SigmaPl*p[i][j+1];
        }
        if (j>=1 && z[i][j] == z[i][j-1])  {
          SigmaPl = vmfs[z[i][j-1]].mu_*vmfs[z[i][j-1]].mu_.transpose();
          Info += SigmaPl;
          xi += SigmaPl*p[i][j-1];
        }
        Eigen::Matrix3f Sigma = Info.inverse();
        Eigen::Vector3f mu = Sigma*xi;
//        std::cout << xi.transpose() << " " << mu.transpose() << std::endl;
        p[i][j] = Normal<float,3>(mu, Sigma).sample(rnd);
      }
    }
//    std::cout << p[N/4][N/2].transpose() << "\t" << p[(3*N)/4][N/2].transpose() << std::endl;
//    std::cout << x[N/4][N/2].transpose() << "\t" << x[(3*N)/4][N/2].transpose() << std::endl;
  }
  return 0;
}


//--C++--
//-- ellipsoid 1D analytical = no orientation
//-- SANS

#include <boost/python.hpp>

#include "mccomponents/kernels/sample/AbstractSQ.h"
#include "mccomponents/kernels/sample/SQAdaptor.h"

extern "C" {
#include "sans/ellipsoid.h"
}

namespace wrap_models{
struct Ellipsoid{
	Ellipsoid(double scale,double radius_a, double radius_b, double contrast, double background,double axis_theta, double axis_phi)
	{
    p.scale = scale;
    p.radius_a = radius_a;
	p.radius_b = radius_b;
    p.contrast = contrast;
	p.background = background;
    p.axis_theta = axis_theta;
	p.axis_phi = axis_phi;
	}

	double operator() (double q) const
	{
		return ellipsoid_analytical_1D( const_cast<EllipsoidParameters *>(&p), q);
	}
	EllipsoidParameters p;
};

void wrap_ellipsoid()
{
	using namespace boost::python;
	typedef Ellipsoid w_t;
	class_<w_t>
		(
		"SANSModel_Ellipsoid",
		init< double, double, double, double, double, double, double >()
			)
		;

	using namespace mccomponents::sample;
	typedef SQAdaptor<w_t> SQ;
	class_<SQ, bases<AbstractSQ> >
		(
		"SANSModel_Ellipsoid_SQAdaptor",
		init< const w_t & >()
		[with_custodian_and_ward<1,2>()] //Jiao, what you mean by the numbers,<1,2>???
		)
		;
	}
}
//2009-06-02 jhcho
// End of file


/* Copyright (c) 2012-2021 Big Ladder Software LLC. All rights reserved.
 * See the LICENSE file for additional terms and conditions. */

#ifndef GROUND_HPP_
#define GROUND_HPP_

#include "Algorithms.hpp"
#include "BoundaryConditions.hpp"
#include "Domain.hpp"
#include "Foundation.hpp"
#include "GroundOutput.hpp"
#include "Mesher.hpp"
#include "libkiva_export.h"

#include <cmath>
#include <memory>
#include <numeric>
#include <string>
#include <vector>

#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCore>

namespace Kiva {

class LIBKIVA_EXPORT Ground {
public:
  // Constructor
  Ground(Foundation &foundation);
  Ground(Foundation &foundation, GroundOutput::OutputMap &outputMap);

  // Destructor
  virtual ~Ground();

  Domain domain;
  Foundation &foundation;
  GroundOutput groundOutput;

  size_t nX, nY, nZ, num_cells;

  std::vector<double> TNew; // solution, n+1
  std::vector<double> TOld; // solution, n

  void buildDomain();

  void calculateBoundaryLayer();
  void setNewBoundaryGeometry();
  void calculate(BoundaryConditions &boundaryConidtions, double ts = 0.0);

  void calculateSurfaceAverages();
  double getSurfaceAverageValue(std::pair<Surface::SurfaceType, GroundOutput::OutputType> output);

private:
  double timestep; // in seconds

  friend class Aggregator;
  BoundaryConditions bcs;
  // Data structures

  // ADE
  std::vector<double> U; // ADE upper sweep, n+1
  std::vector<double> V; // ADE lower sweep, n+1

  // ADI
  std::vector<double> a1; // lower diagonal
  std::vector<double> a2; // main diagonal
  std::vector<double> a3; // upper diagonal
  std::vector<double> b_; // right-hand side
  std::vector<double> x_; // solution

  // Implicit
  Eigen::SparseMatrix<double> Amat;
  std::vector<Eigen::Triplet<double>> tripletList;
  Eigen::VectorXd b, x;

  std::shared_ptr<Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double>>>
      pSolver;

private:
  // Calculators (Called from main calculator)
  void calculateADE();

  void calculateADEUpwardSweep();

  void calculateADEDownwardSweep();

  void calculateExplicit();

  void calculateMatrix(Foundation::NumericalScheme scheme);

  void calculateADI(int dim);

  // Misc. Functions
  void setAmatValue(const std::size_t i, const std::size_t j, const double val);
  void setbValue(const std::size_t i, const double val);
  void setValuesADI(const std::size_t &index, const double &A, const double (&Alt)[2],
                    const double &bVal);
  void solveLinearSystem();
  void clearAmat();
  double getxValue(const int i);
  std::vector<double> getXvalues();

  double getSurfaceArea(Surface::SurfaceType surfaceType);

  void setBoundaryConditions();

  std::vector<std::pair<double, double>> boundaryLayer;

  double getBoundaryValue(double dist);

  double getBoundaryDistance(double val);
  void link_cells_to_temp();
};

} // namespace Kiva

#endif /* GROUND_HPP_ */


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <atomic>
#include <cmath>
#include <functional>
#include <iostream>
#include <iterator>
#include <mutex>
#include <thread>

#include <Eigen/Core>
#include <Eigen/Eigen>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/rate_limiting.h>
#include <franka/robot.h>

namespace {
template <class T, size_t N>
std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
  ostream << "[";
  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
  ostream << "]";
  return ostream;
}
}  // anonymous namespace

int main(int argc, char** argv) {
  // Check whether the required arguments were passed.
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  // Initialize data fields for the print thread.
  struct {
    std::mutex mutex;
    bool has_data;
    franka::RobotState robot_state;
    std::array<double, 7> tau_d_calculated;
    std::array<double, 7> tau_d_last;
    std::array<double, 7> gravity;
    std::array<double, 7> coriolis;
    std::array<double, 42> jacobian_array;
  } print_data{};
  std::atomic_bool running{true};
  const double print_rate = 10.0;
  // Start print thread.
  std::thread print_thread([print_rate, &print_data, &running]() {
    while (running) {
      // Sleep to achieve the desired print rate.
      std::this_thread::sleep_for(
          std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0))));

      // Try to lock data to avoid read write collisions.
      if (print_data.mutex.try_lock()) {
        if (print_data.has_data) {
          // std::array<double, 7> tau_error{};
          // double error_rms(0.0);
          // std::array<double, 7> tau_d_actual{};
          // for (size_t i = 0; i < 7; ++i) {
          //   tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i];
          //   tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i];
          //   error_rms += std::pow(tau_error[i], 2.0) / tau_error.size();
          // }
          // error_rms = std::sqrt(error_rms);
          const Eigen::Matrix<double, 7, 1> mytorques(print_data.robot_state.tau_ext_hat_filtered.data());
          const Eigen::Matrix<double, 6, 7> jacobian(print_data.jacobian_array.data());

          const Eigen::MatrixXd jacobianPinv1 = ( jacobian * jacobian.transpose() ).inverse() * jacobian;
          const Eigen::VectorXd mywrench1= jacobianPinv1 * mytorques;

          Eigen::FullPivLU<Eigen::Matrix<double, 6, 6>> lu_decomp_ = Eigen::FullPivLU<Eigen::Matrix<double, 6, 6>>();
          lu_decomp_.compute( jacobian * jacobian.transpose() );
          const Eigen::MatrixXd jacobianPinv2 = lu_decomp_.inverse() * jacobian;
          const Eigen::VectorXd mywrench2 = jacobianPinv2 * mytorques;

          Eigen::JacobiSVD<Eigen::Matrix<double, 7, 6>> svdT = Eigen::JacobiSVD<Eigen::Matrix<double, 7, 6>>();
          svdT.setThreshold(0.08); //critical singular value threshold is 0.08
          svdT.compute(jacobian.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
          const Eigen::VectorXd mywrench3 = svdT.solve(mytorques);

          const Eigen::MatrixXd U = svdT.matrixU();
          const Eigen::MatrixXd V = svdT.matrixV();
          const Eigen::VectorXd Svec  = svdT.singularValues();
          Eigen::Matrix<double, 6, 6> S = Eigen::Matrix<double, 6, 6>::Zero();
          S.diagonal() = Svec;
          Eigen::Matrix<double, 6, 6> Sinv = Eigen::Matrix<double, 6, 6>::Zero();
          // Sinv = S.inverse();
          for(int i=0; i<6; i++){
            if (Svec(i)>=0.08){
              Sinv(i,i) = 1.0 / Svec(i);
            }
          }
          const Eigen::MatrixXd jacobianPinv4 = V * Sinv * U.transpose();
          const Eigen::VectorXd mywrench4 = jacobianPinv4 * mytorques;

          // Print data to console
          std::cout << "control_command_success_rate: " <<  print_data.robot_state.control_command_success_rate << std::endl
                    << "joint configuration: " <<  print_data.robot_state.q << std::endl
                    // << "jacobian: \n" << jacobian << std::endl
                    // << "U: \n" << U << std::endl
                    // << "V: \n" << V << std::endl
                    // << "S: \n" << S << std::endl
                    // << "Sinv: \n" << Sinv << std::endl
                    // << "jacobianPinv: \n" << jacobianPinv1.row(0) << std::endl
                    // << "jacobianPinv: \n" << jacobianPinv2.row(0) << std::endl
                    // << "jacobianPinv: \n" << jacobianPinv4.row(0) << std::endl
                    // << "jacobian-singularValues: " << svdT.singularValues().transpose() << std::endl
                    << "External torque, filtered: " <<  print_data.robot_state.tau_ext_hat_filtered << std::endl
                    // << "end-effector wrench (base frame)     : " <<  print_data.robot_state.K_F_ext_hat_K << std::endl
                    << "end-effector wrench (stiffness frame): " <<  print_data.robot_state.O_F_ext_hat_K << std::endl
                    << "end-effector wrench (stiffness frame): " <<  mywrench1.transpose() << std::endl
                    << "end-effector wrench (stiffness frame): " <<  mywrench2.transpose() << std::endl
                    << "end-effector wrench (stiffness frame): " <<  mywrench3.transpose() << std::endl
                    << "end-effector wrench (stiffness frame): " <<  mywrench4.transpose() << std::endl
                    // << "tau_error [Nm]: " << tau_error << std::endl
                    // << "tau_commanded [Nm]: " << tau_d_actual << std::endl
                    // << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl
                    // << "root mean square of tau_error [Nm]: " << error_rms << std::endl
                    << "-----------------------" << std::endl
                    ;
          print_data.has_data = false;
        }
        print_data.mutex.unlock();
      }
    }
  });

  try {
    franka::Robot robot(argv[1]);
    robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); //values taken from https://github.com/frankaemika/libfranka/blob/master/examples/examples_common.cpp#L18
    robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); //values taken from https://github.com/frankaemika/libfranka/blob/master/examples/examples_common.cpp#L19
    // robot.setJointImpedance({{100,100,100,100,100,100,100}});
    // robot.setCartesianImpedance({{100,100,100,10,10,10}});
    robot.setCollisionBehavior(
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

    //TODO: be careful with this mode!
    robot.setCollisionBehavior(
        {{2000.0, 2000.0, 1800.0, 1800.0, 1600.0, 1400.0, 1200.0}}, {{2000.0, 2000.0, 1800.0, 1800.0, 1600.0, 1400.0, 1200.0}},
        {{2000.0, 2000.0, 1800.0, 1800.0, 1600.0, 1400.0, 1200.0}}, {{2000.0, 2000.0, 1800.0, 1800.0, 1600.0, 1400.0, 1200.0}},
        {{2000.0, 2000.0, 2000.0, 2500.0, 2500.0, 2500.0}}, {{2000.0, 2000.0, 2000.0, 2500.0, 2500.0, 2500.0}},
        {{2000.0, 2000.0, 2000.0, 2500.0, 2500.0, 2500.0}}, {{2000.0, 2000.0, 2000.0, 2500.0, 2500.0, 2500.0}});

    franka::Model model = robot.loadModel();

    // const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};
    const std::array<double, 7> d_gains = {{15.0, 15.0, 15.0, 15.0, 9.0, 8.0, 4.0}};
    // const std::array<double, 7> d_gains = {{5.0, 5.0, 5.0, 5.0, 3.0, 2.5, 1.5}};
    // const std::array<double, 7> d_gains = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; //TODO pure gravity compensation mode (with coriolis) performs very bad

    // Define callback for the joint torque control loop.
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback =
            [&print_data, &model, d_gains](
                const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {

      const std::array<double, 7> gravity = model.gravity(state);
      const std::array<double, 7> coriolis = model.coriolis(state);
      const std::array<double, 42> jacobian_array = model.zeroJacobian(franka::Frame::kEndEffector, state);

      // Compute torque command
      std::array<double, 7> tau_d_calculated;
      for (size_t i = 0; i < 7; i++) {
        tau_d_calculated[i] = - d_gains[i] * state.dq[i] + coriolis[i];
      }

      // The following line is only necessary for printing the rate limited torque. As we activated
      // rate limiting for the control loop (activated by default), the torque would anyway be
      // adjusted!
      std::array<double, 7> tau_d_rate_limited =
          franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d);

      // Update data to print.
      if (print_data.mutex.try_lock()) {
        print_data.has_data = true;
        print_data.robot_state = state;
        print_data.tau_d_calculated = tau_d_calculated;
        print_data.tau_d_last = tau_d_rate_limited;
        print_data.gravity = gravity;
        print_data.coriolis = coriolis;
        print_data.jacobian_array = jacobian_array;
        print_data.mutex.unlock();
      }

      // Send torque command.
      return tau_d_rate_limited;
    };

    // Start real-time control loop.
    robot.control(impedance_control_callback);

  } catch (const franka::Exception& ex) {
    running = false;
    std::cerr << ex.what() << std::endl;
  }

  if (print_thread.joinable()) {
    print_thread.join();
  }
  return 0;
}


#include <iostream>
#include <fstream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <octomap/octomap.h>    // for octomap

#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿

    ifstream fin("../data/pose.txt");
    if (!fin)
    {
        cerr<<"cannot find pose file"<<endl;
        return 1;
    }

    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "../data/%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

        double data[7] = {0};
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
        }
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }

    // 计算点云并拼接
    // 相机内参
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout<<"正在将图像转换为 Octomap ..."<<endl;

    // octomap tree
    octomap::OcTree tree( 0.03 ); // 参数为分辨率

    for ( int i=0; i<5; i++ )
    {
        cout<<"转换图像中: "<<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];

        octomap::Pointcloud cloud;  // 每张图像的点云

        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;
                // 将世界坐标系的点放入点云
                cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] );
            }

        // 将点云存入八叉树地图，给定原点，这样可以计算投射线
        tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) );
    }

    // 更新中间节点的占据信息并写入磁盘
    tree.updateInnerOccupancy();
    cout<<"saving octomap ... "<<endl;
    tree.writeBinary( "octomap.bt" );
    return 0;
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Demos/Simulation/SimulationModel.h"
#include "Demos/Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Demos/Utils/Utilities.h"
#include "Demos/Simulation/DistanceFieldCollisionDetection.h"
#include "Demos/Utils/OBJLoader.h"
#include "Demos/Utils/Timing.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createMesh();
void render ();
void cleanup();
void reset();
void initShader();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setPoissonRatio(const void *value, void *clientData);
void TW_CALL getPoissonRatio(void *value, void *clientData);
void TW_CALL setNormalizeStretch(const void *value, void *clientData);
void TW_CALL getNormalizeStretch(void *value, void *clientData);
void TW_CALL setNormalizeShear(const void *value, void *clientData);
void TW_CALL getNormalizeShear(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setMaxIterations(const void *value, void *clientData);
void TW_CALL getMaxIterations(void *value, void *clientData);
void TW_CALL setMaxIterationsV(const void *value, void *clientData);
void TW_CALL getMaxIterationsV(void *value, void *clientData);
void TW_CALL setContactTolerance(const void *value, void *clientData);
void TW_CALL getContactTolerance(void *value, void *clientData);
void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData);
void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData);

SimulationModel model;
DistanceFieldCollisionDetection cd;
TimeStepController sim;

const unsigned int width = 30;
const unsigned int depth = 5;
const unsigned int height = 5;
short simulationMethod = 2;
bool doPause = false;
bool renderContacts = false;
std::vector<unsigned int> selectedParticles;
Vector3r oldMousePos;
Shader *shader;
string exePath;
string dataPath;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	exePath = Utilities::getFilePath(argv[0]);
	dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Bar collision demo");
	MiniGL::initLights ();
	MiniGL::initTexture();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);
	initShader();

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarRW(MiniGL::getTweakBar(), "RenderContacts", TW_TYPE_BOOLCPP, &renderContacts, " label='Render contacts' group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");
	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
			" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, &model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "PoissonRatio", TW_TYPE_REAL, setPoissonRatio, getPoissonRatio, &model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, &model, " label='Normalize stretch' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, &model, " label='Normalize shear' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "MaxIter", TW_TYPE_UINT32, setMaxIterations, getMaxIterations, &sim, " label='Max. iterations'  min=1 step=1 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "MaxIterV", TW_TYPE_UINT32, setMaxIterationsV, getMaxIterationsV, &sim, " label='Max. iterations Vel.'  min=1 step=1 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance'  min=0.0 step=0.001 precision=3 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, &model, " label='Contact stiffness RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, &model, " label='Contact stiffness Particle-RB'  min=0.0 step=0.1 precision=2 group=Simulation ");

	glutMainLoop ();

	cleanup ();

	Timing::printAverageTimes();

	return 0;
}

void initShader()
{
	std::string vertFile = dataPath + "/shaders/vs_smooth.glsl";
	std::string fragFile = dataPath + "/shaders/fs_smooth.glsl";
	shader = MiniGL::createShader(vertFile, "", fragFile);

	if (shader == NULL)
		return;

	shader->begin();
	shader->addUniform("modelview_matrix");
	shader->addUniform("projection_matrix");
	shader->addUniform("surface_color");
	shader->addUniform("shininess");
	shader->addUniform("specular_factor");
	shader->end();
}

void cleanup()
{
	delete TimeManager::getCurrent();
	delete shader;
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();
	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);

	model.cleanup();
	buildModel();
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff/h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	if (selectedParticles.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
		sim.step(model);

	for (unsigned int i = 0; i < model.getTetModels().size(); i++)
		model.getTetModels()[i]->updateMeshNormals(model.getParticles());
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (0.005);

	createMesh();

	// create static rigid body
	string fileName = Utilities::normalizePath(dataPath + "/models/cube.obj");
	IndexedFaceMesh mesh;
	VertexData vd;
	OBJLoader::loadObj(fileName, vd, mesh);

	string fileNameTorus = Utilities::normalizePath(dataPath + "/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	OBJLoader::loadObj(fileNameTorus, vdTorus, meshTorus);

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	rb.resize(2);

	// floor
	rb[0] = new RigidBody();
	rb[0]->initBody(1.0,
		Vector3r(0.0, -5.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh,
		Vector3r(100.0, 1.0, 100.0));
	rb[0]->setMass(0.0);

	// torus
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(5.0, -1.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdTorus, meshTorus,
		Vector3r(3.0, 3.0, 3.0));
	rb[1]->setMass(0.0);
	rb[1]->setFrictionCoeff(0.1);

	sim.setCollisionDetection(model, &cd);
	cd.setTolerance(0.05);

	const std::vector<Vector3r> *vertices1 = rb[0]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert1 = static_cast<unsigned int>(vertices1->size());
	cd.addCollisionBox(0, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices1)[0], nVert1, Vector3r(100.0, 1.0, 100.0));

	const std::vector<Vector3r> *vertices2 = rb[1]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert2 = static_cast<unsigned int>(vertices2->size());
	cd.addCollisionTorus(1, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices2)[0], nVert2, Vector2r(3.0, 1.5));

	SimulationModel::TetModelVector &tm = model.getTetModels();
	ParticleData &pd = model.getParticles();
	for (unsigned int i = 0; i < tm.size(); i++)
	{
		const unsigned int nVert = tm[i]->getParticleMesh().numVertices();
		unsigned int offset = tm[i]->getIndexOffset();
		tm[i]->setFrictionCoeff(0.1);
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert);
	}
}

void renderModels()
{
	// Draw simulation model

	const ParticleData &pd = model.getParticles();
	float surfaceColor[4] = { 0.2f, 0.5f, 1.0f, 1 };

	if (shader)
	{
		shader->begin();
		glUniform3fv(shader->getUniform("surface_color"), 1, surfaceColor);
		glUniform1f(shader->getUniform("shininess"), 5.0f);
		glUniform1f(shader->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shader->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shader->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (unsigned int i = 0; i < model.getTetModels().size(); i++)
	{
		TetModel *tetModel = model.getTetModels()[i];
		const IndexedFaceMesh &surfaceMesh = tetModel->getSurfaceMesh();
		Visualization::drawMesh(pd, surfaceMesh, tetModel->getIndexOffset(), surfaceColor);
	}

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	float rbColor[4] = { 0.4f, 0.4f, 0.4f, 1 };
	for (size_t i = 0; i < rb.size(); i++)
	{
		const VertexData &vd = rb[i]->getGeometry().getVertexData();
		const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
		if (shader)
			glUniform3fv(shader->getUniform("surface_color"), 1, rbColor);
		Visualization::drawTexturedMesh(vd, mesh, 0, rbColor);
	}

	if (shader)
		shader->end();
}

void renderContact(ParticleRigidBodyContactConstraint &cc)
{
	float col1[4] = { 0.0f, 0.6f, 0.2f, 1 };
	float col2[4] = { 0.6f, 0.0f, 0.2f, 1 };
	MiniGL::drawPoint(cc.m_constraintInfo.col(0), 5.0f, col1);
	MiniGL::drawPoint(cc.m_constraintInfo.col(1), 5.0f, col2);
	MiniGL::drawVector(cc.m_constraintInfo.col(1), cc.m_constraintInfo.col(1) + cc.m_constraintInfo.col(2), 1.0f, col2);
}

void render ()
{
	MiniGL::coordinateSystem();

	SimulationModel::ParticleRigidBodyContactConstraintVector &contacts = model.getParticleRigidBodyContactConstraints();

	// Draw simulation model
	renderModels();

	float red[4] = { 0.8f, 0.0f, 0.0f, 1 };
	const ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.08f, red);
	}

	if (renderContacts)
	{
		for (unsigned int i = 0; i < contacts.size(); i++)
			renderContact(contacts[i]);
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

void createMesh()
{
	Vector3r points[width*height*depth];
	for (unsigned int i = 0; i < width; i++)
	{
		for (unsigned int j = 0; j < height; j++)
		{
			for (unsigned int k = 0; k < depth; k++)
			{
				points[i*height*depth + j*depth + k] = 0.3*Vector3r((Real)i, (Real)j + 3.0, (Real)k);
			}
		}
	}

	vector<unsigned int> indices;
	for (unsigned int i = 0; i < width - 1; i++)
	{
		for (unsigned int j = 0; j < height - 1; j++)
		{
			for (unsigned int k = 0; k < depth - 1; k++)
			{
				// For each block, the 8 corners are numerated as:
				//     4*-----*7
				//     /|    /|
				//    / |   / |
				//  5*-----*6 |
				//   | 0*--|--*3
				//   | /   | /
				//   |/    |/
				//  1*-----*2
				unsigned int p0 = i*height*depth + j*depth + k;
				unsigned int p1 = p0 + 1;
				unsigned int p3 = (i + 1)*height*depth + j*depth + k;
				unsigned int p2 = p3 + 1;
				unsigned int p7 = (i + 1)*height*depth + (j + 1)*depth + k;
				unsigned int p6 = p7 + 1;
				unsigned int p4 = i*height*depth + (j + 1)*depth + k;
				unsigned int p5 = p4 + 1;

				// Ensure that neighboring tetras are sharing faces
				if ((i + j + k) % 2 == 1)
				{
					indices.push_back(p2); indices.push_back(p1); indices.push_back(p6); indices.push_back(p3);
					indices.push_back(p6); indices.push_back(p3); indices.push_back(p4); indices.push_back(p7);
					indices.push_back(p4); indices.push_back(p1); indices.push_back(p6); indices.push_back(p5);
					indices.push_back(p3); indices.push_back(p1); indices.push_back(p4); indices.push_back(p0);
					indices.push_back(p6); indices.push_back(p1); indices.push_back(p4); indices.push_back(p3);
				}
				else
				{
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p5); indices.push_back(p1);
					indices.push_back(p7); indices.push_back(p2); indices.push_back(p0); indices.push_back(p3);
					indices.push_back(p5); indices.push_back(p2); indices.push_back(p7); indices.push_back(p6);
					indices.push_back(p7); indices.push_back(p0); indices.push_back(p5); indices.push_back(p4);
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p7); indices.push_back(p5);
				}
			}
		}
	}
	model.addTetModel(width*height*depth, (unsigned int)indices.size() / 4u, points, indices.data());

	ParticleData &pd = model.getParticles();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}

	// init constraints
	for (unsigned int cm = 0; cm < model.getTetModels().size(); cm++)
	{
		const unsigned int nTets = model.getTetModels()[cm]->getParticleMesh().numTets();
		const unsigned int *tets = model.getTetModels()[cm]->getParticleMesh().getTets().data();
		const IndexedTetMesh::VertexTets *vTets = model.getTetModels()[cm]->getParticleMesh().getVertexTets().data();
		if (simulationMethod == 1)
		{
			const unsigned int offset = model.getTetModels()[cm]->getIndexOffset();
			const unsigned int nEdges = model.getTetModels()[cm]->getParticleMesh().numEdges();
			const IndexedTetMesh::Edge *edges = model.getTetModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model.addDistanceConstraint(v1, v2);
			}

			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model.addVolumeConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 2)
		{
			TetModel::ParticleMesh &mesh = model.getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model.addFEMTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 3)
		{
			TetModel::ParticleMesh &mesh = model.getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model.addStrainTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 4)
		{
			TetModel::ParticleMesh &mesh = model.getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v[4] = { tets[4 * i], tets[4 * i + 1], tets[4 * i + 2], tets[4 * i + 3] };
				// Important: Divide position correction by the number of clusters
				// which contain the vertex.
				const unsigned int nc[4] = { vTets[v[0]].m_numTets, vTets[v[1]].m_numTets, vTets[v[2]].m_numTets, vTets[v[3]].m_numTets };
				model.addShapeMatchingConstraint(4, v, nc);
			}
		}
		model.getTetModels()[cm]->updateMeshNormals(pd);
	}

	std::cout << "Number of tets: " << indices.size() / 4 << "\n";
	std::cout << "Number of vertices: " << width*height*depth << "\n";

}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*) clientData)->setSolidStiffness(val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getSolidStiffness();
}

void TW_CALL setPoissonRatio(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setSolidPoissonRatio(val);
}

void TW_CALL getPoissonRatio(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getSolidPoissonRatio();
}

void TW_CALL setNormalizeStretch(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setSolidNormalizeStretch(val);
}

void TW_CALL getNormalizeStretch(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getSolidNormalizeStretch();
}

void TW_CALL setNormalizeShear(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setSolidNormalizeShear(val);
}

void TW_CALL getNormalizeShear(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getSolidNormalizeShear();
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}

void TW_CALL setMaxIterations(const void *value, void *clientData)
{
	const unsigned int val = *(const unsigned int *)(value);
	((TimeStepController*)clientData)->setMaxIterations(val);
}

void TW_CALL getMaxIterations(void *value, void *clientData)
{
	*(unsigned int *)(value) = ((TimeStepController*)clientData)->getMaxIterations();
}

void TW_CALL setMaxIterationsV(const void *value, void *clientData)
{
	const unsigned int val = *(const unsigned int *)(value);
	((TimeStepController*)clientData)->setMaxIterationsV(val);
}

void TW_CALL getMaxIterationsV(void *value, void *clientData)
{
	*(unsigned int *)(value) = ((TimeStepController*)clientData)->getMaxIterationsV();
}

void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessRigidBody(val);
}

void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessRigidBody();
}

void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessParticleRigidBody(val);
}

void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessParticleRigidBody();
}

void TW_CALL setContactTolerance(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((DistanceFieldCollisionDetection*)clientData)->setTolerance(val);
}

void TW_CALL getContactTolerance(void *value, void *clientData)
{
	*(Real *)(value) = ((DistanceFieldCollisionDetection*)clientData)->getTolerance();
}


//=======================================================================
// Copyright (c) 2014 Andrzej Pacuk
//
// 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)
//=======================================================================
/**
 * @file lsh_nearest_neighbors_regression_efficient_example.cpp
 * @brief
 * @author Andrzej Pacuk
 * @version 1.0
 * @date 2014-10-08
 */

//! [LSH Nearest Neighbors Regression Efficient Example]
#include "paal/regression/lsh_nearest_neighbors_regression.hpp"
#include "paal/utils/functors.hpp"

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <iostream>
#include <iterator>
#include <utility>
#include <vector>

int main() {
    using coordinate_t = int;
    using result_t = double;
    using point_coordinates_t = std::vector<coordinate_t>;
    using training_point_t = std::tuple<point_coordinates_t, result_t>;

    constexpr paal::utils::tuple_get<0> get_coordinates{};
    constexpr paal::utils::tuple_get<1> get_result{};

    const std::vector<training_point_t> training_points = {
            training_point_t{{0, 0}, 0.0}, training_point_t{{0, 1}, 0.4},
            training_point_t{{1, 0}, 0.6}, training_point_t{{1, 1}, 1.0}};
    const std::vector<point_coordinates_t> test_points =
            {{0, -1}, {2, 1}};

    auto const passes = 50;
    auto const dimensions = std::get<0>(training_points.front()).size();
    auto const hash_functions_per_point = 1;

    auto model = paal::make_lsh_nearest_neighbors_regression_tuple_hash(
            training_points | boost::adaptors::transformed(get_coordinates),
            training_points | boost::adaptors::transformed(get_result),
            passes,
            paal::lsh::hamming_hash_function_generator{dimensions},
            hash_functions_per_point);

    std::vector<result_t> results;
    results.reserve(test_points.size());
    model.test(test_points, std::back_inserter(results));

    std::cout << "Solution:" << std::endl;
    boost::copy(results, std::ostream_iterator<double>(std::cout, ","));
    std::cout << std::endl;

    return 0;
}
//! [LSH Nearest Neighbors Regression Efficient Example]


/*
  This is used for showing topic when SVD is used for LSA
 */
#include <frovedis.hpp>
#include <frovedis/matrix/colmajor_matrix.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace std;

void show_topic(const string& input, const string& dictionary, int num) {
  ifstream dicstr(dictionary.c_str());
  string line;
  vector<string> dic;
  while(getline(dicstr, line)) {
    dic.push_back(line);
  }
  auto matrix = frovedis::colmajor_matrix_local<double>
    (frovedis::make_rowmajor_matrix_local_load<double>(input));
  vector<vector<pair<string,double>>> topic(matrix.local_num_col);
  for(size_t c = 0; c < matrix.local_num_col; c++) {
    vector<pair<double,int>> v;
    for(size_t r = 0; r < matrix.local_num_row; r++) {
      v.push_back(make_pair(abs(matrix.val[matrix.local_num_row * c + r]), r));
    }
    sort(v.begin(), v.end(), greater<pair<double,int>>());
    for(auto it = v.begin(); it != v.end(); ++it) {
      topic[c].push_back(make_pair(dic[it->second], it->first));
    }
  }
  for(size_t t = 0; t < topic.size(); t++) {
    cout << "topic " << t << ": ";
    for(int i = 0; i < num; i++) {
      cout << topic[t][i].first << "(" << topic[t][i].second << ") ";
    }
    cout << endl;
  }
}

int main(int argc, char* argv[]){
  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("input,i", value<string>(), "input vector file")
    ("dictionary,d", value<string>(), "dictionary file")
    ("num,n", value<int>(), "number of words per topic to show");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  string input, dictionary;
  int num;

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input file is not specified" << endl;
    exit(1);
  }

  if(argmap.count("dictionary")){
    dictionary = argmap["dictionary"].as<string>();
  } else {
    cerr << "dictionary file is not specified" << endl;
    exit(1);
  }

  if(argmap.count("num")){
    num = argmap["num"].as<int>();
  } else {
    num = 10;
  }

  show_topic(input, dictionary, num);
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

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

#include "theia/vision/transformation/align_point_clouds.h"
#include "theia/test/benchmark.h"

namespace theia {
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::RowMajor;
using Eigen::Vector3d;

namespace {
double kEpsilon = 1e-6;
}  // namespace

TEST(AlignPointCloudsICP, SimpleTransform) {
  // Make the model a 3x3 grid.
  Matrix<double, 9, 3, RowMajor> left_mat;
  left_mat << 0.4, -3.105, 2.147,
      1.293, 7.1982, -.068,
      -5.34, 0.708, -3.69,
      -.345, 1.987, 0.936,
      0.93, 1.45, 1.079,
      -3.15, -4.73, 2.49,
      2.401, -2.03, -1.87,
      3.192, -.573, 0.1,
      -2.53, 3.07, -5.19;

  // Reference the matrix as a 2d array.
  double* left = left_mat.data();

  // Shift and rotate the points.
  Matrix3d rotation_mat;
  rotation_mat << 0, 0, 1.0,
      -1.0, 0, 0,
      0, -1.0, 0;
  Vector3d translation_vec(0, 2, 2);

  // Rotate the left points.
  Matrix<double, 3, 9> left_mat_transformed =
      rotation_mat*(left_mat.transpose());
  // Translate the left points.
  for (int i = 0; i < 9; i++)
    left_mat_transformed.col(i) += translation_vec;

  // Create a 2d accessor for the transformed points.
  double* right = left_mat_transformed.data();

  double rotation[3 * 3];
  double translation[3];

  // Map the left points to the right points
  AlignPointCloudsICP(9, left, right, rotation, translation);

  // Ensure the calculated transformation is the same as the one we set.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      ASSERT_LT(std::abs(rotation[i * 3 + j] - rotation_mat(i, j)), kEpsilon);
    }
    ASSERT_LT(std::abs(translation[i] - translation_vec(i)), kEpsilon);
  }
}

void UmeyamaSimpleTest() {
  // Make the model a 3x3 grid.
  Matrix<double, 9, 3, RowMajor> left_mat;
  left_mat << 0.4, -3.105, 2.147,
      1.293, 7.1982, -.068,
      -5.34, 0.708, -3.69,
      -.345, 1.987, 0.936,
      0.93, 1.45, 1.079,
      -3.15, -4.73, 2.49,
      2.401, -2.03, -1.87,
      3.192, -.573, 0.1,
      -2.53, 3.07, -5.19;

  // Reference the matrix as a 2d array.
  double* left = left_mat.data();

  // Shift and rotate the points.
  Matrix3d rotation_mat;
  rotation_mat << 0, 0, 1.0,
      -1.0, 0, 0,
      0, -1.0, 0;
  Vector3d translation_vec(0, 2, 2);
  double expected_scale = 1.5;

  // Rotate the left points.
  Matrix<double, 3, 9> left_mat_transformed =
      expected_scale * rotation_mat * (left_mat.transpose());
  // Translate the left points.
  for (int i = 0; i < 9; i++)
    left_mat_transformed.col(i) += translation_vec;

  // Create a 2d accessor for the transformed points.
  double* right = left_mat_transformed.data();

  double rotation[3 * 3];
  double translation[3];
  double scale;

  // Map the left points to the right points
  AlignPointCloudsUmeyama(9, left, right, rotation, translation, &scale);

  // Ensure the calculated transformation is the same as the one we set.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      ASSERT_LT(std::abs(rotation[i * 3 + j] - rotation_mat(i, j)), kEpsilon);
    }
    ASSERT_LT(std::abs(translation[i] - translation_vec(i)), kEpsilon);
  }
  ASSERT_LT(fabs(expected_scale - scale), kEpsilon);
}

TEST(AlignPointCloudsUmeyama, SimpleTransform) {
  UmeyamaSimpleTest();
}

BENCHMARK(AlignPointCloudsUmeyama, benchmark, 100, 1000) {
  UmeyamaSimpleTest();
}

}  // namespace theia