/*
    Copyright 2013 Adobe
    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)
*/
/*************************************************************************************************/

#ifndef ADOBE_ALGORITHM_EQUAL_RANGE_HPP
#define ADOBE_ALGORITHM_EQUAL_RANGE_HPP

#include <adobe/config.hpp>

#include <algorithm>
#include <cassert>
#include <utility>
#include <functional>

#include <boost/next_prior.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

#include <adobe/algorithm/lower_bound.hpp>
#include <adobe/algorithm/upper_bound.hpp>
#include <adobe/functional.hpp>

/*************************************************************************************************/

/*!
\defgroup equal_range equal_range
\ingroup sorting

\see
    - STL documentation for \ref stldoc_equal_range
*/

/*************************************************************************************************/

namespace adobe {
namespace implementation {

/*************************************************************************************************/

template <typename I, // I models ForwardIterator
          typename N, // N models IntegralType
          typename T, // T == result_type(P)
          typename C, // C models StrictWeakOrdering(T, T)
          typename P>
// P models UnaryFunction(value_type(I)) -> T
std::pair<I, I> equal_range_n(I f, N n, const T& x, C c, P p) {
    assert(!(n < 0) && "FATAL (sparent) : n must be >= 0");

    while (n != 0) {
        N h = n >> 1;
        I m = boost::next(f, h);

        if (c(p(*m), x)) {
            f = boost::next(m);
            n -= h + 1;
        } else if (c(x, p(*m))) {
            n = h;
        } else {
            return std::make_pair(
                implementation::lower_bound_n_(f, h, x, c, p),
                implementation::upper_bound_n(boost::next(m), n - (h + 1), x, c, p));
        }
    }
    return std::make_pair(f, f);
}

/*************************************************************************************************/

template <typename I> // I models ForwardRange
struct lazy_range {
    typedef std::pair<typename boost::range_iterator<I>::type,
                      typename boost::range_iterator<I>::type> type;
};

/*************************************************************************************************/

template <typename I> // I models ForwardRange
struct lazy_range_const {
    typedef std::pair<typename boost::range_const_iterator<I>::type,
                      typename boost::range_const_iterator<I>::type> type;
};

/*************************************************************************************************/

} // namespace implementation

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename N, // N models IntegralType
          typename T>
// T == result_type(P)
inline std::pair<I, I> equal_range_n(I f, N n, const T& x) {
    return implementation::equal_range_n(f, n, x, less(), identity<T>());
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename N, // N models IntegralType
          typename T, // T == result_type(P)
          typename C>
// C models StrictWeakOrdering(T, T)
inline std::pair<I, I> equal_range_n(I f, N n, const T& x, C c) {
    return implementation::equal_range_n(f, n, x, std::bind(c, std::placeholders::_1, std::placeholders::_2), identity<T>());
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename N, // N models IntegralType
          typename T, // T == result_type(P)
          typename C, // C models StrictWeakOrdering(T, T)
          typename P>
// P models UnaryFunction(value_type(I)) -> T
inline std::pair<I, I> equal_range_n(I f, N n, const T& x, C c, P p) {
    return implementation::equal_range_n(f, n, x, std::bind(c, std::placeholders::_1, std::placeholders::_2), std::bind(p, std::placeholders::_1));
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename T>
// T == value_type(I)
inline std::pair<I, I> equal_range(I f, I l, const T& x) {
    return std::equal_range(f, l, x);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename T, // T == result_type(P)
          typename C>
// C models StrictWeakOrdering(T, T)
inline std::pair<I, I> equal_range(I f, I l, const T& x, C c) {
    return std::equal_range(f, l, x, std::bind(c, std::placeholders::_1, std::placeholders::_2));
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardIterator
          typename T, // T == result_type(P)
          typename C, // C models StrictWeakOrdering(T, T)
          typename P>
// P models UnaryFunction(value_type(I)) -> T
inline std::pair<I, I> equal_range(I f, I l, const T& x, C c, P p) {
    return equal_range_n(f, std::distance(f, l), x, c, p);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T, // T == result_type(P)
          typename C, // C models StrictWeakOrdering(T, T)
          typename P> // P models UnaryFunction(value_type(I)) -> T
inline typename boost::lazy_disable_if<boost::is_same<I, T>, implementation::lazy_range<I>>::type
equal_range(I& r, const T& x, C c, P p) {
    return adobe::equal_range(boost::begin(r), boost::end(r), x, c, p);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T, // T == result_type(P)
          typename C, // C models StrictWeakOrdering(T, T)
          typename P> // P models UnaryFunction(value_type(I)) -> T
inline typename boost::lazy_disable_if<boost::is_same<I, T>,
                                       implementation::lazy_range_const<I>>::type
equal_range(const I& r, const T& x, C c, P p) {
    return adobe::equal_range(boost::begin(r), boost::end(r), x, c, p);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T> // T == result_type(P)
inline std::pair<typename boost::range_iterator<I>::type, typename boost::range_iterator<I>::type>
equal_range(I& r, const T& x) {
    return std::equal_range(boost::begin(r), boost::end(r), x);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T> // T == result_type(P)
inline std::pair<typename boost::range_const_iterator<I>::type,
                 typename boost::range_const_iterator<I>::type>
equal_range(const I& r, const T& x) {
    return std::equal_range(boost::begin(r), boost::end(r), x);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T, // T == result_type(P)
          typename C> // C models StrictWeakOrdering(T, T)
inline typename boost::lazy_disable_if<boost::is_same<I, T>, implementation::lazy_range<I>>::type
equal_range(I& r, const T& x, C c) {
    return adobe::equal_range(boost::begin(r), boost::end(r), x, c);
}

/*************************************************************************************************/

/*!
    \ingroup equal_range
*/

template <typename I, // I models ForwardRange
          typename T, // T == result_type(P)
          typename C> // C models StrictWeakOrdering(T, T)
inline typename boost::lazy_disable_if<boost::is_same<I, T>,
                                       implementation::lazy_range_const<I>>::type
equal_range(const I& r, const T& x, C c) {
    return adobe::equal_range(boost::begin(r), boost::end(r), x, c);
}

/*************************************************************************************************/

} // namespace adobe

/*************************************************************************************************/

#endif

/*************************************************************************************************/


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

int main(void) {
    vector<int> arr(5);
    for(int i = 0 ; i < 5 ; ++i) {
        cin >> arr[i];
    }
    int k;
    cin >> k;
    // 総当たり
    for(int i = 0 ; i < arr.size() ; ++i) {
        for(int j = 0 ; j < arr.size() ; ++j) {
            int d = abs(arr[i] - arr[j]);
            if(d > k) {
                cout << ":(" << endl;
                return 0;
            }
        }
    }
    cout << "Yay!" << endl;
    return 0;
}


#include <boost/multiprecision/cpp_int/comparison.hpp>


//
//  main.cpp
//  solver
//
//  Created by Alexandre Martin on 07/02/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#include "../solver/solver.hpp"
#include "../solver/expr/tseitin.hpp"
#include "../solver/curry.hpp"
#include "../solver/equality.hpp"
#include "../solver/command_line.hpp"
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <chrono>
#include <iomanip>
#include <boost/heap/fibonacci_heap.hpp>

#define MAKE_WARNING(lno, mess) ("warning line " + std::to_string(lno) + ": " + (mess))

solver parse(std::ifstream & file, options opt) {
    std::string line;
    int line_number = 0;
    auto header = false;
    int V = -1, C = -1;
    int clause_number = 0;
    std::vector<std::unordered_set<int>> res;

    while (std::getline(file, line)) {
        boost::algorithm::trim(line);
        ++line_number;
        if (line.empty())
            continue;
        if(line[0] == 'c')
            continue;
        else if(line[0] == 'p') {
            if (header) {
                std::cout
                    << MAKE_WARNING(line_number, "redefinition of header ignored")
                    << std::endl;
                continue;
            }
            header = true;

            std::string str1, str2;
            std::istringstream ss { line };
            if (!(ss >> str1 >> str2 >> V >> C) || str1 != "p" || str2 != "cnf")
                std::cout << MAKE_WARNING(line_number, "bad header `" + line + "`") << std::endl;
            else if (V < 0)
                std::cout << MAKE_WARNING(line_number, "negative V") << std::endl;
            else if (C < 0)
                std::cout << MAKE_WARNING(line_number, "negative C") << std::endl;
        } else {
            if (!header)
                std::cout << MAKE_WARNING(line_number, "header not defined") << std::endl;
            std::istringstream ss { line };

            int v;
            std::unordered_set<int> clause;
            while (ss >> v) {
                if (v == 0)
                    break;
                if (std::abs(v) > V) {
                    std::cout
                        << MAKE_WARNING(line_number, "out of range variable " + std::to_string(v))
                        << std::endl;
                }
                clause.emplace(v);
            }

            if (v != 0)
                std::cout << MAKE_WARNING(line_number, "line not terminated by 0") << std::endl;

            ++clause_number;
            if (C >= 0 && clause_number == C + 1)
                std::cout << MAKE_WARNING(line_number, "more clauses than announced") << std::endl;
            res.emplace_back(std::move(clause));
        }
    }

    return solver { std::move(res), opt };
}

void trim_expr(std::string & exp) {
    boost::algorithm::replace_all(exp, "\r\n", "\n"); // fuck windows
    boost::algorithm::trim(exp);
    if (boost::algorithm::ends_with(exp, " 0"))
        boost::algorithm::erase_tail(exp, 2);
    boost::algorithm::trim(exp);
}

int main(int argc, const char ** argv) {
    options opt;
    auto tseitin = false;
    bool eq = false;
    std::string file_name;

    parse_command_line(argc, argv, file_name, opt, tseitin, eq);

    if (file_name.empty()) {
        std::cerr << "no input" << std::endl;
        return 1;
    }

    std::ifstream f { file_name };
    if (!f) {
        std::cerr << "file not found `" << file_name << "`" << std::endl;
        return 1;
    }

    bool sat;
    valuation val;
    std::cout << std::fixed << std::setprecision(6);
    if (tseitin) {
        auto tp = std::chrono::system_clock::now();
        std::ostringstream ss;
        ss << f.rdbuf();
        auto exp = ss.str();
        trim_expr(exp);

        auto res = expr::parse(exp);
        if (auto err = boost::get<std::string>(&res)) {
            std::cerr << *err << std::endl;
            return 1;
        }

        auto tseitin = expr::tseitin_transform(*boost::get<expr::logical_expr>(&res));
        solver s { std::move(tseitin.first), opt };

        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "parse time: " << duration.count() / 1000000. << "s" << std::endl;

        tp = std::chrono::system_clock::now();
        val = expr::remove_trailing_variables(s.solve(), tseitin.second);

        duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "solve time: " << duration.count() / 1000000. << "s" << std::endl;

        sat = s.satisfiable();
    } else if (eq) {
        auto tp = std::chrono::system_clock::now();
        std::ostringstream ss;
        ss << f.rdbuf();
        auto exp = ss.str();
        trim_expr(exp);

        auto res = expr::parse_equality(exp);
        if (auto err = boost::get<std::string>(&res)) {
            std::cerr << *err << std::endl;
            return 1;
        }

        auto env = curry_transform(*boost::get<expr::equality_expr>(&res));
        equality_solver eq_s { env.max_constant, std::move(env.atoms) };
        options local_opt = opt;
        local_opt.eq_solver = &eq_s;
        solver s { std::move(env.clauses), local_opt };

        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "parse time: " << duration.count() / 1000000. << "s" << std::endl;

        tp = std::chrono::system_clock::now();
        sat = s.satisfiable();

        duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "solve time: " << duration.count() / 1000000. << "s" << std::endl;
    } else {
        auto tp = std::chrono::system_clock::now();
        auto s = parse(f, opt);

        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "parse time: " << duration.count() / 1000000. << "s" << std::endl;

        tp = std::chrono::system_clock::now();
        val = s.solve();

        duration = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now() - tp
        );
        std::cout << "solve time: " << duration.count() / 1000000. << "s" << std::endl;

        sat = s.satisfiable();
    }

    if (sat) {
        std::cout << "s SATISFIABLE" << std::endl;
        for (auto && v : val)
            std::cout << (v.second ? v.first : -v.first) << " ";
        std::cout << "0" << std::endl;
    }
    else
        std::cout << "s UNSATISFIABLE" << std::endl;

    return 0;
}


#ifndef OCV_CBIR_VISUALIZE_FEATURE_HPP
#define OCV_CBIR_VISUALIZE_FEATURE_HPP

#include "features_indexer.hpp"

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

#include <armadillo>

#include <algorithm>
#include <limits>

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup cbir
 *  @{
 */
namespace cbir{

/**
 *visualize the features of BOVW
 *@tparam T Features type of hdf5
 *@tparam U Type of code book(vocab)
 *@code
 *arma::Mat<float> vocab;
 *vocab.load("ukbench_code_book", arma::arma_ascii);
 *ocv::cbir::visualize_features<float, T> vf;
 *auto result = vf.find_top_results(fi, vocab);
 *std::string const folder = "ukbench_quiz/";
 *size_t index = 0;
 *for(auto const &vecs : result){
 *    ocv::montage montage({64,64}, 4, 4);
 *    for(auto const &res : vecs){
 *        auto img = cv::imread(folder + res.img_name_);
 *        auto const pt = res.kp_.pt;
 *        montage.add_image(ocv::crop_image(img, pt.x,
 *                                          pt.y, 32, 32));
 *    }
 *    cv::imwrite("vocab/img" + std::to_string(index++) + ".jpg",
 *                 montage.get_montage());
 *}
 *@endcode
 */
template<typename T, typename U = double>
class visualize_features
{
public:
    visualize_features()
    {
        static_assert(std::is_arithmetic<T>::value,
                      "T should be arithmetic");
        static_assert(std::is_arithmetic<U>::value,
                      "U should be arithmetic");
    }

    struct vis_point
    {
        vis_point() :
            dist_{std::numeric_limits<double>::max()}
        {}

        vis_point(U dist, std::string img_name,
                  cv::KeyPoint kp) :
            dist_{dist},
            img_name_(std::move(img_name)),
            kp_(kp)
        {}

        friend bool operator<(vis_point const &lhs,
                              vis_point const &rhs)
        {
            return lhs.dist_ < rhs.dist_;
        }

        friend bool operator>(vis_point const &lhs,
                              vis_point const &rhs)
        {
            return !(lhs < rhs);
        }

        friend std::ostream& operator<<(std::ostream &out,
                                        vis_point const &input)
        {
            out<<input.img_name_<<" : "
              <<input.dist_<<" : "
             <<input.kp_.pt;

            return out;
        }

        U dist_;
        std::string img_name_;
        cv::KeyPoint kp_;
    };

    using vis_points = std::vector<vis_point>;

    std::vector<vis_points>
    find_top_results(features_indexer const &fi,
                     arma::Mat<U> const &vocab) const
    {
        std::vector<std::string> img_names;
        fi.read_image_name(img_names);
        //std::cout<<"img size : "<<img_names.size()<<"\n";
        //std::cout<<"vocab size : "<<vocab.n_rows<<", "
        //        <<vocab.n_cols<<"\n";
        std::vector<vis_points> results(vocab.n_cols);
        for(size_t i = 0; i != img_names.size(); ++i){
            cv::Mat img_features;
            std::vector<cv::KeyPoint> keypoints;
            fi.read_image_features(img_features, static_cast<int>(i));
            fi.read_keypoints(keypoints, static_cast<int>(i));
            //std::cout<<img_names[i]<<" : "<<keypoints.size()<<"\n";
            //std::cout<<"img features size : "<<img_features.rows
            //        <<":"<<img_features.cols<<"\n";
            for(int j = 0; j != img_features.rows; ++j){
                arma::Col<U> arma_features(img_features.cols);
                auto *ptr = img_features.ptr<T>(j);
                for(int k = 0; k != img_features.cols; ++k){
                    arma_features(k) = static_cast<U>(ptr[k]);
                }
                auto const distances = euclidean_dist(arma_features,
                                                      vocab);
                //std::cout<<"distances : "<<distances<<std::endl;
                for(arma::uword m = 0; m != vocab.n_cols; ++m){
                    update_vis(distances[m], img_names[i],
                               keypoints[j], results[m]);
                }
            }
        }

        return results;
    }

private:
    arma::Mat<U> euclidean_dist(arma::Col<U> const &x,
                                arma::Mat<U> const &y) const
    {
        return arma::sqrt(arma::sum
                          (arma::square(y.each_col() - x)));
    }

    void update_vis(U distance, std::string const &img_name,
                    cv::KeyPoint kp, vis_points &vis) const
    {
        if(vis.size() < 16){
            vis.emplace_back(distance, img_name, kp);
            if(vis.size() == 16){
                std::sort(std::begin(vis), std::end(vis));
            }
        }else{
            auto it = std::find_if(std::begin(vis), std::end(vis),
                                   [=](vis_point const &val)
            {
                return val.dist_ > distance;
            });
            if(it != std::end(vis)){
                vis.emplace(it, distance, img_name, kp);
                vis.pop_back();
            }
        }
    }

};

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // OCV_CBIR_VISUALIZE_FEATURE_HPP


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/generic/trigo.hpp>
#include <boost/simd/function/restricted.hpp>
#include <boost/simd/meta/is_not_scalar.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( sincosd_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0, A0>                     ;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const
    {
      return detail::trig_base <A0,tag::degree_tag,is_not_scalar_t<A0>,tag::big_tag>::sincosa(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sincosd_
                          , (typename A0)
                          , bd::cpu_
                          , bs::restricted_tag
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE std::pair<A0, A0> operator() (const restricted_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      return detail::trig_base<A0, tag::degree_tag,is_not_scalar_t<A0>,tag::clipped_pio4_tag>::sincosa(a0);
    }
  };
} } }

#endif


#include <Eigen/Core>       // Cholesky decomposition + solving
#include <Eigen/Cholesky>   // of system of linear equations.

#include "nao_igm.h"
#include "maple_functions.h"

/**
 * @brief Get feet positions.
 *
 * @param[out] left_foot_expected 3x1 expected position vector
 * @param[out] right_foot_expected 3x1 expected position vector
 * @param[out] left_foot_computed 3x1 position vector determined using sensor data
 * @param[out] right_foot_computed 3x1 position vector determined using sensor data
 *
 * @note Support foot position is assumed to be correct and there is no difference
 * between the expected and 'real' positions.
 */
void nao_igm::getFeetPositions (
        double *left_foot_expected,
        double *right_foot_expected,
        double *left_foot_computed,
        double *right_foot_computed)
{
    Transform<double, 3> swing_foot_posture;
    getSwingFootPosture (state_sensor, swing_foot_posture.data());

    Vector3d::Map(left_foot_expected) = left_foot_posture.translation();
    Vector3d::Map(right_foot_expected) = right_foot_posture.translation();

    if (support_foot == IGM_SUPPORT_LEFT)
    {
        Vector3d::Map(left_foot_computed) = left_foot_posture.translation();
        Vector3d::Map(right_foot_computed) = swing_foot_posture.translation();
    }
    else
    {
        Vector3d::Map(right_foot_computed) = right_foot_posture.translation();
        Vector3d::Map(left_foot_computed) = swing_foot_posture.translation();
    }
}

/**
 * @brief Set coordinates of center of mass.
 *
 * @param[in] x coordinate
 * @param[in] y coordinate
 * @param[in] z coordinate
 */
void nao_igm::setCoM (const double x, const double y, const double z)
{
    CoM_position[0] = x;
    CoM_position[1] = y;
    CoM_position[2] = z;
}

/**
 * @brief Initialize model.
 *
 * @param[in] support_foot_ current support foot.
 * @param[in] x x coordinate of the support foot.
 * @param[in] y y coordinate of the support foot.
 * @param[in] z z coordinate of the support foot.
 * @param[in] roll roll angle of the support foot.
 * @param[in] pitch pitch angle of the support foot.
 * @param[in] yaw yaw angle of the support foot.
 *
 * @attention Joint angles in state_sensor must be set.
 */
void nao_igm::init (
        const igmSupportFoot support_foot_,
        const double x,
        const double y,
        const double z,
        const double roll,
        const double pitch,
        const double yaw)
{
    support_foot = support_foot_;
    state_model = state_sensor;

    if (support_foot == IGM_SUPPORT_LEFT)
    {
        left_foot_posture =
                Translation<double,3>(x, y, z) *
                AngleAxisd(roll, Vector3d::UnitX()) *
                AngleAxisd(pitch, Vector3d::UnitY()) *
                AngleAxisd(yaw, Vector3d::UnitZ());
    }
    else
    {
        right_foot_posture =
                Translation<double,3>(x, y, z) *
                AngleAxisd(roll, Vector3d::UnitX()) *
                AngleAxisd(pitch, Vector3d::UnitY()) *
                AngleAxisd(yaw, Vector3d::UnitZ());
    }
}

/**
 * @brief Switch support foot.
 */
void nao_igm::switchSupportFoot()
{
    state_model = state_sensor;

    if (support_foot == IGM_SUPPORT_LEFT)
    {
        support_foot = IGM_SUPPORT_RIGHT;
    }
    else
    {
        support_foot = IGM_SUPPORT_LEFT;
    }
}

/**
 * @brief Compute position of the CoM from joint angles.
 *
 * @param[in] joints state of the joints.
 * @param[in,out] CoM_pos 3x1 vector of coordinates.
 */
void nao_igm::getCoM (jointState& joints, double *CoM_pos)
{
    if (support_foot == IGM_SUPPORT_LEFT)
    {
        LLeg2CoM(joints.q, left_foot_posture.data(), CoM_pos);
    }
    else
    {
        RLeg2CoM(joints.q, right_foot_posture.data(), CoM_pos);
    }
}

/**
 * @brief Compute position of the swing foot from joint angles.
 *
 * @param[in] joints state of the joints.
 * @param[in,out] swing_foot_posture 4x4 homogeneous matrix
 */
void nao_igm::getSwingFootPosture (jointState& joints, double *swing_foot_posture)
{
    if (support_foot == IGM_SUPPORT_LEFT)
    {
        LLeg2RLeg(joints.q, left_foot_posture.data(), swing_foot_posture);
    }
    else
    {
        RLeg2LLeg(joints.q, right_foot_posture.data(), swing_foot_posture);
    }
}

/**
 * @brief Solves the Inverse Geometric Problem (IGM).
 *
 * @param[in] ref_angles LOWER_JOINTS_NUM reference joint angles
 * @param[in] mu penalty for difference between solution and reference angles
 * @param[in] tol tolerance
 * @param[in] max_iter maximal number of iterations
 *
 * @return the number of iterations performed until convergence, or a negative number
 * if the algorithm did not converge within max_iter number of iterations.
 *
 * @note It is assumed that the leading matrix of the constraints is nonsingular.
 *
 * @note On input, model_state.q is taken as an initial guess for the joint angles,
 * on output it contains a solution of the inverse kinematics problem (if iter >= 0).
 * Only the joints angles in the lower part of the body are altered.
*/
int nao_igm::igm(
        const double *ref_angles,
        const double mu,
        const double tol,
        const int max_iter)
{
    /*
     * Constraints:
     *  - 3 on the position of the swing foot
     *  - 3 on the orientation of the swing foot
     *  - 3 on the position of the CoM
     *  - 1 to take into account coupled joint L_HIP_YAW_PITCH / R_HIP_YAW_PITCH
     */
    const int num_constraints = 10;

    Map< Matrix<double, LOWER_JOINTS_NUM, 1> > q(state_model.q);
    Map< Matrix<double, LOWER_JOINTS_NUM, 1> > q0(ref_angles);
    Matrix<double, LOWER_JOINTS_NUM, 1> dq;
    Matrix<double, LOWER_JOINTS_NUM, 1> iH; // inverted Hessian

    iH.setConstant(1.0);
    // joints can be penalized here:
    //iH(L_HIP_ROLL) = 0.5;
    //iH(R_HIP_ROLL) = 0.5;

    double out[num_constraints*LOWER_JOINTS_NUM + num_constraints];
    Map< Matrix<double, num_constraints, LOWER_JOINTS_NUM> > A(out);
    Map< Matrix<double, num_constraints, 1> > err(out + num_constraints*LOWER_JOINTS_NUM);

    int iter;
    double norm_dq = 1.0;

    for (iter = 0; (norm_dq > tol) && (iter <= max_iter); ++iter)
    {
        // Form data
        if (support_foot == IGM_SUPPORT_LEFT)
        {
            from_LLeg_3 (
                    state_model.q,
                    left_foot_posture.data(),
                    right_foot_posture.data(),
                    CoM_position,
                    out);
        }
        else
        {
            from_RLeg_3 (
                    state_model.q,
                    right_foot_posture.data(),
                    left_foot_posture.data(),
                    CoM_position,
                    out);
        }

        // Solve KKT system
        dq = mu*(q - q0);
        err = -err - A*dq;
        /// @todo FP underflow occurs in solveInPlace().
        (A*iH.asDiagonal()*A.transpose()).llt().solveInPlace(err);
        dq = -dq - iH.asDiagonal()*A.transpose() * err;
        // Update angles (of legs)
        q += dq;

        // Compute the infinity norm of dq
        // (we want the absolute value of angle change to be detectable by
        // the corresponding sensor, see 'tol' variable above)
        norm_dq = dq.lpNorm<Eigen::Infinity>();
    }

    if (iter > max_iter)
    {
        iter = -1;
    }

    return(iter);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_ADDS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_ADDS_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/adds.hpp>
#include <boost/simd/include/functions/scalar/saturate.hpp>
#include <boost/simd/include/functions/scalar/is_gtz.hpp>
#include <boost/simd/include/functions/scalar/is_greater.hpp>
#include <boost/simd/include/functions/scalar/is_less.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::adds_, tag::cpu_
                            , (A0)
                            , (scalar_< integer_<A0> >)(scalar_< integer_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;
      return static_cast<A0>(boost::simd::saturate<A0>(utype(a0)+utype(a1)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::adds_, tag::cpu_
                            , (A0)
                            , (scalar_< unsigned_<A0> >)(scalar_< unsigned_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;
      return static_cast<A0>(boost::simd::min(utype(boost::simd::Valmax<A0>()), utype(a0+a1)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::adds_, tag::cpu_
                            , (A0)
                            , (scalar_< int64_<A0> >)(scalar_< int64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type utype;

      bool gtza0 = is_gtz(a0);
      bool gtza1 = is_gtz(a1);
      A0 a0pa1 = A0(utype(a0)+utype(a1));
      if (gtza0 && gtza1 && (lt(a0pa1, a0)))
      {
        return Valmax<A0>();
      }
      else if (!gtza0 && !gtza1 && (is_gtz(a0pa1) || (gt(a0pa1, boost::simd::min(a0, a1)))))
      {
        return Valmin<A0>();
      }
      else
      {
        return a0pa1;
      }
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::adds_, tag::cpu_
                            , (A0)
                            , (scalar_< uint64_<A0> >)(scalar_< uint64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0pa1 = a0+a1;
      if (lt(a0pa1, boost::simd::max(a0, a1)))
      {
        return Valmax<A0>();
      }
      else
      {
        return a0pa1;
      }
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::adds_, tag::cpu_
                            , (A0)
                            , (scalar_< uint32_<A0> >)(scalar_< uint32_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0pa1 = a0+a1;
      if (lt(a0pa1, boost::simd::max(a0, a1)))
      {
        return Valmax<A0>();
      }
      else
      {
        return a0pa1;
      }
    }
  };
} } }

#endif


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <boost/array.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <svgpp/parser/detail/common.hpp>
#include <svgpp/policy/transform_events.hpp>

namespace svgpp
{

namespace qi = boost::spirit::qi;

template <
  class Iterator,
  class Context,
  class Number = double,
  class EventsPolicy = policy::transform_events::default_policy<Context> >
class transform_grammar: public qi::grammar<Iterator, void (Context &), detail::character_encoding_namespace::space_type>
{
  typedef transform_grammar<Iterator, Context, Number, EventsPolicy> this_type;
public:
  transform_grammar()
    : this_type::base_type(transforms)
  {
    namespace phx = boost::phoenix;
    using qi::_1;
    using qi::_2;
    using qi::_3;
    using qi::_4;
    using qi::_5;
    using qi::_6;
    using qi::_a;
    using qi::_b;
    using qi::_c;
    using qi::_r1;
    using qi::lit;
    using qi::no_skip;

    transforms
        =  -( ( matrix(_r1)
              | translate(_r1)
              | scale(_r1)
              | rotate(_r1)
              | skewX(_r1)
              | skewY(_r1)
              )
              %
#ifdef SVGPP_STRICT_TRANSFORM_SEPARATOR
                (+no_skip[comma_wsp])
#else
                (*lit(','))
#endif
            );

    matrix
        =   ( lit("matrix")
              >> lit('(')
              >> number >> no_skip[comma_wsp]
              >> number >> no_skip[comma_wsp]
              >> number >> no_skip[comma_wsp]
              >> number >> no_skip[comma_wsp]
              >> number >> no_skip[comma_wsp]
              >> number >> lit(')')
            ) [phx::bind(&call_matrix, _r1, _1, _2, _3, _4, _5, _6)];
    translate
        =   lit("translate")
            >> lit('(')
            >> number [_a = _1]
            >> ( ( no_skip[comma_wsp]
                  >> number [_b = _1]
                  >> lit(')') [phx::bind(&transform_grammar::call_translate, _r1, _a, _b)]
                )
                | lit(')') [phx::bind(&transform_grammar::call_translate_x, _r1, _a)]
              );
    scale
        =   lit("scale")
            >> lit('(')
            >> number [_a = _1]
            >> ( ( no_skip[comma_wsp]
                  >> number [_b = _1]
                  >> lit(')') [phx::bind(&transform_grammar::call_scale, _r1, _a, _b)]
                )
                | lit(')') [phx::bind(&transform_grammar::call_scale_uniform, _r1, _a)]
              );
    rotate
        =   lit("rotate")
            >> lit('(')
            >> number[_a = _1]
            >> ( ( no_skip[comma_wsp]
                  >> number [_b = _1]
                  >> no_skip[comma_wsp]
                  >> number [_c = _1]
                  >> lit(')') [phx::bind(&transform_grammar::call_rotate_about_point, _r1, _a, _b, _c)]
                )
                | lit(')') [phx::bind(&transform_grammar::call_rotate, _r1, _a)]
              );
    skewX
        =   ( lit("skewX")
              >> lit('(')
              >> number
              >> lit(')') ) [phx::bind(&transform_grammar::call_skew_x, _r1, _1)];
    skewY
        =   ( lit("skewY")
              >> lit('(')
              >> number
              >> lit(')') ) [phx::bind(&transform_grammar::call_skew_y, _r1, _1)];
  }

private:
  typename this_type::start_type transforms;
  qi::rule<Iterator, void (Context &), typename this_type::skipper_type>
    matrix, skewX, skewY;
  qi::rule<Iterator, void (Context &), qi::locals<Number, Number>, typename this_type::skipper_type>
    translate, scale;
  qi::rule<Iterator, void (Context &), qi::locals<Number, Number, Number>, typename this_type::skipper_type>
    rotate;
  detail::comma_wsp_rule<Iterator> comma_wsp;
  qi::real_parser<Number, detail::svg_real_policies<Number> > number;

  static void call_matrix(Context & context, Number a, Number b, Number c, Number d, Number e, Number f)
  {
    boost::array<Number, 6> const matrix = {{a, b, c, d, e, f}};
    EventsPolicy::transform_matrix(context, matrix);
  }

  static void call_translate(Context & context, Number tx, Number ty)
  {
    EventsPolicy::transform_translate(context, tx, ty);
  }

  static void call_translate_x(Context & context, Number tx)
  {
    EventsPolicy::transform_translate(context, tx);
  }

  static void call_scale(Context & context, Number sx, Number sy)
  {
    EventsPolicy::transform_scale(context, sx, sy);
  }

  static void call_scale_uniform(Context & context, Number scale)
  {
    EventsPolicy::transform_scale(context, scale);
  }

  static void call_rotate(Context & context, Number angle)
  {
    EventsPolicy::transform_rotate(context, angle);
  }

  static void call_rotate_about_point(Context & context, Number angle, Number cx, Number cy)
  {
    EventsPolicy::transform_rotate(context, angle, cx, cy);
  }

  static void call_skew_x(Context & context, Number angle)
  {
    EventsPolicy::transform_skew_x(context, angle);
  }

  static void call_skew_y(Context & context, Number angle)
  {
    EventsPolicy::transform_skew_y(context, angle);
  }
};

}


#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <opencv2/opencv.hpp>
#include <iomanip>
#include <boost/thread/mutex.hpp>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <map>

using namespace cv;
using namespace std;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
#define toRadian(degree)	((degree) * (M_PI / 180.))
#define toDegree(radian)	((radian) * (180. / M_PI))

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Global variable
boost::mutex mutex[2];
nav_msgs::Odometry g_odom;
sensor_msgs::LaserScan g_scan;
vector<Vec3d> trajectory;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// A template method to check 'nan'
template<typename T>
inline bool isnan(T value)
{
    return value != value;
}

bool checkAngle(int angle, int limit) {
    return (angle + limit/2) % 360 <= limit;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// callback function
void
odomMsgCallback(const nav_msgs::Odometry &msg)
{
    // receive a '/odom' message with the mutex
    mutex[0].lock(); {
        g_odom = msg;
    } mutex[0].unlock();
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// callback function
void
scanMsgCallback(const sensor_msgs::LaserScan& msg)
{
    // receive a '/odom' message with the mutex
    mutex[1].lock(); {
        g_scan = msg;
    } mutex[1].unlock();
}

void resultMsgCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) {
    trajectory.clear();
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
convertOdom2XYZRPY(nav_msgs::Odometry &odom, Vec3d &xyz, Vec3d &rpy)
{
    // 이동 저장
    xyz[0] = odom.pose.pose.position.x;
    xyz[1] = odom.pose.pose.position.y;
    xyz[2] = odom.pose.pose.position.z;

    // 회전 저장
    tf::Quaternion rotationQuat = tf::Quaternion(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
    tf::Matrix3x3(rotationQuat).getEulerYPR(rpy[2], rpy[1], rpy[0]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
convertScan2XYZs(sensor_msgs::LaserScan& lrfScan, vector<Vec3d> &XYZs)
{
    int nRangeSize = (int)lrfScan.ranges.size();
    XYZs.clear();
    XYZs.resize(nRangeSize);

    for(int i=0; i<nRangeSize; i++) {
        double dRange = lrfScan.ranges[i];

        if(isnan(dRange)) {
            XYZs[i] = Vec3d(0., 0., 0.);
        } else {
            double dAngle = lrfScan.angle_min + i*lrfScan.angle_increment;
            XYZs[i] = Vec3d(dRange*cos(dAngle), dRange*sin(dAngle), 0.);
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
saveCurrentPosition(Vec3d &xyz, vector<Vec3d> &trajectory, double dMinDist)
{
    int nSize = (int) trajectory.size();

    if(nSize <= 0) {
        trajectory.push_back(xyz);
    } else {
        Vec3d diff = trajectory[nSize-1] - xyz;
        double len = sqrt(diff.dot(diff));

        if(len > dMinDist) {
            trajectory.push_back(xyz);
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
vector<Vec3d> transform(vector<Vec3d> laserScanXY, double x, double y, double theta)
{
    vector<Vec3d> ret;
    Vec3d newPt;
    double cosTheta = cos(theta);
    double sinTheta = sin(theta);
    int nRangeSize = (int)laserScanXY.size();

    for(int i=0; i<nRangeSize; i++) {
        auto newX = laserScanXY[i][0] - x;
        auto newY = laserScanXY[i][1] - y;
        newPt[0] = cosTheta*newX + -1.*sinTheta*newY;
        newPt[1] = sinTheta*newX + cosTheta*newY;
        newPt[2];
        ret.push_back(newPt);
    }

    return ret;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
initGrid(Mat &display, int nImageSize)
{
    const int nImageHalfSize = nImageSize/2;
    const int nAxisSize = nImageSize/16;
    const Vec2i imageCenterCooord = Vec2i(nImageHalfSize, nImageHalfSize);
    display = Mat::zeros(nImageSize, nImageSize, CV_8UC3);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
drawTrajectory(Mat &display, vector<Vec3d> &trajectory, double dMaxDist)
{
    Vec2i imageHalfSize = Vec2i(display.cols/2, display.rows/2);

    int nSize = (int) trajectory.size();

    for(int i=1; i<nSize; i++) {
        int x = imageHalfSize[0] + cvRound((trajectory[i][0]/dMaxDist)*imageHalfSize[0]);
        int y = imageHalfSize[1] + cvRound((trajectory[i][1]/dMaxDist)*imageHalfSize[1]);

        if(x >= 0 && x < display.cols && y >= 0 && y < display.rows) {
            display.at<Vec3b>(y, x) = Vec3b(128, 128, 128);
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
drawCurrentPositionWithRotation(Mat &display, Vec3d &xyz, Vec3d &rpy, double dMaxDist)
{
    printf("_r = %.3lf, _p = %.3lf, _y = %.3lf\n", toDegree(rpy[0]), toDegree(rpy[1]), toDegree(rpy[2]));

    const int nHeadingSize = 20;
    Vec2i headingDir = Vec2i(nHeadingSize*cos(rpy[2]), nHeadingSize*sin(rpy[2]));
    Vec2i imageHalfSize = Vec2i(display.cols/2, display.rows/2);

    int x = imageHalfSize[0] + cvRound((xyz[0]/dMaxDist)*imageHalfSize[0]);
    int y = imageHalfSize[1] + cvRound((xyz[1]/dMaxDist)*imageHalfSize[1]);

    if(x >= 0 && x < display.cols && y >= 0 && y < display.rows) {
        circle(display, Point(display.cols/2, display.rows/2), nHeadingSize, CV_RGB(255, 255, 255), 1, CV_AA);
        line(display, Point(display.cols/2, display.rows/2), Point(display.cols/2 + nHeadingSize * sqrt(3) / 2, display.rows/2 + nHeadingSize / 2), CV_RGB(255, 255, 255), 1, CV_AA);
        line(display, Point(display.cols/2, display.rows/2), Point(display.cols/2 + nHeadingSize * sqrt(3) / 2, display.rows/2 - nHeadingSize / 2), CV_RGB(255, 255, 255), 1, CV_AA);
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// A callback function. Executed eack time a new pose message arrives.
pair<int, double> drawLRFScan(Mat &display, vector<Vec3d> &laserScanXY, double dMaxDist)
{
    Vec2i imageHalfSize = Vec2i(display.cols/2, display.rows/2);
    int nRangeSize = (int)laserScanXY.size();
    vector<vector<int> > obstacles;
    vector<int> group;
    tuple<double, int, int> nearest = make_tuple(-1., 0, 0);
    const double isSame = 0.25, nearestMin = 1.;
    double prevX, prevY;
    int cnt = 0;
    bool isFront = false;

    //시작점부터 반대방향으로 장애물이 끝날때까지 한번 훑는다
    if(nRangeSize > 0) {
        prevX = laserScanXY[0][0];
        prevY = laserScanXY[0][1];
    }
    while(nRangeSize > 0) {
        double nowX = laserScanXY[--nRangeSize][0];
        double nowY = laserScanXY[nRangeSize][1];
        int x = imageHalfSize[0] + cvRound((nowX/dMaxDist)*imageHalfSize[0]);
        int y = imageHalfSize[1] + cvRound((nowY/dMaxDist)*imageHalfSize[1]);
        if(x >= 0 && x < display.cols && y >= 0 && y < display.rows) {
            if(isSame >= sqrt((prevX-nowX)*(prevX-nowX)+(prevY-nowY)*(prevY-nowY))) {
                if(checkAngle(nRangeSize, 60) && (get<0>(nearest) < 0 || get<0>(nearest) > sqrt(nowX*(-nowX)+(-nowY)*(-nowY))))
                    nearest = make_tuple(sqrt(nowX*nowX+nowY*nowY), 0, nRangeSize);
                if(checkAngle(nRangeSize, 180))
                    isFront = true;
                group.push_back(nRangeSize);
                if(checkAngle(nRangeSize, 180))
                    display.at<Vec3b>(y, x) = Vec3b(255, 255, 0);
                else
                    display.at<Vec3b>(y, x) = Vec3b(80, 80, 80);
            } else {
                nRangeSize++;
                break;
            }
        } else {
            nRangeSize++;
            break;
        }

        prevX = nowX;
        prevY = nowY;
    }

    for(int i=0; i<nRangeSize; i++) {
        double nowX = laserScanXY[i][0];
        double nowY = laserScanXY[i][1];
        int x = imageHalfSize[0] + cvRound((nowX/dMaxDist)*imageHalfSize[0]);
        int y = imageHalfSize[1] + cvRound((nowY/dMaxDist)*imageHalfSize[1]);
        if(x >= 0 && x < display.cols && y >= 0 && y < display.rows) {
            if(i == 0 || isSame >= sqrt((prevX-nowX)*(prevX-nowX)+(prevY-nowY)*(prevY-nowY))) {
                if(checkAngle(i, 180))
                    isFront = true;
                if(checkAngle(i, 60) && (get<0>(nearest) < 0 || get<0>(nearest) > sqrt(nowX*nowX+nowY*nowY)))
                    nearest = make_tuple(sqrt(nowX*nowX+nowY*nowY), cnt, i);
                group.push_back(i);
            } else {
                if(isFront) {
                    obstacles.push_back(group);
                    cnt++;
                }
                if(checkAngle(i, 180))
                    isFront = true;
                else
                    isFront = false;
                group.clear();
                group.push_back(i);
                if(checkAngle(i, 60) && (get<0>(nearest) < 0 || get<0>(nearest) > sqrt(nowX*nowX+nowY*nowY)))
                    nearest = make_tuple(sqrt(nowX*nowX+nowY*nowY), cnt, i);
            }
            if(checkAngle(i, 180)) {
                switch(cnt % 3) {
                case 0:
                    display.at<Vec3b>(y, x) = Vec3b(255, 255, 0);
                    break;
                case 1:
                    display.at<Vec3b>(y, x) = Vec3b(255, 0, 255);
                    break;
                default:
                    display.at<Vec3b>(y, x) = Vec3b(0, 255, 255);
                    break;
                }
            } else
                display.at<Vec3b>(y, x) = Vec3b(80, 80, 80);
        }
        prevX = nowX;
        prevY = nowY;
    }

    if(group.size())
        obstacles.push_back(group);

    bool isDangerous = false;
    if(nRangeSize > 0 && nearestMin >= get<0>(nearest)){
        int nearestObstacle = get<1>(nearest);
        for(auto it = obstacles[nearestObstacle].begin(); it != obstacles[nearestObstacle].end(); ++it) {
            if(checkAngle(*it, 60)) {
                double nowX = laserScanXY[*it][0];
                double nowY = laserScanXY[*it][1];
                if(nearestMin >= sqrt(nowX*nowX+nowY*nowY)){
                    int x = imageHalfSize[0] + cvRound((nowX/dMaxDist)*imageHalfSize[0]);
                    int y = imageHalfSize[1] + cvRound((nowY/dMaxDist)*imageHalfSize[1]);
                    display.at<Vec3b>(y, x) = Vec3b(0, 0, 255);
                    isDangerous = true;
                }
            }
        }
    }

    return make_pair(obstacles.size(), (isDangerous ? get<0>(nearest) : -1.));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
void
printOdometryInfo(nav_msgs::Odometry &odom)
{
    // Display /odom part!
    const ros::Time timestamp = odom.header.stamp;
    const string frame_id = odom.header.frame_id;
    const string child_frame_id = odom.child_frame_id;
    const geometry_msgs::Point translation = odom.pose.pose.position;
    const geometry_msgs::Quaternion rotation = odom.pose.pose.orientation;

    printf("frame_id = %s, child_frame_id = %s\n", frame_id.c_str(), child_frame_id.c_str());
    printf("secs: %d / nsecs: %d\n", timestamp.sec, timestamp.nsec);
    printf("translation = %lf %lf %lf\n", translation.x, translation.y, translation.z);
    printf("rotation = %lf %lf %lf %lf\n\n\n", rotation.x, rotation.y, rotation.z, rotation.w);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
int main(int argc, char **argv)
{
    bool first = true;
    // Initialize the ROS system
    ros::init(argc, argv, "turtle_position_lrf_view");
    ros::NodeHandle nh;

    // Create subscriber objects
    ros::Subscriber subOdom = nh.subscribe("/odom", 100, &odomMsgCallback);
    ros::Subscriber subScan = nh.subscribe("/scan", 10, &scanMsgCallback);
    ros::Subscriber subResult = nh.subscribe("/result", 5, &resultMsgCallback);

    // Display buffer
    Mat display;
    initGrid(display, 801);

    // Odometry buffer
    nav_msgs::Odometry odom;

    // Scan buffer
    sensor_msgs::LaserScan scan;

    // 이동 및 회전 정보
    Vec3d xyz, rpy;

    // 이동궤적

    // LRF scan 정보
    vector<Vec3d> laserScanXY;

    // Mat distance for grid
    const double dGridMaxDist = 4.0;

    // main loop
    while(ros::ok()) {
        // callback 함수을 call!
        ros::spinOnce();

        // receive the global '/odom' message with the mutex
        mutex[0].lock(); {
           odom = g_odom;
        } mutex[0].unlock();

        // odom으로부터 이동 및 회전정보 획득
        convertOdom2XYZRPY(odom, xyz, rpy);

        // 현재의 위치를 저장
        saveCurrentPosition(xyz, trajectory, 0.02);

        // receive the global '/scan' message with the mutex
        mutex[1].lock(); {
           scan = g_scan;
        } mutex[1].unlock();

        // scan으로부터 Cartesian X-Y scan 획득
        convertScan2XYZs(scan, laserScanXY);

        // trajectory를 상대좌표계로 변환
        auto nowTra = transform(trajectory, xyz[0], xyz[1], 2 * M_PI - rpy[2]);

        // 현재 상황을 draw할 display 이미지를 생성
        initGrid(display, 801);
        drawTrajectory(display, nowTra, dGridMaxDist);
        drawCurrentPositionWithRotation(display, xyz, rpy, dGridMaxDist);
        auto ret = drawLRFScan(display, laserScanXY, dGridMaxDist);
        int obstacleSize = ret.first;
        double nearest = ret.second;

        // 2D 영상좌표계에서 top-view 방식의 3차원 월드좌표계로 변환
        transpose(display, display);  // X-Y축 교환
        flip(display, display, 0);  // 수평방향 반전
        flip(display, display, 1);  // 수직방향 반전

        string msg;
        msg = "Number of obstacles: " + to_string(obstacleSize);
        putText(display, msg, Point(10, 30), 0, 0.7, CV_RGB(255, 255, 255));
        if(nearest > 0){
            msg = "The shortest distance to obstacle: " + to_string(nearest);
            putText(display, msg, Point(10, 55), 0, 0.7, CV_RGB(255, 255, 255));
        }

        // 영상 출력!
        imshow("KNU ROS Lecture >> turtle_position_lrf_view", display);
        printOdometryInfo(odom);

        // 사용자의 키보드 입력을 받음!
        int nKey = waitKey(30) % 255;

        if(nKey == 27) {
            // 종료
            break;
        }

        if(nKey == 'c' || nKey == 'C') {
            trajectory.clear();
        }
    }

    return 0;
}


/* Copyright Institue of Sound and Vibration Research - All rights reserved. */

#include <libsignalflows/delay_vector.hpp>

#include <libefl/basic_matrix.hpp>

#include <libpml/initialise_parameter_library.hpp>

#include <librrl/audio_signal_flow.hpp>
#include <librrl/integrity_checking.hpp>

#include <boost/test/unit_test.hpp>

namespace visr
{
namespace signalflows
{
namespace test
{

std::size_t const numChannels =4;

BOOST_AUTO_TEST_CASE( CheckDelayVector )
{
  pml::initialiseParameterLibrary();

  const std::size_t blockSize{64};
  std::size_t const numBlocks{16};

  SignalFlowContext context( blockSize, 48000 );

  signalflows::DelayVector comp( context, "topLevel", nullptr, numChannels,
    1024, "lagrangeOrder3" );

  rrl::AudioSignalFlow flow( comp );

  std::vector<SampleType*> inputPtr( numChannels, nullptr );
  std::vector<SampleType*> outputPtr( numChannels, nullptr );

  efl::BasicMatrix<SampleType> inputData( numChannels, blockSize* numBlocks );
  // TODO: Fill the input data with something useful

  efl::BasicMatrix<SampleType> outputData( numChannels, blockSize* numBlocks );

  for( std::size_t blockIdx(0); blockIdx < numBlocks; ++blockIdx )
  {
    for( std::size_t idx(0); idx < numChannels; ++idx )
    {
      inputPtr[idx] = inputData.row(idx) + blockIdx*blockSize;
      outputPtr[idx] = outputData.row(idx) + blockIdx*blockSize;
    }
    flow.process( &inputPtr[0], &outputPtr[0] );
  }

  // TODO: check output data

}

} // namespace test
} // namespace signalflows
} // namespace visr


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE ExactTransientSolution
#define BOOST_TEST_MAIN
#include <cap/energy_storage_device.h>
#include <cap/mp_values.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/distributions/beta.hpp>
#include <cmath>
#include <iostream>
#include <fstream>
#include <numeric>

namespace cap
{

void compute_parameters(
    std::shared_ptr<boost::property_tree::ptree const> input_database,
    std::shared_ptr<boost::property_tree::ptree> output_database)
{
  double const cm2_to_m2 = 0.0001;
  double const cm_to_m = 0.01;
  double const cross_sectional_area =
      cm2_to_m2 * input_database->get<double>("geometry.geometric_area");
  double const electrode_width =
      cm_to_m *
      input_database->get<double>("geometry.anode_electrode_thickness");
  double const separator_width =
      cm_to_m * input_database->get<double>("geometry.separator_thickness");

  // getting the material parameters values
  std::shared_ptr<boost::property_tree::ptree> material_properties_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("material_properties"));
  cap::MPValuesParameters<2> mp_values_params(material_properties_database);
  std::shared_ptr<boost::property_tree::ptree> geometry_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("geometry"));
  mp_values_params.geometry =
      std::make_shared<cap::DummyGeometry<2>>(geometry_database);
  std::shared_ptr<cap::MPValues<2>> mp_values =
      std::shared_ptr<cap::MPValues<2>>(new cap::MPValues<2>(mp_values_params));
  // build dummy cell itertor and set its material id
  dealii::Triangulation<2> triangulation;
  dealii::GridGenerator::hyper_cube(triangulation);
  dealii::DoFHandler<2> dof_handler(triangulation);
  dealii::DoFHandler<2>::active_cell_iterator cell = dof_handler.begin_active();
  // electrode
  cell->set_material_id(input_database->get<dealii::types::material_id>(
      "geometry.anode_electrode_material_id"));
  std::vector<double> electrode_solid_electrical_conductivity_values(1);
  std::vector<double> electrode_liquid_electrical_conductivity_values(1);
  std::vector<double> electrode_specific_capacitance_values(1);
  std::vector<double> electrode_exchange_current_density_values(1);
  std::vector<double> electrode_electron_thermal_voltage_values(1);
  mp_values->get_values("solid_electrical_conductivity", cell,
                        electrode_solid_electrical_conductivity_values);
  mp_values->get_values("liquid_electrical_conductivity", cell,
                        electrode_liquid_electrical_conductivity_values);
  mp_values->get_values("specific_capacitance", cell,
                        electrode_specific_capacitance_values);
  mp_values->get_values("faradaic_reaction_coefficient", cell,
                        electrode_exchange_current_density_values);
  double const cell_current_density = 1.0;
  double const initial_voltage = 1.0;
  double const dimensionless_cell_current_density =
      cell_current_density * electrode_width *
      (electrode_liquid_electrical_conductivity_values[0] +
       electrode_solid_electrical_conductivity_values[0]) /
      (electrode_liquid_electrical_conductivity_values[0] *
       electrode_solid_electrical_conductivity_values[0] * initial_voltage);
  double const ratio_of_solution_phase_to_matrix_phase_conductivities =
      electrode_liquid_electrical_conductivity_values[0] /
      electrode_solid_electrical_conductivity_values[0];
  if (electrode_exchange_current_density_values[0] != 0.0)
    throw std::runtime_error("test assumes no faradaic processes, "
                             "exchange_current_density has to be zero");

  output_database->put("dimensionless_cell_current_density",
                       dimensionless_cell_current_density);
  output_database->put("ratio_of_solution_phase_to_matrix_phase_conductivities",
                       ratio_of_solution_phase_to_matrix_phase_conductivities);

  output_database->put("position_normalization_factor", electrode_width);
  output_database->put(
      "time_normalization_factor",
      electrode_specific_capacitance_values[0] *
          (1.0 / electrode_solid_electrical_conductivity_values[0] +
           1.0 / electrode_liquid_electrical_conductivity_values[0]) *
          std::pow(electrode_width, 2));
  double const electrode_resistance =
      electrode_width * (electrode_solid_electrical_conductivity_values[0] +
                         electrode_liquid_electrical_conductivity_values[0]) /
      (electrode_solid_electrical_conductivity_values[0] *
       electrode_liquid_electrical_conductivity_values[0]);

  // separator
  cell->set_material_id(input_database->get<dealii::types::material_id>(
      "geometry.separator_material_id"));
  std::vector<double> separator_liquid_electrical_conductivity_values(1);
  mp_values->get_values("liquid_electrical_conductivity", cell,
                        separator_liquid_electrical_conductivity_values);
  double const separator_resitance =
      separator_width / separator_liquid_electrical_conductivity_values[0];

  double const ratio_of_separator_to_electrode_resistances =
      separator_resitance / electrode_resistance;
  output_database->put("ratio_of_separator_to_electrode_resistances",
                       ratio_of_separator_to_electrode_resistances);
  output_database->put("cross_sectional_area", cross_sectional_area);
}

void verification_problem(
    std::shared_ptr<cap::EnergyStorageDevice> dev,
    std::shared_ptr<boost::property_tree::ptree const> database,
    std::ostream &os = std::cout)
{
  double dimensionless_cell_current_density =
      database->get<double>("dimensionless_cell_current_density");
  double ratio_of_solution_phase_to_matrix_phase_conductivities =
      database->get<double>(
          "ratio_of_solution_phase_to_matrix_phase_conductivities");
  double ratio_of_separator_to_electrode_resistances =
      database->get<double>("ratio_of_separator_to_electrode_resistances");

  int const infty =
      database->get<int>("terms_in_truncation_of_infinite_series");
  double const pi = std::acos(-1.0);

  auto compute_dimensionless_overpotential =
      [infty, pi, &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_cell_current_density](double const dimensionless_time,
                                            double const dimensionless_position)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          ((n % 2 == 0 ? 1.0 : -1.0) +
           ratio_of_solution_phase_to_matrix_phase_conductivities) /
          std::pow(n, 2) * std::cos(n * pi * dimensionless_position) *
          std::exp(-std::pow(n, 2) * std::pow(pi, 2) * dimensionless_time);
    }
    return dimensionless_cell_current_density * dimensionless_time +
           dimensionless_cell_current_density *
               (3.0 * std::pow(dimensionless_position, 2) - 1.0) /
               (6.0 *
                (1.0 +
                 ratio_of_solution_phase_to_matrix_phase_conductivities)) +
           dimensionless_cell_current_density *
               ratio_of_solution_phase_to_matrix_phase_conductivities *
               (3.0 * std::pow(dimensionless_position, 2) + 2.0 -
                6.0 * dimensionless_position) /
               (6.0 *
                (1.0 +
                 ratio_of_solution_phase_to_matrix_phase_conductivities)) -
           2.0 * dimensionless_cell_current_density /
               (std::pow(pi, 2) *
                (1.0 +
                 ratio_of_solution_phase_to_matrix_phase_conductivities)) *
               std::accumulate(&(coefficients[1]), &(coefficients[infty]), 0.0);
  };

  auto compute_dimensionless_cell_voltage =
      [infty, pi, &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_cell_current_density,
       &ratio_of_separator_to_electrode_resistances](
          double const dimensionless_time)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          std::pow(((n % 2 == 0 ? 1.0 : -1.0) *
                        ratio_of_solution_phase_to_matrix_phase_conductivities +
                    1.0) /
                       (ratio_of_solution_phase_to_matrix_phase_conductivities +
                        1.0),
                   2) /
          (std::pow(n, 2) * std::pow(pi, 2)) *
          std::exp(-std::pow(n, 2) * std::pow(pi, 2) * dimensionless_time);
    }
    return 1.0 -
           dimensionless_cell_current_density *
               (1.0 / 3.0 + dimensionless_time -
                2.0 * std::accumulate(&(coefficients[1]),
                                      &(coefficients[infty]), 0.0)) -
           0.5 * ratio_of_separator_to_electrode_resistances *
               dimensionless_cell_current_density;
  };

  auto compute_dimensionless_double_layer_current_density =
      [infty, pi, &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_cell_current_density](double const dimensionless_time,
                                            double const dimensionless_position)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          ((n % 2 == 0 ? 1.0 : -1.0) +
           ratio_of_solution_phase_to_matrix_phase_conductivities) *
          std::cos(n * pi * dimensionless_position) *
          std::exp(-std::pow(n, 2) * std::pow(pi, 2) * dimensionless_time);
    }
    return 1.0 +
           2.0 /
               (1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities) *
               std::accumulate(&(coefficients[1]), &(coefficients[infty]), 0.0);
  };

  auto compute_dimensionless_complex_impedance_eq_24ab =
      [&ratio_of_solution_phase_to_matrix_phase_conductivities,
       &ratio_of_separator_to_electrode_resistances](
          double const dimensionless_angular_frequency)
  {
    double const dimensionless_real_impedance =
        (1.0 +
         std::pow(ratio_of_solution_phase_to_matrix_phase_conductivities, 2)) /
            (std::pow(
                 1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities,
                 2) *
             dimensionless_angular_frequency) *
            (std::sinh(dimensionless_angular_frequency) *
                 std::cosh(dimensionless_angular_frequency) -
             std::sin(dimensionless_angular_frequency) *
                 std::cos(dimensionless_angular_frequency)) /
            (std::pow(std::cosh(dimensionless_angular_frequency), 2) -
             std::pow(std::cos(dimensionless_angular_frequency), 2)) +
        2.0 * ratio_of_solution_phase_to_matrix_phase_conductivities /
            (std::pow(
                 1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities,
                 2) *
             dimensionless_angular_frequency) *
            (std::sinh(dimensionless_angular_frequency) *
                 std::cos(dimensionless_angular_frequency) -
             std::cosh(dimensionless_angular_frequency) *
                 std::sin(dimensionless_angular_frequency)) /
            (std::pow(std::cosh(dimensionless_angular_frequency), 2) -
             std::pow(std::cos(dimensionless_angular_frequency), 2)) +
        2.0 * ratio_of_solution_phase_to_matrix_phase_conductivities /
            std::pow(1.0 +
                         ratio_of_solution_phase_to_matrix_phase_conductivities,
                     2) +
        ratio_of_separator_to_electrode_resistances;
    double const dimensionless_imaginary_impedance =
        (1.0 +
         std::pow(ratio_of_solution_phase_to_matrix_phase_conductivities, 2)) /
            (std::pow(
                 1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities,
                 2) *
             dimensionless_angular_frequency) *
            (std::sinh(dimensionless_angular_frequency) *
                 std::cosh(dimensionless_angular_frequency) +
             std::sin(dimensionless_angular_frequency) *
                 std::cos(dimensionless_angular_frequency)) /
            (std::pow(std::cosh(dimensionless_angular_frequency), 2) -
             std::pow(std::cos(dimensionless_angular_frequency), 2)) +
        2.0 * ratio_of_solution_phase_to_matrix_phase_conductivities /
            (std::pow(
                 1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities,
                 2) *
             dimensionless_angular_frequency) *
            (std::sinh(dimensionless_angular_frequency) *
                 std::cos(dimensionless_angular_frequency) +
             std::cosh(dimensionless_angular_frequency) *
                 std::sin(dimensionless_angular_frequency)) /
            (std::pow(std::cosh(dimensionless_angular_frequency), 2) -
             std::pow(std::cos(dimensionless_angular_frequency), 2));
    return std::complex<double>(dimensionless_real_impedance,
                                dimensionless_imaginary_impedance);
  };
  auto compute_dimensionless_complex_impedance_eq22 =
      [&ratio_of_solution_phase_to_matrix_phase_conductivities,
       &ratio_of_separator_to_electrode_resistances](
          double const dimensionless_angular_frequency)
  {
    std::complex<double> sqrt_j_omega_star =
        std::sqrt(std::complex<double>(0.0, dimensionless_angular_frequency));
    return 4.0 * ratio_of_solution_phase_to_matrix_phase_conductivities /
               (std::pow(
                    1.0 +
                        ratio_of_solution_phase_to_matrix_phase_conductivities,
                    2) *
                sqrt_j_omega_star * std::sinh(sqrt_j_omega_star)) +
           2.0 *
               (1.0 +
                std::pow(ratio_of_solution_phase_to_matrix_phase_conductivities,
                         2)) *
               std::cosh(sqrt_j_omega_star) /
               (std::pow(
                    1.0 +
                        ratio_of_solution_phase_to_matrix_phase_conductivities,
                    2) *
                sqrt_j_omega_star * std::sinh(sqrt_j_omega_star)) +
           2.0 * ratio_of_solution_phase_to_matrix_phase_conductivities /
               std::pow(
                   1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities,
                   2) +
           ratio_of_separator_to_electrode_resistances;
  };
  auto const &compute_dimensionless_complex_impedance =
      compute_dimensionless_complex_impedance_eq22;

  // exact vs computed
  double const discharge_current = database->get<double>("discharge_current");
  double const discharge_time = database->get<double>("discharge_time");
  double const time_step = database->get<double>("time_step");
  double const epsilon = time_step * 1.0e-4;
  double const cross_sectional_area =
      database->get<double>("cross_sectional_area");
  double const time_normalization_factor =
      database->get<double>("time_normalization_factor");
  double frequency_normalization_factor = 2.0 / time_normalization_factor;
  double const impedance_normalization_factor =
      dimensionless_cell_current_density;
  double const initial_voltage = database->get<double>("initial_voltage");
  dimensionless_cell_current_density *=
      discharge_current / cross_sectional_area;
  dimensionless_cell_current_density /= 0.5 * initial_voltage;

  std::cout << "I^*   = " << dimensionless_cell_current_density << "\n";
  std::cout << "gamma = "
            << ratio_of_solution_phase_to_matrix_phase_conductivities << "\n";
  std::cout << "beta  = " << ratio_of_separator_to_electrode_resistances
            << "\n";

  // Charge the device
  for (double time = 0.0; time <= 60; time += 2.)
    dev->evolve_one_time_step_constant_voltage(2., initial_voltage);

  double computed_voltage;
  double exact_voltage;
  for (double time = 0.0; time <= discharge_time + epsilon; time += time_step)
  {
    double const dimensionless_time =
        (time + time_step) / time_normalization_factor;
    double const dimensionless_cell_voltage =
        compute_dimensionless_cell_voltage(dimensionless_time);
    exact_voltage = initial_voltage * dimensionless_cell_voltage;
    dev->evolve_one_time_step_constant_current(time_step, -discharge_current);
    dev->get_voltage(computed_voltage);
    if ((std::abs(time + time_step - 1e-3) < 1e-7) ||
        (std::abs(time + time_step - 2e-3) < 1e-7) ||
        (std::abs(time + time_step - 3e-3) < 1e-7) ||
        (std::abs(time + time_step - 4e-3) < 1e-7) ||
        (std::abs(time + time_step - 5e-3) < 1e-7) ||
        (std::abs(time + time_step - 6e-3) < 1e-7) ||
        (std::abs(time + time_step - 7e-3) < 1e-7) ||
        (std::abs(time + time_step - 8e-3) < 1e-7) ||
        (std::abs(time + time_step - 9e-3) < 1e-7) ||
        (std::abs(time + time_step - 10e-3) < 1e-7))
      os << boost::format("  %22.15e  %22.15e  %22.15e  \n") %
                (time + time_step) % exact_voltage % computed_voltage;
  }
  double const percent_tolerance = database->get<double>("percent_tolerance");

  // impedance spectroscopy
  std::fstream fout;
  double frequency_upper_limit = database->get<double>("frequency_upper_limit");
  double frequency_lower_limit = database->get<double>("frequency_lower_limit");
  int steps_per_decade = database->get<int>("steps_per_decade");
  fout.open("impedance_spectroscopy_data_verification", std::fstream::out);
  fout << "# impedance Z(f) = R + i X \n";
  fout << boost::format("# %22s  %22s  %22s  %22s  %22s  \n") %
              "frequency_f_[Hz]" % "resistance_R_[ohm]" % "reactance_X_[ohm]" %
              "magnitude_|Z|_[ohm]" % "phase_arg(Z)_[degree]";
  for (double frequency = frequency_upper_limit;
       frequency >= frequency_lower_limit;
       frequency /= std::pow(10.0, 1.0 / steps_per_decade))
  {
    double const dimensionless_angular_frequency =
        std::sqrt(2.0 * pi * frequency / frequency_normalization_factor);
    std::complex<double> const dimensionless_complex_impedance =
        compute_dimensionless_complex_impedance(
            dimensionless_angular_frequency);
    std::complex<double> const impedance =
        std::conj(impedance_normalization_factor *
                  dimensionless_complex_impedance) /
        cross_sectional_area;
    fout << boost::format("  %22.15e  %22.15e  %22.15e  %22.15e  %22.15e  \n") %
                frequency % impedance.real() % impedance.imag() %
                std::abs(impedance) % (std::arg(impedance) * 180.0 / pi);
  }
  fout.close();

  // energy efficiency
  double const cutoff_voltage = 0.0;
  auto compute_cutoff_time = [&compute_dimensionless_cell_voltage,
                              &dimensionless_cell_current_density,
                              &cutoff_voltage](double const I_star)
  {
    double const tolerance = 1.0e-10;
    boost::uintmax_t iterations = 100;
    dimensionless_cell_current_density = I_star;
    auto min_max = boost::math::tools::bisect(
        [&compute_dimensionless_cell_voltage,
         cutoff_voltage](double const &dimensionless_time)
        {
          return cutoff_voltage -
                 compute_dimensionless_cell_voltage(dimensionless_time);
        },
        0.0, 3600.0,
        [tolerance](double min, double max)
        {
          return std::abs(min - max) <= tolerance;
        },
        iterations);
    return 0.5 * (min_max.first + min_max.second);
  };

  auto compute_energy_efficiency =
      [infty, pi, &compute_cutoff_time,
       &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &ratio_of_separator_to_electrode_resistances](double const I_star,
                                                     double const cutoff_time)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          std::pow(((n % 2 == 0 ? 1.0 : -1.0) *
                        ratio_of_solution_phase_to_matrix_phase_conductivities +
                    1.0) /
                       (ratio_of_solution_phase_to_matrix_phase_conductivities +
                        1.0),
                   2) /
          (std::pow(n, 4) * std::pow(pi, 4)) *
          std::expm1(-std::pow(n, 2) * std::pow(pi, 2) * cutoff_time);
    }
    return I_star * cutoff_time *
               (1.0 - I_star / 3.0 - 0.5 * I_star * cutoff_time -
                0.5 * ratio_of_separator_to_electrode_resistances * I_star) -
           2.0 * std::pow(I_star, 2) *
               std::accumulate(&(coefficients[1]), &(coefficients[infty]), 0.0);
  };
}

} // end namespace cap

BOOST_AUTO_TEST_CASE(test_exact_transient_solution)
{
  // parse input file
  std::shared_ptr<boost::property_tree::ptree> input_database =
      std::make_shared<boost::property_tree::ptree>();
  boost::property_tree::info_parser::read_info("verification_problems.info",
                                               *input_database);

  // remove faradaic processes
  input_database->put(
      "device.material_properties.electrode_material.exchange_current_density",
      0.0);

  // build an energy storage system
  std::shared_ptr<boost::property_tree::ptree> device_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("device"));
  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(boost::mpi::communicator(),
                                      *device_database);

  // measure discharge curve
  std::fstream fout;
  fout.open("verification_problem_data", std::fstream::out);

  std::shared_ptr<boost::property_tree::ptree> verification_problem_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("verification_problem_srinivasan"));

  cap::compute_parameters(device_database, verification_problem_database);

  cap::verification_problem(device, verification_problem_database, fout);

  fout.close();
}


/*=========================================================================

  Program:   Small Body Geophysical Analysis
  Module:    SBGATFilter.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/**
@file SBGATFilter.hpp
@class  SBGATFilter
@author Benjamin Bercovici
@date February 2019

@brief A generic class serving a a shape container and providing a few useful methods
to access the shape's underlying facets, vertices and edges
Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
@copyright MIT License, Benjamin Bercovici and Jay McMahon, (c) Ken Martin, Will Schroeder, Bill Lorensen
*/

#ifndef SBGATFilter_hpp
#define SBGATFilter_hpp

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>

class VTKFILTERSCORE_EXPORT SBGATFilter : public vtkPolyDataAlgorithm{
public:

  /**
   * Constructs with initial values of zero.
   */

  static SBGATFilter *New();

  vtkTypeMacro(SBGATFilter,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Sets the scale factor to 1, indicative that the polydata has its coordinates expressed in meters
  */
  void SetScaleMeters() { this -> scaleFactor = 1;this -> is_in_meters = true;}

  /**
  Sets the scale factor to 1000, indicative that the polydata has its coordinates expressed in kilometers
  */
  void SetScaleKiloMeters() { this -> scaleFactor = 1000; this -> is_in_meters = false;}

  /**
  Return the shape's scale factor
  @return scale factor
  */
  double GetScaleFactor() const {return this -> scaleFactor;}

  /**
  Sets polyhedron density
  @param density bulk density of polyhedron (kg/m^3)
  */
  void SetDensity(const double density){
    this -> density = density;
  }

  /**
  Get polyhedron density
  @return density bulk density of polyhedron (kg/m^3)
  */
  double GetDensity() const{
    return this -> density ;
  }

  /**
  Return coordinates of the three vertices forming a facet
  @param[in] f facet index
  @param[out] r0 first vertex coordinates
  @param[out] r1 first vertex coordinates
  @param[out] r2 first vertex coordinates
  */
  void GetVerticesInFacet(const int & f,double * r0,double * r1, double * r2) const;

  /**
  Return coordinates of point at center of facet
  @param f facet index
  @return coordinates of f-th facet center
  */
  arma::vec::fixed<3> GetFacetCenter(const int & f) const;

  /**
  Return coordinates of the two vertices forming an edge
  @param[in] e edge index
  @param[out] r0 first vertex coordinates
  @param[out] r1 first vertex coordinates
  */
  void GetVerticesOnEdge(const int & e,double * r0,double * r1) const;

  /**
  Return normal of facet f
  @param[in] f facet index
  @param[out] n normal at facet
  */
  void GetFacetNormal(const int & f, double * n) const{n[0] = this -> facet_normals[f][0];n[1] = this -> facet_normals[f][1];n[2] = this -> facet_normals[f][2];}

  /**
  Return the indices of the two facets adjacent to the specified edge
  @param[in] e edge index
  @param[out] f0 index of first facet
  @param[out] f1 index of second facet
  */
  void GetIndicesOfAdjacentFacets(const int & e,int & f0, int & f1) const;

  /**
  Return the indices of the vertices forming the prescribed edge
  @param[in] e edge index
  @param[out] v0 first vertex index
  @param[out] v1 second vertex index
  */
  void GetIndicesVerticesOnEdge(const int & e, int & v0,int & v1) const {v0 = this -> edges[e][0];v1 = this -> edges[e][1];}

  /**
  Return the indices of the vertices forming the prescribed facet
  @param[in] f facet index
  @param[out] v0 first vertex index
  @param[out] v1 second vertex index
  @param[out] v2 third vertex index
  */
  void GetIndicesVerticesInFacet(const int & f, int & v0,int & v1,int & v2) const {v0 = this -> facets[f][0];v1 = this -> facets[f][1];v2 = this -> facets[f][2];}

  /**
  Return the non-normalized facet normal at this facet
  @param f facet index
  @return non-normalized facet normal
  */
  arma::vec::fixed<3> GetNonNormalizedFacetNormal(const int & f) const;

  /**
  Return the vector from the field point to the first vertex on the designated edge
  @param pos field point
  @param e edge index
  @return the vector from the field point to the first vertex on the designated edge
  */
  arma::vec::fixed<3> GetRe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the vector from the field point to the first vertex on the designated edge
  @param pos field point
  @param e edge index
  @return the vector from the field point to the first vertex on the designated edge
  */
  arma::vec::fixed<3> GetRe(const double * pos,const int & e) const;

  /**
  Return the vector from the field point to the first vertex on the designated facet
  @param pos field point
  @param f facet index
  @return the vector from the field point to the first vertex on the designated facet
  */
  arma::vec::fixed<3> GetRf(const arma::vec::fixed<3> & pos,const int & f) const;

  /**
  Return the vector from the field point to the first vertex on the designated facet
  @param pos field point
  @param f facet index
  @return the vector from the field point to the first vertex on the designated facet
  */
  arma::vec::fixed<3> GetRf(const double * pos,const int & f) const;

  /**
  * Return the bounding box (xmin,xmax,ymin,ymax,zmin,zmax) (m)
  @param[in] xmin minimum x-bound of the shape (m)
  @param[in] xmax maximum x-bound of the shape (m)
  @param[in] ymin minimum y-bound of the shape (m)
  @param[in] ymax maximum y-bound of the shape (m)
  @param[in] zmin minimum z-bound of the shape (m)
  @param[in] zmax maximum z-bound of the shape (m)
  */
  void GetBoundingBox(double & xmin,double & xmax,double & ymin,double & ymax,double & zmin,double & zmax) const {
    xmin = bounds[0];
    xmax = bounds[1];
    ymin = bounds[2];
    ymax = bounds[3];
    zmin = bounds[4];
    zmax = bounds[5];
  }

/**
  Return length of considered edge in meters
  @param e edge index
  @return edge length(m )
*/
  double GetEdgeLength(const int & e) const;

  /**
  Return number of facets in shape
  @return number of facets
  */
  int GetN_facets() const{return this -> N_facets;}
  /**
  Return number of edges in shape
  @return number of edges
  */
  int GetN_edges() const{return this -> N_edges;}

  /**
  Return number of vertices in shape
  @return number of vertices
  */
  int GetN_vertices() const{return this -> N_vertices;}

  /**
  Return true if the input shape has its coordinates expressed in meters, false otherwise
  */
  bool GetIsInMeters() const{ return this -> is_in_meters;}

  /**
  Return true if the input shape has its coordinates expressed in kilometers, false otherwise
  */
  bool GetIsInKiloMeters() const{ return !this -> is_in_meters;}

protected:
  SBGATFilter();
  ~SBGATFilter() override;

  void Clear();

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  double ** facet_normals;
  double ** vertices;

  double scaleFactor = 1;
  double density;

  int ** edges;
  int ** facets;
  int ** edge_facets_ids;

  bool is_in_meters = true;

  int N_facets;
  int N_edges;
  int N_vertices;

  double bounds[6];

private:
  SBGATFilter(const SBGATFilter&) = delete;
  void operator=(const SBGATFilter&) = delete;
};

#endif


#define BOOST_TEST_MODULE MyTest
#include <boost/test/unit_test.hpp>
#include <boost/format.hpp>
#include <iostream>
#include <fstream>
#include "Network.h"
#include "hamming.h"

BOOST_AUTO_TEST_CASE(HammingTest) {
    std::ios_base::sync_with_stdio(false);
    std::cin.tie(NULL);
    for (int i = 0; i <= 2; ++i) {
        std::ifstream ifs((boost::format("tests/inHamming%d.txt") % i).str(), std::ios_base::in);
        std::streambuf* cinbuf = std::cin.rdbuf();
        std::cin.rdbuf(ifs.rdbuf());

        HammingOptimizer<uint, int, int> hamming;
        hamming.solve();

        std::cin.rdbuf(cinbuf);
    }

}


#ifndef ASLAM_BACKEND_HOMOGENEOUS_EXPRESSION_NODE_HPP
#define ASLAM_BACKEND_HOMOGENEOUS_EXPRESSION_NODE_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include "TransformationExpressionNode.hpp"
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Core>
#include "EuclideanExpression.hpp"

namespace aslam {
  namespace backend {
    class TransformationExpressionNode;

    /**
     * \class HomogeneousExpressionNode
     * \brief The superclass of all classes representing homogeneous points.
     */
    class HomogeneousExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      HomogeneousExpressionNode();
      virtual ~HomogeneousExpressionNode();

      /// \brief Evaluate the homogeneous matrix.
      Eigen::Vector4d toHomogeneous() const;

      /// \brief Evaluate the Jacobians
      void evaluateJacobians(JacobianContainer & outJacobians) const;

      /// \brief Evaluate the Jacobians and apply the chain rule.
      void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      virtual void getDesignVariables(DesignVariable::set_t & designVariables) const;
    protected:
      // These functions must be implemented by child classes.
      virtual Eigen::Vector4d toHomogeneousImplementation() const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const = 0;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const = 0;

    };

    /**
     * \class HomogeneousExpressionNodeMultiply
     *
     * \brief A class representing the transformation of a homogeneous point
     *
     */
    class HomogeneousExpressionNodeMultiply : public HomogeneousExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      HomogeneousExpressionNodeMultiply(boost::shared_ptr<TransformationExpressionNode> lhs,
				     boost::shared_ptr<HomogeneousExpressionNode> rhs);
      virtual ~HomogeneousExpressionNodeMultiply();

    private:
      virtual Eigen::Vector4d toHomogeneousImplementation() const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

      boost::shared_ptr<TransformationExpressionNode> _lhs;
      mutable Eigen::Matrix4d _T_lhs;
      boost::shared_ptr<HomogeneousExpressionNode> _rhs;
      mutable Eigen::Vector4d _p_rhs;
    };

    class HomogeneousExpressionNodeConstant : public HomogeneousExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      HomogeneousExpressionNodeConstant(const Eigen::Vector4d & p);
      virtual ~HomogeneousExpressionNodeConstant();

        void set(const Eigen::Vector4d & p){ _p = p; }
    private:
      virtual Eigen::Vector4d toHomogeneousImplementation() const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

      Eigen::Vector4d _p;
    };

  class HomogeneousExpressionNodeEuclidean : public HomogeneousExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      HomogeneousExpressionNodeEuclidean( boost::shared_ptr<EuclideanExpressionNode> p);
      virtual ~HomogeneousExpressionNodeEuclidean();

    private:
      virtual Eigen::Vector4d toHomogeneousImplementation() const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

      boost::shared_ptr<EuclideanExpressionNode> _p;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_HOMOGENEOUS_EXPRESSION_NODE_HPP */


#include <boost/python.hpp>
#include "integrand.h"

using namespace Eigen;

void getHband(const VectorXd &x, complex_t &mat,
              const VectorXd &SK, const double &H) {
    // SK : t, t1
    double t = SK[0], t1 = SK[1];
    mat = -2*t*(cos(x(0)) + cos(x(1))) - 4*t1*cos(x(0))*cos(x(1)) - H;
}

GreenIntegrand::GreenIntegrand(const VectorXcd &w_, const double &mu_,
    const MatrixXcd &SE_, const VectorXd &SK_, const double &Hf)
    : md_int::GeneralIntegrand<complex_t>(), mu(mu_),
      H(Hf), wall(w_), SEall(SE_), SK(SK_) {
    set_dim_in(DIM);
    set_dim_out(MSIZE);
}

void GreenIntegrand::set_data(const int &r) {
    w = wall(r);
    SE.setZero();
    for (int i = 0; i < MSIZE; ++i)
        SE(i) = SEall(r, MSIZE*i+i);  // diagonal terms
}

void GreenIntegrand::CalculateOriginalIntegrand(const VectorXd &x,
                                                VectorXcd &y) const {
    y.resize(dim_out());
    complex_t Hband;
    getHband(x, Hband, SK, H);

    for (int L = 0; L < MSIZE; ++L) {
        y(L) = 1. / (w + mu - Hband - SE(L));
    }
}

HIntegrand::HIntegrand(const int &Norder, const VectorXd &SK_,
                       const double &Hf_in)
    : md_int::GeneralIntegrand<complex_t>(), N_order(Norder),
      Hf(Hf_in), SK(SK_) {
    set_dim_in(DIM);
    set_dim_out(N_order*MSIZE);
}

void HIntegrand::CalculateOriginalIntegrand(const VectorXd &x,
                                            VectorXcd &y) const {
    complex_t Hband, Hpow = 1.;
    y.resize(dim_out());
    y.setZero();
    getHband(x, Hband, SK, Hf);
    for (int n = 0; n < N_order; ++n) {
        Hpow *= Hband;
        for (int i = 0; i < MSIZE; ++i)
            y(MSIZE*n+i) = Hpow;
    }
}


#include <iostream>
#include <array>
#include <string>
#include <cstring>
#include <fstream>
#include <random>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/token_functions.hpp>
#include <exception>
#define _USE_MATH_DEFINES
#include <cmath>
#include "port_authority.hpp"
#include "mpi.h"

using namespace std;
using namespace pauth;
using namespace boost;
using namespace boost::program_options;

const unsigned MASTER = 0;

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

  unsigned numruns, numinit, numthreads;
  unsigned long numsteps;
  double eps, delta_max;
  string acc_criterion_str, outfile_str;

  options_description desc("\nTransition state theory assignment.\n\nAllowed arguments");

  desc.add_options()
    ("help,h", "Produce this help message.")
    ("num-runs,n", value<unsigned>(&numruns)->default_value(25),
     "Number of runs for each beta.")
    ("num-steps,s", value<unsigned long>(&numsteps)->default_value(100000),
     "Number of steps for each run.")
    ("num-init,i", value<unsigned>(&numinit)->default_value(100),
     "Number of steps to run in A well for getting random initial conditions.")
    ("num-ompthreads,m", value<unsigned>(&numthreads)->default_value(1),
     "Number of OpenMP threads.")
    ("well-size,z", value<double>(&eps)->default_value(0.2), "Size of well A.")
    ("max-step-size,x", value<double>(&delta_max)->default_value(1.25),
     "Maximum trial move step size.")
    ("acception-criterion,a",
     value<string>(&acc_criterion_str)->default_value("metropolis"),
     "Acceptance criterion (metropolis|kawasaki)")
    ("outfile,o", value<string>(&outfile_str)->default_value("tst.csv"),
     "Output filepath.");

  // override OMP_NUM_THREADS env variable
  omp_set_num_threads(numthreads);

  variables_map vm;
  try {
    store(command_line_parser(argc, argv).options(desc).run(), vm);
    notify(vm);
  } catch(std::exception &e) {
    cout << endl << e.what() << endl;
    cout << desc << endl;
  }

  if (vm.count("help")) {
    cout << "--help specified" << endl;
    cout << desc << endl;
    return 0;
  }

  acc acc_criterion_funct;
  if (acc_criterion_str == "metropolis") acc_criterion_funct = metropolis_acc;
  else if (acc_criterion_str == "kawasaki")
    acc_criterion_funct = kawasaki_acc;
  else {
    cerr << "Error: acceptance-criterion \"" << acc_criterion_str << "\" is "
            "not understood.\n";
    return -1;
  }

  ofstream ostr(outfile_str, ofstream::out);
  std::array<double, 5> Ts = { {
    1.0 / 0.01, 1.0 / 0.1, 1.0, 1.0 / 10.0, 1.0 / 100.0
  } };
  const double kB = 1.0;
  const molecular_id id = molecular_id::Test1;
  const metric m = euclidean;
  const bc boundary = no_bc;
  const_quad_spring_potential pot(1.0, -2.0, 1.0);

  int taskid, numtasks, rc;

  MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &numtasks);
  MPI_Comm_rank(MPI_COMM_WORLD, &taskid);
  cout << "MPI task " << taskid << " has started...\n";

  if (taskid == MASTER) ostr << "beta,M,N,<d(x-q)>,ktst\n";

  random_device rd;
  default_random_engine rng(rd() + taskid);
  uniform_real_distribution<double> dist(0.0, 1.0);

  for (auto T : Ts) {
    long unsigned N_beta = 0;
    long unsigned M_beta = 0;

    for (unsigned k = 0; k < numruns / numtasks; ++k) {
      long unsigned N = 0, N_run = 0;
      long unsigned M = 0, M_run = 0;

      // initialize
      metropolis sim("ktst.xyz", id, 1, 1, 1.0, continuous_trial_move(delta_max),
                     &pot, T, kB, m, boundary,
                     acc_criterion_funct);

      // initialize to random point in well A
      sim.add_stopping_criterion([&](const metropolis &sim) -> bool {
        const auto x0 = sim.positions()(0, 0);
        if (sim.step() % 10 == 0 && x0 < (eps / 2.0 - 1.0)
            && x0 > (-eps / 2.0 - 1.0)) {
          if (dist(rng) < 0.7) return true;
        }
        return false;
      });

      sim.simulate(numsteps / numtasks);
      sim.clear_stopping_criteria();
      sim.reset_step();
      cout << "(task, x0) = (" << taskid << ", " << sim.positions()(0, 0)
           << ")\n";

      // measure
      sim.add_callback([&](const metropolis &sim) {
        const auto x0 = sim.positions()(0, 0);
        if (x0 < eps / 2.0 && x0 > -eps / 2.0) ++M;

        ++N;
      });

      // run main simulation
      sim.simulate(numsteps);

      // post-process and record data
      rc = MPI_Reduce(&N, &N_run, 1, MPI_UNSIGNED_LONG, MPI_SUM,
                      MASTER, MPI_COMM_WORLD);
      if (rc != MPI_SUCCESS) printf("%d: failure on mpc_reduce\n", taskid);

      rc = MPI_Reduce(&M, &M_run, 1, MPI_UNSIGNED_LONG, MPI_SUM,
                      MASTER, MPI_COMM_WORLD);
      if (rc != MPI_SUCCESS) printf("%d: failure on mpc_reduce\n", taskid);

      N_beta += N_run;
      M_beta += M_run;
    }

    if (taskid == MASTER) {
      const double beta = 1 / (T * kB);
      const double dxq = M_beta / (N_beta * eps);
      const double ktst = 0.5 * sqrt(2 / (M_PI * beta * 1.0)) * dxq;
      ostr << beta << ',' << M_beta << ',' << N_beta
           << ',' << dxq << ',' << ktst << '\n';
      cout << "beta       =    " << beta << '\n';
      cout << "M          =    " << M_beta << '\n';
      cout << "N          =    " << N_beta << '\n';
      cout << "<d(x-q)>   =    " << dxq << '\n';
      cout << "ktst       =    " << ktst << '\n';
      cout << '\n';
    }
  }

  MPI_Finalize();

  return 0;
}