/**
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <cmath>
#include <memory>

#include "MaterialLib/MPL/Properties/CapillaryPressureSaturation/SaturationVanGenuchten.h"
#include "MaterialLib/MPL/Properties/SaturationDependentSwelling.h"
#include "MaterialLib/MPL/Property.h"
#include "MaterialLib/SolidModels/LinearElasticIsotropic.h"
#include "MaterialLib/SolidModels/MechanicsBase.h"
#include "MathLib/InterpolationAlgorithms/PiecewiseLinearInterpolation.h"
#include "ParameterLib/ConstantParameter.h"
#include "ProcessLib/RichardsMechanics/ComputeMicroPorosity.h"

using namespace ProcessLib::RichardsMechanics;
namespace MPL = MaterialPropertyLib;

namespace
{
template <int DisplacementDim>
MathLib::KelvinVector::KelvinMatrixType<DisplacementDim>
computeElasticTangentStiffness(
    MaterialLib::Solids::MechanicsBase<DisplacementDim> const& solid_material,
    double const t, ParameterLib::SpatialPosition const& x_position,
    double const dt, double const temperature)
{
    MPL::VariableArray variable_array;
    MPL::VariableArray variable_array_prev;

    auto const null_state = solid_material.createMaterialStateVariables();

    using KV = MathLib::KelvinVector::KelvinVectorType<DisplacementDim>;

    variable_array[static_cast<int>(MPL::Variable::stress)].emplace<KV>(
        KV::Zero());
    variable_array[static_cast<int>(MPL::Variable::mechanical_strain)]
        .emplace<KV>(KV::Zero());
    variable_array[static_cast<int>(MPL::Variable::temperature)]
        .emplace<double>(temperature);

    variable_array_prev[static_cast<int>(MPL::Variable::stress)].emplace<KV>(
        KV::Zero());
    variable_array_prev[static_cast<int>(MPL::Variable::mechanical_strain)]
        .emplace<KV>(KV::Zero());
    variable_array_prev[static_cast<int>(MPL::Variable::temperature)]
        .emplace<double>(temperature);

    auto&& solution = solid_material.integrateStress(
        variable_array_prev, variable_array, t, x_position, dt, *null_state);

    if (!solution)
    {
        OGS_FATAL("Computation of elastic tangent stiffness failed.");
    }

    MathLib::KelvinVector::KelvinMatrixType<DisplacementDim> C =
        std::move(std::get<2>(*solution));

    return C;
}

}  // namespace

TEST(RichardsMechanics, computeMicroPorosity)
{
    static constexpr auto eps = 2e-14;
    constexpr int DisplacementDim = 2;

    //
    // Create properties.
    //

    // Saturation
    constexpr double residual_liquid_saturation = 0;
    constexpr double residual_gas_saturation = 0;
    constexpr double exponent = 0.4;
    constexpr double p_b = 300e6;
    MPL::Property const& saturation_micro = MPL::SaturationVanGenuchten{
        "saturation_micro", residual_liquid_saturation, residual_gas_saturation,
        exponent, p_b};

    // Swelling
    constexpr std::array swelling_pressures{1e7, 1e7, 1e7};
    constexpr std::array swelling_exponents{1.2, 1.2, 1.2};
    constexpr double lower_saturation_limit = 0;
    constexpr double upper_saturation_limit = 1;
    MPL::Property const& swelling_stress_rate =
        MPL::SaturationDependentSwelling{
            "swelling_stress_rate", swelling_pressures,     swelling_exponents,
            lower_saturation_limit, upper_saturation_limit, nullptr};

    // LinearElastic
    ParameterLib::ConstantParameter<double> E{"YoungsModulus", 150e6};
    ParameterLib::ConstantParameter<double> nu{"PoissonsRatio", 0.25};
    auto const solid_material =
        MaterialLib::Solids::LinearElasticIsotropic<DisplacementDim>{{E, nu}};

    //
    // Populate constants
    //
    ParameterLib::SpatialPosition const pos;
    constexpr double t0 = 0;
    constexpr double t_max = 200e3;
    double const dt = 100;
    double const T = 293.15;

    auto const& identity2 = MathLib::KelvinVector::Invariants<
        MathLib::KelvinVector::kelvin_vector_dimensions(
            DisplacementDim)>::identity2;
    auto const C_el =
        computeElasticTangentStiffness(solid_material, t0, pos, dt, T);
    MathLib::KelvinVector::KelvinVectorType<DisplacementDim> const&
        I_2_C_el_inverse = identity2.transpose() * C_el.inverse();
    double const rho_LR_m = 1e3;
    double const mu_LR = 1e-3;
    MicroPorosityParameters const micro_porosity_parameters{
        NumLib::NewtonRaphsonSolverParameters{1000, 1e-8, 1e-15}, 1e-14};

    double const alpha_B = 1;
    double const phi_M = 0.45;

    auto saturation = [&](double const p_L)
    {
        MPL::VariableArray v;
        v[static_cast<int>(MPL::Variable::capillary_pressure)] = -p_L;
        return saturation_micro.template value<double>(v, pos, t0, dt);
    };

    //
    // Drive the computation by macro pressure. Saturation, steady, and
    // desaturation.
    //

    // The time coordinates are simultaneously also the test points.
    std::vector const t_coords{
        // saturation is quick to rich maximum,
        0 * t_max, 0.05 * t_max, 0.2 * t_max, 0.3 * t_max,
        // desaturation takes much longer.
        0.35 * t_max, 0.5 * t_max, t_max};
    std::vector const pressure_M_values{// saturation
                                        -200e6, -200e6, 0., 0.,
                                        // desaturation
                                        0., -200e6, -200e6};
    MathLib::PiecewiseLinearInterpolation const pressure_M{t_coords,
                                                           pressure_M_values};

    //
    // Initial state
    //

    MicroPorosityStateSpace<DisplacementDim> state_prev = {
        0.45 - 0.25,  // phi_m
        0,            // e_sw
        -200e6,       // p_L_m
        {0, 0, 0, 0}  // sigma_sw
    };

    auto state = state_prev;

    std::vector<MicroPorosityStateSpace<DisplacementDim>> results;
    for (double t = t0 + dt; t <= t_max; t += dt)
    {
        double const p_L = pressure_M.getValue(t);
        double const S_L_m_prev = saturation(state_prev.p_L_m);
        // auto const [delta_phi_m, delta_e_sw, delta_sigma_sw, delta_p_L_m] =
        auto const state_increment = computeMicroPorosity<DisplacementDim>(
            I_2_C_el_inverse,
            rho_LR_m,  // for simplification equal to rho_LR_M
            mu_LR, micro_porosity_parameters, alpha_B, phi_M, p_L,
            state_prev.p_L_m, MaterialPropertyLib::VariableArray{}, S_L_m_prev,
            state_prev.phi_m, pos, t, dt, saturation_micro,
            swelling_stress_rate);

        // push back state
        state_prev = state;

        // update
        state += state_increment;

        if (std::find_if(begin(t_coords), end(t_coords),
                         [&](auto const value) {
                             return std::abs(t - value) < eps;
                         }) != end(t_coords))
        {
            results.push_back(state);
            /* Keep for possible result updates
            std::cout << std::setprecision(17)
                      << "MicroPorosityStateSpace<DisplacementDim>{ "
                      << state.phi_m << ", " << state.e_sw << ", "
                      << state.p_L_m << ", {" << state.sigma_sw[0] << ", "
                      << state.sigma_sw[1] << ", " << state.sigma_sw[2] << ", "
                      << state.sigma_sw[3] << " }}\n";
            */
        }
    }

    //
    // Expected results
    //
    std::vector expected_results = {
        MicroPorosityStateSpace<DisplacementDim>{
            0.20000000000000001, 0, -200000000, {0, 0, 0, 0}},
        MicroPorosityStateSpace<DisplacementDim>{
            0.20678794335787565,
            0.012341715196137344,
            -87023875.75987792,
            {-1234171.5196137347, -1234171.5196137347, -1234171.5196137347, 0}},
        MicroPorosityStateSpace<DisplacementDim>{
            0.20984996711734061,
            0.017909031122437243,
            -4693546.2948520193,
            {-1790903.1122437208, -1790903.1122437208, -1790903.1122437208, 0}},
        MicroPorosityStateSpace<DisplacementDim>{
            0.20987701765630837,
            0.017958213920560351,
            23234.984113500199,
            {-1795821.3920560319, -1795821.3920560319, -1795821.3920560319, 0}},
        MicroPorosityStateSpace<DisplacementDim>{
            0.20475472126658667,
            0.0086449477574298151,
            -123250534.84079438,
            {-864494.77574297995, -864494.77574297995, -864494.77574297995, 0}},
        MicroPorosityStateSpace<DisplacementDim>{
            0.20005534659406693,
            0.0001006301710302317,
            -199863745.41244847,
            {-10063.017103021764, -10063.017103021764, -10063.017103021764,
             0}}};

    auto eps_equal = [](double const a, double const b)
    { return std::abs(a - b) < eps; };

    auto const [mismatch_it_expected, mismatch_it_results] = std::mismatch(
        begin(expected_results), end(expected_results), begin(results),
        [&](auto const& a, auto const& b)
        {
            EXPECT_TRUE(eps_equal(a.phi_m, b.phi_m)) << "with eps = " << eps;
            EXPECT_TRUE(eps_equal(a.e_sw, b.e_sw)) << "with eps = " << eps;
            EXPECT_TRUE(eps_equal(a.p_L_m / 1e9, b.p_L_m / 1e9))
                << "with eps = " << eps;
            EXPECT_TRUE((a.sigma_sw - b.sigma_sw).norm() / 1e9 < eps)
                << "with eps = " << eps;
            // Divide pressures and stresses by GPa to get approximately
            // O(1) numbers.
            return eps_equal(a.phi_m, b.phi_m) && eps_equal(a.e_sw, b.e_sw) &&
                   eps_equal(a.p_L_m / 1e9, b.p_L_m / 1e9) &&
                   (a.sigma_sw - b.sigma_sw).norm() / 1e9 < eps;
        });

    ASSERT_TRUE(mismatch_it_expected == end(expected_results))
        << "Mismatch for expected result\n\t" << *mismatch_it_expected
        << "\n\tfound. Corresponding result is\n\t" << *mismatch_it_results;
    ASSERT_TRUE(mismatch_it_results == end(results));
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file dreyfus_wagner_long_test.cpp
 * @brief
 * @author Maciej Andrejczuk
 * @version 1.0
 * @date 2013-08-04
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_steinlib.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/steiner_tree/dreyfus_wagner.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/adaptor/sliced.hpp>

#include <vector>
#include <fstream>

using namespace paal;

BOOST_AUTO_TEST_CASE(dreyfus_wagner_steinlib_long_test) {
    std::vector<steiner_tree_test_with_metric> data;
    LOGLN("READING INPUT...");
    read_steinlib_tests(data);
    // Smaller tests only
    for (auto const &test : data | boost::adaptors::sliced(0, 50)) {
        LOGLN("TEST " << test.test_name);
        auto dw = paal::make_dreyfus_wagner(
            test.metric, test.terminals, test.steiner_points);
        dw.solve();
        int res = dw.get_cost();
        BOOST_CHECK_EQUAL(res, test.optimal);
    }
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-reduction

    returns @ref True if each vector elements have all bits or no bit set.

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

    @par Note

     Using `is_simd_logical(x)` is similar to `all(logical_or(x == Allbits(as(x)), x == Zero(as(x))))`

  **/
  bool is_simd_logical(Value const& x);
} }
#endif

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

#endif


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

#include "multiplication_process.h"

#include <boost/cstdint.hpp>

/**
 * \file multiplication_process.cxx
 *
 * \brief Implementation of the multiplication process.
 */

namespace sprokit
{

class multiplication_process::priv
{
  public:
    typedef int32_t number_t;

    priv();
    ~priv();

    static port_t const port_factor1;
    static port_t const port_factor2;
    static port_t const port_output;
};

process::port_t const multiplication_process::priv::port_factor1 = port_t("factor1");
process::port_t const multiplication_process::priv::port_factor2 = port_t("factor2");
process::port_t const multiplication_process::priv::port_output = port_t("product");

multiplication_process
::multiplication_process(kwiver::vital::config_block_sptr const& config)
  : process(config)
  , d(new priv)
{
  port_flags_t required;

  required.insert(flag_required);

  declare_input_port(
    priv::port_factor1,
    "integer",
    required,
    port_description_t("The first factor to multiply."));
  declare_input_port(
    priv::port_factor2,
    "integer",
    required,
    port_description_t("The second factor to multiply."));
  declare_output_port(
    priv::port_output,
    "integer",
    required,
    port_description_t("Where the product will be available."));
}

multiplication_process
::~multiplication_process()
{
}

void
multiplication_process
::_step()
{
  priv::number_t const factor1 = grab_from_port_as<priv::number_t>(priv::port_factor1);
  priv::number_t const factor2 = grab_from_port_as<priv::number_t>(priv::port_factor2);

  priv::number_t const product = factor1 * factor2;

  push_to_port_as<priv::number_t>(priv::port_output, product);

  process::_step();
}

multiplication_process::priv
::priv()
{
}

multiplication_process::priv
::~priv()
{
}

}


//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -I/Users/arnold/Documents/prg/libraries/cryptopp563/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib -L/Users/arnold/Documents/prg/libraries/cryptopp563/lib -o date date.cpp -lboost_system -lboost_filesystem -lboost_iostreams -lcryptopp

#include <iostream>
#include <boost/filesystem.hpp>
#include <vector>
#include <string>
#include <array>
#include <boost/format.hpp>
#include <utility>

using namespace std;

class Date
{
public:
	explicit Date (unsigned yyyy, unsigned mm, unsigned dd) : val_{yyyy, mm, dd}
	{
		if (1 > yyyy || 9999 < yyyy)
			throw "year out of range";
		if (1 > mm || 12 < mm)
			throw "month out of range";
		if (1 > dd || daysOfMonth(yyyy, mm) < dd) {
			cout << "Days out of range! yyyy:" << yyyy << " mm: " << mm << " dd: " << dd << endl;
			cout.flush();
			throw "days ot of range";
		}
	}	// type conversion
	explicit Date (unsigned yyyymmdd) : Date(yyyymmdd/10000, (yyyymmdd/100)%100, yyyymmdd%100)
	{}
	Date() =default;									// default ctor
	Date(const Date& d) : val_{d.val_.yyyy, d.val_.mm, d.val_.dd}{}							// copy ctor
	//Date(Date&&);								// move ctor
	Date& operator=(const Date& d) {
		if (this != &d) {
			val_.yyyy = d.val_.yyyy;
			val_.mm = d.val_.mm;
			val_.dd = d.val_.dd;
		}
		return *this;
	}				// copy assignment
	//Date& operator=(Date&&);					// move assignment
	~Date ()
	{}

	constexpr unsigned year() const noexcept { return val_.yyyy; }
	constexpr unsigned month() const noexcept { return val_.mm; }
	constexpr unsigned day() const noexcept { return val_.dd; }
	constexpr unsigned yyyymmdd() const noexcept { return year()*10000 + month() * 100 + day(); }
	explicit constexpr operator unsigned() const noexcept { return yyyymmdd(); }

	Date firstOfYear() const noexcept { return Date(year(), 1, 1); }
	Date lastOfYear() const noexcept { return Date(year(), 12, 31); }
	Date firstOfMonth() const noexcept { return Date(year(), month(), 1); }
	Date lastOfMonth() const noexcept { return Date(year(), month(), Date::daysOfMonth(year(), month())); }

	bool isLeapYear() const noexcept { return Date::isLeapYear(year()); }
	unsigned dayOfYear() const noexcept {
		auto d = Date::doma_[month()-1] + day();
		if (month() > 2 && isLeapYear())
			++d;
		return d;
	}

	unsigned weekday() const noexcept {
		// http://www.timeanddate.com/date/doomsday-weekday.html
		int a1 = (year() % 100)/12;
		int a2 = (year() % 100) - (12 * a1);
		int a3 = a2/4;
		int a4 = Date::anchor_[(year()/100)%4];
		int a5 = a1 + a2 + a3 + a4;
		int a6 = a5 % 7;
		auto zz = a6 + static_cast<int>(dayOfYear()) - 31 - static_cast<int>(Date::daysOfMonth(year(), 2));
		auto z = (zz) %7;
		if (zz < 0)
			z = (7 + z)%7;
		//cout << "(" << zz << ", " << z << ")";
		auto wt = std::array<unsigned, 7>{{7, 1, 2, 3, 4, 5, 6}}[z];
		return wt;
	}
	unsigned weekOfYear() const noexcept {
		auto wfoy = firstOfYear().weekday()-1;
		auto doy = dayOfYear();
		auto firstweeknr = (wfoy > 0 && wfoy < 5)?1:0;
		auto nw = (doy / 7) + firstweeknr;
		return nw;
	}

	static bool isLeapYear(unsigned yyyy) noexcept {
		return (yyyy % 4 == 0) && ((yyyy % 100 != 0) || (yyyy % 400 == 0));
	}
	static unsigned daysOfYear(unsigned yyyy) noexcept {
		return isLeapYear(yyyy)?366:365;
	}
	static unsigned daysOfMonth(unsigned yyyy, unsigned mm) {
		auto d = dom_[mm];
		if (2 == mm && isLeapYear(yyyy)) {
			d += 1;
		}
		return d;
	}
	static Date epoch() noexcept { return Date(1,1,1); }
	static constexpr auto d400y = 365 * 400 + 100 - 4 + 1;
	static constexpr auto d100y = 365 * 100 + 25 - 1;
	static constexpr auto d4y = 365 * 4 + 1;
	static constexpr auto d1y = 365;
	constexpr unsigned daysSinceEpoch() const {
		auto yyyy = year();
		auto ys400 = yyyy / 400 * d400y;
		auto yyy = yyyy % 400;
		auto ys100 = yyy / 100 * d100y;
		auto yy = yyy % 100;
		auto ys4 = yy / 4 * d4y;
		auto y = yy % 4;
		auto ys1 = y * d1y;
		unsigned result = ys400 + ys100 + ys4 + ys1 + dayOfYear();
		return result;
	}
	static Date fromDaysSinceEpoch(unsigned d) {
		unsigned tmp = d;
		unsigned yyyy = tmp / d400y * 400;
		tmp %= d400y;
		yyyy += tmp / d100y * 100;
		tmp %= d100y;
		yyyy += tmp / d4y * 4;
		tmp %= d4y;
		yyyy += tmp / d1y;
		tmp %= d1y;
		//yyyy += 1;
		auto ily = isLeapYear(yyyy);
		unsigned m;
		unsigned ld;
		for(m = doma_.size()-1; m != 0; --m) {
			ld = (ily && m>1)?1:0;
			if ((doma_[m]+ld) < tmp)
				break;
		}
		tmp -= doma_[m] +ld;
		++m; // months from 1 - 12
		/*if (tmp == 0) {
			Date dd(yyyy, m, 1);
			--dd;
			return dd;
		}*/
		return Date(yyyy, m, tmp);
	}
	static const char* dayName(unsigned d) {
		if (d < 1 || d > 7)
			throw "Weekday out of range";
		return days_[d];
	}
	Date& operator++() {
		auto d = day() + 1;
		if (d > Date::daysOfMonth(year(), month())) {
			++val_.mm;
			val_.dd = 1;
			if (13 == val_.mm) {
				++val_.yyyy;
				if (9999 < val_.yyyy)
					throw "date is out of range";
				val_.mm = 1;
			}
		} else {
			val_.dd = d;
		}
		return *this;
	}
	Date operator++(int) {
		Date tmp(*this);
		this->operator++();
		return tmp;
	}
	Date& operator--() {
		auto d = day() - 1;
		if (0 == d) {
			auto m = month() - 1;
			if (0 == m) {
				m = 12;
				auto y = year() - 1;
				if (0 == y)
					throw "year is out of range";
				val_.yyyy = y;
			}
			val_.mm = m;
			d = Date::daysOfMonth(year(), month());
		}
		val_.dd = d;
		return *this;
	}
	Date operator--(int) {
		Date tmp(*this);
		this->operator--();
		return tmp;
	}
	unsigned operator-(const Date& d) {
		return this->daysSinceEpoch() - d.daysSinceEpoch();
	}
	constexpr bool operator<(const Date& d) const noexcept {
		return yyyymmdd() < d.yyyymmdd();
	}
	constexpr bool operator==(const Date& d) const noexcept {
		return !((*this < d) && (d < *this));
	}
	constexpr bool operator!=(const Date& d) const noexcept {
		return (*this < d) || (d < *this);
	}
	constexpr bool operator>(const Date& d) const noexcept {
		return d < *this;
	}
	constexpr bool operator<=(const Date& d) const noexcept {
		return !(d < *this);
	}
	constexpr bool operator>=(const Date& d) const noexcept {
		return !(*this < d);
	}
private:
	static const std::array<unsigned, 13> dom_;
	static const std::array<unsigned, 12> doma_;
	static const std::array<unsigned, 4> anchor_;
	static const std::array<const char*, 8> days_;
	struct ct {
		unsigned yyyy : 14;
		unsigned mm   :  4;
		unsigned dd   :  5;
	} val_ {1,1,1};
//	friend void std::swap(Date&, Date&);
};

constexpr const std::array<unsigned, 13> Date::dom_  = {{ 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }};
constexpr const std::array<unsigned, 12> Date::doma_ = {{ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 }};
constexpr const std::array<unsigned, 4> Date::anchor_ = {{ 2, 6, 5, 3 }};
constexpr const std::array<const char*, 8> Date::days_ = {{ "", "Mo", "Di", "Mi", "Do", "Fr", "Sa", "So" }};

ostream& operator<<(ostream& o, const Date& d) {
	return o << boost::format{"%04d-%02d-%02d"} % d.year() % d.month() % d.day();
}

int main (int argc, char const *argv[])
{
	int ret {0};
	cout << sizeof(Date) << endl;
	Date d;
	cout << d << endl;
	Date d2(9999,12,31); cout << d2 << endl;
	Date d3(20161231); cout << d3 << endl;
	/*
	for (unsigned y = 1999; y <= 2020; ++y) {
			cout << "Date::isLeapYear(" << y << "): " << boolalpha << Date::isLeapYear(y) << endl;
	}
	*/
	/*
	for(auto i = 1, n = 0; i < 13; ++i, n += Date::dom_[i-1] )
		cout << i << ": " << n << endl;
	*/
	auto printd = [](const Date& d) {
		auto dse = d.daysSinceEpoch();
		cout << d << " - " << boost::format{"%3d - %d - %s - KW%02s - %6d"}
			% d.dayOfYear() % d.weekday() % Date::dayName(d.weekday()) % d.weekOfYear() % dse << " - ";
		cout.flush();
		try {
			auto dfd = Date::fromDaysSinceEpoch(dse);
			cout << dfd << endl;
		} catch(const char* e) {
			cout << "Exception " << e << endl;
		}
	};
	for(Date d(1,1,1), de(2100,12,31); d < de; ++d) {
		Date dfd = Date::fromDaysSinceEpoch(d.daysSinceEpoch());
		if (dfd != d)
			printd(d);
	}
	printd(Date(10101));
	printd(Date(10102));
	printd(Date(10201));
	printd(Date(10301));
	printd(Date(11231));
	printd(Date(20101));
	printd(Date(30101));
	printd(Date(40101));
	printd(Date(50101));
	printd(Date(990101));
	printd(Date(1000101));
	printd(Date(1010101));
	printd(Date(3990101));
	printd(Date(4000101));
	printd(Date(4010101));
	printd(Date(10000101));
	printd(Date(10010101));
	printd(Date(20000101));
	/*
	for(Date d(1,1,1), e(9999,12,31); d<e; ++d) {
		Date tmp(d.year(), d.month(), d.day());
		if (tmp.month() == 1 and tmp.day() == 1) {
			cout << tmp.year() << " ";
		}
	}
	cout << endl;
	for(Date d(9999,12,31), e(1,1,1); d>e; --d) {
		Date tmp(d.year(), d.month(), d.day());
		if (tmp.month() == 1 and tmp.day() == 1) {
			cout << tmp.year() << " ";
		}
	}
	cout << endl;
	*/
	return ret;
}


#include "Date.hpp"

#include <chrono>
#include <boost/date_time/gregorian/gregorian.hpp>

namespace tk {
Date::Date()
{
	time_t now = time(0);
	tm* ltm = localtime(&now);

	_year = 1900 + ltm->tm_year;
	_month = 1 + ltm->tm_mon;
	_day = ltm->tm_mday;
}

Date::Date(int year, int month, int day)
	: _year(year)
	, _month(month)
	, _day(day)
{
}

Date::Date(const std::string& date_str)
	: _year(0)
	, _month(0)
	, _day(0)
{
	_year = std::stoi(date_str.substr(0, 4));
	_month = std::stoi(date_str.substr(5, 7));
	_day = std::stoi(date_str.substr(8, std::string::npos));
}

std::string Date::ToStringFull() const
{
	//	boost::gregorian::date d(boost::gregorian::from_simple_string(date_in));
	boost::gregorian::date d(_year, _month, _day);
	return std::string(d.day_of_week().as_long_string()) + " " + std::string(d.month().as_long_string()) + " "
		+ std::to_string(d.day());
}

std::string Date::ToString() const
{
	//	boost::gregorian::date d(boost::gregorian::from_simple_string(date_in));
	boost::gregorian::date d(_year, _month, _day);
	return boost::gregorian::to_iso_extended_string(d);
}
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-ieee
    This function object returns a safe mininimal value relative to the input, i.e. a
    value which will not underflow when divided by the input argument.

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

    @par Example:

      @snippet safe_min.cpp safe_min

    @par Possible output:

      @snippet safe_min.txt safe_min

  **/
  IEEEValue safe_min(IEEEValue const& x);
} }
#endif

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

#endif


#pragma once

#include <Eigen/Dense>

namespace RRT {

/**
 * @brief Handles a grid of obstacles laid over a continuous 2d plane.
 * @details The state space is broken up into a grid with the given discrete
 * height and widths.
 */
class ObstacleGrid {
public:
    ObstacleGrid(double width, double height, int discretizedWidth,
                 int discretizedHeight);
    ~ObstacleGrid();

    Eigen::Vector2i gridSquareForLocation(const Eigen::Vector2d& loc) const;

    /**
     * Finds the distance from state to its neareset obstacle. Only searches up
     *to
     * maxDist around
     * state so as to not waste time checking far away and irrelevant obstacles.
     *
     * @param state The location to search with respect to for the nearest
     * obstacle dist
     * @param maxDist The maximum vertical and horizontal distance from state to
     * search for obstacles
     */
    double nearestObstacleDist(const Eigen::Vector2d& state,
                              double maxDist) const;
    void clear();
    bool& obstacleAt(int x, int y);
    bool obstacleAt(int x, int y) const;
    bool& obstacleAt(const Eigen::Vector2i& gridLoc);
    bool obstacleAt(const Eigen::Vector2i& gridLoc) const;

    int discretizedWidth() const;
    int discretizedHeight() const;
    double width() const;
    double height() const;

private:
    int _discretizedWidth, _discretizedHeight;
    double _width, _height;

    /// 2d array of obstacles
    bool* _obstacles;
};

}  // namespace RRT


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/reverse.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

using namespace boost::simd;

STF_CASE_TPL( "Unary reverse shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 2
  {
    bs::pack<T,2> a{ Valmax<T>(), T(42) };
    STF_ALL_EQUAL( (shuffle<1,0>(a)), bs::reverse(a) );
  }

  // cardinal 4
  {
    bs::pack<T,4> a{ Valmax<T>(), T(42), T(69), Valmin<T>() };
    STF_ALL_EQUAL( (shuffle<3,2,1,0>(a)), bs::reverse(a) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a { boost::simd::Valmax<T>(),T(66),T(99),T(55)
                    , T(-1),T(77), T(23), boost::simd::Valmin<T>()
                    };

    STF_ALL_EQUAL( (shuffle<7,6,5,4,3,2,1,0>(a)), bs::reverse(a) );
  }
}

STF_CASE_TPL( "Binary reverse shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 2
  {
    bs::pack<T,2> a{ Valmax<T>(), T(42)       };
    bs::pack<T,2> b{ T(69)      , Valmin<T>() };

    STF_ALL_EQUAL( (shuffle<1,0>(a,b)), bs::reverse(a) );
    STF_ALL_EQUAL( (shuffle<3,2>(a,b)), bs::reverse(b) );
  }

  // cardinal 4
  {
    bs::pack<T,4> a{ Valmax<T>(), T(42), T(69), Valmin<T>() };
    bs::pack<T,4> b{ T(13), Valmin<T>(), T(37), Valmax<T>() };

    STF_ALL_EQUAL( (shuffle<3,2,1,0>(a,b)), bs::reverse(a) );
    STF_ALL_EQUAL( (shuffle<7,6,5,4>(a,b)), bs::reverse(b) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a { boost::simd::Valmax<T>(),T(66),T(99),T(55)
                    , T(-1),T(77), T(23), boost::simd::Valmin<T>()
                    };

    bs::pack<T,8> b { T(36),T(27), boost::simd::Valmax<T>(),T(66)
                    , T(10), boost::simd::Valmin<T>(),T(-37), T(14)
                    };

    STF_ALL_EQUAL( (shuffle<7,6,5,4,3,2,1,0>(a,b)), bs::reverse(a) );
    STF_ALL_EQUAL( (shuffle<15,14,13,12,11,10,9,8>(a,b)), bs::reverse(b) );
  }
}


#include <boost/random/discard_block.hpp>


#ifndef TEST_UNIT_TORSTEN_PK_ONECPT_EFFCPT_MODEL_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_PK_ONECPT_EFFCPT_MODEL_TEST_FIXTURE

#include <stan/math/rev/core/recover_memory.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <stan/math/torsten/test/unit/test_fixture_model.hpp>
#include <stan/math/torsten/pmx_solve_onecpt_effcpt.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

template<typename T>
struct test_onecpt_effcpt : public TorstenPMXTest<test_onecpt_effcpt<T>> {
  test_onecpt_effcpt() {
    this -> ncmt = 3;

    this -> reset_events(10);

    this -> theta.resize(1);
    this -> theta[0] = {10, 80, 1.2, 0.5};
    this -> biovar.resize(1);
    this -> biovar[0] = {1, 1, 1};
    this -> tlag.resize(1);
    this -> tlag[0] = {0, 0, 0};
    this -> pMatrix.resize(1);
    this -> pMatrix[0].resize(3, 3);
    this -> pMatrix[0] << - this -> theta[0][2], 0.0, 0.0,
      this -> theta[0][2], - this -> theta[0][0]/ this -> theta[0][1], 0.0,
      0.0, this -> theta[0][3], - this -> theta[0][3];

    this -> time[0] = 0.0;
    for(int i = 0; i < this -> nt; i++) {
      this -> time[i] = i * 0.25;
    }
    this -> time[this -> nt - 1] = 4.0;

    this -> amt[0] = 1000.0;
    this -> cmt[0] = 1;
    this -> evid[0] = 1;
    this -> ii[0] = 12;
    this -> addl[0] = 14;
  }
};

#endif


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * (C) 2009-2018, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "pteros/python/compiled_plugin.h"
#include "pteros/core/pteros_error.h"
#include "pteros/core/distance_search.h"
#include <boost/lexical_cast.hpp>
#include <fstream>

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif

using namespace std;
using namespace pteros;
using namespace Eigen;

class rdf: public pteros::Compiled_plugin_base {
public:
    rdf(pteros::Trajectory_processor* pr, const pteros::Options& opt): Compiled_plugin_base(pr,opt) {}

    string help(){
        return  "Purpose:\n"
                "\tComputes radial distribution function of two selections.\n"
                "Output:\n"
                "\tFile <label>.dat containing the following columns:\n"
                "\ttime RMSD\n"
                "\tAlso reports mean RMSD in the file header.\n"
                "Options:\n"
                "\t-selection <string>\n"
                "\t\tSelection text\n"
                "\t-nojump <true|false>. Default: true\n"
                "\t\tRemove jumps of atoms over periodic box boundary.\n"
                "\t\tSetting this to false is only meaningful is very special cases."
                ;
    }

protected:

    void pre_process(){
        sel1.modify(system,options("sel1").as_string() );
        sel2.modify(system,options("sel2").as_string() );
        do_cm = options("cm","false").as_bool();
        n_bins = options("bins","50").as_int();
        min_dist = options("min","0.0").as_float();
        // Max dist will be set later when we get the box dimension
        mass_weighted = options("mass_weighted","true").as_bool();

        data.resize(n_bins);
        data.fill(0.0);

        max_dist = 0.5*options("max",
                           boost::lexical_cast<string>(system.Box(0).extents().minCoeff())
                          ).as_float();
        bin_sz = (max_dist-min_dist)/float(n_bins);
    }

    void process_frame(const pteros::Frame_info &info){
        float d;

        sel1.apply();
        sel2.apply();

        density = (sel1.size()*sel2.size())/system.Box(0).volume();

        vector<Vector2i> bon;
        vector<float> dist_vec;
        search_contacts(max_dist,sel1,sel2,bon,true,true,&dist_vec);

        // Atom-atom rdf
        int bin;
        float r;
        for(int i=0;i<dist_vec.size();++i){
            bin = int(floor(dist_vec[i]/bin_sz));
            if(bin<n_bins){
                r = (0.5+bin)*bin_sz;
                data[bin] += 1.0/( 4.0*M_PI*r*r*density*bin_sz );
            }
        }

    }

    void post_process(const pteros::Frame_info &info){
        // Output
        string fname = label+".dat";

        ofstream f(fname.c_str());
        f << "# RDF of selections [" << sel1.get_text() << "] and ["
          << sel2.get_text() << "]"
          << endl;
        for(int i=0; i<data.size(); ++i){
            float r = 0.5*bin_sz+i*bin_sz;
            f << r << " " << data[i]/(float)info.valid_frame << endl;
        }
        f.close();
    }

private:
    VectorXf data;
    pteros::Selection sel1, sel2;
    bool do_cm;
    bool mass_weighted;
    float bin_sz;
    int n_bins;
    float min_dist, max_dist;
    float density;
};

CREATE_COMPILED_PLUGIN(rdf)


/**
 * @file KinematicsInterface.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <vector>
#include <memory>
#include <Eigen/Eigen>

#include <momentumopt/setting/PlannerSetting.hpp>
//#include <solver/interface/NlpDescription.hpp>
//#include <momentumopt/dynopt/DynamicsState.hpp>
#include <momentumopt/kinopt/KinematicsState.hpp>

namespace momentumopt {

  class KinematicsOptimizer;

  class KinematicsInterface// : public virtual solver::NlpDescription
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  KinematicsInterface(){}
      ~KinematicsInterface(){}

      // pure virtual functions to be implemented
      virtual void initialize(PlannerSetting& planner_setting) = 0;
      virtual Eigen::Vector3d logarithmicMap(const Eigen::Vector4d quat_wxyz) = 0;
      virtual KinematicsState integratePosture(KinematicsState& kin_state, double dt) = 0;
      virtual void displayPosture(const KinematicsState& kin_state, double time_step) = 0;
      virtual KinematicsState updateJacobiansAndState(KinematicsState& kin_state, double dt) = 0;
      virtual KinematicsState differentiatePostures(KinematicsState& start_state, KinematicsState& end_state, double timestep) = 0;

      // center of mass and momentum jacobians
      Eigen::MatrixXd& centerOfMassJacobian() { return center_of_mass_jacobian_; }
      Eigen::MatrixXd& centroidalMomentumMatrix() { return centroidal_momentum_matrix_; }
      const Eigen::MatrixXd& centerOfMassJacobian() const { return center_of_mass_jacobian_; }
      const Eigen::MatrixXd& centroidalMomentumMatrix() const { return centroidal_momentum_matrix_; }
      void centerOfMassJacobian(const Eigen::MatrixXd& center_of_mass_jacobian) { center_of_mass_jacobian_ = center_of_mass_jacobian; }
      void centroidalMomentumMatrix(const Eigen::MatrixXd& centroidal_mometum_matrix) { centroidal_momentum_matrix_ = centroidal_mometum_matrix; }

      Eigen::MatrixXd& centroidalMomentumMatrixVariation() { return centroidal_momentum_matrix_variation_; }
      const Eigen::MatrixXd& centroidalMomentumMatrixVariation() const { return centroidal_momentum_matrix_variation_; }
      void centroidalMomentumMatrixVariation(const Eigen::MatrixXd& centroidal_momentum_matrix_variation) { centroidal_momentum_matrix_variation_ = centroidal_momentum_matrix_variation; }

      // endeffector jacobians
      Eigen::MatrixXd& endeffectorJacobian(int eff_id) { return endeffector_jacobians_[eff_id]; }
      const std::vector<Eigen::MatrixXd>& endeffectorJacobians() const { return endeffector_jacobians_; }
      const Eigen::MatrixXd& endeffectorJacobian(int eff_id) const { return endeffector_jacobians_[eff_id]; }
      void endeffectorJacobians(const std::vector<Eigen::MatrixXd>& endeffector_jacobians) { endeffector_jacobians_ = endeffector_jacobians; }

      // bodies non-penetration constraints
      const Eigen::VectorXd& constraintsVector() const { return constraints_vector_; }
      const Eigen::MatrixXd& constraintsMatrix() const { return constraints_matrix_; }
      void constraintsVector(const Eigen::VectorXd& constraints_vector) { constraints_vector_ = constraints_vector; }
      void constraintsMatrix(const Eigen::MatrixXd& constraints_matrix) { constraints_matrix_ = constraints_matrix; }

    private:
      friend class KinematicsOptimizer;

      /*! internal initialization function */
      void internalInitialization(PlannerSetting& planner_setting);

      /*! Getter and setter methods for getting the planner variables  */
      inline PlannerSetting& getSetting() { return *planner_setting_; }
      inline const PlannerSetting& getSetting() const { return *planner_setting_; }

    private:
      /*! Variables required for optimization */
      PlannerSetting* planner_setting_;

      /*! Jacobians and problem matrices */
      Eigen::VectorXd constraints_vector_;
      std::vector<Eigen::MatrixXd> endeffector_jacobians_;
      Eigen::MatrixXd centroidal_momentum_matrix_, centroidal_momentum_matrix_variation_, center_of_mass_jacobian_, constraints_matrix_;
  };

}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 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_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_DETAILS_ULP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_DETAILS_ULP_HPP_INCLUDED

#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/predecessor.hpp>
#include <boost/simd/include/functions/simd/successor.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>

namespace boost{ namespace simd{ namespace details {

  template<class A0>
  inline A0 ulp(A0 const& a0)
  {
    A0 xp = boost::simd::predecessor(a0);
    A0 xs = boost::simd::successor(a0);
    return if_allbits_else( is_equal(a0, Inf<A0>())
                          , boost::simd::min(a0-xp,xs-a0)
                          );
  }

} } }

#endif


#include <iostream>
#include <armadillo>

#include "Isomap.h"

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

    isomap::Isomap isomap;

    // Format is
    std::srand(0);
    arma::fmat input = arma::randn<arma::fmat>(20000, 100).t();

    auto start = std::chrono::steady_clock::now();
    auto knn = isomap.knn(input, 10);
    auto end = std::chrono::steady_clock::now();

    std::cout << double(std::chrono::nanoseconds(end - start).count()) / double(std::nano::den) << std::endl;

    //std::tie(knnIndex, knnDistance) = kNearestNeighbours(); // TODO parameters (k epsilon)

    // Work out how much GPU memory we have
    // We need to split input into an appropriate sized chunks
    // These chunks need to be sized that (K * n * sizeof(size_t)) + (K * n * sizeof(double)) + 2x < gpumemory

    // Allocate a matrix to hold the results of the KNN operation (K * n doubles), (k * n size_t)

    // Call the KNN kernel for each chunk of memory passing in each input and output chunk

    return 0;
}


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

#include <boost/test/unit_test.hpp>
#include <boost/time_series/delta_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/characteristic_series.hpp>
#include <boost/time_series/numeric/numeric.hpp>
#include <boost/time_series/ordered_inserter.hpp>

using namespace boost::unit_test;
using namespace boost::time_series;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;
typedef rrs::unit_run<std::ptrdiff_t> rng;

///////////////////////////////////////////////////////////////////////////////
// test_series_multiplies
//
void test_series_multiplies()
{
    // check multiplication of a sparse time series and a dense time series yields
    // a sparse time series
    dense_series<int> ss1;
    sparse_series<int> ss2;

    make_ordered_inserter(ss1)(3, 1)(4, 2).commit();
    make_ordered_inserter(ss2)(4, 2)(5, 3).commit();

    BOOST_CHECK_EQUAL(2, std::distance(seq::begin(ss1), seq::end(ss1)));
    BOOST_CHECK_EQUAL(2, std::distance(seq::begin(ss2), seq::end(ss2)));

    sparse_series<int> ss3 = ss1 * ss2;
    BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss3), seq::end(ss3)));
    BOOST_CHECK_EQUAL(16, seq::elements(ss3)(*seq::begin(ss3)));
    BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss3)(*seq::begin(ss3))));

    ss2 *= ss1;
    BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss2), seq::end(ss2)));
    BOOST_CHECK_EQUAL(16, seq::elements(ss2)(*seq::begin(ss2)));
    BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss2)(*seq::begin(ss2))));

    //// check multiplication of a dense time series and a delta time series yields
    //// a scaled delta time series
    //descriptor<> disc2(size = 4, offset = -1);
    //dense_series<int> ss4(disc2);
    //delta_unit_series<int> ss5(disc2, 2);
    //rrs::set_at(ss4, rng(1), 3);
    //rrs::set_at(ss4, rng(2), 4);
    //BOOST_CHECK_EQUAL(4, std::distance(seq::begin(ss4), seq::end(ss4)));
    //BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss5), seq::end(ss5)));

    //time_series_facade<int, scaled_storage_tag<delta_storage_tag>, daily> ss6 = ss4 * ss5;
    //BOOST_CHECK_EQUAL(1, std::distance(seq::begin(ss6), seq::end(ss6)));
    //BOOST_CHECK_EQUAL(16, seq::elements(ss6)(*seq::begin(ss6)));
    //BOOST_CHECK_EQUAL(2, rrs::offset(rrs::runs(ss6)(*seq::begin(ss6))));
}

///////////////////////////////////////////////////////////////////////////////
// test_series_multiplies2
//
void test_series_multiplies2()
{
    piecewise_constant_series<int> discounts;
    make_ordered_inserter(discounts)
        (1, -inf, 1)
        (2, 1, 7)
        (3, 7, 30)
        (4, 30, 60)
        (5, 60, 90)
    .commit();

    piecewise_constant_series<int> expected;
    make_ordered_inserter(expected)
        (1, 0, 1)
        (2, 1, 7)
        (3, 7, 30)
    .commit();

    characteristic_unit_series<int> clip_range(0, 30);
    piecewise_constant_series<double> result = discounts * clip_range;

    BOOST_CHECK_EQUAL(expected, result);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("series multiplies test");

    test->add(BOOST_TEST_CASE(&test_series_multiplies));
    test->add(BOOST_TEST_CASE(&test_series_multiplies2));

    return test;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

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

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/indepmeasllh.h>
#include <tomographer/densedm/tspacefigofmerit.h>
#include <tomographer/tools/eigenutil.h>
#include <tomographer/mhrw_valuehist_tools.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

// -----------------------------------------------------------------------------
// fixture(s)

template<typename T>
void save_and_reload(const T & a, T & b)
{
  std::string buf;
  // write
  { std::stringstream sstream;
    boost::archive::text_oarchive oa(sstream);
    oa << a;
    buf = sstream.str();
  }
  BOOST_MESSAGE("DATA: " << buf) ;
  // and read
  { std::istringstream sstream(buf);
    boost::archive::text_iarchive ia(sstream);
    ia >> b;
  }
}

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(serializing)

BOOST_AUTO_TEST_CASE(indepmeasllh)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  1./std::sqrt(2.0),  0,
    0.5, 0.5, -1./std::sqrt(2.0),  0,
    0.5, 0.5,  0,         1./std::sqrt(2.0),
    0.5, 0.5,  0,        -1./std::sqrt(2.0),
    1,   0,    0,         0,
    0,   1,    0,         0
    ;
  IndepMeasLLH::FreqListType Nx(6);
  Nx << 1500, 800, 300, 300, 10, 30;

  dat.setMeas(Exn, Nx);

  IndepMeasLLH * dat2 = NULL;

  save_and_reload(&dat, dat2);

  // compare
  BOOST_CHECK_EQUAL(dat.dmt.dim(), dat2->dmt.dim()) ;
  MY_BOOST_CHECK_EIGEN_EQUAL(dat.Exn(), dat2->Exn(), tol);
  MY_BOOST_CHECK_EIGEN_EQUAL(dat.Nx(), dat2->Nx(), tol);

  delete dat2;
}

BOOST_AUTO_TEST_SUITE(tspacefigofmerit)

BOOST_AUTO_TEST_CASE(fidelitytorefcalculator)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  typedef Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes> TheType;
  DMTypes::MatrixType T_ref;
  T_ref << std::sqrt(0.2), std::complex<double>(0,std::sqrt(0.1)),
    std::complex<double>(-std::sqrt(0.4), std::sqrt(0.2)), -std::sqrt(0.1) ;
  TheType a(T_ref);

  TheType b;
  save_and_reload(a, b);

  MY_BOOST_CHECK_FLOATS_EQUAL(a.getValue(T_ref), b.getValue(T_ref), tol) ;
}

BOOST_AUTO_TEST_CASE(purifdisttorefcalculator)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  typedef Tomographer::DenseDM::TSpace::PurifDistToRefCalculator<DMTypes> TheType;
  DMTypes::MatrixType T_ref;
  T_ref << std::sqrt(0.2), std::complex<double>(0,std::sqrt(0.1)),
    std::complex<double>(-std::sqrt(0.4), std::sqrt(0.2)), -std::sqrt(0.1) ;
  TheType a(T_ref);

  TheType b;
  save_and_reload(a, b);

  MY_BOOST_CHECK_FLOATS_EQUAL(a.getValue(T_ref), b.getValue(T_ref), tol) ;
}

BOOST_AUTO_TEST_CASE(trdisttorefcalculator)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  typedef Tomographer::DenseDM::TSpace::TrDistToRefCalculator<DMTypes> TheType;
  DMTypes::MatrixType rho_ref;
  rho_ref << 0.8, std::complex<double>(0,0.2),
    std::complex<double>(0,-0.2), 0.2 ;
  TheType a(rho_ref);

  TheType b;
  save_and_reload(a, b);

  DMTypes::MatrixType T_ref;
  T_ref << std::sqrt(0.2), std::complex<double>(0,std::sqrt(0.1)),
    std::complex<double>(-std::sqrt(0.4), std::sqrt(0.2)), -std::sqrt(0.1) ;
  MY_BOOST_CHECK_FLOATS_EQUAL(a.getValue(T_ref), b.getValue(T_ref), tol) ;
}

BOOST_AUTO_TEST_CASE(observablevaluecalculator)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;
  typedef Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes> TheType;
  DMTypes::MatrixType A;
  A << 2, std::complex<double>(0,-1),
    std::complex<double>(0,1), -1 ;
  TheType a(dmt, A);

  TheType b;
  save_and_reload(a, b);

  DMTypes::MatrixType rho_ref;
  rho_ref << 0.8, std::complex<double>(0,0.2),
    std::complex<double>(0,-0.2), 0.2 ;

  DMTypes::MatrixType T_ref;
  T_ref << std::sqrt(0.2), std::complex<double>(0,std::sqrt(0.1)),
    std::complex<double>(-std::sqrt(0.4), std::sqrt(0.2)), -std::sqrt(0.1) ;
  MY_BOOST_CHECK_FLOATS_EQUAL(a.getValue(T_ref), b.getValue(T_ref), tol) ;
}

BOOST_AUTO_TEST_SUITE_END() // tspacefigofmerit

BOOST_AUTO_TEST_CASE(valuehisttools_cdata)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;
  typedef Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes> ValCalc;
  DMTypes::MatrixType A;
  A << 2, std::complex<double>(0,-1),
    std::complex<double>(0,1), -1 ;
  ValCalc valcalc(dmt, A);

  typedef Tomographer::MHRWTasks::ValueHistogramTools::CDataBase<ValCalc>  CDataBaseType;

  CDataBaseType cdata(valcalc,
                      CDataBaseType::HistogramParams(0.0,1.0,100),
                      9,
                      CDataBaseType::MHRWParamsType(0.04, 24, 1024, 32768),
                      123000456u);

  CDataBaseType cdata2;

  save_and_reload(cdata, cdata2);

  DMTypes::MatrixType T_ref;
  T_ref << std::sqrt(0.2), std::complex<double>(0,std::sqrt(0.1)),
    std::complex<double>(-std::sqrt(0.4), std::sqrt(0.2)), -std::sqrt(0.1) ;
  MY_BOOST_CHECK_FLOATS_EQUAL(valcalc.getValue(T_ref), cdata2.valcalc.getValue(T_ref), tol) ;
  MY_BOOST_CHECK_FLOATS_EQUAL(cdata.histogram_params.min, cdata2.histogram_params.min, tol) ;
  MY_BOOST_CHECK_FLOATS_EQUAL(cdata.histogram_params.max, cdata2.histogram_params.max, tol) ;
  BOOST_CHECK_EQUAL(cdata.histogram_params.num_bins, cdata2.histogram_params.num_bins) ;
  BOOST_CHECK_EQUAL(cdata.binningNumLevels.value, cdata2.binningNumLevels.value) ;

  MY_BOOST_CHECK_FLOATS_EQUAL(cdata.mhrw_params.mhwalker_params.step_size,
                              cdata2.mhrw_params.mhwalker_params.step_size, tol) ;
  BOOST_CHECK_EQUAL(cdata.mhrw_params.n_sweep, cdata2.mhrw_params.n_sweep) ;
  BOOST_CHECK_EQUAL(cdata.mhrw_params.n_therm, cdata2.mhrw_params.n_therm) ;
  BOOST_CHECK_EQUAL(cdata.mhrw_params.n_run, cdata2.mhrw_params.n_run) ;

  BOOST_CHECK_EQUAL(cdata.base_seed, cdata2.base_seed) ;
}

BOOST_AUTO_TEST_SUITE_END() // serializing


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/floor.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <scalar_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL( "Check return type from floor", STF_NUMERIC_TYPES)
{
  using bs::floor;
  STF_TYPE_IS(decltype(floor(T())), T);
}

STF_CASE_TPL( "Check floor behavior on IEEE types",  STF_IEEE_TYPES)
{
  using bs::floor;

  STF_EQUAL(floor(T(2.5)), T(2));
  STF_EQUAL(floor(T(1.6)), T(1));
  STF_EQUAL(floor(T(1.5)), T(1));
  STF_EQUAL(floor(T(1.4)), T(1));
  STF_EQUAL(floor(T(0)), T(0));
  STF_EQUAL(floor(T(-0.4)), T(-1));
  STF_EQUAL(floor(T(-0.5)), T(-1));
  STF_EQUAL(floor(T(-0.6)), T(-1));
  STF_EQUAL(floor(T(-1.5)), T(-2));
}

STF_CASE_TPL( "Check floor behavior on limit cases", STF_NUMERIC_TYPES)
{
  using bs::floor;
  using bs::Valmin;
  using bs::Valmax;

  STF_EQUAL(floor(Valmin<T>()), Valmin<T>());
  STF_EQUAL(floor(Valmax<T>()), Valmax<T>());
}

STF_CASE_TPL( "Check floor behavior on IEEE limit cases", STF_IEEE_TYPES)
{
  using bs::floor;
  using bs::Valmin;
  using bs::Valmax;
  using bs::Inf;
  using bs::Minf;
  using bs::Nan;
  using bs::Maxflint;

  STF_EQUAL(floor(Inf<T>()) , Inf<T>());
  STF_EQUAL(floor(Minf<T>()), Minf<T>());
  STF_IEEE_EQUAL(floor(Nan<T>()) , Nan<T>());
  STF_EQUAL(floor(Maxflint<T>()+1) , Maxflint<T>()+1);
  STF_EQUAL(floor(Maxflint<T>()  ) , Maxflint<T>()  );
  STF_EQUAL(floor(Maxflint<T>()-1) , Maxflint<T>()-1);
}

STF_CASE_TPL( "Check floor behavior on integral types",  STF_INTEGRAL_TYPES)
{
  using bs::floor;

  STF_EQUAL(floor(T(2)), T(2));
  STF_EQUAL(floor(T(1)), T(1));
  STF_EQUAL(floor(T(0)), T(0));
  STF_EQUAL(floor(T(-1)), T(-1));
  STF_EQUAL(floor(T(-2)), T(-2));
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.boolean toolbox - negif/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.boolean components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <boost/simd/toolbox/boolean/include/functions/negif.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/toolbox/constant/constant.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/include/functions/load.hpp>
#include <boost/simd/include/functions/is_nez.hpp>

NT2_TEST_CASE_TPL ( negif_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::negif;
  using boost::simd::tag::negif_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  using boost::simd::logical;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef native< logical<T>, ext_t>             vlT;
  typedef typename boost::dispatch::meta::call<negif_(vlT,vT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
   NT2_TEST_EQUAL(negif(boost::simd::False<vlT>(),boost::simd::splat<vT>(1))[0], 1);
   NT2_TEST_EQUAL(negif(boost::simd::True<vlT>(),boost::simd::splat<vT>(1))[0], -1);
   NT2_TEST_EQUAL(negif((boost::simd::splat<vlT>(T(1))),boost::simd::splat<vT>(1))[0], -1);
} // end of test for floating_

NT2_TEST_CASE_TPL ( negif_signed_int__2_0,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::negif;
  using boost::simd::tag::negif_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  using boost::simd::logical;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef native< logical<T>, ext_t>             vlT;
  typedef typename boost::dispatch::meta::call<negif_(vlT,vT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
  NT2_TEST_EQUAL(negif(boost::simd::False<vlT>(),boost::simd::splat<vT>(1))[0], 1);
  NT2_TEST_EQUAL(negif(boost::simd::True<vlT>(),boost::simd::splat<vT>(1))[0], -1);
  NT2_TEST_EQUAL(negif((boost::simd::splat<vlT>(1)),boost::simd::splat<vT>(1))[0], -1);
} // end of test for signed_int_


#pragma once
#ifndef DATA_HPP
#define DATA_HPP

// c++ libaries
#include <vector>
// eigen
#include <Eigen/Dense>
// ann - util
# include "src/util/typedef.hpp"

class MLData{
private:
	int size_;
	int dIn_,dOut_;
	std::vector<VecXd> in_;
	std::vector<VecXd> out_;
public:
	//==== constructors/destructors ====
	MLData():size_(0),dIn_(0),dOut_(0){}
	MLData(int size, int dIn, int dOut){resize(size,dIn,dOut);}
	~MLData(){}

	//==== access ====
	const int& size()const{return size_;}
	const int& dIn()const{return dIn_;}
	const int& dOut()const{return dOut_;}
	VecXd& in(int n){return in_[n];}
	const VecXd& in(int n)const{return in_[n];}
	std::vector<VecXd>& in(){return in_;}
	const std::vector<VecXd>& in()const{return in_;}
	VecXd& out(int n){return out_[n];}
	const VecXd& out(int n)const{return out_[n];}
	std::vector<VecXd>& out(){return out_;}
	const std::vector<VecXd>& out()const{return out_;}

	//==== member functions ====
	void clear();
	void resize(int dIn, int dOut);
	void resize(int size, int dIn, int dOut);
	void push(const Eigen::VectorXd& in, const Eigen::VectorXd& out);
};

#endif


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================
#include <nt2/include/functions/asinh.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( asinh_real,  NT2_REAL_TYPES)
{
  using nt2::asinh;
  using nt2::tag::asinh_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<asinh_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), nt2::Zero<T>())), cT(nt2::Nan <T>(), nt2::Zero<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Inf<T>(),  nt2::Pio_2<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(),  nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Inf <T>(), nt2::Zero<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Inf <T>(), nt2::Pi<T>()/4), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Inf <T>(), nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), nt2::Inf<T>())),  cT(nt2::Minf <T>(), nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::One  <T>(), -nt2::Inf <T>())), cT(nt2::Inf<T>(),  -nt2::Pio_2<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::One  <T>(), -nt2::Nan <T>())), cT(nt2::Nan<T>(),  -nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), -nt2::One <T>())), cT(nt2::Inf <T>(), -nt2::Zero<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), -nt2::Inf<T>())),  cT(nt2::Inf <T>(), -nt2::Pi<T>()/4), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Inf  <T>(), -nt2::Nan<T>())),  cT(nt2::Inf <T>(), -nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), -nt2::One<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), -nt2::Inf<T>())),  cT(nt2::Minf <T>(), -nt2::Nan<T>()), 0.75);
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Nan  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
#endif
   NT2_TEST_ULP_EQUAL(nt2::asinh(cT(nt2::Zero<T>(),  nt2::Zero<T>())), cT(nt2::Zero<T>(), nt2::Zero<T>()), 0.75);
}


#include <algorithm>
#include <iostream>
#include <string>

#include <boost/program_options.hpp>

#include "sigmoid.hh"

namespace po = boost::program_options;
using namespace machina;

int main(int argc, char *argv[])
{
  std::string funct;
  int width = 0;
  int depth = 0;
  int scale = 0;

  try {
    po::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help")
      ("funct,f", po::value<std::string>(&funct)->required(), "Function")
      ("width,w", po::value<int>(&width)->default_value(8), "Width")
      ("depth,d", po::value<int>(&depth)->default_value(4096), "Depth")
      ("scale,s", po::value<int>(&scale)->default_value(255), "Scale");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc;
      return 0;
    }
    po::notify(vm);
  } catch (const po::error& ex) {
    std::cerr << "ERROR: " << ex.what() << std::endl;
    return 1;
  }

  std::transform(funct.begin(), funct.end(), funct.begin(), ::tolower);

  bool prime;
  if (funct.compare("sigmoid") == 0) {
    prime = false;
  } else if (funct.compare("sigmoid-prime") == 0) {
    prime = true;
  } else {
    std::cerr << "ERROR: unknown function: " << funct << std::endl;
    return 1;
  }

  Sigmoid memory(width, scale, prime);
  for (int val = 0; val < depth >> 1; val++)
    memory << val;
  for (int val = -(depth >> 1); val < 0; val++)
    memory << val;
  std::cout << memory;

  return 0;
}


#ifndef TRANSFORM_HPP
#define TRANSFORM_HPP
#include <stdio.h>
#include <armadillo>

class Transform
{
public:
    Transform();
    arma::fmat T(float tx, float ty, float tz);
    arma::fmat S(float sx, float sy, float sz);
    arma::fmat R(float ax, float ay, float az, float angle);
};

#endif // FACE_HPP


//------------------------------------------------------------------------------
// pointInPolygon
//
// Test if a given point is in a polygon or not.  This is based on code I found
// at
//
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
//
// which I have modified here somewhat to use Spheral structures and such.
//------------------------------------------------------------------------------
#include "pointInPolygon.hh"
#include "testBoxIntersection.hh"
#include "pointOnPolygon.hh"
#include "pointDistances.hh"
#include "lineSegmentIntersections.hh"

// #include "boost/geometry.hpp"
// #include "boost/geometry/geometries/point_xy.hpp"
// #include "boost/geometry/geometries/polygon.hpp"
// #include <boost/geometry/geometries/adapted/boost_tuple.hpp>
// BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

// #include <boost/geometry/geometries/adapted/c_array.hpp>
// BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

#include <limits>
using std::vector;
using std::string;
using std::pair;
using std::make_pair;
using std::cout;
using std::cerr;
using std::endl;
using std::min;
using std::max;
using std::abs;

//------------------------------------------------------------------------------
// GeomVector<2> -> Boost.Geometry
//------------------------------------------------------------------------------
// namespace boost
// {
//   namespace geometry
//   {
//     namespace traits
//     {
//       // Adapt Spheral::GeomVector<2> to Boost.Geometry

//       template<> struct tag<Spheral::GeomVector<2>>
//       { typedef point_tag type; };

//       template<> struct coordinate_type<Spheral::GeomVector<2>>
//       { typedef double type; };

//       template<> struct coordinate_system<Spheral::GeomVector<2>>
//       { typedef cs::cartesian type; };

//       template<> struct dimension<Spheral::GeomVector<2>> : boost::mpl::int_<2> {};

//       template<>
//       struct access<Spheral::GeomVector<2>, 0>
//       {
//         static Spheral::GeomVector<2>::double get(Spheral::GeomVector<2> const& p)
//         {
//           return p.x();
//         }

//         static void set(Spheral::GeomVector<2>& p, Spheral::GeomVector<2>::double const& value)
//         {
//           p.x(value);
//         }
//       };

//       template<>
//       struct access<Spheral::GeomVector<2>, 1>
//       {
//         static Spheral::GeomVector<2>::double get(Spheral::GeomVector<2> const& p)
//         {
//           return p.y();
//         }

//         static void set(Spheral::GeomVector<2>& p, Spheral::GeomVector<2>::double const& value)
//         {
//           p.y(value);
//         }
//       };
//     }
//   }
// } // namespace boost::geometry::traits

namespace Spheral {

//------------------------------------------------------------------------------
// Work with a closed polygon defined by it's vertices.
// We assume the caller has ordered the vertices (clockwise or
// counter-clockwise), and should have already performed the box exclusion
// test for efficiency.
//------------------------------------------------------------------------------
bool pointInPolygon(const Dim<2>::Vector& p,
                    const std::vector<Dim<2>::Vector>& vertices) {
  typedef Dim<2>::Vector Vector;

  // Now we do the test of casting a semi-infinite ray in the x direction from
  // the point and counting intersections with the polygon.
  // Note this bit of code will reject points lying exactly on the boundary
  // of a polygon.
  unsigned i, j;
  bool result = false;
  const unsigned nvertices = vertices.size();
  const double px = p.x();
  const double py = p.y();
  for (i = 0, j = nvertices - 1; i < nvertices; j = i++) {
    if ( ((vertices[i].y() > py) != (vertices[j].y() > py)) &&
	 (px < (vertices[j].x() - vertices[i].x()) * (py - vertices[i].y()) / (vertices[j].y() - vertices[i].y()) + vertices[i].x()) )
      result = not result;
  }

  // That's it.
  return result;
}

//------------------------------------------------------------------------------
// Test a polygon.
//------------------------------------------------------------------------------
bool pointInPolygon(const Dim<2>::Vector& p,
                    const Dim<2>::FacetedVolume& polygon,
                    const bool countBoundary,
                    const double tol) {

  // Do the quick box rejection test.
  if (not testPointInBox(p, polygon.xmin(), polygon.xmax(), tol)) return false;

  // Check if the point is on the boundary (within tolerance).
  if (pointOnPolygon(p, polygon, tol)) return countBoundary;

  typedef Dim<2>::Vector Vector;
  typedef Dim<2>::FacetedVolume::Facet Facet;
  const vector<Vector>& pverts = polygon.vertices();
  const vector<Facet>& facets = polygon.facets();
  const unsigned nfacets = facets.size();
  CHECK(nfacets >= 3);

  // If there's just one loop it's an easy check.
  if (facets.back().ipoint2() == 0) {
    vector<Vector> verts(pverts.begin(), pverts.end());
    verts.push_back(verts[0]);
    return pointInPolygon(p, verts);
  }

  // If there are multiple loops, we can still do them in one go, but we have to
  // insert (0,0) coordinates between loops.  See the discussion at the above
  // website source for all this for more information.
  vector<Vector> verts(1, Vector::zero);
  verts.reserve(int(1.05*pverts.size()));
  unsigned ifacet = 0;
  while (ifacet < nfacets) {
    const unsigned istart = facets[ifacet].ipoint1();
    verts.push_back(pverts[istart]);
    while (ifacet < nfacets and facets[ifacet].ipoint2() != istart) {
      verts.push_back(facets[ifacet].point2());
      ++ifacet;
    }
    verts.push_back(pverts[istart]);
    verts.push_back(Vector::zero);
    ++ifacet;
  }
  return pointInPolygon(p, verts);
}

//------------------------------------------------------------------------------
// Test a polygon in 3-D.
//------------------------------------------------------------------------------
bool pointInPolygon(const Dim<3>::Vector& p,
                    const vector<Dim<3>::Vector>& vertices,
                    const Dim<3>::Vector& normal) {
  typedef Dim<3>::Vector Vector;

  // Prerequisites.
  const unsigned npts = vertices.size();
  unsigned i, j, k;
  BEGIN_CONTRACT_SCOPE
  {
    REQUIRE(vertices.size() > 2);
    const double normmag = normal.magnitude();
    Vector normi;
    for (i = 1; i != npts; ++i) {
      j = (i + 1) % npts;
      k = (i + 2) % npts;
      normi = (vertices[j] - vertices[i]).cross(vertices[k] - vertices[i]);
      REQUIRE(fuzzyEqual(std::abs(normi.dot(normal)), normmag*normi.magnitude(), 1.0e-10));
    }
    REQUIRE(fuzzyEqual(pointPlaneDistance(p, vertices[0], normal.unitVector()), 0.0, 1.0e-10));
  }
  END_CONTRACT_SCOPE

  bool result = false;

  const double px = p.x();
  const double py = p.y();
  const double pz = p.z();

  double fxmin =  1e50, fymin =  1e50, fzmin =  1e50;
  double fxmax = -1e50, fymax = -1e50, fzmax = -1e50;
  for (i = 0; i != npts; ++i) {
    fxmin = std::min(fxmin, vertices[i].x());
    fymin = std::min(fymin, vertices[i].y());
    fzmin = std::min(fzmin, vertices[i].z());
    fxmax = std::max(fxmax, vertices[i].x());
    fymax = std::max(fymax, vertices[i].y());
    fzmax = std::max(fzmax, vertices[i].z());
  }
  if (px >= fxmin and px <= fxmax and
      py >= fymin and py <= fymax and
      pz >= fzmin and pz <= fzmax) {

    // Check if the point is on the boundary.
    for (i = 0; i != npts; ++i) {
      j = (i + 1) % npts;
      if ((closestPointOnSegment(p, vertices[i], vertices[j]) - p).magnitude2() < 1.0e-10) return true;
    }

    if (std::abs(normal.x()) >= std::abs(normal.y()) and std::abs(normal.x()) >= std::abs(normal.z())) {

      // x plane -- use (y,z) coordinates.
      for (i = 0, j = npts - 1; i < npts; j = i++) {
        if ( ((vertices[i].z() > pz) != (vertices[j].z() > pz)) &&
             (py < (vertices[j].y() - vertices[i].y()) * (pz - vertices[i].z()) / (vertices[j].z() - vertices[i].z()) + vertices[i].y()) )
          result = not result;
      }

    } else if (std::abs(normal.y()) >= std::abs(normal.x()) and std::abs(normal.y()) >= std::abs(normal.z())) {

      // y plane -- use (z,x) coordinates.
      for (i = 0, j = npts - 1; i < npts; j = i++) {
        if ( ((vertices[i].x() > px) != (vertices[j].x() > px)) &&
             (pz < (vertices[j].z() - vertices[i].z()) * (px - vertices[i].x()) / (vertices[j].x() - vertices[i].x()) + vertices[i].z()) )
          result = not result;
      }

    } else {
      CHECK(std::abs(normal.z()) >= std::abs(normal.x()) and std::abs(normal.z()) >= std::abs(normal.y()));

      // z plane -- use (x,y) coordinate.
      for (i = 0, j = npts - 1; i < npts; j = i++) {
        if ( ((vertices[i].y() > py) != (vertices[j].y() > py)) &&
             (px < (vertices[j].x() - vertices[i].x()) * (py - vertices[i].y()) / (vertices[j].y() - vertices[i].y()) + vertices[i].x()) )
          result = not result;
      }

    }
  }

  // That's it.
  return result;
}

//------------------------------------------------------------------------------
// Test a polygon in 3-D.
// This version allows you to pass in an indirect addressing definition of the
// polygon vertices.
// Note -- does not currently work for polygons with more than one loop!
//------------------------------------------------------------------------------
bool pointInPolygon(const Dim<3>::Vector& p,
                    const vector<Dim<3>::Vector>& vertices,
                    const vector<unsigned>& ipoints,
                    const Dim<3>::Vector& normal,
                    const bool countBoundary,
                    const double tol) {

  typedef Dim<3>::Vector Vector;
  // typedef boost::tuple<double, double> BGPoint;
  // typedef boost::geometry::model::polygon<BGPoint> BGPolygon;

  // Prerequisites.
  const auto npts = ipoints.size();
  unsigned i, j, ik, jk;
  // BEGIN_CONTRACT_SCOPE
  // {
  //   REQUIRE(ipoints.size() > 2);
  //   const double normmag = normal.magnitude();
  //   Vector normi;
  //   for (i = 1; i != npts; ++i) {
  //     j = (i + 1) % npts;
  //     k = (i + 2) % npts;
  //     normi = (vertices[ipoints[j]] - vertices[ipoints[i]]).cross(vertices[ipoints[k]] - vertices[ipoints[i]]);
  //     REQUIRE2(fuzzyEqual(std::abs(normi.dot(normal)), normmag*normi.magnitude(), 1.0e-5), normi << " " << normal << " " << normi.dot(normal) << " " << normmag*normi.magnitude());
  //   }
  //   // REQUIRE2(fuzzyEqual(pointPlaneDistance(p, vertices[ipoints[0]], normal.unitVector()), 0.0, 1.0e-3), pointPlaneDistance(p, vertices[ipoints[0]], normal.unitVector()));
  // }
  // END_CONTRACT_SCOPE

  bool result = false;

  const double px = p.x();
  const double py = p.y();
  const double pz = p.z();

  // Find the bounding box for the polygon.
  double fxmin = std::numeric_limits<double>::max(),    fymin = std::numeric_limits<double>::max(),   fzmin =  std::numeric_limits<double>::max();
  double fxmax = std::numeric_limits<double>::lowest(), fymax = std::numeric_limits<double>::lowest(), fzmax = std::numeric_limits<double>::lowest();
  for (const auto i: ipoints) {
    fxmin = std::min(fxmin, vertices[i].x());
    fymin = std::min(fymin, vertices[i].y());
    fzmin = std::min(fzmin, vertices[i].z());
    fxmax = std::max(fxmax, vertices[i].x());
    fymax = std::max(fymax, vertices[i].y());
    fzmax = std::max(fzmax, vertices[i].z());
  }
  if (px >= fxmin and px <= fxmax and
      py >= fymin and py <= fymax and
      pz >= fzmin and pz <= fzmax) {

    // Check if the point is on the boundary (within tolerance).
    if (pointOnPolygon(p, vertices, ipoints, tol)) return countBoundary;

    // Figure out which plane we're going to project to.
    const double nmax = normal.maxAbsElement();
    bool facetTest = false;

    // x plane -- use (y,z) coordinates.
    if (std::abs(normal.x()) > 0.9*nmax) {
      // vector<BGPoint> points;
      // for (i = 0; i != npts; ++i) points.push_back(BGPoint(vertices[ipoints[i]].y(), vertices[ipoints[i]].z()));
      // points.push_back(points[0]);
      // BGPolygon poly;
      // boost::geometry::append(poly, points);
      // result = boost::geometry::within(BGPoint(py, pz), poly);
      for (ik = 0, jk = npts - 1; ik < npts; jk = ik++) {
        i = ipoints[ik];
        j = ipoints[jk];
        if ( ((vertices[i].z() > pz) != (vertices[j].z() > pz)) &&
             (py < (vertices[j].y() - vertices[i].y()) * (pz - vertices[i].z()) / (vertices[j].z() - vertices[i].z()) + vertices[i].y()) )
          result = not result;
      }

    // y plane -- use (z,x) coordinates.
    } else if (std::abs(normal.y()) > 0.5*nmax) {
      // vector<BGPoint> points;
      // for (i = 0; i != npts; ++i) points.push_back(BGPoint(vertices[ipoints[i]].z(), vertices[ipoints[i]].x()));
      // points.push_back(points[0]);
      // BGPolygon poly;
      // boost::geometry::append(poly, points);
      // result = boost::geometry::within(BGPoint(pz, px), poly);
      for (ik = 0, jk = npts - 1; ik < npts; jk = ik++) {
        i = ipoints[ik];
        j = ipoints[jk];
        if ( ((vertices[i].x() > px) != (vertices[j].x() > px)) &&
             (pz < (vertices[j].z() - vertices[i].z()) * (px - vertices[i].x()) / (vertices[j].x() - vertices[i].x()) + vertices[i].z()) )
          result = not result;
      }

    // z plane -- use (x,y) coordinate.
    } else {
      // vector<BGPoint> points;
      // for (i = 0; i != npts; ++i) points.push_back(BGPoint(vertices[ipoints[i]].x(), vertices[ipoints[i]].y()));
      // points.push_back(points[0]);
      // BGPolygon poly;
      // boost::geometry::append(poly, points);
      // result = boost::geometry::within(BGPoint(px, py), poly);
      for (ik = 0, jk = npts - 1; ik < npts; jk = ik++) {
        i = ipoints[ik];
        j = ipoints[jk];
        if ( ((vertices[i].y() > py) != (vertices[j].y() > py)) &&
             (px < (vertices[j].x() - vertices[i].x()) * (py - vertices[i].y()) / (vertices[j].y() - vertices[i].y()) + vertices[i].x()) )
          result = not result;
      }
    }

  }

  // That's it.
  return result;
}

}


#ifndef UQ_PCELLS_HASH_TABLE_HPP
#define UQ_PCELLS_HASH_TABLE_HPP

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include "mfem.hpp"

#include "cells_hash_table.hpp"

using namespace mfem;

//! Defines parallel hash table
class ParCellsHashTable
{
public:
    //! Constructor
    ParCellsHashTable(MPI_Comm& comm,
                      int Nx, double xl, double xr,
                      int Ny, double yl, double yr);

    //! Destructor
    ~ ParCellsHashTable() {}

    //! Searches key in the hash table
    //! returns the cell indices
    std::pair<int, int> search(Eigen::Vector2d&);

    //! Inserts a key in the hash table
    void insert(int, double, Eigen::Vector4d&);

    //! Modifies a key in the hash table
    int modify(int, Eigen::Vector4d&);

    //! Displays the hash table
    void display()
    {
        if (m_cartRank == 0) { m_cellsHashTable->display(); }
    }

    //! Returns the total number of elements
    inline int get_numElements() {

        int numElements_ = m_cellsHashTable
                ->get_numElements(m_iStart, m_iEnd,
                                  m_jStart, m_jEnd);

        int numElements = 0;
        MPI_Allreduce(&numElements_, &numElements, 1,
                      MPI_INT, MPI_SUM, m_cartComm);

        return numElements;
    }

    //! Returns the number of cells along x-axis
    inline int get_numCellsInX() {
        return m_Nx;
    }

    //! Returns the number of cells along y-axis
    inline int get_numCellsInY() {
        return m_Ny;
    }

    //! Returns x-axis left bound
    inline double get_xLeft() {
        return m_xl;
    }

    //! Returns x-axis right bound
    inline double get_xRight() {
        return m_xr;
    }

    //! Returns y-axis left bound
    inline double get_yLeft() {
        return m_yl;
    }

    //! Returns y-axis right bound
    inline double get_yRight() {
        return m_yr;
    }

    //! Returns the communicator
    inline MPI_Comm get_comm() {
        return m_cartComm;
    }

    //! Returns local hash table
    inline std::shared_ptr<CellsHashTable>
    get_localHashTable() {
        return m_cellsHashTable;
    }

private:
    MPI_Comm m_cartComm;
    int m_cartRank;

    int m_Nx;
    double m_xl, m_xr;
    int m_iStart, m_iEnd;

    int m_Ny;
    double m_yl, m_yr;
    int m_jStart, m_jEnd;

    std::shared_ptr<CellsHashTable> m_cellsHashTable;
};

//! Makes parallel hash table for a given mesh
std::shared_ptr<ParCellsHashTable>
make_hashTable
(MPI_Comm& comm,
 std::shared_ptr<Mesh>& mesh,
 int Nx, double xl, double xr,
 int Ny, double yl, double yr);

//! Updates the velocity data in hash table
void update_hashTable
(std::shared_ptr<ParCellsHashTable>&,
 std::shared_ptr<Mesh>& mesh,
 GridFunction *vx, GridFunction *vy);

#endif /// UQ_PCELLS_HASH_TABLE_HPP


// Author: Francesco Regazzoni - MOX, Politecnico di Milano
// Email:  francesco.regazzoni@polimi.it
// Date:   2020

#include <cmath>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <Eigen/Dense>

#include "model_RDQ20_MF.hpp"

model_RDQ20_MF::model_RDQ20_MF(std::string parameters_file)
    : sarcomere("RDQ20-MF") {
  // Read JSON options file
  boost::property_tree::ptree root;
  boost::property_tree::read_json(parameters_file, root);

  prm_LA = root.get_child("geometry")
               .get_child("LA")
               .get_value<double>(); // [micro m]
  prm_LM = root.get_child("geometry")
               .get_child("LM")
               .get_value<double>(); // [micro m]
  prm_LB = root.get_child("geometry")
               .get_child("LB")
               .get_value<double>(); // [micro m]
  prm_SL0 = root.get_child("geometry")
                .get_child("SL0")
                .get_value<double>(); // [micro m]
  prm_Q = root.get_child("RU_steady_state")
              .get_child("Q")
              .get_value<double>(); // [-]
  prm_Kd0 = root.get_child("RU_steady_state")
                .get_child("Kd0")
                .get_value<double>(); // [micro M]
  prm_alphaKd = root.get_child("RU_steady_state")
                    .get_child("alphaKd")
                    .get_value<double>(); // [micro M / micro m]
  prm_mu = root.get_child("RU_steady_state")
               .get_child("mu")
               .get_value<double>(); // [-]
  prm_gamma = root.get_child("RU_steady_state")
                  .get_child("gamma")
                  .get_value<double>(); // [-]
  prm_Koff = root.get_child("RU_kinetics")
                 .get_child("Koff")
                 .get_value<double>(); // [s^-1]
  prm_Kbasic = root.get_child("RU_kinetics")
                   .get_child("Kbasic")
                   .get_value<double>(); // [s^-1]
  prm_r0 = root.get_child("XB_cycling")
               .get_child("r0")
               .get_value<double>(); // [s^-1]
  prm_alpha = root.get_child("XB_cycling")
                  .get_child("alpha")
                  .get_value<double>(); // [-]
  prm_mu0_fP = root.get_child("XB_cycling")
                   .get_child("mu0_fP")
                   .get_value<double>(); // [s^-1]
  prm_mu1_fP = root.get_child("XB_cycling")
                   .get_child("mu1_fP")
                   .get_value<double>(); // [s^-1]
  prm_a_XB = root.get_child("upscaling")
                 .get_child("a_XB")
                 .get_value<double>(); // [kPa]

  initialize_rates();

  n_variables = 20;
  initial_state.push_back(1.0);
  for (unsigned int i = 0; i < n_variables - 1; ++i)
    initial_state.push_back(0.0);
}

void model_RDQ20_MF::solve_time_step(std::vector<double> &state,
                                     const double &calcium,
                                     const double &sarcomere_length,
                                     const double &dSL_dt, const double &dt) {

  // Deserialize state
  deserialize_state(state);

  // Update RU transition rates
  RU_update_rates(calcium, sarcomere_length);

  // Advance RU state
  double RU_dt = 0.0;
  double time_advanced = 0.0;
  while (time_advanced <=
         dt - 1e-10) // Cover the time-step up to a given tolerance
  {
    RU_dt = std::min(prm_time_step_update_RU_state, dt - time_advanced);
    RU_update_state(RU_dt);
    time_advanced += RU_dt;
  }

  // Advance XB state
  XB_update_state(dSL_dt, dt);

  // Re-serialize state
  serialize_state(state);
}

void model_RDQ20_MF::deserialize_state(const std::vector<double> &state) {
  unsigned int i_current = 0;
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC)
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          state_RU[TL][TC][TR][CC] = state[i_current];
          i_current++;
        }

  for (i_XB = 0; i_XB < 4; ++i_XB) {
    state_XB[i_XB] = state[i_current];
    i_current++;
  }
}

void model_RDQ20_MF::serialize_state(std::vector<double> &state) {
  unsigned int i_current = 0;
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC)
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          state[i_current] = state_RU[TL][TC][TR][CC];
          i_current++;
        }

  for (i_XB = 0; i_XB < 4; ++i_XB) {
    state[i_current] = state_XB[i_XB];
    i_current++;
  }
}

double model_RDQ20_MF::get_active_tension(const std::vector<double> &state,
                                          const double &sarcomere_length) {
  return prm_a_XB * (state[17] + state[19]) *
         fraction_single_overlap(sarcomere_length);
}

double model_RDQ20_MF::get_active_stiffness(const std::vector<double> &state,
                                            const double &sarcomere_length) {
  return prm_a_XB * (state[16] + state[18]) *
         fraction_single_overlap(sarcomere_length);
}

void model_RDQ20_MF::initialize_rates() {
  rates_C[1][0] = prm_Koff;
  rates_C[1][1] = prm_Koff / prm_mu;

  int permissive_neighbors;
  for (TL = 0; TL < 2; ++TL)
    for (TR = 0; TR < 2; ++TR) {
      permissive_neighbors = TL + TR;
      rates_T[TL][1][TR][0] =
          prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
      rates_T[TL][1][TR][1] =
          prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
      rates_T[TL][0][TR][0] = prm_Q * prm_Kbasic *
                              std::pow(prm_gamma, permissive_neighbors) /
                              prm_mu;
      rates_T[TL][0][TR][1] =
          prm_Q * prm_Kbasic * std::pow(prm_gamma, permissive_neighbors);
    }
}

void model_RDQ20_MF::RU_update_rates(const double &calcium,
                                     const double &sarcomere_length) {
  for (unsigned int i = 0; i < 2; ++i)
    rates_C[0][i] = prm_Koff /
                    (prm_Kd0 - prm_alphaKd * (2.15 - sarcomere_length)) *
                    calcium;
}

void model_RDQ20_MF::RU_update_state(const double &dt) {
  // Compute fluxes associated with center unit
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC)
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          // PhiT_C(a,b,c,B) =  P( T_i^t+dt =~ b,
          // (T_{i-1},T_i,T_{i+1},C_i)^t = (a,b,c,B) ) / dt
          PhiT_C[TL][TC][TR][CC] =
              state_RU[TL][TC][TR][CC] * rates_T[TL][TC][TR][CC];
          // PhiC_C(a,b,c,B) =  P( C_i^t+dt =~ B,
          // (T_{i-1},T_i,T_{i+1},C_i)^t = (a,b,c,B) ) / dt
          PhiC_C[TL][TC][TR][CC] = state_RU[TL][TC][TR][CC] * rates_C[CC][TC];
        }

  double flux_tot;
  double prob_tot;

  // Compute rates associated with left unit
  // rates_T_L(a,b) ~= P( T_{i-1}^t+dt =~ a | (T_{i-1},T_i)^t = (a,b) ) / dt
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC) {
      flux_tot = 0.0;
      prob_tot = 0.0;
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          flux_tot += PhiT_C[TL][TC][TR][CC];
          prob_tot += state_RU[TL][TC][TR][CC];
        }
      if (prob_tot > 1e-12) {
        rates_T_L[TL][TC] = flux_tot / prob_tot;
      } else {
        rates_T_L[TL][TC] = 0.0;
      }
    }

  // Compute rates associated with right unit
  // rates_T_L(c,b) ~= P( T_{i+1}^t+dt =~ c | (T_i,T_{i+1})^t = (b,c) ) / dt
  for (TR = 0; TR < 2; ++TR)
    for (TC = 0; TC < 2; ++TC) {
      flux_tot = 0.0;
      prob_tot = 0.0;
      for (TL = 0; TL < 2; ++TL)
        for (CC = 0; CC < 2; ++CC) {
          flux_tot += PhiT_C[TL][TC][TR][CC];
          prob_tot += state_RU[TL][TC][TR][CC];
        }
      if (prob_tot > 1e-12) {
        rates_T_R[TR][TC] = flux_tot / prob_tot;
      } else {
        rates_T_R[TR][TC] = 0.0;
      }
    }

  // Compute fluxes associated with external units
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC)
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          // PhiT_L(a,b,c,B) ~= P( T_{i-1}^t+dt =~ a,
          // (T_{i-1},T_i,T_{i+1},C_i)^t = (a,b,c,B) ) / dt
          PhiT_L[TL][TC][TR][CC] = state_RU[TL][TC][TR][CC] * rates_T_L[TC][TL];
          // PhiT_R(a,b,c,B) ~= P( T_{i+1}^t+dt =~ c,
          // (T_{i-1},T_i,T_{i+1},C_i)^t = (a,b,c,B) ) / dt
          PhiT_R[TL][TC][TR][CC] = state_RU[TL][TC][TR][CC] * rates_T_R[TC][TR];
        }

  // Forward Euler advance
  for (TL = 0; TL < 2; ++TL)
    for (TC = 0; TC < 2; ++TC)
      for (TR = 0; TR < 2; ++TR)
        for (CC = 0; CC < 2; ++CC) {
          state_RU[TL][TC][TR][CC] +=
              dt * (-PhiT_L[TL][TC][TR][CC] + PhiT_L[1 - TL][TC][TR][CC] -
                    PhiT_C[TL][TC][TR][CC] + PhiT_C[TL][1 - TC][TR][CC] -
                    PhiT_R[TL][TC][TR][CC] + PhiT_R[TL][TC][1 - TR][CC] -
                    PhiC_C[TL][TC][TR][CC] + PhiC_C[TL][TC][TR][1 - CC]);
        }
}

void model_RDQ20_MF::XB_update_state(const double &dSL_dt, const double &dt) {
  double v = -dSL_dt / prm_SL0;

  double permissivity = 0.0;
  double flux_PN = 0.0;
  double flux_NP = 0.0;

  for (TL = 0; TL < 2; ++TL)
    for (TR = 0; TR < 2; ++TR)
      for (CC = 0; CC < 2; ++CC) {
        permissivity += state_RU[TL][1][TR][CC];
        flux_PN += state_RU[TL][1][TR][CC] * rates_T[TL][1][TR][CC];
        flux_NP += state_RU[TL][0][TR][CC] * rates_T[TL][0][TR][CC];
      }

  double k_PN = 0.0;
  double k_NP = 0.0;
  if (permissivity >= 1e-12)
    k_PN = flux_PN / permissivity;
  if (1 - permissivity >= 1e-12)
    k_NP = flux_NP / (1 - permissivity);

  double r = prm_r0 + prm_alpha * std::abs(v);
  double diag_P = r + k_PN;
  double diag_N = r + k_NP;

  Eigen::Matrix<double, 4, 4> XB_A;
  Eigen::Matrix<double, 4, 1> XB_rhs, XB_sol;

  // Fill matrix
  XB_A(0, 0) = -diag_P;
  XB_A(1, 1) = -diag_P;
  XB_A(2, 2) = -diag_N;
  XB_A(3, 3) = -diag_N;
  XB_A(0, 2) = k_NP;
  XB_A(1, 3) = k_NP;
  XB_A(2, 0) = k_PN;
  XB_A(3, 1) = k_PN;
  XB_A(1, 0) = -v;
  XB_A(3, 2) = -v;

  XB_A *= -dt;
  for (i_XB = 0; i_XB < 4; ++i_XB)
    XB_A(i_XB, i_XB) += 1.0;

  // Fill rhs
  XB_rhs(0) = state_XB[0] + dt * permissivity * prm_mu0_fP;
  XB_rhs(1) = state_XB[1] + dt * permissivity * prm_mu1_fP;
  XB_rhs(2) = state_XB[2];
  XB_rhs(3) = state_XB[3];

  // Implicit Euler advance
  XB_sol = XB_A.colPivHouseholderQr().solve(XB_rhs);

  for (i_XB = 0; i_XB < 4; ++i_XB)
    state_XB[i_XB] = XB_sol(i_XB);
}

double model_RDQ20_MF::fraction_single_overlap(const double &SL) const {
  double LMh = (prm_LM - prm_LB) * 0.5;
  if (SL > prm_LA && SL <= prm_LM)
    return (SL - prm_LA) / LMh;
  if (SL > prm_LM && SL <= 2 * prm_LA - prm_LB)
    return (SL + prm_LM - 2 * prm_LA) * 0.5 / LMh;
  if (SL > 2 * prm_LA - prm_LB && SL <= 2 * prm_LA + prm_LB)
    return 1.0;
  if (SL > 2 * prm_LA + prm_LB && SL <= 2 * prm_LA + prm_LM)
    return (prm_LM + 2 * prm_LA - SL) * 0.5 / LMh;
  return 0.0;
}


//==================================================================================================
/*!
  @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_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SQRT_HPP_INCLUDED

#include <boost/simd/function/raw.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( sqrt_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const raw_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 r = if_else_zero(a0, a0 * raw_(rsqrt)(a0));
      #ifndef BOOST_SIMD_NO_INFINITIES
      return if_else(a0 == Inf<A0>(), Inf<A0>(), r);
      #else
      return r;
      #endif
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( sqrt_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const raw_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( bs::assert_all(is_positive(a0))
                      , "sqrt integer domain is restricted to positive integer."
                      );
      return sqrt(a0);
    }
  };
} } }

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <sstream>
#include <iomanip>

#ifndef WIN64
#define EIGEN_DONT_ALIGN_STATICALLY
#endif
#include <Eigen/Dense>

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

#include "mex.h"
#include "matrix.h"
#include "ImageAlign.cpp"
#include "tum_benchmark.hpp"

// cv::Mat LoadGray(const std::string &filename)
// {
//     cv::Mat imgGray = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
//     // convert gray to float
//     cv::Mat gray;
//     imgGray.convertTo(gray, CV_32FC1, 1.0f / 255.0f);
//     return gray;
// }

// cv::Mat LoadDepth(const std::string &filename)
// {
//     //fill/read 16 bit depth image
//     cv::Mat imgDepthIn = cv::imread(filename, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
//     cv::Mat imgDepth;
//     imgDepthIn.convertTo(imgDepth, CV_32FC1, (1.0 / 5000.0));
//     return imgDepth;
// }

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

	if (nlhs != 1) {
			mexErrMsgIdAndTxt("MATLAB:outputmismatch",
												"Output arguments must be 1!");
	}

	if (nrhs != 6) {
			mexErrMsgIdAndTxt("MATLAB:Inputmismatch",
												"Input arguments must be 6!");
	}

	char *depthImg1 = mxArrayToString(prhs[0]);

   if (depthImg1 == NULL)
     mexErrMsgTxt("Not enough heap space to hold converted string.");

 	char *depthImg2 = mxArrayToString(prhs[1]);

   if (depthImg2 == NULL)
     mexErrMsgTxt("Not enough heap space to hold converted string.");

 	char *colorImg1 = mxArrayToString(prhs[2]);
   if (colorImg1 == NULL)
     mexErrMsgTxt("Not enough heap space to hold converted string.");

 	char *colorImg2 = mxArrayToString(prhs[3]);
   if (colorImg2 == NULL)
     mexErrMsgTxt("Not enough heap space to hold converted string.");

 	cv::Mat grayRef = loadGray(colorImg1);
 	cv::Mat depthRef = loadDepth(depthImg1);
 	cv::Mat grayCur = loadGray(colorImg2);
 	cv::Mat depthCur = loadDepth(depthImg2);

  size_t n = mxGetN(prhs[4]);
  size_t m = mxGetM(prhs[4]);

  if (n != 3 || m != 3){
    mexErrMsgIdAndTxt("MATLAB:inputmismatch",
                        "wrong matrix dimension!");
  }

 	double *A = mxGetPr(prhs[4]);
	Eigen::Matrix3f K;
	for (size_t i=0; i < n*m; i++){
		K(i%n, i/m) = A[i];
	}

  size_t n_p = mxGetN(prhs[5]);
  size_t m_p = mxGetM(prhs[5]);

	if (n_p != 4 || m_p != 4){
		mexErrMsgIdAndTxt("MATLAB:inputmismatch",
                        "wrong initial pose dimension!");
  }

   	double *B = mxGetPr(prhs[5]);
	Eigen::Matrix4f relPose;
	for (size_t i=0; i < n_p*m_p; i++){
		relPose(i%n_p, i/m_p) = B[i];
	}

    // K <<    517.3, 0.0, 318.6,
    //         0.0, 516.5, 255.3,
    //         0.0, 0.0, 1.0;

	Eigen::Matrix4f poses = PoseEst(grayRef, depthRef, grayCur, depthCur, K, relPose);

	double *D;
	plhs[0] = mxCreateDoubleMatrix(4,4, mxREAL);
	D = mxGetPr(plhs[0]);
	size_t len = poses.size();
	for (size_t j=0; j< len; j++){
			D[j] = poses(j%4, j/4);
		}

}