// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSION_TREE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSION_TREE_HPP

#include <cstddef>
#include <type_traits>

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/mp11/list.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

enum class operator_types {
    sum, difference, product, abs, no_op, max, min
};

enum class operator_arities { nullary, unary, binary };

constexpr int sign_uncertain = -2;

struct sum_error_type {};
struct product_error_type {};
struct no_error_type {};

template <typename... Children>
struct internal_node
{
    static constexpr bool is_leaf = false;
    static constexpr bool non_negative = false;
    using all_children = boost::mp11::mp_list<Children...>; //for convenience
};

template <typename Left, typename Right>
struct internal_binary_node : internal_node<Left, Right>
{
    using left  = Left;
    using right = Right;
    static constexpr operator_arities operator_arity = operator_arities::binary;
};

template <typename Child>
struct internal_unary_node : internal_node<Child>
{
    using child = Child;
    static constexpr operator_arities operator_arity = operator_arities::unary;
};

template <typename Left, typename Right>
struct sum : public internal_binary_node<Left, Right>
{
    static constexpr bool sign_exact = Left::is_leaf && Right::is_leaf;
    static constexpr bool non_negative = Left::non_negative && Right::non_negative;
    static constexpr operator_types operator_type = operator_types::sum;
    using error_type = sum_error_type;
};

template <typename Left, typename Right>
struct difference : public internal_binary_node<Left, Right>
{
    static constexpr bool sign_exact = Left::is_leaf && Right::is_leaf;
    static constexpr bool non_negative = false;
    static constexpr operator_types operator_type = operator_types::difference;
    using error_type = sum_error_type;
};

template <typename Left, typename Right>
struct product : public internal_binary_node<Left, Right>
{
    static constexpr bool sign_exact = Left::sign_exact && Right::sign_exact;
    static constexpr bool non_negative =
           (Left::non_negative && Right::non_negative)
        || std::is_same<Left, Right>::value;
    static constexpr operator_types operator_type = operator_types::product;
    using error_type = product_error_type;
};

template <typename Left, typename Right>
struct max : public internal_binary_node<Left, Right>
{
    static constexpr bool sign_exact = Left::sign_exact && Right::sign_exact;
    static constexpr bool non_negative =
        Left::non_negative || Right::non_negative;
    static constexpr operator_types operator_type = operator_types::max;
    using error_type = no_error_type;
};

template <typename Left, typename Right>
struct min : public internal_binary_node<Left, Right>
{
    static constexpr bool sign_exact = Left::sign_exact && Right::sign_exact;
    static constexpr bool non_negative =
        Left::non_negative && Right::non_negative;
    static constexpr operator_types operator_type = operator_types::min;
    using error_type = no_error_type;
};

template <typename Child>
struct abs : public internal_unary_node<Child>
{
    using error_type = no_error_type;
    static constexpr operator_types operator_type = operator_types::abs;
    static constexpr bool sign_exact = Child::sign_exact;
    static constexpr bool non_negative = true;
};

struct leaf
{
    static constexpr bool is_leaf = true;
    static constexpr bool sign_exact = true;
    static constexpr bool non_negative = false;
    static constexpr operator_types operator_type = operator_types::no_op;
    static constexpr operator_arities operator_arity = operator_arities::nullary;
};

template <std::size_t Argn>
struct argument : public leaf
{
    static constexpr std::size_t argn = Argn;
};

template <typename NumberType>
struct static_constant_interface : public leaf
{
    using value_type = NumberType;
    static constexpr NumberType value = 0; //override
    static constexpr std::size_t argn = 0;
};

template <typename Node>
using is_leaf = boost::mp11::mp_bool<Node::is_leaf>;

template
<
    typename In,
    typename Out,
    template <typename> class Anchor = is_leaf,
    bool IsBinary = In::operator_arity == operator_arities::binary,
    bool AtAnchor = Anchor<In>::value
>
struct post_order_impl;

template
<
    typename In,
    typename Out,
    template <typename> class Anchor,
    bool IsBinary
>
struct post_order_impl<In, Out, Anchor, IsBinary, true>
{
    using type = Out;
};

template <typename In, typename Out, template <typename> class Anchor>
struct post_order_impl<In, Out, Anchor, true, false>
{
    using leftl = typename post_order_impl
            <
                typename In::left,
                boost::mp11::mp_list<>,
                Anchor
            >::type;
    using rightl = typename post_order_impl
            <
                typename In::right,
                boost::mp11::mp_list<>,
                Anchor
            >::type;
    using merged = boost::mp11::mp_append<Out, leftl, rightl>;
    using type   =
        boost::mp11::mp_unique<boost::mp11::mp_push_back<merged, In>>;
};

template <typename In, typename Out, template <typename> class Anchor>
struct post_order_impl<In, Out, Anchor, false, false>
{
    using childl = typename post_order_impl
            <
                typename In::child,
                boost::mp11::mp_list<>,
                Anchor
            >::type;
    using merged = boost::mp11::mp_append<Out, childl>;
    using type   =
        boost::mp11::mp_unique<boost::mp11::mp_push_back<merged, In>>;
};

template <typename In, template <typename> class Anchor = is_leaf>
using post_order =
    typename post_order_impl<In, boost::mp11::mp_list<>, Anchor>::type;

template <typename Node, typename IsLeaf = is_leaf<Node>>
struct max_argn_impl;

template <typename Node> using max_argn = typename max_argn_impl<Node>::type;

template <typename Node>
struct max_argn_impl<Node, boost::mp11::mp_false>
{
private:
    using children_list = boost::mp11::mp_rename<Node, boost::mp11::mp_list>;
    using children_max_argn =
        boost::mp11::mp_transform<max_argn, children_list>;
public:
    using type = boost::mp11::mp_max_element
        <
            children_max_argn,
            boost::mp11::mp_less
        >;
};

template <typename Node>
struct max_argn_impl<Node, boost::mp11::mp_true>
{
    using type = boost::mp11::mp_size_t<Node::argn>;
};

using  _1 = argument<1>;
using  _2 = argument<2>;
using  _3 = argument<3>;
using  _4 = argument<4>;
using  _5 = argument<5>;
using  _6 = argument<6>;
using  _7 = argument<7>;
using  _8 = argument<8>;
using  _9 = argument<9>;
using _10 = argument<10>;
using _11 = argument<11>;
using _12 = argument<12>;

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSION_TREE_HPP


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::gamma::derivative::log_unnormalized_pdf //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <cmath>
#include <limits>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{
namespace math{

template <class T, class P>
inline T derivative_log_unnormalized_pdf(
    const boost::math::gamma_distribution<T, P>& dist,
    const T& x
)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function
        = "log_unnormalized_pdf(const gamma_distribution<%1%>&, %1%)";

   T shape = dist.shape();
   T scale = dist.scale();

   T result;
   if(false == boost::math::detail::check_gamma(
    function, scale, shape, &result, P()))
      return result;
   if(false == boost::math::detail::check_gamma_x(function, x, &result, P()))
      return result;

   if(x == 0)
   {
      // TODO check. Also see log_unnormalized_pdf
      return std::numeric_limits<T>::infinity();
   }
   static T one_ = static_cast<T>(1);
   result = (shape - one_) / x - one_ / scale;
   return result;
} // derivative_log_unnormalized_pdf

}// math
}// boost

#endif


#include <armadillo>
#include <boost/program_options.hpp>
#include <HSMM.hpp>
#include <Multivariate_Gaussian_emission.hpp>
#include <random>

using namespace arma;
using namespace hsmm;
using namespace robotics::random;
using namespace std;
namespace po = boost::program_options;

// MLE transition in the HMM case. This means self-transitions are allowed.
mat getHmmTransitionFromLabels(const field<ivec>& labels_seq, int nstates) {
    mat hmm_transition(nstates, nstates, fill::zeros);
    for(const ivec &s : labels_seq)
        for(int i = 0; i < s.n_elem - 1; i++)
            hmm_transition(s(i), s(i + 1))++;

    for(int i = 0; i < nstates; i++)
        hmm_transition.row(i) = hmm_transition.row(i) / accu(
                hmm_transition.row(i));
    return hmm_transition;
}

// MLE categorical distribution over labels for the IID case.
mat getMixtureModelTransitionFromLabels(const field<ivec>& labels_seq,
        int nstates) {
    mat transition(nstates, nstates, fill::zeros);
    for(const ivec &s : labels_seq)
        for(int i = 0; i < s.n_elem; i++)
            transition.col(s(i)) += 1;

    for(int i = 0; i < nstates; i++)
        transition.row(i) = transition.row(i) / accu(transition.row(i));
    return transition;
}

// Equivalent to np.fft.rfftfreq(512, 1.0 / 128.0) in Python.
vec discreteFourierTransformSampleFrequencies() {
    return linspace(0, 64, 257);
}

vec extract_frequency_features(vec input) {
    vec nu = discreteFourierTransformSampleFrequencies();
    uvec delta_band = find(nu > 0.4 && nu < 4.1);
    uvec theta_band = find(nu > 5.9 && nu < 10.1);
    uvec alpha_band = find(nu > 10 && nu < 15.1);
    uvec all_bands = find(nu > 3.9 && nu < 40.1);

    cx_mat t = fft(input);

    // Ensuring the same size as np.fft.rfft(input).
    t = t.head_rows(input.n_elem / 2 + 1);
    mat norm = abs(t);
    norm = norm % norm;
    vec features = {accu(norm.elem(delta_band)), accu(norm.elem(theta_band)),
            accu(norm.elem(alpha_band)), accu(norm.elem(all_bands))};
    return features;
}

vec extract_eeg_features(vec eeg) {
    return extract_frequency_features(eeg).head_rows(3);
}

vec extract_emg_features(vec emg) {
    return extract_frequency_features(emg).tail_rows(1);
}

field<vec> getFeatureVectors(const mat& eeg1, const mat& eeg2, const mat& emg) {
    int nobs = eeg1.n_cols;
    assert(nobs == eeg2.n_cols);
    assert(nobs == emg.n_cols);
    field<vec> features(nobs);
    for(int i = 0; i < nobs; i++) {
        vec f = join_vert(extract_eeg_features(eeg1.col(i)),
                extract_eeg_features(eeg2.col(i)));
        features(i) = join_vert(f, extract_emg_features(emg.col(i)));
    }
    return features;
}

ivec predict_labels_iid(shared_ptr<MultivariateGaussianEmission> e,
        const mat& test_input, const vec& class_prior) {
    assert(class_prior.n_elem == e->getNumberStates());
    ivec ret(test_input.n_cols);
    double seq_loglikelihood = 0;
    for(int i = 0; i < test_input.n_cols; i++) {
        vec loglikelihoods(e->getNumberStates());
        for(int j = 0; j < e->getNumberStates(); j++)
            loglikelihoods(j) = e->loglikelihood(j,
                    test_input.col(i)) + log(class_prior(j));
        ret(i) = (int) loglikelihoods.index_max();
        seq_loglikelihood += logsumexp(loglikelihoods);
    }
    cout << "IIDtestloglikelihood " << seq_loglikelihood << endl;
    return ret;
}

ivec predict_labels_from_filtering(const mat& filtering_state_marginals) {
    ivec ret(filtering_state_marginals.n_cols);
    for(int i = 0; i < ret.n_elem; i++)
        ret(i) = (int) filtering_state_marginals.col(i).index_max();
    return ret;
}

vector<NormalDist> get_normal_distributions(int nstates, int ndurations,
        int min_duration, int ndimension) {
    mt19937 gen(0);
    vector<NormalDist> states;
    vector<vec> samples;
    vector<int> labels;
    for(int i = 0; i < nstates; i++) {
        vec mean = ones<vec>(ndimension) * i * 10;
        mat cov = eye(ndimension, ndimension);
        NormalDist a(mean, cov);
        states.push_back(a);

        // Generating toy data.
        int nsamples = 100;
        vector<vec> s = sample_multivariate_normal(gen, a, nsamples);
        vector<int> l = conv_to<vector<int>>::from(ones<ivec>(nsamples) * i);
        samples.insert(samples.end(), s.begin(), s.end());
        labels.insert(labels.end(), l.begin(), l.end());
    }

    // Toy data handling.
    ivec labels_vec = conv_to<ivec>::from(labels);
    field<vec> obs_field(samples.size());
    for(int i = 0; i < samples.size(); i++)
        obs_field(i) = samples.at(i);

    // Debug
    shared_ptr<MultivariateGaussianEmission> emission(
            new MultivariateGaussianEmission(states));
    OnlineHSMMRunlengthBased model(emission, nstates, ndurations, min_duration);
    for(int i = 0; i < samples.size(); i++)
        model.addNewObservation(samples.at(i));
    return states;
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    vector<string> input_features, input_labels;
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<vector<string>>(&input_features)->multitoken(),
                "Path to the (multiple) input features")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("nstates", po::value<int>(), "Number of hidden states for the HSMM")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("labels,l", po::value<vector<string>>(&input_labels)->multitoken(),
                "Path to input labels")
        ("nodur", "Flag to deactivate the learning of durations")
        ("iid", "Flag to deactivate the learning of transitions")
        ("alphadurprior", po::value<double>(),
                "Alpha for Dirichlet prior for the duration")
        ("filteringprediction", po::value<string>(), "Path to predicted labels"
                " based on the filtering distribution over states")
        ("mr", po::value<string>(), "Runlength marginals output filename")
        ("ms", po::value<string>(), "States marginals output filename")
        ("ms2", po::value<string>(), "States marginals output filename. This "
                "one is based on the residual time posterior instead of the "
                "runlength posterior")
        ("md", po::value<string>(), "Duration marginals output filename")
        ("ml", po::value<string>(), "Remaining runlength marginals output"
                " filename")
        ("leaveoneout", po::value<int>(), "Index of the sequence that will be"
                " left out for validation")
        ("savefiletype", po::value<string>()->default_value("arma_binary"),
                "File type to save the matrices after inference");
    assert(input_features.size() == input_labels.size());
    vector<string> required_fields = {"input", "labels", "leaveoneout",
            "nstates", "mindur", "ndur"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    int nstates = vm["nstates"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    int leaveoneout = vm["leaveoneout"].as<int>();
    int min_duration = vm["mindur"].as<int>();
    int nseq = input_features.size();

    field<ivec> labels_seq(nseq - 1);
    field<mat> obs_seq(nseq - 1);
    ivec test_labels;
    mat test_obs;
    int train_idx = 0;
    for(int i = 0; i < nseq; i++) {
        if (i == leaveoneout) {
            test_obs.load(input_features[i], raw_ascii);
            test_labels.load(input_labels[i], raw_ascii);
        }
        else {
            labels_seq(train_idx).load(input_labels[i], raw_ascii);
            obs_seq(train_idx).load(input_features[i], raw_ascii);
            train_idx++;
        }
    }
    assert(train_idx == nseq - 1);
    int ndimension = test_obs.n_rows;

    // Creating normal distributions for the emission process.
    vector<NormalDist> states = get_normal_distributions(nstates, ndurations,
            min_duration, ndimension);

    // Creating the emission process.
    shared_ptr<MultivariateGaussianEmission> emission(
            new MultivariateGaussianEmission(states));

    // Training the emission based on the labels.
    emission->fitFromLabels(obs_seq, labels_seq);

    // Creating the online HSMM whose emission process doesnt take into account
    // the total segment duration. The pmfs are uniformly initialized.
    OnlineHSMMRunlengthBased model(emission, nstates, ndurations, min_duration);

    // Setting a Dirichlet prior over the durations.
    if (vm.count("alphadurprior")) {
        mat alphas = ones<mat>(nstates, ndurations) *
            vm["alphadurprior"].as<double>();
        model.setDurationDirichletPrior(alphas);
    }

    // Learning the HSMM parameters from the labels.
    if (!vm.count("iid")) {
        if (min_duration == 1 && ndurations == 1)
            model.setTransition(getHmmTransitionFromLabels(labels_seq,nstates));
        else
            model.setTransitionFromLabels(labels_seq);
    }
    else
        model.setTransition(getMixtureModelTransitionFromLabels(labels_seq,
                    nstates));
    if (!vm.count("nodur"))
        model.setDurationFromLabels(labels_seq);

    if (vm.count("output")) {
        ofstream output_params(vm["output"].as<string>());
        nlohmann::json current_params = model.to_stream();
        output_params << std::setw(4) << current_params << endl;
        output_params.close();
    }
    mat runlength_marginals;
    mat state_marginals, state_marginals_2;
    mat remaining_runlength_marginals;
    mat duration_marginals;

    if (vm.count("mr"))
        runlength_marginals = zeros<mat>(min_duration + ndurations - 1,
                test_obs.n_cols);
    if (vm.count("ms") || vm.count("filteringprediction"))
        state_marginals = zeros<mat>(nstates, test_obs.n_cols);
    if (vm.count("ms2"))
        state_marginals_2 = zeros<mat>(nstates, test_obs.n_cols);
    if (vm.count("md"))
        duration_marginals = zeros<mat>(ndurations, test_obs.n_cols);
    if (vm.count("ml"))
        remaining_runlength_marginals = zeros<mat>(
                min_duration + ndurations - 1, test_obs.n_cols);
    vec loglikelihoods(test_obs.n_cols);
    for(int i = 0; i < test_obs.n_cols; i++) {
        loglikelihoods(i) = model.oneStepAheadLoglikelihood(test_obs.col(i));
        model.addNewObservation(test_obs.col(i));
        if (vm.count("mr"))
            runlength_marginals.col(i) = model.getRunlengthMarginal();
        if (vm.count("ms") || vm.count("filteringprediction"))
            state_marginals.col(i) = model.getStateMarginal();
        if (vm.count("ms2"))
            state_marginals_2.col(i) = model.getStateMarginal2();
        if (vm.count("ml"))
            remaining_runlength_marginals.col(i) =
                    model.getResidualTimeMarginal();
    }

    // The test log-likelihood.
    cout << accu(loglikelihoods) << endl;

    // Saving the filtering inferences.
    auto file_type = vm["savefiletype"].as<string>().compare(
            "arma_binary") == 0 ? arma_binary : raw_ascii;
    if (vm.count("mr"))
        runlength_marginals.save(vm["mr"].as<string>(), file_type);
    if (vm.count("ms"))
        state_marginals.save(vm["ms"].as<string>(), file_type);
    if (vm.count("ms2"))
        state_marginals_2.save(vm["ms2"].as<string>(), file_type);
    if (vm.count("ml"))
        remaining_runlength_marginals.save(vm["ml"].as<string>(), file_type);
    if (vm.count("filteringprediction")) {
        ivec filtering_prediction = predict_labels_from_filtering(
                state_marginals);
        filtering_prediction.save(vm["filteringprediction"].as<string>(),
                raw_ascii);
    }
    return 0;
}


#include "util/coordinate_calculation.hpp"

#include "util/string_util.hpp"
#include "util/trigonometry_table.hpp"

#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include <cmath>

#include <limits>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

double haversineDistance(const int lat1, const int lon1, const int lat2, const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;
    const double dlat1 = lt1 * (RAD);

    const double dlong1 = ln1 * (RAD);
    const double dlat2 = lt2 * (RAD);
    const double dlong2 = ln2 * (RAD);

    const double dlong = dlong1 - dlong2;
    const double dlat = dlat1 - dlat2;

    const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
    return EARTH_RADIUS * charv;
}

double haversineDistance(const FixedPointCoordinate coordinate_1,
                         const FixedPointCoordinate coordinate_2)
{
    return haversineDistance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                             coordinate_2.lon);
}

double greatCircleDistance(const FixedPointCoordinate coordinate_1,
                           const FixedPointCoordinate coordinate_2)
{
    return greatCircleDistance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                               coordinate_2.lon);
}

double greatCircleDistance(const int lat1, const int lon1, const int lat2, const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const double float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const double float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const double float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const double float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
    const double y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * EARTH_RADIUS;
}

double perpendicularDistance(const FixedPointCoordinate source_coordinate,
                             const FixedPointCoordinate target_coordinate,
                             const FixedPointCoordinate query_location)
{
    double ratio;
    FixedPointCoordinate nearest_location;

    return perpendicularDistance(source_coordinate, target_coordinate, query_location,
                                 nearest_location, ratio);
}

double perpendicularDistance(const FixedPointCoordinate segment_source,
                             const FixedPointCoordinate segment_target,
                             const FixedPointCoordinate query_location,
                             FixedPointCoordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    return perpendicularDistanceFromProjectedCoordinate(
        segment_source, segment_target, query_location,
        {mercator::latToY(query_location.lat / COORDINATE_PRECISION),
         query_location.lon / COORDINATE_PRECISION},
        nearest_location, ratio);
}

double
perpendicularDistanceFromProjectedCoordinate(const FixedPointCoordinate source_coordinate,
                                             const FixedPointCoordinate target_coordinate,
                                             const FixedPointCoordinate query_location,
                                             const std::pair<double, double> projected_coordinate)
{
    double ratio;
    FixedPointCoordinate nearest_location;

    return perpendicularDistanceFromProjectedCoordinate(source_coordinate, target_coordinate,
                                                        query_location, projected_coordinate,
                                                        nearest_location, ratio);
}

double
perpendicularDistanceFromProjectedCoordinate(const FixedPointCoordinate segment_source,
                                             const FixedPointCoordinate segment_target,
                                             const FixedPointCoordinate query_location,
                                             const std::pair<double, double> projected_coordinate,
                                             FixedPointCoordinate &nearest_location,
                                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    // initialize values
    const double x = projected_coordinate.first;
    const double y = projected_coordinate.second;
    const double a = mercator::latToY(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = mercator::latToY(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, new_y;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.0 + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    new_y = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(new_y) < (1.0 / COORDINATE_PRECISION))
    {
        new_y = 0.0;
    }

    // compute ratio
    ratio = static_cast<double>((p - new_y * a) /
                                c); // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.0 : 0.0);
    }
    else if (std::abs(ratio) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 0.0;
    }
    else if (std::abs(ratio - 1.0) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 1.0;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.0)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.0)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(mercator::yToLat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.IsValid());

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}

double bearing(const FixedPointCoordinate first_coordinate,
               const FixedPointCoordinate second_coordinate)
{
    const double lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const double lon_delta = degToRad(lon_diff);
    const double lat1 = degToRad(first_coordinate.lat / COORDINATE_PRECISION);
    const double lat2 = degToRad(second_coordinate.lat / COORDINATE_PRECISION);
    const double y = std::sin(lon_delta) * std::cos(lat2);
    const double x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    double result = radToDeg(std::atan2(y, x));
    while (result < 0.0)
    {
        result += 360.0;
    }

    while (result >= 360.0)
    {
        result -= 360.0;
    }
    return result;
}

double computeAngle(const FixedPointCoordinate first,
                    const FixedPointCoordinate second,
                    const FixedPointCoordinate third)
{
    using namespace boost::math::constants;
    using namespace coordinate_calculation;

    const double v1x = (first.lon - second.lon) / COORDINATE_PRECISION;
    const double v1y = mercator::latToY(first.lat / COORDINATE_PRECISION) -
                       mercator::latToY(second.lat / COORDINATE_PRECISION);
    const double v2x = (third.lon - second.lon) / COORDINATE_PRECISION;
    const double v2y = mercator::latToY(third.lat / COORDINATE_PRECISION) -
                       mercator::latToY(second.lat / COORDINATE_PRECISION);

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. / pi<double>();

    while (angle < 0.)
    {
        angle += 360.;
    }

    return angle;
}

namespace mercator
{
double yToLat(const double value)
{
    using namespace boost::math::constants;

    return 180. * (1. / pi<long double>()) *
           (2. * std::atan(std::exp(value * pi<double>() / 180.)) - half_pi<double>());
}

double latToY(const double latitude)
{
    using namespace boost::math::constants;

    return 180. * (1. / pi<double>()) *
           std::log(std::tan((pi<double>() / 4.) + latitude * (pi<double>() / 180.) / 2.));
}
} // ns mercato // ns mercatorr
} // ns coordinate_calculation
} // ns util
} // ns osrm


#ifndef RLSS_DISCRETESEARCH_HPP
#define RLSS_DISCRETESEARCH_HPP

#include <rlss/internal/Util.hpp>
#include <rlss/OccupancyGrid.hpp>
#include <rlss/CollisionShapes/CollisionShape.hpp>
#include <libMultiRobotPlanning/a_star.hpp>
#include <boost/functional/hash/hash.hpp>
#include <optional>
#include <iostream>

namespace rlss {

namespace internal {

template<typename T, unsigned int DIM>
std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
            const typename OccupancyGrid<T, DIM>::Coordinate& start_coordinate,
            const typename OccupancyGrid<T, DIM>::Coordinate& goal_coordinate,
            const OccupancyGrid<T, DIM>& occupancy_grid,
            const AlignedBox<T, DIM>& workspace,
            std::shared_ptr<rlss::CollisionShape<T,DIM>> collision_shape
) {
    using VectorDIM = VectorDIM<T, DIM>;
    using OccupancyGrid = OccupancyGrid<T, DIM>;
    using AlignedBox = AlignedBox<T, DIM>;
    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;
    using Index = typename OccupancyGrid::Index;
    using Coordinate = typename OccupancyGrid::Coordinate;
    using CollisionShape = rlss::CollisionShape<T, DIM>;

    struct State {
        Coordinate position;
        Index dir;
        explicit State(Coordinate c) : position(c), dir(Index::Zero()) {}
        State(Coordinate c, Index d) : position(c), dir(d) {}
        bool operator==(const State& rhs) const {
            return position == rhs.position && dir == rhs.dir;
        }
    };

    struct StateHasher {
        std::size_t operator()(const State& s) const {
            std::size_t seed = 0;
            for(unsigned int d = 0; d < DIM; d++) {
                boost::hash_combine(seed, s.position(d));
                boost::hash_combine(seed, s.dir(d));
            }
            return seed;
        }
    };

    enum class Action {
        FORWARD,
        ROTATE,
        ROTATEFORWARD
    };

    class Environment {
    public:

        Environment(
            const OccupancyGrid& occ,
            const AlignedBox& works,
            std::shared_ptr<CollisionShape> cols,
            const Coordinate& goal
            )
            : m_occupancy_grid(occ),
              m_workspace(works),
              m_collision_shape(cols),
              m_goal(goal)
        {}

        // get distance of current state pos to goal pos
        int admissibleHeuristic(const State& s) {
            int h = 0;
            Index state_idx = m_occupancy_grid.getIndex(s.position);
            Index goal_idx = m_occupancy_grid.getIndex(m_goal);
            return (state_idx - goal_idx).norm();
//            for(unsigned int d = 0; d < DIM; d++) {
//                h += std::abs(state_idx(d) - goal_idx(d));
//            }
//            return h;
        }

        // check if current pos is the goal pos
        bool isSolution(const State& s) { return s.position == m_goal; }

        // get neighbouring voxels
        void getNeighbors(
            const State& s,
            std::vector<libMultiRobotPlanning::Neighbor<State, Action, int> >&
                neighbors) {

            neighbors.clear();

            Coordinate s_center = m_occupancy_grid.getCenter(s.position);
            Index s_idx = m_occupancy_grid.getIndex(s_center);

            if(this->actionValid(s.position, m_goal)) {
                neighbors.emplace_back(
                    State(m_goal, Index::Zero()),
                    Action::ROTATEFORWARD,
                    1 + (s_idx - m_occupancy_grid.getIndex(m_goal))
                                  .norm()
//                                .cwiseAbs().sum()
                );
            }

            if(s_center != s.position) { // get into grid
                if(this->actionValid(s.position, s_center)) {
                    neighbors.emplace_back(
                        State(s_center, Index::Zero()),
                        Action::ROTATEFORWARD,
                        2
                    );
                }

                std::vector<Index> neigh_indexes
                        = m_occupancy_grid.getNeighbors(s_idx);

                for(const auto& neigh_idx: neigh_indexes) {
                    auto neigh_center = m_occupancy_grid.getCenter(neigh_idx);
                    if(this->actionValid(s.position, neigh_center)) {
                        neighbors.emplace_back(
                                State(neigh_center, Index::Zero()),
                                Action::ROTATEFORWARD,
                                2
                        );
                    }
                }
            } else {
                if(s.dir == Index::Zero()) {
                    std::vector<Index> neigh_indexes
                            = m_occupancy_grid.getNeighbors(s_idx);
                    for(const auto& neigh_idx : neigh_indexes) {
                        if (this->actionValid(s_idx, neigh_idx)) {
                            neighbors.emplace_back(
                                State(
                                    m_occupancy_grid.getCenter(neigh_idx),
                                    neigh_idx - s_idx
                                ),
                                Action::FORWARD,
                                1
                            );
                        }
                    }
                } else {
                    Index idx = s_idx + s.dir;
                    if(this->actionValid(s_idx, idx)) {
                        neighbors.emplace_back(
                            State(m_occupancy_grid.getCenter(idx), s.dir),
                            Action::FORWARD,
                            1
                        );
                    }

                    for(unsigned int d = 0; d < DIM; d++) {
                        if(s.dir(d) == 0) {
                            Index dir = Index::Zero();
                            dir(d) = 1;
                            neighbors.emplace_back(
                                State(m_occupancy_grid.getCenter(s_idx), dir),
                                Action::ROTATE,
                                1
                            );
                            dir(d) = -1;
                            neighbors.emplace_back(
                                State(m_occupancy_grid.getCenter(s_idx), dir),
                                Action::ROTATE,
                                1
                            );
                        }
                    }
                }
            }

        }

        void onExpandNode(const State& /*s*/, int /*fScore*/, int /*gScore*/) {}

        void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {}

    public:

        bool actionValid(const Coordinate& start, const Coordinate& end) {
            return rlss::internal::segmentValid<T, DIM>(
                    m_occupancy_grid,
                    m_workspace,
                    start,
                    end,
                    m_collision_shape
            );
        }

        bool actionValid(const Index& from_idx, const Index& to_idx) {
            Coordinate from_center = m_occupancy_grid.getCenter(from_idx);
            Coordinate to_center = m_occupancy_grid.getCenter(to_idx);

            return this->actionValid(from_center, to_center);
        }

        bool positionValid(const Coordinate& pos) {
            AlignedBox robot_box = m_collision_shape->boundingBox(pos);
            return !m_occupancy_grid.isOccupied(robot_box)
                    && m_workspace.contains(robot_box);
        }

        bool indexValid(const Index& idx) {
            Coordinate center = m_occupancy_grid.getCenter(idx);
            return this->positionValid(center);
        }

    private:
        const OccupancyGrid& m_occupancy_grid;
        const AlignedBox& m_workspace;
        std::shared_ptr<CollisionShape> m_collision_shape;
        const Coordinate& m_goal;
    };

    Environment env(
            occupancy_grid,
            workspace,
            collision_shape,
            goal_coordinate
    );
    libMultiRobotPlanning::AStar<State, Action, int, Environment, StateHasher>
            astar(env);
    libMultiRobotPlanning::PlanResult<State, Action, int> solution;

    if(env.positionValid(start_coordinate)) {
        State start_state(start_coordinate);
        bool success = astar.search(start_state, solution);
        if(!success) {
            return std::nullopt;
        }

        StdVectorVectorDIM segments;
        segments.push_back(solution.states[0].first.position);
        for(std::size_t i = 0; i < solution.actions.size(); i++) {
            std::string action_text;
            if(solution.actions[i].first == Action::ROTATE) {
                action_text = "ROTATE";
            } else if(solution.actions[i].first == Action::FORWARD) {
                action_text = "FORWARD";
            } else if(solution.actions[i].first == Action::ROTATEFORWARD) {
                action_text = "ROTATEFORWARD";
            }
            debug_message(solution.states[i].first.position.transpose(),
                    ",", solution.states[i].first.dir.transpose(), " > ",
                    solution.states[i+1].first.position.transpose(), ",",
                    solution.states[i+1].first.dir.transpose(), ", action: ",
                      action_text);
            if(solution.actions[i].first == Action::ROTATE) {
                segments.push_back(solution.states[i+1].first.position);
            } else if (solution.actions[i].first == Action::ROTATEFORWARD) {
                if(i != 0
                    && segments.back() != solution.states[i].first.position)
                    segments.push_back(solution.states[i].first.position);
                if(i != solution.actions.size() - 1
                    && segments.back() != solution.states[i+1].first.position)
                    segments.push_back(solution.states[i+1].first.position);
            }
        }
        segments.push_back(solution.states.back().first.position);

        return segments;
    }

    return std::nullopt;
}

} // namespace internal
} // namespace rlss

#endif // RLSS_DISCRETESEARCH_HPP


/*--
    Solver.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    This program 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.

    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "Solver.h"
#include <Eigen/Cholesky>
#include <iostream> //TODO: TMP
#include <cmath>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

LSSolver::LSSolver(LSProblem *problem, const vector<LSBoxConstraint> &constraints)
: _problem(problem), _constraints(constraints), _damping(1.), _maxIter(100),
  _increaseDampingAfter(0), _dampingIncreaseFactor(1.)
{
};

VectorXd LSSolver::solve(const VectorXd &guess, bool *is_valid)
{
	if (is_valid != nullptr)
		*is_valid = true;

    VectorXd best;
    double bestError = 1e100;
    VectorXd x = guess;
    LSEvalData *evalData = _problem->createEvalData();

    set<LSBoxConstraint> activeSet = _clamp(x);

    VectorXd delta;
    int iter;
    for(iter = 0; iter < _maxIter; ++iter)
    {
        if(iter > _increaseDampingAfter)
            _damping *= _dampingIncreaseFactor;
        _problem->eval(x, evalData);

        double error = evalData->error();
        //printf("Iter = %d, error = %lf\n", iter, error);
        if(error < bestError)
        {
            bestError = error;
            best = x;

            if(error < 1e-10)
                break;
        }

        set<LSBoxConstraint> prevActiveSet = activeSet;
        evalData->solveForDelta(_damping, delta, activeSet);

		double delta_norm = delta.squaredNorm();
		if (isnan(delta_norm)) {
			if (is_valid != nullptr)
				*is_valid = false;
			break;
		}

		if(delta_norm < 1e-14)
            break;

        int newConstraint = _project(x, delta, prevActiveSet);

        if(newConstraint != -1)
            activeSet.insert(_constraints[newConstraint]);

        x += delta;

        int halvings = 0;
        while(_problem->error(x, evalData) > error && delta.squaredNorm() > 1e-8)
        {
            //printf("Halving\n");
            delta *= 0.5;
            x -= delta;
            ++halvings;
        }
        if(halvings > 0) //halve again -- won't hurt and may actually help
        {
            delta *= 0.5;
            x -= delta;
        }
    }

    double error = _problem->error(x, evalData);
    if(iter > 5)
        Debugging::get()->printf("After %d iterations, error = %lf", iter, sqrt(error));
    if(error < bestError)
    {
        best = x;
    }

    delete evalData;
    return best;
}

set<LSBoxConstraint> LSSolver::_clamp(VectorXd &x)
{
    set<LSBoxConstraint> out;
    for(int i = 0; i < (int)_constraints.size(); ++i)
    {
        const LSBoxConstraint &c = _constraints[i];
        if(c.sign == 0 || (x[c.index] - c.value) * c.sign < 0.)
        {
            x[c.index] = c.value;
            out.insert(c);
            //cout << "Clamping constraint " << i << endl;
        }
    }
    return out;
}

int LSSolver::_project(const VectorXd &from, VectorXd &delta, const set<LSBoxConstraint> &activeSet)
{
    int closestConstraint = -1;
    double minScale = 1.;

    for(int i = 0; i < (int)_constraints.size(); ++i)
    {
        const LSBoxConstraint &c = _constraints[i];

        if(c.sign == 0)
            delta[c.index] = 0; //just in case

        if(activeSet.count(c))
            continue; //already constrained

        double scale = (c.value - from[c.index]) / delta[c.index];

        if((from[c.index] + delta[c.index] - c.value) * c.sign >= 0.)
            continue;

        if(scale < minScale)
        {
            minScale = scale;
            closestConstraint = i;
        }
    }

    if(closestConstraint >= 0)
    {
        delta *= minScale;
        //cout << "Projecting up to constraint " << closestConstraint << " by " << minScale << endl;
    }

    return closestConstraint;
}

bool LSSolver::verifyDerivatives(const Eigen::VectorXd &pt, double eps) const
{
    LSEvalData *evalData = _problem->createEvalData();
    _problem->eval(pt, evalData);

    MatrixXd exactDer = evalData->errVecDer();
    MatrixXd numDer = exactDer;

    for(int i = 0; i < numDer.cols(); ++i)
    {
        VectorXd mod = pt;
        mod[i] += eps;

        _problem->eval(mod, evalData);
        VectorXd plus = evalData->errVec();

        mod[i] = pt[i] - eps;
        _problem->eval(mod, evalData);
        VectorXd minus = evalData->errVec();

        numDer.col(i) = (plus - minus) / (2 * eps);
    }

    double err = (numDer - exactDer).norm();

    //TODO: just print the error for now
    Debugging::get()->printf("Derivative Error = %lf", err);
#if 0
    for(int i = 0; i < numDer.cols(); ++i)
        Debugging::get()->printf("Col %d err = %lf", i, (numDer.col(i) - exactDer.col(i)).norm());
    for(int i = 0; i < numDer.rows(); ++i)
        Debugging::get()->printf("Row %d err = %lf", i, (numDer.row(i) - exactDer.row(i)).norm());
#endif
    delete evalData;

    return true;
}

void LSDenseEvalData::solveForDelta(double damping, VectorXd &out, set<LSBoxConstraint> &constraints)
{
    int vars = (int)_errDer.cols();
    if(constraints.empty())
    {
        LDLT<MatrixXd> ldlt(MatrixXd::Identity(vars, vars) * damping + _errDer.transpose() * _errDer);
        out = ldlt.solve(-_errDer.transpose() * _err);
    }
    else
    {
        VectorXd rhs = -_err;

        vector<bool> constraintIndices(vars, false);
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it)
            constraintIndices[it->index] = true;

        if(vars > (int)constraints.size())
        {
            MatrixXd lhs(_errDer.rows(), vars - (int)constraints.size());
            int offs = 0;
            for(int i = 0; i < vars; ++i)
            {
                if(constraintIndices[i])
                {
                    ++offs;
                    continue;
                }
                lhs.col(i - offs) = _errDer.col(i);
            }

            LDLT<MatrixXd> ldlt(MatrixXd::Identity(lhs.cols(), lhs.cols()) * damping + lhs.transpose() * lhs);
            VectorXd x = ldlt.solve(lhs.transpose() * rhs);

            out.resize(_errDer.cols());
            offs = 0;

            for(int i = 0; i < vars; ++i)
            {
                if(constraintIndices[i])
                {
                    out[i] = 0.;
                    ++offs;
                    continue;
                }
                out[i] = x[i - offs];
            }
        }
        else //as many variables as constraints
        {
            out = VectorXd::Zero(_errDer.cols());
        }

        //check which constraints we don't need
        VectorXd gradient = _errDer.transpose() * (_errDer * out - rhs);
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); )
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(gradient[it->index] * it->sign < 0) //if sign is zero, constraint will not get erased
            {
                //cout << "Unsetting constraint on variable at index " << it->index << endl;
                constraints.erase(it);
            }
            it = next;
        }
    }
}

END_NAMESPACE_Cornu


//
// Created by jianping on 17-9-18.
//

#include <iostream>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <fstream>
#include <Eigen/Geometry>

void saveXYZ(const char* filename, const cv::Mat& mat,float tx)
{
    const double max_z = 2000;
    FILE* fp = fopen(filename, "wt");
    for(int y = 0; y < mat.rows; y++)
    {
        for(int x = 0; x < mat.cols; x++)
        {
            cv::Vec3f point = mat.at<cv::Vec3f>(y, x);
            if(fabs(fabs(point[2]) - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
        }
    }
    fclose(fp);
}

void readParams(const std::string& intrinsicFile,
                Eigen::Matrix3d& K0,
                Eigen::Matrix3d& K1)
{
    std::ifstream ifs(intrinsicFile.c_str(),std::ios_base::in);
    char line[256];
    ifs.getline(line,256);
    sscanf(line,"cam0=[%lf %lf %lf; %lf %lf %lf; %lf %lf %lf]",
           &K0(0,0),&K0(0,1),&K0(0,2),&K0(1,0),&K0(1,1),&K0(1,2),&K0(2,0),&K0(2,1),&K0(2,2));
    std::cout<<"K0\n"<<K0<<"\n";

    ifs.getline(line,256);
    sscanf(line,"cam1=[%lf %lf %lf; %lf %lf %lf; %lf %lf %lf]",
           &K1(0,0),&K1(0,1),&K1(0,2),&K1(1,0),&K1(1,1),&K1(1,2),&K1(2,0),&K1(2,1),&K1(2,2));
    std::cout<<"K1\n"<<K1<<"\n";
}

const int scale = 1;
int main() {
    Eigen::Matrix3d K0;
    Eigen::Matrix3d K1;
    readParams("/home/jianping/workspace/datas/stereo/calib.txt",K0,K1);

    LOG(INFO)<<"matching"<<std::endl;

    cv::Mat image_ref_origin = cv::imread("/home/jianping/workspace/datas/stereo/im0.png");
    cv::Mat image_source_origin = cv::imread("/home/jianping/workspace/datas/stereo/im1.png");
    cv::StereoSGBM sgbm(0,640,
                        8,8*3*3*3,32*3*3*3,2,
                        16,5,100,
                        2,false
    );

    Eigen::Matrix3d C;
    C = Eigen::Matrix3d::Identity();
    Eigen::Vector3d r0,r1,r(100,0,0);

    cv::Mat Q;
    cv::Rect roi1, roi2;
    cv::Mat M0,M1,R01(3,3,CV_64FC1),t(3,1,CV_64FC1),R0__,R1__,P0__,P1__;
    cv::Mat D0(5,1,CV_64FC1),D1(5,1,CV_64FC1);
    D0.setTo(0);
    D1.setTo(0);
    cv::eigen2cv(K0,M0);
    cv::eigen2cv(K1,M1);
    cv::eigen2cv(C,R01);
    cv::eigen2cv(r,t);

    cv::Size size(image_ref_origin.cols, image_ref_origin.rows);
    cv::stereoRectify(M0,D0,M1,D1,
                      size,R01,t,
                      R0__,R1__,
                      P0__,P1__,
                      Q,cv::CALIB_FIX_INTRINSIC,-1,
                      size, &roi1, &roi2
    );

    cv::Mat disp,disp8,dispnorm(image_ref_origin.rows,image_ref_origin.cols,CV_64FC1);
    sgbm(image_ref_origin,image_source_origin,disp);

    cv::normalize(disp,dispnorm,255,0,cv::NORM_MINMAX);
    dispnorm.convertTo(disp8, CV_8U);
    cv::namedWindow("re",cv::WINDOW_NORMAL);
    cv::imshow("re",disp8);

    cv::namedWindow("ref",cv::WINDOW_NORMAL);
    cv::namedWindow("source",cv::WINDOW_NORMAL);

    for (int i = 0; i < image_ref_origin.rows; i += image_ref_origin.rows/20 ) {
        cv::line(image_ref_origin,cv::Point(0,i),cv::Point(image_ref_origin.cols,i),cv::Scalar(0,255,0),3);
        cv::line(image_source_origin,cv::Point(0,i),cv::Point(image_ref_origin.cols,i),cv::Scalar(0,255,0),3);

    }

    cv::imshow("ref",image_source_origin);
    cv::imshow("source",image_ref_origin);

    cv::Mat xyz,xyz8;
    reprojectImageTo3D(disp, xyz, Q, false);

    saveXYZ("./pts.xyz", xyz, r(0));
    cv::waitKey(0);
    return 0;
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/include/functions/aligned_load.hpp>
#include <boost/simd/include/functions/aligned_store.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct rgb2grey_simd
{
  template<typename Setup>
  rgb2grey_simd(Setup const& s)
                    :  height(boost::fusion::at_c<0>(s))
                    ,  width(boost::fusion::at_c<1>(s))
                    ,  size_(height*width)
  {
    gr.resize(size_);
    r.resize(size_);
    g.resize(size_);
    b.resize(size_);
    for(std::size_t i=0; i<size_; i++)
      r[i] = g[i] = b[i] = gr[i] = T(i);
  }

  void operator()()
  {
    using boost::simd::pack;

    typedef pack<T> type;
    std::size_t aligned_sz = size_ & ~(type::static_size-1);
    std::size_t it         = 0;

    type cr(0.3f), cg(0.59f),cb(0.11f);

    for(std::size_t m=aligned_sz; it != m; it+=type::static_size)
    {
      type vr(&r[it]), vg(&g[it]), vb(&b[it]);

      boost::simd::aligned_store( cr * vr + cg * vg + cb * vb, &gr[it] );
    }

    for(std::size_t m=size_; it != m; it++)
    {
      gr[it] = T(0.3) * r[it] + T(0.59) * g[it] + T(0.11) * b[it];
    }
  }

  friend std::ostream& operator<<(std::ostream& os, rgb2grey_simd<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

  private:
  std::size_t height, width, size_;
  std::vector<T,boost::simd::allocator<T> > r, g, b, gr;
};

NT2_REGISTER_BENCHMARK_TPL( rgb2grey_simd, (float) )
{
  std::size_t hmin  = args("hmin" ,  32   );
  std::size_t hmax  = args("hmax" , 128   );
  std::size_t hstep = args("hstep",   2   );
  std::size_t wmin  = args("wmin" , hmin  );
  std::size_t wmax  = args("wmax" , hmax  );
  std::size_t wstep = args("wstep", hstep );

  run_during_with< rgb2grey_simd<T> > ( 1.
                                        , and_( geometric(hmin,hmax,hstep)
                                              , geometric(wmin,wmax,wstep)
                                              )
                                        , cycles_per_element<stats::median_>()
                                        );
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <ql/experimental/volatility/stickybpvolswaptioncube.hpp>
#include <ql/experimental/volatility/multiplicativesmilesection.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/flatextrapolation2d.hpp>
#include <ql/quotes/simplequote.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

StickyBpVolSwaptionCube::StickyBpVolSwaptionCube(
    const boost::shared_ptr<SwaptionVolatilityCube> &sourceCube,
    Handle<Quote> atmVolatilitySpread)
    : SwaptionVolatilityCube(
          sourceCube->atmVol(), sourceCube->optionTenors(),
          sourceCube->swapTenors(), sourceCube->strikeSpreads(),
          sourceCube->volSpreads(), sourceCube->swapIndexBase(),
          sourceCube->shortSwapIndexBase(), sourceCube->vegaWeightedSmileFit()),
      sourceCube_(sourceCube),
      atmVolatilitySpread_(atmVolatilitySpread) {

    registerWith(sourceCube_);
    registerWith(atmVolatilitySpread_);

    boost::shared_ptr<SwaptionVolatilityDiscrete> atm =
        boost::dynamic_pointer_cast<SwaptionVolatilityDiscrete>(
            *sourceCube->atmVol());

    // this should never happen (?)
    QL_REQUIRE(atm != NULL, "atm is not of type SwaptionVolatilityDiscrete");

    atmLevel_ = Matrix(optionDates().size(), swapTenors().size());

    for (Size j = 0; j < atm->swapTenors().size(); ++j)
        for (Size i = 0; i < atm->optionDates().size(); ++i)
            atmLevel_[i][j] = sourceCube->atmStrike(atm->optionDates()[i],
                                                    atm->swapTenors()[j]);

    originalAtm_ = FlatExtrapolator2D(boost::make_shared<BilinearInterpolation>(
        atm->swapLengths().begin(), atm->swapLengths().end(),
        atm->optionTimes().begin(), atm->optionTimes().end(), atmLevel_));

    originalAtm_.enableExtrapolation();
}

boost::shared_ptr<SmileSection>
StickyBpVolSwaptionCube::smileSectionImpl(Time optionTime,
                                          Time swapLength) const {

    boost::shared_ptr<SmileSection> source =
        sourceCube_->smileSection(optionTime, swapLength, true);

    Real newAtm = source->atmLevel();

    QL_REQUIRE(newAtm != Null<Real>(),
               "source smile section does not provide atm level");

    Real mul = originalAtm_(optionTime, swapLength) / newAtm;
    Real add = 0.0;
    if(!atmVolatilitySpread_.empty()) {
        if(atmVolatilitySpread_->isValid()) {
            add = atmVolatilitySpread_->value() * mul;
        }
    }

    boost::shared_ptr<SmileSection> tmp =
        boost::make_shared<MultiplicativeSmileSection>(
            source, Handle<Quote>(boost::make_shared<SimpleQuote>(mul)),
            Handle<Quote>(boost::make_shared<SimpleQuote>(add)));
    return tmp;
}

Volatility StickyBpVolSwaptionCube::volatilityImpl(Time optionTime,
                                                   Time swapLength,
                                                   Rate strike) const {
    return smileSectionImpl(optionTime, swapLength)->volatility(strike);
}
}


#ifndef RNDCMP_INCLUDE_FIXED_HPP_
#define RNDCMP_INCLUDE_FIXED_HPP_

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

namespace rndcmp {
    template<typename INT_T, int FRACT_SIZE = 0, int POW = 2>
    class Fixed {
    public:
        Fixed() = default;

        /* constructors */

        template<typename T>
        Fixed(T v, std::enable_if_t<std::is_floating_point<T>::value, bool> = true) {
            setValueFromT(v);
        }

        template<typename T>
        Fixed(T v, std::enable_if_t<std::is_integral_v<T>, int> = 0) {
            setValueFromT(static_cast<double>(v));
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        operator T() const {
            return static_cast<T>(value) / (1 << FRACT_SIZE);
        }

        /* + operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed operator+(const T& rhs) const {
            return Fixed(T(*this) + rhs);
        }

        Fixed operator+(const Fixed& rhs) const {
            double val = static_cast<double>(*this) + static_cast<double>(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend Fixed operator+(T lhs, const Fixed& rhs) {
            T val = lhs + T(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed& operator+=(const T& rhs) {
            T sum = T(*this) + rhs;
            setValueFromT<T>(sum);
            return *this;
        }

        Fixed& operator+=(const Fixed& rhs) {
            double sum = static_cast<double>(*this) + static_cast<double>(rhs);
            setValueFromT(sum);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed operator+(const T& rhs) const {
            return Fixed(double(*this) + double(rhs));
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend Fixed operator+(T lhs, const Fixed& rhs) {
            double val = double(lhs) + double(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed& operator+=(const T& rhs) {
            double val = double(*this) + double(rhs);
            setValueFromT<double>(val);
            return *this;
        }

        /* - operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed operator-(const T& rhs) const {
            return Fixed(T(*this) - rhs);
        }

        Fixed operator-(const Fixed& rhs) const {
            double val = static_cast<double>(*this) - static_cast<double>(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed& operator-=(const T& rhs) {
            T sub = T(*this) - rhs;
            setValueFromT<T>(sub);
            return *this;
        }

        Fixed& operator-=(const Fixed& rhs) {
            double val = static_cast<double>(*this) - static_cast<double>(rhs);
            setValueFromT(val);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend Fixed operator-(T lhs, const Fixed& rhs) {
            T val = lhs - T(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed operator-(const T& rhs) const {
            return Fixed(double(*this) - double(rhs));
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend Fixed operator-(T lhs, const Fixed& rhs) {
            double val = double(lhs) - double(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed& operator-=(const T& rhs) {
            double val = double(*this) - double(rhs);
            setValueFromT<double>(val);
            return *this;
        }

        /* multiply operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed operator*(const T& rhs) const {
            return Fixed(T(*this) * rhs);
        }

        Fixed operator*(const Fixed& rhs) const {
            return Fixed(double(*this) * double(rhs));
        }

        Fixed& operator*=(const Fixed& rhs) {
            double val = static_cast<double>(*this) * static_cast<double>(rhs);
            setValueFromT(val);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed& operator*=(const T& rhs) {
            T mul = T(*this) * rhs;
            setValueFromT<T>(mul);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend Fixed operator*(T lhs, const Fixed& rhs) {
            T val = lhs * T(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed operator*(const T& rhs) const {
            return Fixed(double(*this) * double(rhs));
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend Fixed operator*(T lhs, const Fixed& rhs) {
            double val = double(lhs) * double(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed& operator*=(const T& rhs) {
            double val = double(*this) * double(rhs);
            setValueFromT<double>(val);
            return *this;
        }

        /* divide operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed operator/(const T& rhs) const {
            return Fixed(T(*this) / rhs);
        }

        Fixed operator/(const Fixed& rhs) const {
            return Fixed(double(*this) / double(rhs));
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        Fixed& operator/=(const T& rhs) {
            T div = T(*this) / rhs;
            setValueFromT<T>(div);
            return *this;
        }

        Fixed& operator/=(const Fixed& rhs) {
            double val = static_cast<double>(*this) / static_cast<double>(rhs);
            setValueFromT(val);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend Fixed operator/(T lhs, const Fixed& rhs) {
            T val = lhs / T(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed operator/(const T& rhs) const {
            return Fixed(double(*this) / double(rhs));
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend Fixed operator/(T lhs, const Fixed& rhs) {
            double val = double(lhs) / double(rhs);
            return Fixed(val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        Fixed& operator/=(const T& rhs) {
            double val = double(*this) / double(rhs);
            setValueFromT<double>(val);
            return *this;
        }

        /* comparison operators */

        /* less */

        bool operator<(const Fixed& rhs) const {
            return value < rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return static_cast<double>(*this) < rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return static_cast<double>(*this) < rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) < static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) < static_cast<double>(lhs);
        }

        /* greater */

        bool operator>(const Fixed& rhs) const {
            return value > rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return static_cast<double>(*this) > rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return static_cast<double>(*this) > rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) > static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) > static_cast<double>(lhs);
        }

        /* equal */

        bool operator==(const Fixed& rhs) const {
            return value == rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return static_cast<double>(*this) == rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return static_cast<double>(*this) == rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) == static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) == static_cast<double>(lhs);
        }

        /* not equal */

        bool operator!=(const Fixed& rhs) const {
            return value != rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return static_cast<double>(*this) != rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return static_cast<double>(*this) != rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) != static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) != static_cast<double>(lhs);
        }

        /* less or equal */

        bool operator<=(const Fixed& rhs) const {
            return value <= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return static_cast<double>(*this)  <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return static_cast<double>(*this)  <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) <= static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) <= static_cast<double>(lhs);
        }

        /* greater or equal */

        bool operator>=(const Fixed& rhs) const {
            return value >= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return static_cast<double>(*this) >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return static_cast<double>(*this) >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) >= static_cast<double>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const Fixed& lhs) {
            return static_cast<double>(rhs) >= static_cast<double>(lhs);
        }

        /* unary minus */

        Fixed operator-() const {
            double v = - static_cast<double>(*this);
            return Fixed(v);
        }

        /* ostream overload */
        friend std::ostream& operator<<(std::ostream& os, const Fixed& v) {
            os << double(v);
            return os;
        }

        /* Trigonometric functions */
        friend inline Fixed cos(const Fixed&  x)  { return cos(static_cast<double>(x)); }
        friend inline Fixed sin(const Fixed&  x)  { return sin(static_cast<double>(x)); }
        friend inline Fixed tan(const Fixed&  x)  { return tan(static_cast<double>(x)); }
        friend inline Fixed acos(const Fixed&  x)  { return acos(static_cast<double>(x)); }
        friend inline Fixed asin(const Fixed&  x)  { return asin(static_cast<double>(x)); }
        friend inline Fixed atan(const Fixed&  x)  { return atan(static_cast<double>(x)); }

        /* Hyperbolic functions */
        friend inline Fixed cosh(const Fixed&  x)  { return cosh(static_cast<double>(x)); }
        friend inline Fixed sinh(const Fixed&  x)  { return sinh(static_cast<double>(x)); }
        friend inline Fixed tanh(const Fixed&  x)  { return tanh(static_cast<double>(x)); }
        friend inline Fixed acosh(const Fixed&  x)  { return acosh(static_cast<double>(x)); }
        friend inline Fixed asinh(const Fixed&  x)  { return asinh(static_cast<double>(x)); }
        friend inline Fixed atanh(const Fixed&  x)  { return atanh(static_cast<double>(x)); }

        /* Exponential and logarithmic functions */
        friend inline Fixed exp(const Fixed&  x)  { return exp(static_cast<double>(x)); }
        friend inline Fixed log(const Fixed&  x)  { return log(static_cast<double>(x)); }
        friend inline Fixed log10(const Fixed&  x)  { return log10(static_cast<double>(x)); }
        friend inline Fixed logb(const Fixed&  x)  { return logb(static_cast<double>(x)); }

        /* Power functions */
        friend inline Fixed pow(const Fixed&  base, double exponent)  { return pow(static_cast<double>(base), exponent); }
        friend inline Fixed sqrt(const Fixed&  x)  { return sqrt(static_cast<double>(x)); }
        friend inline Fixed cbrt(const Fixed&  x)  { return cbrt(static_cast<double>(x)); }

        friend inline Fixed scalbn(const Fixed&  x, int n)  { return scalbn(static_cast<double>(x), n); }

        /* Other functions */
        friend inline Fixed abs(const Fixed&  x)  { return abs(static_cast<double>(x)); }
        friend inline Fixed fabs(const Fixed&  x)  { return fabs(static_cast<double>(x)); }
        friend inline Fixed abs2(const Fixed& x)  { return x*x; }

        friend inline Fixed copysign(const Fixed&  x1, const Fixed& x2)  { return copysign(static_cast<double>(x1), static_cast<double>(x2)); }
        friend inline Fixed fmax(const Fixed&  x1, const Fixed&  x2) { return x1 < x2 ? x1 : x2; }
        friend inline bool isfinite(const Fixed& x) { return true; }

    protected:
        INT_T value;
    private:
        template<typename T>
        void setValueFromT(T v) {
            value = static_cast<INT_T>(v * (1 << FRACT_SIZE));
        }
    };
}

namespace Eigen {
    // Inheritance from float is a temporary bad solution. Need specify all NumTraits explicitly
    template<typename INT_T, int FRACT_SIZE, int POW> struct NumTraits<rndcmp::Fixed<INT_T, FRACT_SIZE, POW>>: NumTraits<float> {
        typedef rndcmp::Fixed<INT_T, FRACT_SIZE, POW> Real;
        typedef rndcmp::Fixed<INT_T, FRACT_SIZE, POW> NonInteger;
        typedef rndcmp::Fixed<INT_T, FRACT_SIZE, POW> Nested;

        enum {
            IsComplex = 0,
            IsInteger = 0,
            IsSigned = 1,
            RequireInitialization = 1,
            ReadCost = 1,
            AddCost = 4,
            MulCost = 4
        };
    };
}

#endif  // RNDCMP_INCLUDE_FIXED_HPP_


//
// Copyright (c) 2018 CNRS
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/algorithm/regressor.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_static_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  VectorXd q = randomConfiguration(model);
  regressor::computeStaticRegressor(model,data,q);

  VectorXd phi(4*(model.njoints-1));
  for(int k = 1; k < model.njoints; ++k)
  {
    const Inertia & Y = model.inertias[(size_t)k];
    phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever();
  }

  Vector3d com = centerOfMass(model,data_ref,q);
  Vector3d static_com_ref;
  static_com_ref <<  com;

  Vector3d static_com = data.staticRegressor * phi;

  BOOST_CHECK(static_com.isApprox(static_com_ref));
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/26/problem26.hpp"

BOOST_AUTO_TEST_SUITE( Problem26 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem26::solve(10);
        BOOST_CHECK_EQUAL(res, 7);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem26::solve();
        BOOST_CHECK_EQUAL(res, 983);
    }

BOOST_AUTO_TEST_SUITE_END()


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_compute_index_reordering.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_compute_index_reordering);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef ublas::vector<size_t>   idx_vector_type;

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(2) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(4) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(6) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(9) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(1) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(3) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(8) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(0) = OpenTissue::math::optimization::IN_UPPER;
  bitmask(5) = OpenTissue::math::optimization::IN_UPPER;
  bitmask(7) = OpenTissue::math::optimization::IN_UPPER;

  idx_vector_type old2new;
  idx_vector_type new2old;

  OpenTissue::math::optimization::compute_index_reordering( bitmask, old2new, new2old );

  BOOST_CHECK( old2new( 2 ) == 0 );
  BOOST_CHECK( old2new( 4 ) == 1 );
  BOOST_CHECK( old2new( 6 ) == 2 );
  BOOST_CHECK( old2new( 9 ) == 3 );
  BOOST_CHECK( old2new( 1 ) == 4 );
  BOOST_CHECK( old2new( 3 ) == 5 );
  BOOST_CHECK( old2new( 8 ) == 6 );
  BOOST_CHECK( old2new( 0 ) == 7 );
  BOOST_CHECK( old2new( 5 ) == 8 );
  BOOST_CHECK( old2new( 7 ) == 9 );
  BOOST_CHECK( new2old( 0 ) == 2 );
  BOOST_CHECK( new2old( 1 ) == 4 );
  BOOST_CHECK( new2old( 2 ) == 6 );
  BOOST_CHECK( new2old( 3 ) == 9 );
  BOOST_CHECK( new2old( 4 ) == 1 );
  BOOST_CHECK( new2old( 5 ) == 3 );
  BOOST_CHECK( new2old( 6 ) == 8 );
  BOOST_CHECK( new2old( 7 ) == 0 );
  BOOST_CHECK( new2old( 8 ) == 5 );
  BOOST_CHECK( new2old( 9 ) == 7 );

}

BOOST_AUTO_TEST_SUITE_END();


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/equal.hpp>
#include <boost/hana/functional/demux.hpp>
#include <boost/hana/functional/placeholder.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;
using hana::_;

constexpr auto f = hana::demux(hana::make_tuple)(
    _ + _,
    _ - _,
    _ * _,
    _ / _
);

static_assert(
    f(10, 4) == hana::make_tuple(
        10 + 4,
        10 - 4,
        10 * 4,
        10 / 4
    )
, "");

int main() { }


#include "Demos/Utils/Config.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Utils/TimeManager.h"
#include <Eigen/Dense>
#include "TriangleModel.h"
#include "TimeStepTriangleModel.h"
#include <iostream>

// Enable memory leak detection
#ifdef _DEBUG
	#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 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 setXXStiffness(const void *value, void *clientData);
void TW_CALL getXXStiffness(void *value, void *clientData);
void TW_CALL setYYStiffness(const void *value, void *clientData);
void TW_CALL getYYStiffness(void *value, void *clientData);
void TW_CALL setXYStiffness(const void *value, void *clientData);
void TW_CALL getXYStiffness(void *value, void *clientData);
void TW_CALL setXYPoissonRatio(const void *value, void *clientData);
void TW_CALL getXYPoissonRatio(void *value, void *clientData);
void TW_CALL setYXPoissonRatio(const void *value, void *clientData);
void TW_CALL getYXPoissonRatio(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 setBendingStiffness(const void *value, void *clientData);
void TW_CALL getBendingStiffness(void *value, void *clientData);
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(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);

TriangleModel model;
TimeStepTriangleModel simulation;

const int nRows = 30;
const int nCols = 30;
const float width = 10.0f;
const float height = 10.0f;
bool doPause = true;
std::vector<unsigned int> selectedParticles;
Eigen::Vector3f oldMousePos;

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

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

	buildModel ();

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

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, 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, &simulation, " 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, &simulation, " label='Simulation method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_FLOAT, setStiffness, getStiffness, &model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Distance constraints' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XXStiffness", TW_TYPE_FLOAT, setXXStiffness, getXXStiffness, &model, " label='Stiffness XX'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YYStiffness", TW_TYPE_FLOAT, setYYStiffness, getYYStiffness, &model, " label='Stiffness YY'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYStiffness", TW_TYPE_FLOAT, setXYStiffness, getXYStiffness, &model, " label='Stiffness XY'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XXStiffnessFEM", TW_TYPE_FLOAT, setXXStiffness, getXXStiffness, &model, " label='Youngs modulus XX'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YYStiffnessFEM", TW_TYPE_FLOAT, setYYStiffness, getYYStiffness, &model, " label='Youngs modulus YY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYStiffnessFEM", TW_TYPE_FLOAT, setXYStiffness, getXYStiffness, &model, " label='Youngs modulus XY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYPoissonRatioFEM", TW_TYPE_FLOAT, setXYPoissonRatio, getXYPoissonRatio, &model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YXPoissonRatioFEM", TW_TYPE_FLOAT, setYXPoissonRatio, getYXPoissonRatio, &model, " label='Poisson ratio YX'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	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' ");
	TwType enumType3 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType3, setBendingMethod, getBendingMethod, &simulation, " label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}' group=Bending");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingStiffness", TW_TYPE_FLOAT, setBendingStiffness, getBendingStiffness, &model, " label='Bending stiffness'  min=0.0 step=0.01 precision=4 group=Bending ");

	glutMainLoop ();

	cleanup ();

	return 0;
}

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

void reset()
{
	model.reset();
	simulation.reset(model);
	TimeManager::getCurrent()->setTime(0.0);
}

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

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

	ParticleData &pd = model.getParticleMesh().getVertexData();
	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.getParticleMesh().getVertexData();
	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 < 4; i++)
		simulation.step(model);
	model.getParticleMesh().updateVertexNormals();
}

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

	createMesh();
}

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

	// Draw simulation model

	// mesh
	const ParticleData &pd = model.getParticleMesh().getVertexData();
	const IndexedFaceMesh<ParticleData> &mesh = model.getParticleMesh();
	const unsigned int *faces = mesh.getFaces().data();
	const unsigned int nFaces = mesh.numFaces();
	const Eigen::Vector3f *vertexNormals = mesh.getVertexNormals().data();
	const Eigen::Vector2f *uvs = mesh.getUVs().data();

	float surfaceColor[4] = { 0.2f, 0.5f, 1.0f, 1 };
	float speccolor[4] = { 1.0, 1.0, 1.0, 1.0 };
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, surfaceColor);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, surfaceColor);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, speccolor);
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 100.0);
	glColor3fv(surfaceColor);

	MiniGL::bindTexture();

	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, &pd.getPosition(0)[0]);
	glTexCoordPointer(2, GL_FLOAT, 0, &uvs[0][0]);
	glNormalPointer(GL_FLOAT, 0, &vertexNormals[0][0]);
	glDrawElements(GL_TRIANGLES, (GLsizei)3 * mesh.numFaces(), GL_UNSIGNED_INT, mesh.getFaces().data());
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_TEXTURE_COORD_ARRAY);

	MiniGL::unbindTexture();

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

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

/** Create a particle model mesh
*/
void createMesh()
{
	TriangleModel::ParticleMesh::UVs uvs;
	uvs.resize(nRows*nCols);

	const float dy = width / (float)(nCols-1);
	const float dx = height / (float)(nRows-1);

	Eigen::Vector3f points[nRows*nCols];
	for (int i = 0; i < nRows; i++)
	{
		for (int j = 0; j < nCols; j++)
		{
			const float y = (float)dy*j;
			const float x = (float)dx*i;
			points[i*nCols + j] = Eigen::Vector3f(x, 1.0, y);

			uvs[i*nCols + j][0] = x/width;
			uvs[i*nCols + j][1] = y/height;
		}
	}
	const int nIndices = 6 * (nRows - 1)*(nCols - 1);

	TriangleModel::ParticleMesh::UVIndices uvIndices;
	uvIndices.resize(nIndices);

	unsigned int indices[nIndices];
	int index = 0;
	for (int i = 0; i < nRows - 1; i++)
	{
		for (int j = 0; j < nCols - 1; j++)
		{
			int helper = 0;
			if (i % 2 == j % 2)
				helper = 1;

			indices[index] = i*nCols + j;
			indices[index + 1] = i*nCols + j + 1;
			indices[index + 2] = (i + 1)*nCols + j + helper;

			uvIndices[index] = i*nCols + j;
			uvIndices[index + 1] = i*nCols + j + 1;
			uvIndices[index + 2] = (i + 1)*nCols + j + helper;
			index += 3;

			indices[index] = (i + 1)*nCols + j + 1;
			indices[index + 1] = (i + 1)*nCols + j;
			indices[index + 2] = i*nCols + j + 1 - helper;

			uvIndices[index] = (i + 1)*nCols + j + 1;
			uvIndices[index + 1] = (i + 1)*nCols + j;
			uvIndices[index + 2] = i*nCols + j + 1 - helper;
			index += 3;
		}
	}
	model.setGeometry(nRows*nCols, &points[0], nIndices / 3, &indices[0], uvIndices, uvs);

	TriangleModel::ParticleMesh &mesh = model.getParticleMesh();
	ParticleData &pd = mesh.getVertexData();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}

	// Set mass of points to zero => make it static
	pd.setMass(0, 0.0);
	pd.setMass((nRows-1)*nCols, 0.0);

	model.initConstraints();

	std::cout << "Number of triangles: " << nIndices / 3 << "\n";
	std::cout << "Number of vertices: " << nRows*nCols << "\n";

}

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

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

void TW_CALL setStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*) clientData)->setStiffness(val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getStiffness();
}

void TW_CALL setXXStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setXXStiffness(val);
}

void TW_CALL getXXStiffness(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getXXStiffness();
}

void TW_CALL setYYStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setYYStiffness(val);
}

void TW_CALL getYYStiffness(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getYYStiffness();
}

void TW_CALL setXYStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setXYStiffness(val);
}

void TW_CALL getXYStiffness(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getXYStiffness();
}

void TW_CALL setYXPoissonRatio(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setYXPoissonRatio(val);
}

void TW_CALL getYXPoissonRatio(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getYXPoissonRatio();
}

void TW_CALL setXYPoissonRatio(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setXYPoissonRatio(val);
}

void TW_CALL getXYPoissonRatio(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getXYPoissonRatio();
}

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

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

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

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

void TW_CALL setBendingStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((TriangleModel*)clientData)->setBendingStiffness(val);
}

void TW_CALL getBendingStiffness(void *value, void *clientData)
{
	*(float *)(value) = ((TriangleModel*)clientData)->getBendingStiffness();
}

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

void TW_CALL getBendingMethod(void *value, void *clientData)
{
	*(short *)(value) = (short) ((TimeStepTriangleModel*)clientData)->getBendingMethod();
}

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

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

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

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