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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2012 Ferdinando Ametrano
 Copyright (C) 2006 Mark Joshi
 Copyright (C) 2006 StatPro Italia srl
 Copyright (C) 2007 Cristina Duminuco
 Copyright (C) 2007 Chiara Fornarola

 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/pricingengines/blackformula.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <boost/math/special_functions/atanh.hpp>

namespace {
    void checkParameters(QuantLib::Real strike,
                         QuantLib::Real forward,
                         QuantLib::Real displacement)
    {
        QL_REQUIRE(displacement >= 0.0, "displacement ("
                                            << displacement
                                            << ") must be non-negative");
        QL_REQUIRE(strike + displacement >= 0.0,
                   "strike + displacement (" << strike << " + " << displacement
                                             << ") must be non-negative");
        QL_REQUIRE(forward + displacement > 0.0, "forward + displacement ("
                                                     << forward << " + "
                                                     << displacement
                                                     << ") must be positive");
    }
}

namespace QuantLib {

    Real blackFormula(Option::Type optionType,
                      Real strike,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        if (stdDev==0.0)
            return std::max((forward-strike)*optionType, Real(0.0))*discount;

        forward = forward + displacement;
        strike = strike + displacement;

        // since displacement is non-negative strike==0 iff displacement==0
        // so returning forward*discount is OK
        if (strike==0.0)
            return (optionType==Option::Call ? forward*discount : 0.0);

        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        Real d2 = d1 - stdDev;
        CumulativeNormalDistribution phi;
        Real nd1 = phi(optionType*d1);
        Real nd2 = phi(optionType*d2);
        Real result = discount * optionType * (forward*nd1 - strike*nd2);
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real blackFormula(const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement) {
        return blackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount, displacement);
    }

    Real blackFormulaImpliedStdDevApproximation(Option::Type optionType,
                                                Real strike,
                                                Real forward,
                                                Real blackPrice,
                                                Real discount,
                                                Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(blackPrice>=0.0,
                   "blackPrice (" << blackPrice << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        Real stdDev;
        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==forward)
            // Brenner-Subrahmanyan (1988) and Feinstein (1988) ATM approx.
            stdDev = blackPrice/discount*std::sqrt(2.0 * M_PI)/forward;
        else {
            // Corrado and Miller extended moneyness approximation
            Real moneynessDelta = optionType*(forward-strike);
            Real moneynessDelta_2 = moneynessDelta/2.0;
            Real temp = blackPrice/discount - moneynessDelta_2;
            Real moneynessDelta_PI = moneynessDelta*moneynessDelta/M_PI;
            Real temp2 = temp*temp-moneynessDelta_PI;
            if (temp2<0.0) // approximation breaks down, 2 alternatives:
                // 1. zero it
                temp2=0.0;
                // 2. Manaster-Koehler (1982) efficient Newton-Raphson seed
                //return std::fabs(std::log(forward/strike))*std::sqrt(2.0);
            temp2 = std::sqrt(temp2);
            temp += temp2;
            temp *= std::sqrt(2.0 * M_PI);
            stdDev = temp/(forward+strike);
        }
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDevApproximation(
                      const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real blackPrice,
                      Real discount,
                      Real displacement) {
        return blackFormulaImpliedStdDevApproximation(payoff->optionType(),
            payoff->strike(), forward, blackPrice, discount, displacement);
    }

    class BlackImpliedStdDevHelper {
      public:
        BlackImpliedStdDevHelper(Option::Type optionType,
                                 Real strike,
                                 Real forward,
                                 Real undiscountedBlackPrice,
                                 Real displacement = 0.0)
        : halfOptionType_(0.5*optionType), signedStrike_(optionType*(strike+displacement)),
          signedForward_(optionType*(forward+displacement)),
          undiscountedBlackPrice_(undiscountedBlackPrice)
        {
            checkParameters(strike, forward, displacement);
            QL_REQUIRE(undiscountedBlackPrice>=0.0,
                       "undiscounted Black price (" <<
                       undiscountedBlackPrice << ") must be non-negative");
            signedMoneyness_ = optionType*std::log((forward+displacement)/(strike+displacement));
        }
        Real operator()(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            if (stdDev==0.0)
                return std::max(signedForward_-signedStrike_, Real(0.0))
                                                   - undiscountedBlackPrice_;
            Real temp = halfOptionType_*stdDev;
            Real d = signedMoneyness_/stdDev;
            Real signedD1 = d + temp;
            Real signedD2 = d - temp;
            Real result = signedForward_ * N_(signedD1)
                - signedStrike_ * N_(signedD2);
            // numerical inaccuracies can yield a negative answer
            return std::max(Real(0.0), result) - undiscountedBlackPrice_;
        }
        Real derivative(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            Real signedD1 = signedMoneyness_/stdDev + halfOptionType_*stdDev;
            return signedForward_*N_.derivative(signedD1);
        }
      private:
        Real halfOptionType_;
        Real signedStrike_, signedForward_;
        Real undiscountedBlackPrice_, signedMoneyness_;
        CumulativeNormalDistribution N_;
    };

    Real blackFormulaImpliedStdDev(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real blackPrice,
                                   Real discount,
                                   Real displacement,
                                   Real guess,
                                   Real accuracy,
                                   Natural maxIterations)
    {
        checkParameters(strike, forward, displacement);

        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        QL_REQUIRE(blackPrice>=0.0,
                   "option price (" << blackPrice << ") must be non-negative");
        // check the price of the "other" option implied by put-call paity
        Real otherOptionPrice = blackPrice - optionType*(forward-strike)*discount;
        QL_REQUIRE(otherOptionPrice>=0.0,
                   "negative " << Option::Type(-1*optionType) <<
                   " price (" << otherOptionPrice <<
                   ") implied by put-call parity. No solution exists for " <<
                   optionType << " strike " << strike <<
                   ", forward " << forward <<
                   ", price " << blackPrice <<
                   ", deflator " << discount);

        // solve for the out-of-the-money option which has
        // greater vega/price ratio, i.e.
        // it is numerically more robust for implied vol calculations
        if (optionType==Option::Put && strike>forward) {
            optionType = Option::Call;
            blackPrice = otherOptionPrice;
        }
        if (optionType==Option::Call && strike<forward) {
            optionType = Option::Put;
            blackPrice = otherOptionPrice;
        }

        strike = strike + displacement;
        forward = forward + displacement;

        if (guess==Null<Real>())
            guess = blackFormulaImpliedStdDevApproximation(
                optionType, strike, forward, blackPrice, discount, displacement);
        else
            QL_REQUIRE(guess>=0.0,
                       "stdDev guess (" << guess << ") must be non-negative");
        BlackImpliedStdDevHelper f(optionType, strike, forward,
                                   blackPrice/discount);
        NewtonSafe solver;
        solver.setMaxEvaluations(maxIterations);
        Real minSdtDev = 0.0, maxStdDev = 24.0; // 24 = 300% * sqrt(60)
        Real stdDev = solver.solve(f, accuracy, guess, minSdtDev, maxStdDev);
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDev(
                        const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real blackPrice,
                        Real discount,
                        Real displacement,
                        Real guess,
                        Real accuracy,
                        Natural maxIterations) {
        return blackFormulaImpliedStdDev(payoff->optionType(), payoff->strike(),
            forward, blackPrice, discount, displacement, guess, accuracy, maxIterations);
    }

    Real blackFormulaCashItmProbability(Option::Type optionType,
                                        Real strike,
                                        Real forward,
                                        Real stdDev,
                                        Real displacement) {
        checkParameters(strike, forward, displacement);
        if (stdDev==0.0)
            return (forward*optionType > strike*optionType ? 1.0 : 0.0);

        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==0.0)
            return (optionType==Option::Call ? 1.0 : 0.0);
        Real d2 = std::log(forward/strike)/stdDev - 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return phi(optionType*d2);
    }

    Real blackFormulaCashItmProbability(
                        const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        return blackFormulaCashItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev , displacement);
    }

    Real blackFormulaVolDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real expiry,
                                      Real discount,
                                      Real displacement)
    {
        return  blackFormulaStdDevDerivative(strike,
                                     forward,
                                     stdDev,
                                     discount,
                                     displacement)*std::sqrt(expiry);
    }

    Real blackFormulaStdDevDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real discount,
                                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        forward = forward + displacement;
        strike = strike + displacement;

        if (stdDev==0.0 || strike==0.0)
            return 0.0;

        Real d1 = std::log(forward/strike)/stdDev + .5*stdDev;
        return discount * forward *
            CumulativeNormalDistribution().derivative(d1);
    }

    Real blackFormulaStdDevDerivative(
                        const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount,
                        Real displacement) {
        return blackFormulaStdDevDerivative(payoff->strike(), forward,
                                     stdDev, discount, displacement);
    }

    Real bachelierBlackFormula(Option::Type optionType,
                               Real strike,
                               Real forward,
                               Real stdDev,
                               Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");
        Real d = (forward-strike)*optionType, h = d/stdDev;
        if (stdDev==0.0)
            return discount*std::max(d, 0.0);
        CumulativeNormalDistribution phi;
        Real result = discount*(stdDev*phi.derivative(h) + d*phi(h));
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real bachelierBlackFormula(
                        const boost::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount) {
        return bachelierBlackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount);
    }

    static Real h(Real eta) {

        const static Real  A0          = 3.994961687345134e-1;
        const static Real  A1          = 2.100960795068497e+1;
        const static Real  A2          = 4.980340217855084e+1;
        const static Real  A3          = 5.988761102690991e+2;
        const static Real  A4          = 1.848489695437094e+3;
        const static Real  A5          = 6.106322407867059e+3;
        const static Real  A6          = 2.493415285349361e+4;
        const static Real  A7          = 1.266458051348246e+4;

        const static Real  B0          = 1.000000000000000e+0;
        const static Real  B1          = 4.990534153589422e+1;
        const static Real  B2          = 3.093573936743112e+1;
        const static Real  B3          = 1.495105008310999e+3;
        const static Real  B4          = 1.323614537899738e+3;
        const static Real  B5          = 1.598919697679745e+4;
        const static Real  B6          = 2.392008891720782e+4;
        const static Real  B7          = 3.608817108375034e+3;
        const static Real  B8          = -2.067719486400926e+2;
        const static Real  B9          = 1.174240599306013e+1;

        QL_REQUIRE(eta>=0.0,
                       "eta (" << eta << ") must be non-negative");

        const Real num = A0 + eta * (A1 + eta * (A2 + eta * (A3 + eta * (A4 + eta
                    * (A5 + eta * (A6 + eta * A7))))));

        const Real den = B0 + eta * (B1 + eta * (B2 + eta * (B3 + eta * (B4 + eta
                    * (B5 + eta * (B6 + eta * (B7 + eta * (B8 + eta * B9))))))));

        return std::sqrt(eta) * (num / den);

    }

    Real bachelierBlackFormulaImpliedVol(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real tte,
                                   Real bachelierPrice,
                                   Real discount) {

        const static Real SQRT_QL_EPSILON = std::sqrt(QL_EPSILON);

        QL_REQUIRE(tte>0.0,
                   "tte (" << tte << ") must be positive");

        Real forwardPremium = bachelierPrice/discount;

        Real straddlePremium;
        if (optionType==Option::Call){
            straddlePremium = 2.0 * forwardPremium - (forward - strike);
        } else {
            straddlePremium = 2.0 * forwardPremium + (forward - strike);
        }

        Real nu = (forward - strike) / straddlePremium;
        QL_REQUIRE(nu<=1.0,
                   "nu (" << nu << ") must be <= 1.0");
        QL_REQUIRE(nu>=-1.0,
                     "nu (" << nu << ") must be >= -1.0");

        nu = std::max(-1.0 + QL_EPSILON, std::min(nu,1.0 - QL_EPSILON));

        // nu / arctanh(nu) -> 1 as nu -> 0
        Real eta = (std::fabs(nu) < SQRT_QL_EPSILON) ? 1.0 : nu / boost::math::atanh(nu);

        Real heta = h(eta);

        Real impliedBpvol = std::sqrt(M_PI / (2 * tte)) * straddlePremium * heta;

        return impliedBpvol;
    }
}


// Copyright (c) 2017, Pawel Bylica
// All rights reserved.

#include <boost/random.hpp>

int main() {
  boost::random::mt19937 rng;
  boost::random::uniform_int_distribution<> six(1,6);
  return six(rng);
}


/**
 * @author     : Zhao Chonyyao (cyzhao@zju.edu.cn)
 * @date       : 2021-04-30
 * @description: some basic energy function.
 * @version    : 1.0
 */
#include <iostream>

#include <Eigen/SparseCore>

#include "basic_energy.h"

using namespace std;
using namespace Eigen;

namespace PhysIKA {
/******************************************momentum*******************************/

template <typename T, size_t dim_>
size_t momentum<T, dim_>::Nx() const
{
    return dim_ * dof_;
}

template <typename T, size_t dim_>
momentum<T, dim_>::momentum(const size_t dof, const Matrix<T, -1, 1>& mass_vec, const T& dt)
    : dof_(dof), dispk_(Matrix<T, -1, 1>::Zero(dim_ * dof)), vk_(Matrix<T, -1, 1>::Zero(dim_ * dof)), dt_(dt), d1dt_(1 / dt), d1dtdt_(1 / dt / dt), mass_vec_(dim_ * dof)
{

#pragma omp parallel for
    for (size_t i = 0; i < dof; ++i)
    {
        for (size_t j = 0; j < dim_; ++j)
        {
            mass_vec_(i * dim_ + j) = mass_vec(i);
        }
    }
}

template <typename T, size_t dim_>
momentum<T, dim_>::momentum(const T* rest, const size_t dof, const Matrix<T, -1, 1>& mass_vec, const T& dt)
    : dof_(dof), vk_(Matrix<T, -1, 1>::Zero(dim_ * dof)), dt_(dt), d1dt_(1 / dt), d1dtdt_(1 / dt / dt), mass_vec_(dim_ * dof)
{

    dispk_ = Eigen::Map<const Matrix<T, -1, 1>>(rest, dim_ * dof);
#pragma omp parallel for
    for (size_t i = 0; i < dof; ++i)
    {
        for (size_t j = 0; j < dim_; ++j)
        {
            mass_vec_(i * dim_ + j) = mass_vec(i);
        }
    }
}

template <typename T, size_t dim_>
int momentum<T, dim_>::Val(const T* x, data_ptr<T, dim_>& data) const
{
    Eigen::Map<const Matrix<T, -1, 1>> _x(x, dim_ * dof_);
    const Matrix<T, -1, 1>             acce = (_x - dispk_) * d1dt_ - vk_;
    data->save_val(0.5 * acce.dot(mass_vec_.cwiseProduct(acce)));

    return 0;
}
template <typename T, size_t dim_>
int momentum<T, dim_>::Gra(const T* x, data_ptr<T, dim_>& data) const
{
    Eigen::Map<const Matrix<T, -1, 1>> _x(x, dim_ * dof_);

    const Matrix<T, -1, 1> acce = (_x - dispk_) * d1dtdt_ - vk_ * d1dt_;
    data->save_gra(mass_vec_.cwiseProduct(acce));

    return 0;
}

template <typename T, size_t dim_>
int momentum<T, dim_>::Hes(const T* x, data_ptr<T, dim_>& data) const
{
#pragma omp parallel for
    for (size_t i = 0; i < dim_ * dof_; ++i)
        data->save_hes(i, i, d1dtdt_ * mass_vec_(i));

    return 0;
}

template <typename T, size_t dim_>
int momentum<T, dim_>::update_location_and_velocity(const T* new_dispk_ptr, const T* new_velo_ptr)
{
    Eigen::Map<const Matrix<T, -1, 1>> new_dispk(new_dispk_ptr, dim_ * dof_);
    if (new_velo_ptr == nullptr)
        vk_ = (new_dispk - dispk_) * d1dt_;
    else
        vk_ = Map<const Matrix<T, -1, 1>>(new_velo_ptr, dim_ * dof_);

    dispk_ = new_dispk;
    return 0;
}

template <typename T, size_t dim_>
int momentum<T, dim_>::set_initial_velocity(const Matrix<T, dim_, 1>& velo)
{
    Eigen::Map<Matrix<T, -1, -1>> myvelo(vk_.data(), dim_, dof_);
    for (size_t i = 0; i < dim_; ++i)
    {
        myvelo.row(i) = Matrix<T, 1, -1>::Ones(dof_) * velo(i);
    }
    return 0;
}

/******************************************momentum*******************************/
/******************************************position_constraint*******************************/
template <typename T, size_t dim_>
position_constraint<T, dim_>::position_constraint(const size_t dof, const T& w, const vector<size_t>& cons)
    : w_(w), cons_(cons), dof_(dof)
{
    rest_ = Matrix<T, -1, -1>::Zero(dim_, dof);
}

template <typename T, size_t dim_>
position_constraint<T, dim_>::position_constraint(const T* rest, const size_t dof, const T& w, const std::vector<size_t>& cons)
    : w_(w), cons_(cons), dof_(dof)
{
    rest_ = Eigen::Map<const Matrix<T, -1, -1>>(rest, dim_, dof_);
}

//TODO: simplify _x
template <typename T, size_t dim_>
int position_constraint<T, dim_>::Val(const T* x, data_ptr<T, dim_>& data) const
{
    Eigen::Map<const Matrix<T, -1, -1>> deformed(x, dim_, dof_);
    Matrix<T, -1, -1>                   _x = deformed - rest_;
    for (auto iter_c = cons_.begin(); iter_c != cons_.end(); ++iter_c)
    {
        data->save_val(w_ * _x.col(*iter_c).dot(_x.col(*iter_c)));
    }

    return 0;
}

template <typename T, size_t dim_>
int position_constraint<T, dim_>::Gra(const T* x, data_ptr<T, dim_>& data) const
{
    Eigen::Map<const Matrix<T, -1, -1>> deformed(x, dim_, dof_);
    Matrix<T, -1, -1>                   _x = deformed - rest_;

    for (auto iter_c = cons_.begin(); iter_c != cons_.end(); ++iter_c)
        data->save_gra(*iter_c, 2.0 * w_ * _x.col(*iter_c));
    return 0;
}

template <typename T, size_t dim_>
int position_constraint<T, dim_>::Hes(const T* x, data_ptr<T, dim_>& data) const
{
    for (auto iter_c = cons_.begin(); iter_c != cons_.end(); ++iter_c)
    {
        for (size_t j = 0; j < dim_; ++j)
        {
            data->save_hes(*iter_c * dim_ + j, *iter_c * dim_ + j, 2 * w_);
        }
    }
    return 0;
}

template <typename T, size_t dim_>
size_t position_constraint<T, dim_>::Nx() const
{
    return dim_ * dof_;
}
/******************************************position_constraint*******************************/

/******************************************gravity*******************************/
//dof here is not about dim_
template <typename T, size_t dim_>
gravity_energy<T, dim_>::gravity_energy(const size_t dof, const T& w_g, const T& gravity, const Matrix<T, -1, 1>& mass, const char& axis)
    : w_g_(w_g), dof_(dof), gravity_(gravity), mass_(mass), axis_(axis)
{
}

template <typename T, size_t dim_>
int gravity_energy<T, dim_>::Val(const T* x, data_ptr<T, dim_>& data) const
{

    Eigen::Map<const Matrix<T, -1, -1>> _x(x, dim_, dof_);
    size_t                              which_axis = size_t(axis_ - 'x');
    data->save_val((_x.row(which_axis).transpose().array() * mass_.array()).sum() * w_g_ * gravity_);
    return 0;
}

template <typename T, size_t dim_>
int gravity_energy<T, dim_>::Gra(const T* x, data_ptr<T, dim_>& data) const
{
    size_t which_axis = size_t(axis_ - 'x');

    Matrix<T, -1, -1> g(dim_, dof_);
    g.setZero();
    g.row(which_axis) = Matrix<T, -1, 1>::Constant(dof_, gravity_ * w_g_).cwiseProduct(mass_).transpose();
    Eigen::Map<const Matrix<T, -1, 1>> g_(g.data(), dim_ * dof_);
    data->save_gra(g_);
    return 0;
}
template <typename T, size_t dim_>
int gravity_energy<T, dim_>::Hes(const T* x, data_ptr<T, dim_>& data) const
{
    return 0;
}

template <typename T, size_t dim_>
size_t gravity_energy<T, dim_>::Nx() const
{
    return dim_ * dof_;
}
/******************************************gravity*******************************/

/*************************************collision*********************************/
template <typename T, size_t dim_>
collision<T, dim_>::collision(const size_t dof_, const T& w_coll, const char& ground_axis, const T& ground_pos, const size_t& num_surf_point, const shared_ptr<Matrix<T, -1, -1>>& init_points_ptr)
    : ground_axis_(ground_axis), ground_pos_(ground_pos), w_coll_(w_coll), num_surf_point_(num_surf_point), dof_(dof_), init_points_ptr_(init_points_ptr)
{
}

template <typename T, size_t dim_>
int collision<T, dim_>::Val(const T* x, data_ptr<T, dim_>& data) const
{
    const size_t which_axis = size_t(ground_axis_ - 'x');

    Eigen::Map<const Matrix<T, -1, -1>> _x(x, dim_, dof_);
#pragma omp parallel for
    for (size_t i = 0; i < dof_; ++i)
    {
        const T position_now = _x(which_axis, i) + (*init_points_ptr_)(which_axis, i);
        if ((position_now - ground_pos_) < 0)
        {

            data->save_val(w_coll_ * pow((ground_pos_ - position_now), 2));
        }
    }
    return 0;
}
template <typename T, size_t dim_>
int collision<T, dim_>::Gra(const T* x, data_ptr<T, dim_>& data) const
{
    const size_t which_axis = size_t(ground_axis_ - 'x');

    Eigen::Map<const Matrix<T, -1, -1>> _x(x, dim_, dof_);
#pragma omp parallel for
    for (size_t i = 0; i < dof_; ++i)
    {
        const T position_now = _x(which_axis, i) + (*init_points_ptr_)(which_axis, i);
        if ((position_now - ground_pos_) < 0)
        {
            data->save_gra(i * dim_ + which_axis, 2 * w_coll_ * (position_now - ground_pos_));
        }
    }
    return 0;
}
template <typename T, size_t dim_>
int collision<T, dim_>::Hes(const T* x, data_ptr<T, dim_>& data) const
{
    const size_t                        which_axis = size_t(ground_axis_ - 'x');
    Eigen::Map<const Matrix<T, -1, -1>> _x(x, dim_, dof_);
#pragma omp parallel for
    for (size_t i = 0; i < dof_; ++i)
    {
        const T position_now = _x(which_axis, i) + (*init_points_ptr_)(which_axis, i);
        if ((position_now - ground_pos_) < 0)
        {
            for (size_t j = 0; j < dim_; ++j)
            {
                data->save_hes(i * dim_ + j, i * dim_ + j, 2 * w_coll_);
            }
        }
    }

    return 0;
}
template <typename T, size_t dim_>
size_t collision<T, dim_>::Nx() const
{
    return dim_ * dof_;
}

/*************************************collision*********************************/

template class position_constraint<double, 3>;
template class position_constraint<float, 3>;
template class position_constraint<float, 1>;
template class position_constraint<double, 1>;
template class momentum<double, 3>;
template class momentum<float, 3>;
template class gravity_energy<double, 3>;
template class gravity_energy<float, 3>;
// template class collision<FLOAT_TYPE, 3>;

}  //namespace PhysIKA


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2015 by Wenzel Jakob

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Nori 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 <nori/mesh.h>
#include <nori/bbox.h>
#include <nori/bsdf.h>
#include <nori/emitter.h>
#include <nori/warp.h>
#include <Eigen/Geometry>

NORI_NAMESPACE_BEGIN

Mesh::Mesh() { }

Mesh::~Mesh() {
    delete m_bsdf;
    delete m_emitter;
}

void Mesh::activate() {
    if (!m_bsdf) {
        /* If no material was assigned, instantiate a diffuse BRDF */
        m_bsdf = static_cast<BSDF *>(
            NoriObjectFactory::createInstance("diffuse", PropertyList()));
    }
	m_dPdf.reserve(getTriangleCount());//the discrete pdf will have an entry for each triangle
	float meshSurfaceArea = getMeshSurfaceArea();
	for (int i = 0; i < getTriangleCount(); i++)
	{
		m_dPdf.append(surfaceArea(i) * meshSurfaceArea);//each triangle's pdf will be the triangle surface area / mesh surface area
	}
}

Point2f squareToUniformBary(const Point2f& sample)
{
	return Point2f(1 - std::sqrt(1 - sample[0]), sample[1] * std::sqrt(1 - sample[0]));
}

//return pdf of the entire mesh which is the reciprocal of the surface area of the entire mesh
float Mesh::getMeshSurfaceArea() const
{
	float meshSurfaceArea = 0;
	for (int i = 0; i < getTriangleCount(); i++)
	{
		meshSurfaceArea += surfaceArea(i);
	}
	return 1.f / meshSurfaceArea;
}

SurfaceSample Mesh::getSurfaceSample(const Point2f& sample,const float sample2) const
{
	SurfaceSample res;
	int sampleTriangleIndex = m_dPdf.sample(sample2);

	//now we get the vertices of the face (triangle) using the index
	uint32_t i0 = m_F(0, sampleTriangleIndex), i1 = m_F(1, sampleTriangleIndex), i2 = m_F(2, sampleTriangleIndex);

	//now we have the indices of the vertices that can be used with m_V
	const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);//using these indices we take the 3 vertex points of the triangle
	Point2f baryCoords(squareToUniformBary(sample));
	//now we calculate the position p on the surface of the triangle using the barycentric coordinates
	res.p = baryCoords[0] * p0 + baryCoords[1] * p1 + (1 - baryCoords[0] - baryCoords[1]) * p2;
	res.pdf = getMeshSurfaceArea();
	//now we calculate the surface normal at point p.
	const MatrixXf &N = getVertexNormals();
	//in case the mesh provides per-vertex normals
	if (m_N.size() > 0)
	{
		res.n = (baryCoords[0] * N.col(i0) + baryCoords[1] * N.col(i1) + (1 - baryCoords[0] - baryCoords[1]) * N.col(i2)).normalized();
	}
	else {
		//mesh doesn't provide per-vertex normal so we use the face
		res.n = (p1 - p0).cross(p2 - p0).normalized();//is this the right normal direction?
	}
	return res;
}

float Mesh::surfaceArea(uint32_t index) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);

    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    return 0.5f * Vector3f((p1 - p0).cross(p2 - p0)).norm();
}

bool Mesh::rayIntersect(uint32_t index, const Ray3f &ray, float &u, float &v, float &t) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);
    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    /* Find vectors for two edges sharing v[0] */
    Vector3f edge1 = p1 - p0, edge2 = p2 - p0;

    /* Begin calculating determinant - also used to calculate U parameter */
    Vector3f pvec = ray.d.cross(edge2);

    /* If determinant is near zero, ray lies in plane of triangle */
    float det = edge1.dot(pvec);

    if (det > -1e-8f && det < 1e-8f)
        return false;
    float inv_det = 1.0f / det;

    /* Calculate distance from v[0] to ray origin */
    Vector3f tvec = ray.o - p0;

    /* Calculate U parameter and test bounds */
    u = tvec.dot(pvec) * inv_det;
    if (u < 0.0 || u > 1.0)
        return false;

    /* Prepare to test V parameter */
    Vector3f qvec = tvec.cross(edge1);

    /* Calculate V parameter and test bounds */
    v = ray.d.dot(qvec) * inv_det;
    if (v < 0.0 || u + v > 1.0)
        return false;

    /* Ray intersects triangle -> compute t */
    t = edge2.dot(qvec) * inv_det;

    return t >= ray.mint && t <= ray.maxt;
}

BoundingBox3f Mesh::getBoundingBox(uint32_t index) const {
    BoundingBox3f result(m_V.col(m_F(0, index)));
    result.expandBy(m_V.col(m_F(1, index)));
    result.expandBy(m_V.col(m_F(2, index)));
    return result;
}

Point3f Mesh::getCentroid(uint32_t index) const {
    return (1.0f / 3.0f) *
        (m_V.col(m_F(0, index)) +
         m_V.col(m_F(1, index)) +
         m_V.col(m_F(2, index)));
}

void Mesh::addChild(NoriObject *obj) {
    switch (obj->getClassType()) {
        case EBSDF:
            if (m_bsdf)
                throw NoriException(
                    "Mesh: tried to register multiple BSDF instances!");
            m_bsdf = static_cast<BSDF *>(obj);
            break;

        case EEmitter: {
                Emitter *emitter = static_cast<Emitter *>(obj);
                if (m_emitter)
                    throw NoriException(
                        "Mesh: tried to register multiple Emitter instances!");
                m_emitter = emitter;
            }
            break;

        default:
            throw NoriException("Mesh::addChild(<%s>) is not supported!",
                                classTypeName(obj->getClassType()));
    }
}

std::string Mesh::toString() const {
    return tfm::format(
        "Mesh[\n"
        "  name = \"%s\",\n"
        "  vertexCount = %i,\n"
        "  triangleCount = %i,\n"
        "  bsdf = %s,\n"
        "  emitter = %s\n"
        "]",
        m_name,
        m_V.cols(),
        m_F.cols(),
        m_bsdf ? indent(m_bsdf->toString()) : std::string("null"),
        m_emitter ? indent(m_emitter->toString()) : std::string("null")
    );
}

std::string Intersection::toString() const {
    if (!mesh)
        return "Intersection[invalid]";

    return tfm::format(
        "Intersection[\n"
        "  p = %s,\n"
        "  t = %f,\n"
        "  uv = %s,\n"
        "  shFrame = %s,\n"
        "  geoFrame = %s,\n"
        "  mesh = %s\n"
        "]",
        p.toString(),
        t,
        uv.toString(),
        indent(shFrame.toString()),
        indent(geoFrame.toString()),
        mesh ? mesh->toString() : std::string("null")
    );
}

NORI_NAMESPACE_END


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file L2LossFunction.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_L2_LOSS_FUNCTION_HPP
#define STEAM_L2_LOSS_FUNCTION_HPP

#include <Eigen/Core>

#include <steam/problem/lossfunc/LossFunctionBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief 'L2' loss function
//////////////////////////////////////////////////////////////////////////////////////////////
class L2LossFunc : public LossFunctionBase
{
public:

  /// Convenience typedefs
  typedef std::shared_ptr<L2LossFunc> Ptr;
  typedef std::shared_ptr<const L2LossFunc> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  L2LossFunc() {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Cost function (basic evaluation of the loss function)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double cost(double whitened_error_norm) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Weight for iteratively reweighted least-squares (influence function div. by error)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double weight(double whitened_error_norm) const;
};

} // steam

#endif // STEAM_L2_LOSS_FUNCTION_HPP


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TSegment3D.h>
#include <mrpt/math/geometry.h>	 // distance()
#include <mrpt/serialization/CArchive.h>  // impl of << operator

#include <Eigen/Dense>

using namespace mrpt::math;

void TSegment3D::generate2DObject(TSegment2D& s) const
{
	s = TSegment2D(*this);
}

double TSegment3D::length() const { return math::distance(point1, point2); }
double TSegment3D::distance(const TPoint3D& point) const
{
	return std::min(
		std::min(math::distance(point, point1), math::distance(point, point2)),
		TLine3D(*this).distance(point));
}
double TSegment3D::distance(const TSegment3D& segment) const
{
	Eigen::Vector3d u, v, w;
	TPoint3D diff_vect = point2 - point1;
	diff_vect.asVector(u);
	diff_vect = segment.point2 - segment.point1;
	diff_vect.asVector(v);
	diff_vect = point1 - segment.point1;
	diff_vect.asVector(w);
	double a = u.dot(u);  // always >= 0
	double b = u.dot(v);
	double c = v.dot(v);  // always >= 0
	double d = u.dot(w);
	double e = v.dot(w);
	double D = a * c - b * b;  // always >= 0
	double sc, sN, sD = D;	// sc = sN / sD, default sD = D >= 0
	double tc, tN, tD = D;	// tc = tN / tD, default tD = D >= 0

	// compute the line parameters of the two closest points
	if (D < 0.00000001)
	{  // the lines are almost parallel
		sN = 0.0;  // force using point P0 on segment S1
		sD = 1.0;  // to prevent possible division by 0.0 later
		tN = e;
		tD = c;
	}
	else
	{  // get the closest points on the infinite lines
		sN = (b * e - c * d);
		tN = (a * e - b * d);
		if (sN < 0.0)
		{  // sc < 0 => the s=0 edge is visible
			sN = 0.0;
			tN = e;
			tD = c;
		}
		else if (sN > sD)
		{  // sc > 1 => the s=1 edge is visible
			sN = sD;
			tN = e + b;
			tD = c;
		}
	}

	if (tN < 0.0)
	{  // tc < 0 => the t=0 edge is visible
		tN = 0.0;
		// recompute sc for this edge
		if (-d < 0.0) sN = 0.0;
		else if (-d > a)
			sN = sD;
		else
		{
			sN = -d;
			sD = a;
		}
	}
	else if (tN > tD)
	{  // tc > 1 => the t=1 edge is visible
		tN = tD;
		// recompute sc for this edge
		if ((-d + b) < 0.0) sN = 0;
		else if ((-d + b) > a)
			sN = sD;
		else
		{
			sN = (-d + b);
			sD = a;
		}
	}
	// finally do the division to get sc and tc
	sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
	tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);

	// get the difference of the two closest points
	const auto dP = (w + (sc * u) - (tc * v)).eval();
	return dP.norm();  // return the closest distance
}
bool TSegment3D::contains(const TPoint3D& point) const
{
	// Not very intuitive, but very fast, method.
	return std::abs(
			   math::distance(point1, point) + math::distance(point2, point) -
			   math::distance(point1, point2)) < getEpsilon();
}

bool TSegment3D::operator<(const TSegment3D& s) const
{
	if (point1 < s.point1) return true;
	else if (s.point1 < point1)
		return false;
	else
		return point2 < s.point2;
}

mrpt::serialization::CArchive& mrpt::math::operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TSegment3D& s)
{
	return in >> s.point1 >> s.point2;
}
mrpt::serialization::CArchive& mrpt::math::operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TSegment3D& s)
{
	return out << s.point1 << s.point2;
}


//-------------------------------------------------------------------------//
//
// Copyright 2017 Sascha Kaden
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//-------------------------------------------------------------------------//

#ifndef POINTLIST_H
#define POINTLIST_H

#include <Eigen/Core>

#include <ippp/types.h>
#include <ippp/util/Logging.h>
#include <ippp/util/UtilGeo.hpp>

namespace ippp {

/*!
* \brief   Data structure for 2D or 3D point list
* \details Typedefs has to be used otherwise the variadic template constructor will not work.
*          Number of input points have to be the size of the PointList.
* \author  Sascha Kaden
* \date    2016-12-19
*/
template <class T, unsigned int P>
class PointList {
  public:
    PointList() = default;

    /*!
    *  \brief      Last constructor of PointList
    *  \param[in]  index
    *  \param[in]  point
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    template <class... Args>
    PointList(unsigned int index, T pt) {
        assert(index == P - 1);
        m_p[index] = pt;
    }

    /*!
    *  \brief      First constructor of PointList
    *  \param[in]  point
    *  \param[in]  remaining list of points
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    template <class... Args>
    PointList(T pt, Args... fargs) : PointList(1, fargs...) {
        m_p[0] = pt;
    }

    /*!
    *  \brief      Second constructor of PointList
    *  \param[in]  index
    *  \param[in]  point
    *  \param[in]  remaining list of points
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    template <class... Args>
    PointList(unsigned int index, T pt, Args... fargs) : PointList(index + 1, fargs...) {
        assert(index < P - 1);
        m_p[index] = pt;
    }

    /*!
    *  \brief      Set point of PointList from index (index starts with 1)
    *  \param[in]  index
    *  \param[out] point
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    void setP(const T p, const unsigned int index) {
        assert(index > 0);
        assert(index <= P);
        m_p[index - 1] = p;
    }

    /*!
    *  \brief      Return point of PointList from index
    *  \param[in]  index
    *  \param[out] point
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    T getP(const unsigned int index) const {
        assert(index > 0);
        assert(index <= P);
        return m_p[index - 1];
    }

    /*!
    *  \brief      Transform Pointlist itself by passed rotation matrix and translation vector
    *  \param[in]  rotation matrix R
    *  \param[in]  translation vector t
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    void transform(const MatrixX &R, const T &t) {
        assert(R.rows() == m_p[0].rows());

        for (unsigned int i = 0; i < P; ++i)
            m_p[i] = R * m_p[i] + t;
    }

    /*!
    *  \brief      Transform Pointlist itself by passed transformation matrix.
    *  \param[in]  transformation matrix T
    *  \author     Sascha Kaden
    *  \date       2016-12-19
    */
    void transform(const MatrixX &trafo) {
        assert(trafo.rows() == m_p[0].rows() + 1);
        MatrixX R;
        T t;
        util::decomposeT(trafo, R, t);

        for (unsigned int i = 0; i < P; ++i)
            m_p[i] = R * m_p[i] + t;
    }

  private:
    T m_p[P];
};

typedef PointList<Vector2, 2> Line2D;
typedef PointList<Vector3, 2> Line3D;
typedef PointList<Vector2, 3> Triangle2D;
typedef PointList<Vector3, 3> Triangle3D;
typedef PointList<Vector2, 4> Square2D;
typedef PointList<Vector3, 4> Square3D;
typedef PointList<Vector2, 5> Pentagon2D;
typedef PointList<Vector3, 5> Pentagon3D;
typedef PointList<Vector2, 6> Hexagon2D;
typedef PointList<Vector3, 6> Hexagon3D;
typedef PointList<Vector2, 7> Heptagon2D;
typedef PointList<Vector3, 7> Heptagon3D;

} /* namespace ippp */

#endif    // POINTLIST_H


#ifndef SPARSEMATRIX_HPP_1BWMMLC8
#define SPARSEMATRIX_HPP_1BWMMLC8

#include <algorithm>
#include <cassert>
#include <fstream>
#include <iterator>
#include <map>
#include <stdexcept>
#include <tuple>
#include <unordered_map>
#include <vector>
#include <vector>
#include <iostream>

#include <Eigen/Sparse>

namespace cask {
  namespace sparse {

    using EigenSparseMatrix = Eigen::SparseMatrix<double, Eigen::RowMajor, int32_t>;

    template<typename value_type>
  class SparkCooMatrix {

        public:
        using CoordType = std::tuple<int, int, value_type>;

        int n, m;
        std::vector<CoordType> data;

        SparkCooMatrix(int _n, int _m) : n(_n), m(_m) {}
      };
  }

// A dense vector representation
class Vector {
public:
  std::vector<double> data;

  Vector(int n) : data(n, 0) {}
  Vector(std::initializer_list<double> l) : data(l) {}
  Vector(const std::vector<double>& v) : data(v.begin(), v.end()) {}

  Vector operator-(const Vector& other) const {
    int n = size();
    if (other.size() != data.size()) {
      throw std::invalid_argument("Attempt to subtract vectors of different lengths: " +
                                  std::to_string(other.size()) + " != " + std::to_string(n));
    }
    Vector v(n);
    for (int i = 0; i < n; i++) {
      v[i] = data[i] - other[i];
    }
    return v;
  }

  int size() const {
    return data.size();
  }

  bool operator==(const Vector& other) const {
    return data == other.data;
  }

  const double& operator[](int i) const {
    return data[i];
  }

  double& operator[](int i) {
    return data[i];
  }

  void print(std::string label="") const {
    std::cout << label;
    for (int i : data)
      std::cout << i << " ";
    std::cout << std::endl;
  }

  double norm() const {
    double residual;
    for (double d : data)
      residual += d * d;
    return std::sqrt(residual);
  }

  double distance(const Vector& exp) const {
    return (*this - exp).norm();
  }

  void writeToFile(std::string path) {
    std::ofstream f{path};
    if (!f)
      throw std::invalid_argument("Could not open file for writing");
    for (auto v : data)
      f << v << std::endl;
  }
};

/**
 * Sparse matrix representations for some common storage formats:
 * - DoK (Dictionary of Keys) -- the de facto format for all/many construction tasks; random read/write access is
 * efficient, but memory footprint is large
 * - CSR -- an efficient format for when only row slicing operations are required; random read access is supported but inefficient,
 * random write access is only supported in update mode (cannot insert new nonzeros), memory footprint is minimal
 *
 * Some underlying assumptions that are respected by all matrix formats:
 *
 * - all storage is 0 indexed; it is incredibly awkward and error-prone to support 1-based indexing in a language like C++;
 * - for interfacing with certain codes that do not support 0 based indexing (e.g. if using BLAS from Fortran), some
 * formats provide a getIndexed() method to obtain the corresponding representation in 1 based indexing. Read the specific
 * of each method to understand the storage format and prevent bugs when interfacing with low-level C/Fortran APIs
 *
 * TODOs
 * - a consistent, safe mechanism for handling symmetric matrices?
 */
class DokMatrix {

 public:
  int n, m;
  int nnzs;

  // use of std::map important: values are sorted by column index internaly
  std::unordered_map<int, std::map<int, double>> dok;

  DokMatrix() : n(0), m(0), nnzs(0) {}

  DokMatrix(int _n, int m) : n(_n), m(m), nnzs(0) {}

  DokMatrix(int _n, int m, int _nnzs) : n(_n), m(m), nnzs(_nnzs) {}

  // Initialize the matrix as a dense matrix, with the given values, in order;
  // matrix is assumed square with n = sqrt(pattern.size()); 0 entries must be
  // entered explicitly
  DokMatrix(const std::initializer_list<double>& pattern) :
      DokMatrix(floor((sqrt(pattern.size()))), pattern) { }

  // Initialize the matrix as a dense matrix, with the given values, in order;
  // matrix is assumed of size n rows by pattern.size() / n columns;
  // 0 entries must be entered explicitly
  DokMatrix(int n, const std::initializer_list<double>& pattern) : n(n) {
    nnzs = std::count_if(pattern.begin(), pattern.end(), [](double x){return x != 0;});
    auto it = pattern.begin();
    m = pattern.size() / n;
    for (int i = 0; i < n; i++)
      for (int j = 0; j < m; j++) {
        double value = *it;
        if (value != 0) {
          dok[i][j] = value;
        }
        it++;
      }
  }

  DokMatrix explicitSymmetric() {
    int newNnzs = 0;
    DokMatrix m(n, this->m);
    for (auto &&s : dok) {
      for (auto &&p : s.second) {
        int i = s.first;
        int j = p.first;
        double value = p.second;
        m.dok[i][j] = value;
        newNnzs++;

        // check the transpose entry does not exist
        // TODO replace dok.find with dok.at
        if (i == j)
          continue;
        if (dok.find(j) != dok.end()) {
          if (dok[j].find(i) != dok[j].end()) {
            if (dok[j][i] != p.second) {
              throw std::invalid_argument("Matrix is not symmetric");
            } else {
              std::cout << "Warning! Matrix already contains transpose entry for "
                        << s.first << " " << p.first << std::endl;
            }
          }
        }

        // add the transpose entry
        m.dok[j][i] = value;
        newNnzs++;
      }
    }
    m.nnzs = newNnzs;
    return m;
  }

  bool operator==(const DokMatrix& other) const {
    return n == other.n && m == other.m && nnzs == other.nnzs && dok == other.dok;
  }

  void pretty_print() const {
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < m; j++) {
        std::cout << at(i, j) << " ";
      }
      std::cout << "\n";
    }
  }

  double at(int i, int j) const {
    assert(i < n && j < m);
    if (dok.count(i) == 0)
      return 0;
    if (dok.at(i).count(j) == 0)
      return 0;
    return dok.at(i).at(j);
  }

  void set(int i, int j, double val) {
    assert(i < n && j < m);
    dok[i][j] = val;
    nnzs++;
  }

  bool isNnz(int i, int j) const {
    if (dok.count(i) == 0)
      return false;
    if (dok.at(i).count(j) == 0)
        return false;
    return dok.at(i).at(j) != 0;
  }

  DokMatrix getLowerTriangular() const {
    DokMatrix lowerTriangular(n, m);
    for (auto &e : dok) {
      for (auto &ee : e.second) {
        int i = e.first;
        int j = ee.first;
        double value = ee.second;
        if (j <= i)
          lowerTriangular.set(i, j, value);
      }
    }
    return lowerTriangular;
  }

  DokMatrix getUpperTriangular() const {
    DokMatrix lowerTriangular(n, m);
    for (auto &e : dok) {
      for (auto &ee : e.second) {
        int i = e.first;
        int j = ee.first;
        double value = ee.second;
        if (i <= j)
          lowerTriangular.set(i, j, value);
      }
    }
    return lowerTriangular;
  }

  Vector dot(const Vector& b) const {
    Vector result(b.size());
    for (auto &p : dok) {
      int row = p.first;
      for (auto &e : p.second) {
        result[row] += b[e.first] * e.second;
      }
    }
    return result;
  }

};

// TODO verify preconditions:
// with 1 based indexing
// lower triangular
// row entries in increasing column order
class CsrMatrix {
  // TODO move to include/, make API
 public:
  int n, m;
  int nnzs;
  std::vector<double> values;
  std::vector<int> col_ind;
  std::vector<int> row_ptr;

  CsrMatrix() : n(0), m(0), nnzs(0) {}

  CsrMatrix(std::initializer_list<double> mat) : CsrMatrix(DokMatrix(mat)){
  }

  CsrMatrix(int n, std::initializer_list<double> mat) : CsrMatrix(DokMatrix(n, mat)){
  }

  CsrMatrix(const DokMatrix &m) {
    nnzs = m.nnzs;
    this->n = m.n;
    this->m = m.m;
    int pos = 0;
    for (int i = 0; i < n; i++) {
      row_ptr.push_back(pos);
      if (m.dok.count(i) != 0) {
        for (auto &entries : m.dok.at(i)) {
          col_ind.push_back(entries.first);
          values.push_back(entries.second);
          pos++;
        }
      }
    }
    row_ptr.push_back(nnzs);
  }

  CsrMatrix(int _n, int _m, int _nnzs, double *_values, int *_col_ind, int *_row_ptr) :
      n(_n), m(_m),
      nnzs(_nnzs) {
    values.assign(_values, _values + _nnzs);
    col_ind.assign(_col_ind, _col_ind + _nnzs);
    row_ptr.assign(_row_ptr, _row_ptr + n + 1);
  }

  CsrMatrix(int n, int m, int nnzs,
            const std::vector<double> &values,
            const std::vector<int> &col_ind,
            const std::vector<int> &row_ptr) :
      n(n), m(m), nnzs(nnzs), values(values), col_ind(col_ind), row_ptr(row_ptr) {}

  // Prints all matrix values
  void pretty_print() const {
    for (int i = 0; i < n; i++) {
      int col_ptr = row_ptr[i];
      for (int j = 0; j < n; j++) {
        if (col_ptr < row_ptr[i + 1] && col_ind[col_ptr] == j) {
          std::cout << values[col_ptr] << " ";
          col_ptr++;
          continue;
        }
        std::cout << "0 ";
      }
      std::cout << "\n";
    }
  }

  void print() {
    std::cout << "CSRMatrix( n= " << n << " nnzs= " << nnzs << ")" << std::endl;
    std::cout << "values = ";
    std::copy(values.begin(), values.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
    std::cout << "col_ind = ";
    std::copy(col_ind.begin(), col_ind.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
    std::cout << "row_ptr = ";
    std::copy(row_ptr.begin(), row_ptr.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
  }

  double &get(int i, int j) {
    for (int k = row_ptr[i]; k < row_ptr[i + 1]; k++) {
      if (col_ind[k] == j) {
        return values[k];
      }
    }

    throw std::invalid_argument("No nonzero at row col:"
                                    + std::to_string(i) + " "
                                    + std::to_string(j));
  }

  bool isNnz(int i, int j) {
    // TODO efficiency could be improved
    try {
      get(i, j);
    } catch (std::invalid_argument) {
      return false;
    }
    return true;
  }

  bool isSymmetric() const {
    return true;
  }

  DokMatrix toDok() const {
    DokMatrix m(n, this->m, nnzs);
    for (int i = 0; i < n; i++) {
      for (int k = row_ptr[i]; k < row_ptr[i + 1]; k++) {
        m.dok[i][col_ind[k]] = values[k];
      }
    }
    return m;
  }

  bool operator==(const CsrMatrix& o) const {
    return n == o.n &&
        m == o.m &&
        nnzs == o.nnzs &&
        values == o.values &&
        row_ptr == o.row_ptr &&
        col_ind == o.col_ind;
  }

  std::vector<int> getRowPtrWithOneBasedIndex() const {
    std::vector<int> rowIndexed;
    std::transform(
        row_ptr.begin(), row_ptr.end(), std::back_inserter(rowIndexed),
        [](int x){return x + 1;}
    );
    return rowIndexed;
  }

  std::vector<int> getColIndWithOneBasedIndex() const {
    std::vector<int> colIndexed;
    std::transform(
        col_ind.begin(), col_ind.end(), std::back_inserter(colIndexed),
        [](int x){return x + 1;}
    );
    return colIndexed;

  }

  CsrMatrix getLowerTriangular() const {
    return CsrMatrix(toDok().getLowerTriangular());
  }

  CsrMatrix getUpperTriangular() const {
    return CsrMatrix(toDok().getUpperTriangular());
  }

  Vector dot(const Vector& b) const {
    return toDok().dot(b);
  }

  CsrMatrix sliceRows(int startRow, int nRows) const {
    //startPos = row_ptr[startRow];
    std::vector<int> newRowPtr, newColInd;
    std::vector<double> newValues;
    int nnzs = 0;
    int offset = row_ptr[startRow];
    for (int i = startRow; i < startRow + nRows; i++) {
      int r = row_ptr[i];
      newRowPtr.push_back(r - offset);
      for (int j = r; j < row_ptr[i + 1]; j++) {
        newValues.push_back(values[j]);
        newColInd.push_back(col_ind[j]);
        nnzs++;
      }
    }
    newRowPtr.push_back(newValues.size());
    return CsrMatrix(nRows, this->m, nnzs, newValues, newColInd, newRowPtr);
  }

  /** Slices columns from this matrix into blocks. Each block has as
      many rows as this matrix but at most blockSize columns.  All
      blocks except the last one have exactly blockSize columns.

      Example:

      This matrix:
      1 2 3 4
      5 6 7 8
      sliceColumns(3) returns:
      1 2 3 | 4
      5 6 7 | 8
  */
  std::vector<cask::CsrMatrix> sliceColumns(int blockSize) const {
    // XXX constructing a CSR matrix this way is not safe because m and n are not updated
    int nBlocks = m / blockSize + (m % blockSize == 0 ? 0 : 1);
    std::vector<cask::CsrMatrix> partitions(nBlocks);
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < nBlocks; j++) {
        auto& p = partitions[j].row_ptr;
        if (p.size() == 0)
          p.push_back(0);
        else
          p.push_back(p.back());
      }
      //std::cout << "i = " << i << std::endl;
      //std::cout << "colptr" << colptr[i] << std::endl;
      for (int j = row_ptr[i]; j < row_ptr[i+1]; j++) {
        auto& p = partitions[col_ind[j] / blockSize];
        int idxInPartition = col_ind[j] - (col_ind[j] / blockSize ) * blockSize;
        p.col_ind.push_back(idxInPartition);
        p.values.push_back(values[j]);
        p.row_ptr.back()++;
      }
    }
    return partitions;
  }

};

/** A symmetric matrix for which only the lower triangle is stored explicitly, in CSR format */
class SymCsrMatrix {
 public:
  int n, m;
  int nnzs;
  CsrMatrix matrix;

  // Construct a symmetric matrix from a lower triangular matrix in DoK format
  explicit SymCsrMatrix(const DokMatrix& l) : n(l.n), m(l.m), matrix(l) {
    int diagNnzs = 0;
    for (int i = 0; i < l.n; i++)
      if (l.at(i, i) != 0)
        diagNnzs++;
    nnzs = 2 * (l.nnzs - diagNnzs) + diagNnzs;
  }

  void print() {
    // matrix.print();
  }

  void pretty_print() {
    std::cout << "Stored matrix: " << std::endl;
    matrix.pretty_print();
    std::cout << "Implicit values: " << std::endl;
    matrix.toDok().explicitSymmetric().pretty_print();
  }

  Vector dot(const Vector& b) const {
    return matrix.toDok().explicitSymmetric().dot(b);
  }

};

}

#endif /* end of include guard: SPARSEMATRIX_HPP_1BWMMLC8 */


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of integral class.
 */
#include "num_collect/base/concepts/integral.h"

#include <cstdint>

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>

TEST_CASE("num_collect::concepts::integral") {
    using num_collect::concepts::integral;

    SECTION("check") {
        STATIC_REQUIRE(integral<int>);
        STATIC_REQUIRE(integral<std::int64_t>);
        STATIC_REQUIRE(integral<std::uint16_t>);
        STATIC_REQUIRE_FALSE(integral<float>);
        STATIC_REQUIRE_FALSE(integral<double>);
        STATIC_REQUIRE_FALSE(integral<Eigen::MatrixXd>);
    }
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// 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 "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/dominator_tree.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_dominator_tree
{
    template <class Graph, class PredMap>
    void operator()(const Graph& g, size_t entry, PredMap pred_map) const
    {
        lengauer_tarjan_dominator_tree(g, vertex(entry,g), pred_map);
    }
};

typedef property_map_types::apply<mpl::vector<int32_t>,
                                  GraphInterface::vertex_index_map_t,
                                  mpl::bool_<false> >::type
    pred_properties;

void dominator_tree(GraphInterface& gi, size_t entry, boost::any pred_map)
{
    run_action<graph_tool::detail::always_directed>()
        (gi, std::bind(get_dominator_tree(), std::placeholders::_1, entry,
                       std::placeholders::_2),
         pred_properties())(pred_map);
}


// (C) Copyright Jeremy Siek 2001.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
//  Sample output:
//
//  bits(0) = 00
//  bits(1) = 01
//  bits(2) = 10
//  bits(3) = 11

#include <iostream>
#include <boost/dynamic_bitset.hpp>

int main()
{
  const boost::dynamic_bitset<> b0(2, 0ul);
  std::cout << "bits(0) = " << b0 << std::endl;

  const boost::dynamic_bitset<> b1(2, 1ul);
  std::cout << "bits(1) = " << b1 << std::endl;

  const boost::dynamic_bitset<> b2(2, 2ul);
  std::cout << "bits(2) = " << b2 << std::endl;

  const boost::dynamic_bitset<> b3(2, 3ul);
  std::cout << "bits(3) = " << b3 << std::endl;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::common::concept::aggregate::expectations.hpp                   //
//                                                                              //
//  (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_COMMON_CONCEPT_AGGREGATE_EXPECTATIONS_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_CONCEPT_AGGREGATE_EXPECTATIONS_HPP_ER_2009
#include <boost/mpl/empty_base.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/mean.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/variance.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace common{
namespace concept{

    template<
        typename B = mpl::empty_base
    >
    class Expectations : public
        Mean<
            Variance<
                B
            >
        >
    {
    };

}// concept
}// common
}// distribution

}// statistics
}// detail
}// boost

#endif


#pragma once

#include <Eigen/Dense>
#include <iostream>
#include <stdio.h>

#include "pnc/robot_system/robot_system.hpp"
#include "utils/util.hpp"

/// class Contact
class Contact {
public:
  /// \{ \name Constructor and Destructor
  Contact(RobotSystem *_robot, const int _dim, const std::string _target_id,
          const double _mu) {
    robot_ = _robot;

    dim = _dim;
    target_id = _target_id;
    mu_ = _mu;
    rf_z_max = 1000.;

    jacobian = Eigen::MatrixXd::Zero(dim, robot_->n_q_dot);
    jacobian_dot_q_dot = Eigen::VectorXd::Zero(dim);
  }

  virtual ~Contact() {}
  /// \}

  /// Contact dimension
  int dim;

  /// Contact frame
  std::string target_id;

  /// Maximum normal force
  double rf_z_max;

  /// Contact jacobian
  Eigen::MatrixXd jacobian;

  /// Contact jacobian times qdot
  Eigen::MatrixXd jacobian_dot_q_dot;

  /// Contact constraint matrix
  Eigen::MatrixXd cone_constraint_mat;

  /// Contact constraint vector
  Eigen::VectorXd cone_constraint_vec;

  /// Update contact jacobian and contact constraint.
  void update_contact() {
    this->_update_jacobian();
    this->_update_cone_constraint();
  };

protected:
  RobotSystem *robot_;

  virtual void _update_jacobian() = 0;
  virtual void _update_cone_constraint() = 0;

  double mu_;
};


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>
#include <vector>

#include "src/hierarchies/abstract_hierarchy.h"
#include "src/mixings/logit_sb_mixing.h"
#include "src/utils/io_utils.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

TEST(logit_sb, misc) {
  // Initialize parameters
  auto &rng = bayesmix::Rng::Instance().get();
  rng.seed(20201124);
  unsigned int dim = 2;
  unsigned int n_comp = 3;
  unsigned int n_data = 90;
  unsigned int n_iter = 20;

  // DATA GENERATION
  // True coefficients
  Eigen::MatrixXd cov_centers(dim, n_comp);
  cov_centers.col(0) << -5.0, 0.0;
  cov_centers.col(1) << 5.0, 0.0;
  cov_centers.col(2) << 0.0, 5.0;
  // Allocations
  std::vector<unsigned int> allocations(n_data);
  for (int i = 0; i < n_data / 3; i++) {
    allocations[n_data / 3 + i] = 1;
    allocations[2 * n_data / 3 + i] = 2;
  }
  // Covariates, based on cluster allocation
  Eigen::MatrixXd covariates(n_data, dim);
  for (int i = 0; i < n_data; i++) {
    covariates.row(i) =
        stan::math::multi_normal_rng(cov_centers.col(allocations[i]),
                                     Eigen::MatrixXd::Identity(dim, dim), rng);
  }
  // INITIALIZATION
  // Prior parameters
  double step = 0.025;
  auto prior_mean = Eigen::VectorXd::Zero(dim);
  auto cov = 5.0 * Eigen::MatrixXd::Identity(dim, dim);
  // Set parameters to mixing object
  LogitSBMixing mix;
  bayesmix::LogSBPrior prior;
  prior.set_step_size(step);
  prior.set_num_components(n_comp);
  bayesmix::to_proto(prior_mean, prior.mutable_normal_prior()->mutable_mean());
  bayesmix::to_proto(cov, prior.mutable_normal_prior()->mutable_var());
  mix.get_mutable_prior()->CopyFrom(prior);
  mix.set_covariates(&covariates);
  mix.initialize();

  // TESTING
  // Test prior read/write
  google::protobuf::Message *prior_out = mix.get_mutable_prior();
  ASSERT_EQ(prior.DebugString(), prior_out->DebugString());
  // Test initialization of state
  Eigen::MatrixXd coeffs = mix.get_state().regression_coeffs;
  for (int i = 0; i < coeffs.cols(); i++) {
    // Check equality of i-th column, element-by-element
    for (int j = 0; j < prior_mean.size(); j++) {
      ASSERT_DOUBLE_EQ(prior_mean(j), coeffs(j, i));
    }
  }
  // M-H run
  std::vector<std::shared_ptr<AbstractHierarchy>> hierarchies(n_comp);
  for (int i = 0; i < n_iter; i++) {
    mix.update_state(hierarchies, allocations);
    // std::cout << i << "\n" << mix.get_state().regression_coeffs <<
    // std::endl;
  }
  std::cout << "acceptance rates: " << mix.get_acceptance_rates().transpose()
            << std::endl;
  // Weights with test set
  Eigen::VectorXd test1(2);
  test1 << -5, 0;
  Eigen::VectorXd test2(2);
  test2 << 5, 0;
  Eigen::VectorXd test3(2);
  test3 << 0, 5;
  std::cout << "test1: " << mix.get_weights(test1).transpose() << std::endl;
  std::cout << "test2: " << mix.get_weights(test2).transpose() << std::endl;
  std::cout << "test3: " << mix.get_weights(test3).transpose() << std::endl;
}


// Copyright (c) 2022, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#include "estimators/fundamental_matrix.h"

#include <cfloat>
#include <complex>
#include <vector>

#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "base/polynomial.h"
#include "estimators/utils.h"
#include "util/logging.h"

namespace colmap {

std::vector<FundamentalMatrixSevenPointEstimator::M_t>
FundamentalMatrixSevenPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), 7);
  CHECK_EQ(points2.size(), 7);

  // Note that no normalization of the points is necessary here.

  // Setup system of equations: [points2(i,:), 1]' * F * [points1(i,:), 1]'.
  Eigen::Matrix<double, 7, 9> A;
  for (size_t i = 0; i < 7; ++i) {
    const double x0 = points1[i](0);
    const double y0 = points1[i](1);
    const double x1 = points2[i](0);
    const double y1 = points2[i](1);
    A(i, 0) = x1 * x0;
    A(i, 1) = x1 * y0;
    A(i, 2) = x1;
    A(i, 3) = y1 * x0;
    A(i, 4) = y1 * y0;
    A(i, 5) = y1;
    A(i, 6) = x0;
    A(i, 7) = y0;
    A(i, 8) = 1;
  }

  // 9 unknowns with 7 equations, so we have 2D null space.
  Eigen::JacobiSVD<Eigen::Matrix<double, 7, 9>> svd(A, Eigen::ComputeFullV);
  const Eigen::Matrix<double, 9, 9> f = svd.matrixV();
  Eigen::Matrix<double, 1, 9> f1 = f.col(7);
  Eigen::Matrix<double, 1, 9> f2 = f.col(8);

  f1 -= f2;

  // Normalize, such that lambda + mu = 1
  // and add constraint det(F) = det(lambda * f1 + (1 - lambda) * f2).

  const double t0 = f1(4) * f1(8) - f1(5) * f1(7);
  const double t1 = f1(3) * f1(8) - f1(5) * f1(6);
  const double t2 = f1(3) * f1(7) - f1(4) * f1(6);
  const double t3 = f2(4) * f2(8) - f2(5) * f2(7);
  const double t4 = f2(3) * f2(8) - f2(5) * f2(6);
  const double t5 = f2(3) * f2(7) - f2(4) * f2(6);

  Eigen::Vector4d coeffs;
  coeffs(0) = f1(0) * t0 - f1(1) * t1 + f1(2) * t2;
  coeffs(1) = f2(0) * t0 - f2(1) * t1 + f2(2) * t2 -
              f2(3) * (f1(1) * f1(8) - f1(2) * f1(7)) +
              f2(4) * (f1(0) * f1(8) - f1(2) * f1(6)) -
              f2(5) * (f1(0) * f1(7) - f1(1) * f1(6)) +
              f2(6) * (f1(1) * f1(5) - f1(2) * f1(4)) -
              f2(7) * (f1(0) * f1(5) - f1(2) * f1(3)) +
              f2(8) * (f1(0) * f1(4) - f1(1) * f1(3));
  coeffs(2) = f1(0) * t3 - f1(1) * t4 + f1(2) * t5 -
              f1(3) * (f2(1) * f2(8) - f2(2) * f2(7)) +
              f1(4) * (f2(0) * f2(8) - f2(2) * f2(6)) -
              f1(5) * (f2(0) * f2(7) - f2(1) * f2(6)) +
              f1(6) * (f2(1) * f2(5) - f2(2) * f2(4)) -
              f1(7) * (f2(0) * f2(5) - f2(2) * f2(3)) +
              f1(8) * (f2(0) * f2(4) - f2(1) * f2(3));
  coeffs(3) = f2(0) * t3 - f2(1) * t4 + f2(2) * t5;

  Eigen::VectorXd roots_real;
  Eigen::VectorXd roots_imag;
  if (!FindPolynomialRootsCompanionMatrix(coeffs, &roots_real, &roots_imag)) {
    return {};
  }

  std::vector<M_t> models;
  models.reserve(roots_real.size());

  for (Eigen::VectorXd::Index i = 0; i < roots_real.size(); ++i) {
    const double kMaxRootImag = 1e-10;
    if (std::abs(roots_imag(i)) > kMaxRootImag) {
      continue;
    }

    const double lambda = roots_real(i);
    const double mu = 1;

    Eigen::MatrixXd F = lambda * f1 + mu * f2;

    F.resize(3, 3);

    const double kEps = 1e-10;
    if (std::abs(F(2, 2)) < kEps) {
      continue;
    }

    F /= F(2, 2);

    models.push_back(F.transpose());
  }

  return models;
}

void FundamentalMatrixSevenPointEstimator::Residuals(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2,
    const M_t& F, std::vector<double>* residuals) {
  ComputeSquaredSampsonError(points1, points2, F, residuals);
}

std::vector<FundamentalMatrixEightPointEstimator::M_t>
FundamentalMatrixEightPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());

  // Center and normalize image points for better numerical stability.
  std::vector<X_t> normed_points1;
  std::vector<Y_t> normed_points2;
  Eigen::Matrix3d points1_norm_matrix;
  Eigen::Matrix3d points2_norm_matrix;
  CenterAndNormalizeImagePoints(points1, &normed_points1, &points1_norm_matrix);
  CenterAndNormalizeImagePoints(points2, &normed_points2, &points2_norm_matrix);

  // Setup homogeneous linear equation as x2' * F * x1 = 0.
  Eigen::Matrix<double, Eigen::Dynamic, 9> cmatrix(points1.size(), 9);
  for (size_t i = 0; i < points1.size(); ++i) {
    cmatrix.block<1, 3>(i, 0) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 0) *= normed_points2[i].x();
    cmatrix.block<1, 3>(i, 3) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 3) *= normed_points2[i].y();
    cmatrix.block<1, 3>(i, 6) = normed_points1[i].homogeneous();
  }

  // Solve for the nullspace of the constraint matrix.
  Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> cmatrix_svd(
      cmatrix, Eigen::ComputeFullV);
  const Eigen::VectorXd cmatrix_nullspace = cmatrix_svd.matrixV().col(8);
  const Eigen::Map<const Eigen::Matrix3d> ematrix_t(cmatrix_nullspace.data());

  // Enforcing the internal constraint that two singular values must non-zero
  // and one must be zero.
  Eigen::JacobiSVD<Eigen::Matrix3d> fmatrix_svd(
      ematrix_t.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector3d singular_values = fmatrix_svd.singularValues();
  singular_values(2) = 0.0;
  const Eigen::Matrix3d F = fmatrix_svd.matrixU() *
                            singular_values.asDiagonal() *
                            fmatrix_svd.matrixV().transpose();

  const std::vector<M_t> models = {points2_norm_matrix.transpose() * F *
                                   points1_norm_matrix};
  return models;
}

void FundamentalMatrixEightPointEstimator::Residuals(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2,
    const M_t& E, std::vector<double>* residuals) {
  ComputeSquaredSampsonError(points1, points2, E, residuals);
}

}  // namespace colmap


// std includes
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <cmath>
#include <ctime>
#include <memory>

// Armadillo includes and preprocessors
#include <armadillo>
#include <viennacl/ocl/forwards.h>
// ViennaCL includes
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/ilu.hpp"
//#include "viennacl/linalg/ilu/chow_patel_ilu.hpp"
#include "viennacl/linalg/lu.hpp"
#include "viennacl/linalg/amg.hpp"
#include "viennacl/linalg/sum.hpp"
#include "viennacl/linalg/maxmin.hpp"
#include "viennacl/tools/timer.hpp"
#include "viennacl/forwards.h"

// Main CoMFi include
#include "comfi.hpp"

using namespace std;

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

  // Create intermediate variable but use constant for safety
	comfi::types::Settings __set;
	if (argc > 1) {
		comfi::util::interpret_arguments(__set, argc, argv);
	}
  const comfi::types::Settings settings = __set;

  // Create context
  comfi::types::Context ctx(1,
                            400,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN);
  unique_ptr<vcl_mat> xn_vcl(new vcl_mat(comfi::util::shock_tube_ic(ctx)));
  //unique_ptr<vcl_mat> xn_vcl(new vcl_mat(comfi::util::ot_vortex_ic(ctx)));
  unique_ptr<vcl_mat> xn1_vcl(new vcl_mat(*xn_vcl));
  comfi::util::save_solution(*xn_vcl, ctx);

  // Initial std output
  cout << "Begin CoMFi Simulation" << endl;
  cout << "======================" << endl
       << endl;
  #ifdef VIENNACL_WITH_OPENCL
    cout << "Device Info:" << endl
         << "------------" << endl
         << viennacl::ocl::current_device().info()
         << endl;
  #endif
  #ifdef _OPENMP
    cout << "OpenMP Info:" << endl
         << "------------" << endl
         << "- OMP Max threads: " << omp_get_max_threads() << endl
         << "- OMP Devices: " << omp_get_default_device() << "/" << omp_get_num_devices() << endl
         << endl;
  #endif
  cout << "Parameters:" << endl;
  cout << "-----------" << endl;
  cout << "- Grid size: (" << ctx.nz << ", " << ctx.nx << ") " << endl
       << "- Normalization constants:" << endl
       << " - " << "n_0: " << ctx.n_0 << " m^-3" << endl
       << " - " << "L_0: " << ctx.l_0 << " m" << endl
       << " - " << "t_0: " << ctx.t_0 << " s" << endl
       << " - " << "V_0: " << ctx.V_0 << " m/s" << endl
       << " - " << "B_0: " << ctx.B_0/0.1e-3 << " G" << endl
       << " - " << "e_0: " << ctx.e_0 << " C" << endl
       << " - " << "T_0: " << ctx.T_0 << " K" << endl
       << " - " << "p_0: "<< ctx.p_0 << " Pa" << endl
       << " - " << "Width: " << ctx.width << " m" << endl
       << " - " << "Height: " << ctx.height << " m" << endl
       << " - " << "dx: " << ctx.dx*ctx.l_0 << " m | " << "dz: " << ctx.dz*ctx.l_0 << " m | "
                << "ds: " << ctx.ds*ctx.l_0 << " m" << endl
       << endl;

  //Initiate
  viennacl::tools::timer vcl_timer[2]; // timers
  //auto init = mhdsim::util::calcInitialCondition(op);
  //auto init = mhdsim::util::calcReconnectionIC(op);
  //auto init = comfi::util::calcShockTubeIC(op);
  //auto init = comfi::util::calcSolerIC(op);
  //arma::vec x0 = std::get<0>(init);
  //const comfi::types::BgData bg;

  cout << "Compiling kernels ... ";
  std::ifstream phi_file("kernels_ocl/phi.c");
  string phi_code((std::istreambuf_iterator<char>(phi_file)),
                   std::istreambuf_iterator<char>()
                 );
  viennacl::ocl::program & phi_prog = viennacl::ocl::current_context().add_program(phi_code.c_str(), "fluxl"); // compile flux opencl kernel
  std::ifstream eig_file("kernels_ocl/element_max.c");
  string eig_code((std::istreambuf_iterator<char>(eig_file)),
                   std::istreambuf_iterator<char>()
                 );
  viennacl::ocl::program & eig_prog = viennacl::ocl::current_context().add_program(eig_code.c_str(), "element_max"); // compile eigenvalue opencl kernel
  cout << "built." << endl;

  // Full execution timer
  vcl_timer[0].start();

  // Begin solving/advancing
  while (((ctx.time_step() < settings.max_time_steps) || (settings.max_time_steps < 0)) &&
         ((ctx.time_elapsed() < settings.max_time) || (settings.max_time < 0.0))) {

    double solve_time=0.0, build_time=0.0;

    // Figure out time step
    const double V = comfi::util::getmaxV(*xn_vcl, ctx);
    cout << "Char speed: "  << V << " V_0" << endl;
    const double ds = (ctx.dx>ctx.dz)*ctx.dz + (ctx.dz>=ctx.dx)*ctx.dx;
    ctx.set_dt(0.8*0.5*ds/V);
    cout << "dt: " << ctx.dt() << " t_0";
    cout << "\t| Time: " << ctx.time_elapsed() << " t_0" << endl;

    /* //Update loop stuff */
    /* t(ctx.time_step()) = ctx.time_elapsed(); */
    /* dt_n(ctx.time_step()) = ctx.dt(); */

    cout << ctx.time_step() << ": " ;

    vcl_timer[1].start();
    //const arma::sp_mat Ri_cpu = mhdsim::routines::computeRi(*xn_vcl, op);
    //static const arma::sp_mat one = arma::speye(num_of_elem, num_of_elem);
    //const arma::sp_mat LHS_cpu = one + Ri_cpu*dt; // Euler
    //vcl_sp_mat LHS;
    //viennacl::copy(LHS_cpu, LHS);
    build_time = vcl_timer[1].get();

    vcl_timer[1].start();
    //GMRES
    // solve (e.g. using GMRES solver)
    // create and compute preconditioner:
    //viennacl::linalg::ilu0_tag ilu0_config(true);
    //viennacl::linalg::block_ilu_precond<vcl_sp_mat, viennacl::linalg::ilu0_tag> vcl_precond(LHS, ilu0_config);
    //viennacl::linalg::ilu0_precond< vcl_sp_mat > vcl_precond(LHS, viennacl::linalg::ilu0_tag());
    //viennacl::linalg::gmres_tag my_solver_tag(tolerance, 100, 20);
    //viennacl::linalg::bicgstab_tag my_solver_tag(tolerance, 100, 20);
    //unique_ptr<vcl_vec> x0_vcl(new vcl_vec(viennacl::linalg::solve(LHS, RHS, my_solver_tag, vcl_precond)));
    /* unique_ptr<vcl_mat> x0_vcl(new vcl_mat(comfi::routines::computeRHS_Euler(*xn_vcl, ctx))); */
    unique_ptr<vcl_mat> x0_vcl(new vcl_mat(comfi::routines::computeRHS_RK4(*xn_vcl, ctx)));
    ctx.advance();
    solve_time = vcl_timer[1].get();

//    cout << " GMRES(" << my_solver_tag.iters();
//    cout << "," << my_solver_tag.error();
//    cout << ") | Build Time:" << build_time;
//    cout << "s\t| Sol Time:" << solve_time << "s" << endl;
//    const double sol_error = my_solver_tag.error();
    /* double sol_error = 0; */

    //mhdsim::util::save_solution(x0_vcl, -1, op);
    //mhdsim::util::save_solution(xn_vcl, -2, op);
    //mhdsim::util::save_solution(xn1_vcl, -3, op);

    //Save solution every save_dt time
    static double time_since_last_save = 0.0;
    time_since_last_save += ctx.dt();
    if ((time_since_last_save > settings.save_dt) && (settings.save_dt > 0.0)) {
      comfi::util::save_solution(*x0_vcl, ctx);
      time_since_last_save = 0.0;
    }

    //Save solution every save_dn steps
    if ((ctx.time_step()%settings.save_dn == 0) && (settings.save_dn > 0)) {
      comfi::util::save_solution(*x0_vcl, ctx);
    }

    // Move pointers before starting next step
    xn1_vcl = std::move(xn_vcl);
    xn_vcl = std::move(x0_vcl);

  }

  cout << "Total exec time: " << vcl_timer[0].get() << endl;

  return 0;
}

/*
vim: tabstop=2
vim: shiftwidth=2
vim: smarttab
vim: expandtab
*/


//sensor_msgs/Imu

#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <vector>
#include <iostream>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "sensor_msgs/Imu.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Bool.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace cv;
using namespace Eigen;

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
roll = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
if (fabs(sinp) >= 1)
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
pitch = asin(sinp);

// yaw (z-axis rotation)
double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny_cosp, cosy_cosp);
}
static float toAngle(double& r)
{
    return 180 * r / M_PI;
}
class message_sync_ros_node
{
private:
    ros::NodeHandle node_;
    ros::Subscriber init_sub,yt_sub,imu_sub;
    typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::Imu> slamSyncPolicy;
    message_filters::Subscriber<nav_msgs::Odometry> *yt_sub_;
    message_filters::Subscriber<sensor_msgs::Imu> *imu_sub_;
    message_filters::Synchronizer<slamSyncPolicy> *sync_;
    std::string yt_topic;
    std::string imu_topic;
    std::string root_path;
    bool is_record,is_init;
    ofstream map_points;
    stringstream gss;
    float init_yt_x,init_yt_y,init_yt_z,init_imu_x,init_imu_y,init_imu_z;

public:
    message_sync_ros_node();
    ~message_sync_ros_node();
    void callback(const nav_msgs::Odometry::ConstPtr &yt_data,const sensor_msgs::Imu::ConstPtr &imu_data);
    void update();
    void initCB(const std_msgs::Bool::ConstPtr &msg);
    void ytCB(const nav_msgs::Odometry::ConstPtr &yt_data);
    void imuCB(const sensor_msgs::Imu::ConstPtr &imu_data);
    void isDirectory(string path);
};

message_sync_ros_node::message_sync_ros_node()
{
    is_record = false;
    is_init = false;
    //map_points.open("/home/li/capture/0919/record.txt", ios_base::app);
    ros::param::get("/sync_imu/yt_topic", yt_topic);
    ros::param::get("/sync_imu/imu_topic", imu_topic);
    ros::param::get("/sync_imu/root_path", root_path);
    std::cout << "yt_topic:" << yt_topic << std::endl;
    std::cout << "imu_topic:" << imu_topic << std::endl;
    std::cout << "root_path:" << root_path << std::endl;

    isDirectory(root_path);
    string record_path = root_path + "/record.txt";
    map_points.open(record_path, ios_base::app);
    init_sub = node_.subscribe<std_msgs::Bool>("sync_imu/init", 1, &message_sync_ros_node::initCB, this);
    yt_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(node_, yt_topic, 1);
    imu_sub_ = new message_filters::Subscriber<sensor_msgs::Imu>(node_, imu_topic, 1);
    sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(20), *yt_sub_, *imu_sub_);
    sync_->registerCallback(boost::bind(&message_sync_ros_node::callback, this, _1, _2));

    //sub
    yt_sub = node_.subscribe<nav_msgs::Odometry>(yt_topic, 1, &message_sync_ros_node::ytCB, this);
    imu_sub = node_.subscribe<sensor_msgs::Imu>(imu_topic, 1, &message_sync_ros_node::imuCB, this);
}

void message_sync_ros_node::initCB(const std_msgs::Bool::ConstPtr &msg){
    std::cout << "initCB " << std::endl;
     is_init = true;
}

message_sync_ros_node::~message_sync_ros_node()
{
    map_points.close();
}

void message_sync_ros_node::update()
{
   std::cout << "update " << std::endl;
}

void message_sync_ros_node::isDirectory(string path)
{
    //判断根目录下是否存在子目录，不存在创建
    if (!boost::filesystem::exists(path))
    {
        boost::filesystem::create_directory(path);
    }
}

void message_sync_ros_node::callback(const nav_msgs::Odometry::ConstPtr &yt_data,const sensor_msgs::Imu::ConstPtr &imu_data)
{
    std::cout << "sync message" << std::endl;
    // nav_msgs::Odometry current_pose;
    // current_pose = *odom_data;
    float angle_x = yt_data->pose.pose.position.x;
    float angle_y = yt_data->pose.pose.position.y;
    float angle_z = yt_data->pose.pose.position.z;
    //std::cout << "angle_x:" << angle_x<< " angle_y:" << angle_y<< " angle_z:" << angle_z << std::endl;
    //
    //float imu_x = imu_data->orientation.x;
    geometry_msgs::Quaternion qua = imu_data->orientation;
    Eigen::Quaterniond q(qua.w, qua.x, qua.y, qua.z);
    double roll,pitch,yaw;
    toEulerAngle(q,roll,pitch,yaw);
    //std::cout << "roll:" << roll<< " pitch:" << pitch<< " yaw:" << yaw << std::endl;
    if(is_init){
        std::cout << "init:" << std::endl;
        init_yt_x = angle_x;
        init_yt_y = angle_y;
        init_yt_z = angle_z;
        init_imu_x = toAngle(roll);
        init_imu_y = toAngle(pitch);
        init_imu_z = toAngle(yaw);
        is_init = false;
    }
    float dx = abs(angle_x - init_yt_x)/100;
    float dy = abs(angle_y - init_yt_y)/100;
    float dz = abs(angle_z - init_yt_z)/100;
    std::cout << "ptz dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    map_points << "ptz dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    dx = abs(toAngle(roll) - init_imu_x);
    dy = abs(toAngle(pitch) - init_imu_y);
    dz = abs(toAngle(yaw) - init_imu_z);
    //std::cout << "roll:" << toAngle(roll)<< " pitch:" << toAngle(pitch) << " yaw:" << toAngle(yaw) << std::endl;
    std::cout << "imu dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    map_points << "imu dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
}

void message_sync_ros_node::ytCB(const nav_msgs::Odometry::ConstPtr &yt_data)
{
    float angle_x = abs(yt_data->pose.pose.position.x)/100;
    float angle_y = abs(yt_data->pose.pose.position.y)/100;
    float angle_z = abs(yt_data->pose.pose.position.z)/100;
    std::cout << "PTZ data receive time:" << yt_data->header.stamp << " angle_x:" << angle_x<< " angle_y:" << angle_y << " angle_z:" << angle_z << std::endl;
    map_points << "PTZ data receive time:" << yt_data->header.stamp << " angle_x:" << angle_x<< " angle_y:" << angle_y << " angle_z:" << angle_z << std::endl;
}

void message_sync_ros_node::imuCB(const sensor_msgs::Imu::ConstPtr &imu_data)
{
    geometry_msgs::Quaternion qua = imu_data->orientation;
    Eigen::Quaterniond q(qua.w, qua.x, qua.y, qua.z);
    double roll,pitch,yaw;
    toEulerAngle(q,roll,pitch,yaw);
    if(is_init){
        std::cout << "init:" << std::endl;
        init_imu_x = toAngle(roll);
        init_imu_y = toAngle(pitch);
        init_imu_z = toAngle(yaw);
        is_init = false;
    }
    float dx = abs(toAngle(roll) - init_imu_x);
    float dy = abs(toAngle(pitch) - init_imu_y);
    float dz = abs(toAngle(yaw) - init_imu_z);
    std::cout << "IMU data receive time:" << imu_data->header.stamp << " roll:" << dx<< " pitch:" << dy << " yaw:" << dz << std::endl;
    map_points << "IMU data receive time:" << imu_data->header.stamp << " roll:" << dx<< " pitch:" << dy << " yaw:" << dz << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "message_sync_node");
    message_sync_ros_node node;

    ROS_INFO("message_sync_node node started...");
    ros::Rate rate(10);

    while (ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}