#include "mesh.h"

#include <fstream>

#include <Eigen/Dense>

Mesh::Mesh()
{}

void Mesh::SaveOBJ(const char* filename, const Camera& camera, bool readjust) {
	auto& vertices = vertices_;
	auto& faces = faces_;
	std::ofstream os(filename);
	for (int i = 0; i < vertices.size(); ++i) {
		auto v = vertices[i];
		if (readjust)
			camera.Undistort(v);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}
	for (int i = 0; i < faces.size(); ++i) {
		auto& f = faces[i];
		os << "f " << f[0] + 1 << " " << f[1] + 1 << " " << f[2] + 1 << "\n";
	}
	os.close();
}

Eigen::Vector3d PerspectiveInterp(const Eigen::Vector3d& a, const Eigen::Vector3d& b, double step, bool perspective) {
	if (!perspective)
		return a * (1 - step) + b * step;
	Eigen::Vector3d v0(a[0] * a[2],a[1] * a[2],a[2]);
	Eigen::Vector3d v1(b[0] * b[2],b[1] * b[2],b[2]);
	Eigen::Vector3d n1 = (v1 - v0);
	if (n1.norm() == 0)
		return a;
	n1 /= n1.norm();

	Eigen::Vector3d dir1(a[0] * (1 - step) + b[0] * step, a[1] * (1 - step) + b[1] * step, 1);

	Eigen::Vector3d dir = dir1 - dir1.dot(n1) * n1;

	double d_norm = dir.squaredNorm();
	double z = v0.dot(dir) / d_norm;

	dir1[2] = z;
	return dir1;
}

void Mesh::LoadFromFile(const char* filename) {
	auto& vertices = vertices_;
	auto& faces = faces_;
	vertices.clear();
	faces.clear();
	char buffer[1024];
	std::ifstream is(filename);
	while (is >> buffer) {
		if (strcmp(buffer, "v") == 0) {
			Eigen::Vector3d v;
			is >> v[0] >> v[1] >> v[2];
			vertices.push_back(v);
		}
		else if (strcmp(buffer, "f") == 0) {
			Eigen::Vector3i f;
			for (int i = 0; i < 3; ++i) {
				is >> buffer;
				int t = 0;
				int p = 0;
				int l = strlen(buffer);
				while (p != l && buffer[p] >= '0' && buffer[p] <= '9') {
					t = t * 10 + (buffer[p] - '0');
					p += 1;
				}
				f[i] = t - 1;
			}
			faces.push_back(f);
		}
	}
	is.close();
}

void Mesh::BoundaryClip(int dim, double clamp_thres, int comp, bool perspective) {
	auto& vertices = vertices_;
	auto& faces = faces_;

	int i = 0;
	int faces_num = faces.size();
	while (i < faces_num) {
		Eigen::Vector3d v[3];
		for (int j = 0; j < 3; ++j)
			v[j] = vertices[faces[i][j]];
		int valids[3];
		for (int j = 0; j < 3; ++j) {
			if ((comp == 0 && v[j][dim] < clamp_thres + -1e-6)
			 || (comp == 1 && v[j][dim] > clamp_thres + 1e-6)) {
				valids[j] = 0;
			} else {
				valids[j] = 1;
			}
		}
		int sum = valids[0] + valids[1] + valids[2];
		if (sum == 0) {
			faces[i] = faces.back();
			faces.pop_back();
			faces_num = faces.size();
			continue;
		}
		else if (sum == 3) {
		}
		else if (sum == 1) {
			int j = 0;
			while (valids[j] == 0)
				j += 1;
			double step0 = 0, step1 = 0;
			if (comp == 0) {
				step0 = (clamp_thres - v[j][dim]) / (v[(j + 2) % 3][dim] - v[j][dim]);
				step1 = (clamp_thres - v[j][dim]) / (v[(j + 1) % 3][dim] - v[j][dim]);
			}
			else {
				step0 = (clamp_thres - v[j][dim]) / (v[(j + 2) % 3][dim] - v[j][dim]);
				step1 = (clamp_thres - v[j][dim]) / (v[(j + 1) % 3][dim] - v[j][dim]);
			}
			int nx = vertices.size();
			int nx2 = vertices.size() + 1;

			vertices.push_back(PerspectiveInterp(v[j], v[(j+2)%3], step0, perspective));
			vertices.push_back(PerspectiveInterp(v[j], v[(j+1)%3], step1, perspective));

			faces[i] = Eigen::Vector3i(nx, faces[i][j], nx2);
		}
		else {
			int j = 0;
			while (valids[j] == 1)
				j += 1;
			double step0 = 0, step1 = 0;
			if (comp == 0) {
				step0 = (clamp_thres - v[j][dim]) / (v[(j + 2) % 3][dim] - v[j][dim]);
				step1 = (clamp_thres - v[j][dim]) / (v[(j + 1) % 3][dim] - v[j][dim]);
			}
			else {
				step0 = (clamp_thres - v[j][dim]) / (v[(j + 2) % 3][dim] - v[j][dim]);
				step1 = (clamp_thres - v[j][dim]) / (v[(j + 1) % 3][dim] - v[j][dim]);
			}
			int v0 = faces[i][(j + 2) % 3];
			int nx = vertices.size();
			int nx2 = vertices.size() + 1;
			int v1 = faces[i][(j + 1) % 3];

			vertices.push_back(PerspectiveInterp(v[j], v[(j+2)%3], step0, perspective));
			vertices.push_back(PerspectiveInterp(v[j], v[(j+1)%3], step1, perspective));

			faces[i] = Eigen::Vector3i(v0, nx, nx2);
			faces.push_back(Eigen::Vector3i(v0, nx2, v1));
		}
		i += 1;
	}
}

void Mesh::Recenter() {
	auto& vertices = vertices_;
	for (int i = 0; i < vertices.size(); ++i) {
		vertices[i][0] -= 0.5;
		vertices[i][1] -= 0.5;
	}
}

void Mesh::ComputeNormals()
{
	auto& face_normals = face_normals_;
	auto& vertices = vertices_;
	auto& faces = faces_;

	face_normals.resize(faces.size());
	for (int i = 0; i < faces.size(); ++i) {
		auto v0 = vertices[faces[i][0]];
		auto v1 = vertices[faces[i][1]];
		auto v2 = vertices[faces[i][2]];
		v0[0] *= v0[2];
		v0[1] *= v0[2];
		v1[0] *= v1[2];
		v1[1] *= v1[2];
		v2[0] *= v2[2];
		v2[1] *= v2[2];
		Eigen::Vector3d d1 = v1 - v0;
		Eigen::Vector3d d2 = v2 - v0;
		Eigen::Vector3d n = d1.cross(d2);
		if (n.norm() > 0)
			n = n.normalized();
		face_normals[i] = n;
	}
}

void Mesh::ComputePlaneParameters() {
	auto& faces = faces_;
	auto& vertices = vertices_;
	auto& params = params_;

	params_.resize(faces.size());
	for (int i = 0; i < faces.size(); ++i) {
		params[i] = PlaneParam(vertices[faces[i][0]],vertices[faces[i][1]],vertices[faces[i][2]]);
	}
}


//
// $Id$
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "TruncatedLorentzianParameters.hpp"
#include "pwiz/utility/misc/unit.hpp"
#include <boost/filesystem/operations.hpp>
#include "pwiz/utility/misc/Std.hpp"
#include <cstring>

using namespace pwiz::util;
using namespace pwiz::frequency;

ostream* os_ = 0;

void testParameterConversion()
{
    TruncatedLorentzianParameters tlp;
    tlp.f0 = 666;
    tlp.alpha = complex<double>(100);
    double shift = 666;
    double scale = 50;

    ublas::vector<double> p = tlp.parameters(-shift, 1/scale);
    unit_assert(p(TruncatedLorentzian::AlphaR) == 2);
    unit_assert(p(TruncatedLorentzian::AlphaI) == 0);
    unit_assert(p(TruncatedLorentzian::Tau) == 1);
    unit_assert(p(TruncatedLorentzian::F0) == 0);

    ublas::vector<double> p2(4);
    p2(TruncatedLorentzian::AlphaR) = 3;
    p2(TruncatedLorentzian::AlphaI) = 0;
    p2(TruncatedLorentzian::Tau) = 0;
    p2(TruncatedLorentzian::F0) = 1;

    tlp.parameters(p2, shift, scale);
    unit_assert(tlp.alpha == 3.*scale);
    unit_assert(tlp.tau == 0);
    unit_assert(tlp.f0 == 1+shift);
}

void testIO()
{
    TruncatedLorentzianParameters tlp;
    tlp.T = 2;
    tlp.tau = 3;
    tlp.f0 = 666;
    tlp.alpha = complex<double>(100);

    const char* filename = "TruncatedLorentzianTest.test.tlp";
    tlp.write(filename);
    TruncatedLorentzianParameters tlp2(filename);

    unit_assert(tlp2.T == tlp.T);
    unit_assert(tlp2.tau == tlp.tau);
    unit_assert(tlp2.f0 == tlp.f0);
    unit_assert(tlp2.alpha == tlp.alpha);

    boost::filesystem::remove(filename);
}

void testEquality()
{
    TruncatedLorentzianParameters tlp;
    TruncatedLorentzianParameters tlp2;

    tlp.f0 = 666;
    unit_assert(tlp != tlp2);
    tlp2.f0 = 666;
    unit_assert(tlp == tlp2);
}

void testSamples()
{
    TruncatedLorentzianParameters tlp;
    tlp.T = 2;
    tlp.tau = 3;
    tlp.f0 = 666;
    tlp.alpha = complex<double>(100);

    double start = 660;
    double step = .2;
    int count = 60;

    tlp.writeSamples(cout, start, step, count);
}

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

    try
    {
        if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
        if (os_) *os_ << "TruncatedLorentzianParametersTest\n";
        testParameterConversion();
        testIO();
        testEquality();
        //testSamples();
    }
    catch (exception& e)
    {
        TEST_FAILED(e.what())
    }
    catch (...)
    {
        TEST_FAILED("Caught unknown exception.")
    }

    TEST_EPILOG
}


#include <algorithm>
#include <chrono>
#include <iostream>
#include <vector>

// #include <boost/align/aligned_allocator.hpp>
// using boost::alignment::aligned_allocator;
using high_resolution_clock = std::chrono::high_resolution_clock;
using time_point = std::chrono::high_resolution_clock::time_point;

#include "autotune/autotune.hpp"

// defines kernel, put in single compilation unit
AUTOTUNE_KERNEL(void(std::vector<double> &, std::vector<double> &), square,
                "examples/jit_only_kernel")

int main(void) {

  autotune::square.set_verbose(true);
  // builder.set_verbose(true);

  size_t N = 2048 * 2048; // 32MB
  size_t repetitions = 100;
  std::vector<double> origin(N);
  std::fill(origin.begin(), origin.end(), 2.0);
  std::vector<double> dest_slow(N);
  std::vector<double> dest_fast(N);

  std::vector<double> dest_reference(N);
  std::fill(dest_reference.begin(), dest_reference.end(), 4.0);

  double duration_slow = 0.0;
  auto &builder = autotune::square.get_builder<cppjit::builder::gcc>();
  {
    builder.set_cpp_flags("-std=c++17");
    autotune::square.compile();

    for (size_t i = 0; i < repetitions; i++) {
      time_point start = high_resolution_clock::now();
      autotune::square(origin, dest_slow);
      time_point end = high_resolution_clock::now();
      duration_slow += std::chrono::duration<double>(end - start).count();
    }
    std::cout << "avr. copy duration (native not specified): "
              << (duration_slow / repetitions) << std::endl;
    bool is_eqal = std::equal(dest_reference.begin(), dest_reference.end(),
                              dest_slow.begin());
    if (is_eqal) {
      std::cout << "ranges are equal!" << std::endl;
    } else {
      std::cout << "ranges are NOT equal!" << std::endl;
    }
  }

  double duration_fast = 0.0;
  {
    builder.set_cpp_flags("-std=c++17 -O3 -march=native"); // -mtune=native
    autotune::square.compile();
    for (size_t i = 0; i < repetitions; i++) {
      time_point start = high_resolution_clock::now();
      autotune::square(origin, dest_fast);
      time_point end = high_resolution_clock::now();
      duration_fast += std::chrono::duration<double>(end - start).count();
    }
    std::cout << "avr. copy duration (native specified): "
              << (duration_fast / repetitions) << std::endl;
    bool is_eqal = std::equal(dest_reference.begin(), dest_reference.end(),
                              dest_fast.begin());
    if (is_eqal) {
      std::cout << "ranges are equal!" << std::endl;
    } else {
      std::cout << "ranges are NOT equal!" << std::endl;
    }
  }

  std::cout << "avr. speedup (slow / fast): " << (duration_slow / duration_fast)
            << "x" << std::endl;

  return 0;
}


#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <sstream>                     // std::stringstream
#include <utility>                     // std::pair
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "networks.hpp"                // WAE_Decoder
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// -------------------
// Synthesis Function
// -------------------
void synth(po::variables_map &vm, torch::Device &device, WAE_Decoder &dec){

    constexpr std::string_view extension = "png";  // the extension of file name to save sample images
    constexpr std::pair<float, float> output_range = {-1.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    size_t max_counter;
    float value;
    std::string path, result_dir;
    std::stringstream ss;
    torch::Tensor z, output, outputs;

    // (1) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["synth_load_epoch"].as<std::string>() + "_dec.pth";
    torch::load(dec, path, device);

    // (2) Image Generation
    dec->eval();
    max_counter = (int)(vm["synth_sigma_max"].as<float>() / vm["synth_sigma_inter"].as<float>() * 2) + 1;
    z = torch::full({1, (long int)vm["nz"].as<size_t>()}, /*value=*/-vm["synth_sigma_max"].as<float>(), torch::TensorOptions().dtype(torch::kFloat)).to(device);
    outputs = dec->forward(z);
    for (size_t i = 1; i < max_counter; i++){
        value = -vm["synth_sigma_max"].as<float>() + (float)i * vm["synth_sigma_inter"].as<float>();
        z = torch::full({1, (long int)vm["nz"].as<size_t>()}, /*value=*/value, torch::TensorOptions().dtype(torch::kFloat)).to(device);
        output = dec->forward(z);
        outputs = torch::cat({outputs, output}, /*dim=*/0);
    }
    result_dir = vm["synth_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ss.str(""); ss.clear(std::stringstream::goodbit);
    ss << result_dir << "/Generated_Image."  << extension;
    visualizer::save_image(outputs.detach(), ss.str(), /*range=*/output_range, /*cols=*/max_counter);

    // End Processing
    return;

}


/* [auto_generated]
 boost/numeric/odeint/stepper/controlled_runge_kutta.hpp

 [begin_description]
 The default controlled stepper which can be used with all explicit Runge-Kutta error steppers.
 [end_description]

 Copyright 2010-2013 Karsten Ahnert
 Copyright 2010-2015 Mario Mulansky
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED

#include <cmath>

#include <boost/config.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template
<
class Value ,
class Algebra ,
class Operations
>
class default_error_checker
{
public:

    typedef Value value_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;

    default_error_checker(
            value_type eps_abs = static_cast< value_type >( 1.0e-6 ) ,
            value_type eps_rel = static_cast< value_type >( 1.0e-6 ) ,
            value_type a_x = static_cast< value_type >( 1 ) ,
            value_type a_dxdt = static_cast< value_type >( 1 ))
        : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt )
    { }

    template< class State , class Deriv , class Err, class Time >
    value_type error( const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
    {
        return error( algebra_type() , x_old , dxdt_old , x_err , dt );
    }

    template< class State , class Deriv , class Err, class Time >
    value_type error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
    {
        using std::abs;
        // this overwrites x_err !
        algebra.for_each3( x_err , x_old , dxdt_old ,
                typename operations_type::template rel_error< value_type >( m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt * abs(get_unit_value( dt )) ) );

        // value_type res = algebra.reduce( x_err ,
        //        typename operations_type::template maximum< value_type >() , static_cast< value_type >( 0 ) );
        return algebra.norm_inf( x_err );
    }

private:

    value_type m_eps_abs;
    value_type m_eps_rel;
    value_type m_a_x;
    value_type m_a_dxdt;

};

template< typename Value, typename Time >
class default_step_adjuster
{
public:
    typedef Time time_type;
    typedef Value value_type;

    default_step_adjuster(const time_type max_dt=static_cast<time_type>(0))
            : m_max_dt(max_dt)
    {}

    time_type decrease_step(time_type dt, const value_type error, const int error_order) const
    {
        // returns the decreased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        dt *= max
        BOOST_PREVENT_MACRO_SUBSTITUTION(
                static_cast<value_type>( static_cast<value_type>(9) / static_cast<value_type>(10) *
                                         pow(error, static_cast<value_type>(-1) / (error_order - 1))),
                static_cast<value_type>( static_cast<value_type>(1) / static_cast<value_type> (5)));
        if(m_max_dt != static_cast<time_type >(0))
            // limit to maximal stepsize even when decreasing
            dt = detail::min_abs(dt, m_max_dt);
        return dt;
    }

    time_type increase_step(time_type dt, value_type error, const int stepper_order) const
    {
        // returns the increased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        // adjust the size if dt is smaller than max_dt (providede max_dt is not zero)
        if(error < 0.5)
        {
            // error should be > 0
            error = max BOOST_PREVENT_MACRO_SUBSTITUTION (
                    static_cast<value_type>( pow( static_cast<value_type>(5.0) , -static_cast<value_type>(stepper_order) ) ) ,
                    error);
            // time_type dt_old = dt;   unused variable warning
            //error too small - increase dt and keep the evolution and limit scaling factor to 5.0
            dt *= static_cast<value_type>(9)/static_cast<value_type>(10) *
                  pow(error, static_cast<value_type>(-1) / stepper_order);
            if(m_max_dt != static_cast<time_type >(0))
                // limit to maximal stepsize
                dt = detail::min_abs(dt, m_max_dt);
        }
        return dt;
    }

    bool check_step_size_limit(const time_type dt)
    {
        if(m_max_dt != static_cast<time_type >(0))
            return detail::less_eq_with_sign(dt, m_max_dt, dt);
        return true;
    }

    time_type get_max_dt() { return m_max_dt; }

private:
    time_type m_max_dt;
};

/*
 * error stepper category dispatcher
 */
template<
class ErrorStepper ,
class ErrorChecker = default_error_checker< typename ErrorStepper::value_type ,
    typename ErrorStepper::algebra_type ,
    typename ErrorStepper::operations_type > ,
class StepAdjuster = default_step_adjuster< typename ErrorStepper::value_type ,
    typename ErrorStepper::time_type > ,
class Resizer = typename ErrorStepper::resizer_type ,
class ErrorStepperCategory = typename ErrorStepper::stepper_category
>
class controlled_runge_kutta ;

/*
 * explicit stepper version
 *
 * this class introduces the following try_step overloads
    * try_step( sys , x , t , dt )
    * try_step( sys , x , dxdt , t , dt )
    * try_step( sys , in , t , out , dt )
    * try_step( sys , in , dxdt , t , out , dt )
 */
/**
 * \brief Implements step size control for Runge-Kutta steppers with error
 * estimation.
 *
 * This class implements the step size control for standard Runge-Kutta
 * steppers with error estimation.
 *
 * \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
 * \tparam ErrorChecker The error checker
 * \tparam Resizer The resizer policy type.
 */
template<
class ErrorStepper,
class ErrorChecker,
class StepAdjuster,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster, Resizer ,
        explicit_error_stepper_tag >
{

public:

    typedef ErrorStepper stepper_type;
    typedef typename stepper_type::state_type state_type;
    typedef typename stepper_type::value_type value_type;
    typedef typename stepper_type::deriv_type deriv_type;
    typedef typename stepper_type::time_type time_type;
    typedef typename stepper_type::algebra_type algebra_type;
    typedef typename stepper_type::operations_type operations_type;
    typedef Resizer resizer_type;
    typedef ErrorChecker error_checker_type;
    typedef StepAdjuster step_adjuster_type;
    typedef explicit_controlled_stepper_tag stepper_category;

#ifndef DOXYGEN_SKIP
    typedef typename stepper_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;

    typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster ,
            Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif //DOXYGEN_SKIP

    /**
     * \brief Constructs the controlled Runge-Kutta stepper.
     * \param error_checker An instance of the error checker.
     * \param stepper An instance of the underlying stepper.
     */
    controlled_runge_kutta(
            const error_checker_type &error_checker = error_checker_type( ) ,
            const step_adjuster_type &step_adjuster = step_adjuster_type() ,
            const stepper_type &stepper = stepper_type( )
    )
        : m_stepper(stepper), m_error_checker(error_checker) , m_step_adjuster(step_adjuster)
    { }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The overloads are needed to solve the forwarding problem
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /**
     * \brief Tries to perform one step. Solves the forwarding problem and
     * allows for using boost range as state_type.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful. Can be a boost range.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /*
     * Version 2 : try_step( sys , x , dxdt , t , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut , class DerivIn >
    controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
        }
        return res;
    }

    /*
     * Version 3 : try_step( sys , in , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     *
     * the disable is needed to avoid ambiguous overloads if state_type = time_type
     */
    /**
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
        sys( in , m_dxdt.m_v , t );
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Version 4 : try_step( sys , in , dxdt , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class DerivIn , class StateOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
    {
        if( !m_step_adjuster.check_step_size_limit(dt) )
        {
            // given dt was above step size limit - adjust and return fail;
            dt = m_step_adjuster.get_max_dt();
            return fail;
        }

        m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        // do one step with error calculation
        m_stepper.do_step( system , in , dxdt , t , out , dt , m_xerr.m_v );

        value_type max_rel_err = m_error_checker.error( m_stepper.algebra() , in , dxdt , m_xerr.m_v , dt );

        if( max_rel_err > 1.0 )
        {
            // error too big, decrease step size and reject this step
            dt = m_step_adjuster.decrease_step(dt, max_rel_err, m_stepper.error_order());
            return fail;
        } else
        {
            // otherwise, increase step size and accept
            t += dt;
            dt = m_step_adjuster.increase_step(dt, max_rel_err, m_stepper.stepper_order());
            return success;
        }
    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_m_xerr_impl( x );
        resize_m_dxdt_impl( x );
        resize_m_xnew_impl( x );
        m_stepper.adjust_size( x );
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    stepper_type& stepper( void )
    {
        return m_stepper;
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    const stepper_type& stepper( void ) const
    {
        return m_stepper;
    }

private:

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        sys( x , m_dxdt.m_v ,t );
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    template< class StateIn >
    bool resize_m_xerr_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    stepper_type m_stepper;
    error_checker_type m_error_checker;
    step_adjuster_type m_step_adjuster;

    resizer_type m_dxdt_resizer;
    resizer_type m_xerr_resizer;
    resizer_type m_xnew_resizer;

    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_xerr;
    wrapped_state_type m_xnew;
};

/*
 * explicit stepper fsal version
 *
 * the class introduces the following try_step overloads
    * try_step( sys , x , t , dt )
    * try_step( sys , in , t , out , dt )
    * try_step( sys , x , dxdt , t , dt )
    * try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
 */
/**
 * \brief Implements step size control for Runge-Kutta FSAL steppers with
 * error estimation.
 *
 * This class implements the step size control for FSAL Runge-Kutta
 * steppers with error estimation.
 *
 * \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
 * \tparam ErrorChecker The error checker
 * \tparam Resizer The resizer policy type.
 */
template<
class ErrorStepper ,
class ErrorChecker ,
class StepAdjuster ,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster , Resizer , explicit_error_stepper_fsal_tag >
{

public:

    typedef ErrorStepper stepper_type;
    typedef typename stepper_type::state_type state_type;
    typedef typename stepper_type::value_type value_type;
    typedef typename stepper_type::deriv_type deriv_type;
    typedef typename stepper_type::time_type time_type;
    typedef typename stepper_type::algebra_type algebra_type;
    typedef typename stepper_type::operations_type operations_type;
    typedef Resizer resizer_type;
    typedef ErrorChecker error_checker_type;
    typedef StepAdjuster step_adjuster_type;
    typedef explicit_controlled_stepper_fsal_tag stepper_category;

#ifndef DOXYGEN_SKIP
    typedef typename stepper_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;

    typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster , Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif // DOXYGEN_SKIP

    /**
     * \brief Constructs the controlled Runge-Kutta stepper.
     * \param error_checker An instance of the error checker.
     * \param stepper An instance of the underlying stepper.
     */
    controlled_runge_kutta(
            const error_checker_type &error_checker = error_checker_type() ,
            const step_adjuster_type &step_adjuster = step_adjuster_type() ,
            const stepper_type &stepper = stepper_type()
    )
    : m_stepper( stepper ) , m_error_checker( error_checker ) , m_step_adjuster(step_adjuster) ,
      m_first_call( true )
    { }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The two overloads are needed in order to solve the forwarding problem
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t , dt );
    }

    /**
     * \brief Tries to perform one step. Solves the forwarding problem and
     * allows for using boost range as state_type.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful. Can be a boost range.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t , dt );
    }

    /*
     * Version 2 : try_step( sys , in , t , out , dt );
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     *
     * The disabler is needed to solve ambiguous overloads
     */
    /**
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        if( m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
        {
            initialize( system , in , t );
        }
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Version 3 : try_step( sys , x , dxdt , t , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut , class DerivInOut >
    controlled_step_result try_step( System system , StateInOut &x , DerivInOut &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        m_dxdt_new_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_new_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , m_dxdtnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
            boost::numeric::odeint::copy( m_dxdtnew.m_v , dxdt );
        }
        return res;
    }

    /*
     * Version 4 : try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type &t ,
            StateOut &out , DerivOut &dxdt_out , time_type &dt )
    {
        if( !m_step_adjuster.check_step_size_limit(dt) )
        {
            // given dt was above step size limit - adjust and return fail;
            dt = m_step_adjuster.get_max_dt();
            return fail;
        }

        m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        //fsal: m_stepper.get_dxdt( dxdt );
        //fsal: m_stepper.do_step( sys , x , dxdt , t , dt , m_x_err );
        m_stepper.do_step( system , in , dxdt_in , t , out , dxdt_out , dt , m_xerr.m_v );

        // this potentially overwrites m_x_err! (standard_error_checker does, at least)
        value_type max_rel_err = m_error_checker.error( m_stepper.algebra() , in , dxdt_in , m_xerr.m_v , dt );

        if( max_rel_err > 1.0 )
        {
            // error too big, decrease step size and reject this step
            dt = m_step_adjuster.decrease_step(dt, max_rel_err, m_stepper.error_order());
            return fail;
        }
        // otherwise, increase step size and accept
        t += dt;
        dt = m_step_adjuster.increase_step(dt, max_rel_err, m_stepper.stepper_order());
        return success;
    }

    /**
     * \brief Resets the internal state of the underlying FSAL stepper.
     */
    void reset( void )
    {
        m_first_call = true;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param deriv The initial derivative of the ODE.
     */
    template< class DerivIn >
    void initialize( const DerivIn &deriv )
    {
        boost::numeric::odeint::copy( deriv , m_dxdt.m_v );
        m_first_call = false;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The initial state of the ODE which should be solved.
     * \param t The initial time.
     */
    template< class System , class StateIn >
    void initialize( System system , const StateIn &x , time_type t )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        sys( x , m_dxdt.m_v , t );
        m_first_call = false;
    }

    /**
     * \brief Returns true if the stepper has been initialized, false otherwise.
     *
     * \return true, if the stepper has been initialized, false otherwise.
     */
    bool is_initialized( void ) const
    {
        return ! m_first_call;
    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_m_xerr_impl( x );
        resize_m_dxdt_impl( x );
        resize_m_dxdt_new_impl( x );
        resize_m_xnew_impl( x );
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    stepper_type& stepper( void )
    {
        return m_stepper;
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    const stepper_type& stepper( void ) const
    {
        return m_stepper;
    }

private:

    template< class StateIn >
    bool resize_m_xerr_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_new_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        if( m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
        {
            initialize( system , x , t );
        }
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    stepper_type m_stepper;
    error_checker_type m_error_checker;
    step_adjuster_type m_step_adjuster;

    resizer_type m_dxdt_resizer;
    resizer_type m_xerr_resizer;
    resizer_type m_xnew_resizer;
    resizer_type m_dxdt_new_resizer;

    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_xerr;
    wrapped_state_type m_xnew;
    wrapped_deriv_type m_dxdtnew;
    bool m_first_call;
};

/********** DOXYGEN **********/

/**** DEFAULT ERROR CHECKER ****/

/**
 * \class default_error_checker
 * \brief The default error checker to be used with Runge-Kutta error steppers
 *
 * This class provides the default mechanism to compare the error estimates
 * reported by Runge-Kutta error steppers with user defined error bounds.
 * It is used by the controlled_runge_kutta steppers.
 *
 * \tparam Value The value type.
 * \tparam Time The time type.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 */

    /**
     * \fn default_error_checker( value_type eps_abs , value_type eps_rel , value_type a_x , value_type a_dxdt ,
     * time_type max_dt)
     * \brief Constructs the error checker.
     *
     * The error is calculated as follows: ????
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param a_x Factor for the weight of the state.
     * \param a_dxdt Factor for the weight of the derivative.
     * \param max_dt Maximum allowed step size.
     */

    /**
     * \fn error( const State &x_old , const Deriv &dxdt_old , Err &x_err , time_type dt ) const
     * \brief Calculates the error level.
     *
     * If the returned error level is greater than 1, the estimated error was
     * larger than the permitted error bounds and the step should be repeated
     * with a smaller step size.
     *
     * \param x_old State at the beginning of the step.
     * \param dxdt_old Derivative at the beginning of the step.
     * \param x_err Error estimate.
     * \param dt Time step.
     * \return error
     */

    /**
     * \fn error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , time_type dt ) const
     * \brief Calculates the error level using a given algebra.
     *
     * If the returned error level is greater than 1, the estimated error was
     * larger than the permitted error bounds and the step should be repeated
     * with a smaller step size.
     *
     * \param algebra The algebra used for calculation of the error.
     * \param x_old State at the beginning of the step.
     * \param dxdt_old Derivative at the beginning of the step.
     * \param x_err Error estimate.
     * \param dt Time step.
     * \return error
     */

    /**
     * \fn time_type decrease_step(const time_type dt, const value_type error, const int error_order)
     * \brief Returns a decreased step size based on the given error and order
     *
     * Calculates a new smaller step size based on the given error and its order.
     *
     * \param dt The old step size.
     * \param error The computed error estimate.
     * \param error_order The error order of the stepper.
     * \return dt_new The new, reduced step size.
     */

    /**
     * \fn time_type increase_step(const time_type dt, const value_type error, const int error_order)
     * \brief Returns an increased step size based on the given error and order.
     *
     * Calculates a new bigger step size based on the given error and its order. If max_dt != 0, the
     * new step size is limited to max_dt.
     *
     * \param dt The old step size.
     * \param error The computed error estimate.
     * \param error_order The order of the stepper.
     * \return dt_new The new, increased step size.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED


// File: vector1.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    // Define dense vector of doubles with 10 elements all set to 0.0.
    dense_vector<double>   v(10, 0.0);

    // Set element 7 to 3.0.
    v[7]= 3.0;

    std::cout << "v is " << v << "\n";
    return 0;
}


/* Boost interval/policies.hpp template implementation file
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_INTERVAL_POLICIES_HPP
#define BOOST_NUMERIC_INTERVAL_POLICIES_HPP

#include <boost/numeric/interval/interval.hpp>

namespace boost {
namespace numeric {
namespace interval_lib {

/*
 * policies class
 */

template<class Rounding, class Checking>
struct policies
{
  typedef Rounding rounding;
  typedef Checking checking;
};

/*
 * policies switching classes
 */

template<class OldInterval, class NewRounding>
class change_rounding
{
  typedef typename OldInterval::base_type T;
  typedef typename OldInterval::traits_type p;
  typedef typename p::checking checking;
public:
  typedef interval<T, policies<NewRounding, checking> > type;
};

template<class OldInterval, class NewChecking>
class change_checking
{
  typedef typename OldInterval::base_type T;
  typedef typename OldInterval::traits_type p;
  typedef typename p::rounding rounding;
public:
  typedef interval<T, policies<rounding, NewChecking> > type;
};

/*
 * Protect / unprotect: control whether the rounding mode is set/reset
 * at each operation, rather than once and for all.
 */

template<class OldInterval>
class unprotect
{
  typedef typename OldInterval::base_type T;
  typedef typename OldInterval::traits_type p;
  typedef typename p::rounding r;
  typedef typename r::unprotected_rounding newRounding;
public:
  typedef typename change_rounding<OldInterval, newRounding>::type type;
};

} // namespace interval_lib
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_POLICIES_HPP


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

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

#include "gtest/gtest.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/sfm/estimators/estimate_triangulation.h"

namespace theia {

namespace {

using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

void CreateObservations(
    const int num_observations,
    const int num_outliers,
    std::vector<Matrix3x4d>* projection_matrices,
    std::vector<Vector2d>* features) {
  const Vector4d point3d(0, 0, 8, 1.0);
  projection_matrices->resize(num_observations + num_outliers);
  features->resize(num_observations + num_outliers);

  // Initialize a random rotation and position within a 2x2x2 box around the
  // origin.
  for (int i = 0; i < num_observations; i++) {
    const Matrix3d rotation = Matrix3d::Identity() + 0.1 * Matrix3d::Random();
    const Vector3d position =
        Vector3d::Random() + Vector3d(i / 2.0, i / 2.0, i / 2.0);
    const Vector3d translation = -rotation * position;
    projection_matrices->at(i) << rotation, translation;
    features->at(i) = (projection_matrices->at(i) * point3d).hnormalized();
  }

  for (int i = 0; i < num_outliers; i++) {
    projection_matrices->at(i) = Matrix3x4d::Random();
    features->at(i) = (projection_matrices->at(i) * point3d).hnormalized();
  }
}

}  // namespace

TEST(EstimateTriangulation, InsufficientObservations) {
  std::vector<Matrix3x4d> projection_matrices;
  std::vector<Vector2d> features;
  CreateObservations(1, 0, &projection_matrices, &features);

  RansacParameters params;
  params.error_thresh = 1.0 * 1.0;
  RansacSummary summary;
  Vector4d triangulated_point;
  EXPECT_FALSE(EstimateTriangulation(params,
                                     projection_matrices,
                                     features,
                                     &triangulated_point,
                                     &summary));
}

TEST(EstimateTriangulation, TwoViews) {
  std::vector<Matrix3x4d> projection_matrices;
  std::vector<Vector2d> features;
  CreateObservations(2, 0, &projection_matrices, &features);

  RansacParameters params;
  params.error_thresh = 1.0 * 1.0;
  RansacSummary summary;
  Vector4d triangulated_point;
  EXPECT_TRUE(EstimateTriangulation(params,
                                    projection_matrices,
                                    features,
                                    &triangulated_point,
                                    &summary));

  const Vector4d gt_point(0, 0, 8, 1);
  static const double kTolerance = 1e-6;
  EXPECT_LT((triangulated_point.hnormalized() - gt_point.hnormalized()).norm(),
            kTolerance);
  EXPECT_EQ(summary.inliers.size(), 2);
}

TEST(EstimateTriangulation, WithOutliers) {
  std::vector<Matrix3x4d> projection_matrices;
  std::vector<Vector2d> features;
  CreateObservations(10, 2, &projection_matrices, &features);

  RansacParameters params;
  params.error_thresh = 1.0 * 1.0;
  RansacSummary summary;
  Vector4d triangulated_point;
  EXPECT_TRUE(EstimateTriangulation(params,
                                    projection_matrices,
                                    features,
                                    &triangulated_point,
                                    &summary));

  const Vector4d gt_point(0, 0, 8, 1);
  static const double kTolerance = 1e-6;
  EXPECT_LT((triangulated_point.hnormalized() - gt_point.hnormalized()).norm(),
            kTolerance);
  EXPECT_GE(summary.inliers.size(), 6);
}

}  // namespace theia


#ifndef F_M_HPP
#define F_M_HPP

#include <armadillo>
using namespace arma ;

irowvec f(irowvec x) ;
void g() ;

irowvec f(irowvec x)
{
  irowvec y ;
  y = x+2 ;
  return y ;
}

void g()
{
  irowvec x, y ;
  sword _x [] = {1, 2, 3} ;
  x = irowvec(_x, 3, false) ;
  y = f(x) ;
}
#endif


// Copyright 2005 Alexander Nasonov.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/mpl/at.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/promote.hpp>

namespace mpl = boost::mpl;

int main()
{
    using namespace mpl::placeholders;

    typedef mpl::vector< char
                       , signed char          // 1
                       , unsigned char
                       , short int const      // 3
                       , unsigned short int
                       , int volatile         // 5
                       , unsigned int         // 6
                       , long                 // 7
                       , unsigned long        // 8
                       , float const          // 9
                       > types;

    typedef mpl::transform< types
                          , mpl::lambda< boost::promote<_> >::type
                          >::type promoted;

    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,1>::type, int           >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,3>::type, int const     >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,5>::type, int volatile  >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,6>::type, unsigned int  >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,7>::type, long          >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,8>::type, unsigned long >::value ));
    BOOST_STATIC_ASSERT(( ::boost::is_same< mpl::at_c<promoted,9>::type, double const  >::value ));

    return 0;
}


/*
 * network_state.hpp
 *
 *  Created on: Apr 25, 2010
 *      Author: stewie
 */

#ifndef NETWORK_STATE_HPP_
#define NETWORK_STATE_HPP_

#include <cstddef>

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

namespace bn {

/**
 * Type that represents the state of a network.
 */
typedef boost::dynamic_bitset<> State;

class BitsetHasher {
private:
	template<class BlockType> class BlockHasher {
	private:
		BlockType& acc;

	public:

		BlockHasher(BlockType& acc) :
			acc(acc) {
		}

		void operator()(const BlockType b) const {
			acc ^= b;
		}
	};

public:

	template<class BlockType> std::size_t operator()(
			const boost::dynamic_bitset<BlockType>& b) const {
		BlockType h = 0;
		boost::to_block_range(
				b,
				boost::make_function_output_iterator(BlockHasher<BlockType> (h)));
		return h;
	}
};

static const BitsetHasher bitset_hash = BitsetHasher();

} // namespace bn

#endif /* NETWORK_STATE_HPP_ */


//==================================================================================================
/*!

  Copyright 2015 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/scalar/iround.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" iround real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::iround;
  using r_t = decltype(iround(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(iround(bs::Inf<T>()), bs::Inf<r_t>());
  STF_EQUAL(iround(bs::Minf<T>()), bs::Minf<r_t>());
  STF_EQUAL(iround(bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_EQUAL(iround(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(iround(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(iround(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for floating_

STF_CASE_TPL (" iround unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::iround;
  using r_t = decltype(iround(T()));

  // return type conformity test
   STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  STF_EQUAL(iround(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(iround(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" iround signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::iround;
  using r_t = decltype(iround(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  STF_EQUAL(iround(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(iround(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(iround(bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP

#include <cstddef>

#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/exception.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/tag_cast.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/strategies/centroid.hpp>
#include <boost/geometry/strategies/concepts/centroid_concept.hpp>
#include <boost/geometry/views/closeable_view.hpp>

#include <boost/geometry/util/for_each_coordinate.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#if ! defined(BOOST_GEOMETRY_CENTROID_NO_THROW)

/*!
\brief Centroid Exception
\ingroup centroid
\details The centroid_exception is thrown if the free centroid function is called with
    geometries for which the centroid cannot be calculated. For example: a linestring
    without points, a polygon without points, an empty multi-geometry.
\qbk{
[heading See also]
\* [link geometry.reference.algorithms.centroid the centroid function]
}

 */
class centroid_exception : public geometry::exception
{
public:

    inline centroid_exception() {}

    virtual char const* what() const throw()
    {
        return "Boost.Geometry Centroid calculation exception";
    }
};

#endif

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace centroid
{

struct centroid_point
{
    template<typename Point, typename PointCentroid, typename Strategy>
    static inline void apply(Point const& point, PointCentroid& centroid,
            Strategy const&)
    {
        geometry::convert(point, centroid);
    }
};

template
<
    typename Indexed,
    typename Point,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct centroid_indexed_calculator
{
    typedef typename select_coordinate_type
        <
            Indexed, Point
        >::type coordinate_type;
    static inline void apply(Indexed const& indexed, Point& centroid)
    {
        coordinate_type const c1 = get<min_corner, Dimension>(indexed);
        coordinate_type const c2 = get<max_corner, Dimension>(indexed);
        coordinate_type m = c1 + c2;
        coordinate_type const two = 2;
        m /= two;

        set<Dimension>(centroid, m);

        centroid_indexed_calculator
            <
                Indexed, Point,
                Dimension + 1, DimensionCount
            >::apply(indexed, centroid);
    }
};

template<typename Indexed, typename Point, std::size_t DimensionCount>
struct centroid_indexed_calculator<Indexed, Point, DimensionCount, DimensionCount>
{
    static inline void apply(Indexed const& , Point& )
    {
    }
};

struct centroid_indexed
{
    template<typename Indexed, typename Point, typename Strategy>
    static inline void apply(Indexed const& indexed, Point& centroid,
            Strategy const&)
    {
        centroid_indexed_calculator
            <
                Indexed, Point,
                0, dimension<Indexed>::type::value
            >::apply(indexed, centroid);
    }
};

// There is one thing where centroid is different from e.g. within.
// If the ring has only one point, it might make sense that
// that point is the centroid.
template<typename Point, typename Range>
inline bool range_ok(Range const& range, Point& centroid)
{
    std::size_t const n = boost::size(range);
    if (n > 1)
    {
        return true;
    }
    else if (n <= 0)
    {
#if ! defined(BOOST_GEOMETRY_CENTROID_NO_THROW)
        throw centroid_exception();
#endif
        return false;
    }
    else // if (n == 1)
    {
        // Take over the first point in a "coordinate neutral way"
        geometry::convert(*boost::begin(range), centroid);
        return false;
    }
    return true;
}

/*!
    \brief Calculate the centroid of a ring.
*/
template <closure_selector Closure>
struct centroid_range_state
{
    template<typename Ring, typename Strategy>
    static inline void apply(Ring const& ring,
            Strategy const& strategy, typename Strategy::state_type& state)
    {
        typedef typename closeable_view<Ring const, Closure>::type view_type;

        typedef typename boost::range_iterator<view_type const>::type iterator_type;

        view_type view(ring);
        iterator_type it = boost::begin(view);
        iterator_type end = boost::end(view);

        for (iterator_type previous = it++;
            it != end;
            ++previous, ++it)
        {
            strategy.apply(*previous, *it, state);
        }
    }
};

template <closure_selector Closure>
struct centroid_range
{
    template<typename Range, typename Point, typename Strategy>
    static inline void apply(Range const& range, Point& centroid,
            Strategy const& strategy)
    {
        if (range_ok(range, centroid))
        {
            typename Strategy::state_type state;
            centroid_range_state<Closure>::apply(range, strategy, state);
            strategy.result(state, centroid);
        }
    }
};

/*!
    \brief Centroid of a polygon.
    \note Because outer ring is clockwise, inners are counter clockwise,
    triangle approach is OK and works for polygons with rings.
*/
struct centroid_polygon_state
{
    template<typename Polygon, typename Strategy>
    static inline void apply(Polygon const& poly,
            Strategy const& strategy, typename Strategy::state_type& state)
    {
        typedef typename ring_type<Polygon>::type ring_type;
        typedef centroid_range_state<geometry::closure<ring_type>::value> per_ring;

        per_ring::apply(exterior_ring(poly), strategy, state);

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(poly);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_ring::apply(*it, strategy, state);
        }
    }
};

struct centroid_polygon
{
    template<typename Polygon, typename Point, typename Strategy>
    static inline void apply(Polygon const& poly, Point& centroid,
            Strategy const& strategy)
    {
        if (range_ok(exterior_ring(poly), centroid))
        {
            typename Strategy::state_type state;
            centroid_polygon_state::apply(poly, strategy, state);
            strategy.result(state, centroid);
        }
    }
};

}} // namespace detail::centroid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct centroid: not_implemented<Tag>
{};

template <typename Geometry>
struct centroid<Geometry, point_tag>
    : detail::centroid::centroid_point
{};

template <typename Box>
struct centroid<Box, box_tag>
    : detail::centroid::centroid_indexed
{};

template <typename Segment>
struct centroid<Segment, segment_tag>
    : detail::centroid::centroid_indexed
{};

template <typename Ring>
struct centroid<Ring, ring_tag>
    : detail::centroid::centroid_range<geometry::closure<Ring>::value>
{};

template <typename Linestring>
struct centroid<Linestring, linestring_tag>
    : detail::centroid::centroid_range<closed>
 {};

template <typename Polygon>
struct centroid<Polygon, polygon_tag>
    : detail::centroid::centroid_polygon
 {};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc{centroid} \brief_strategy
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_strategy_reasons
\tparam Geometry \tparam_geometry
\tparam Point \tparam_point
\tparam Strategy \tparam_strategy{Centroid}
\param geometry \param_geometry
\param c \param_point \param_set{centroid}
\param strategy \param_strategy{centroid}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{[include reference/algorithms/centroid_strategies.qbk]}
}

*/
template<typename Geometry, typename Point, typename Strategy>
inline void centroid(Geometry const& geometry, Point& c,
        Strategy const& strategy)
{
    //BOOST_CONCEPT_ASSERT( (geometry::concept::CentroidStrategy<Strategy>) );

    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    typedef typename point_type<Geometry>::type point_type;

    // Call dispatch apply method. That one returns true if centroid
    // should be taken from state.
    dispatch::centroid<Geometry>::apply(geometry, c, strategy);
}

/*!
\brief \brief_calc{centroid}
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_default_strategy
\tparam Geometry \tparam_geometry
\tparam Point \tparam_point
\param geometry \param_geometry
\param c The calculated centroid will be assigned to this point reference

\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{
[heading Example]
[centroid]
[centroid_output]
}
 */
template<typename Geometry, typename Point>
inline void centroid(Geometry const& geometry, Point& c)
{
    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    typedef typename strategy::centroid::services::default_strategy
        <
            typename cs_tag<Geometry>::type,
            typename tag_cast
                <
                    typename tag<Geometry>::type,
                    pointlike_tag,
                    linear_tag,
                    areal_tag
                >::type,
            dimension<Geometry>::type::value,
            Point,
            Geometry
        >::type strategy_type;

    centroid(geometry, c, strategy_type());
}

/*!
\brief \brief_calc{centroid}
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_return{centroid}.
\tparam Point \tparam_point
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{centroid}

\qbk{[include reference/algorithms/centroid.qbk]}
 */
template<typename Point, typename Geometry>
inline Point return_centroid(Geometry const& geometry)
{
    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    Point c;
    centroid(geometry, c);
    return c;
}

/*!
\brief \brief_calc{centroid} \brief_strategy
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_return{centroid}. \details_strategy_reasons
\tparam Point \tparam_point
\tparam Geometry \tparam_geometry
\tparam Strategy \tparam_strategy{centroid}
\param geometry \param_geometry
\param strategy \param_strategy{centroid}
\return \return_calc{centroid}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{[include reference/algorithms/centroid_strategies.qbk]}
 */
template<typename Point, typename Geometry, typename Strategy>
inline Point return_centroid(Geometry const& geometry, Strategy const& strategy)
{
    //BOOST_CONCEPT_ASSERT( (geometry::concept::CentroidStrategy<Strategy>) );

    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    Point c;
    centroid(geometry, c, strategy);
    return c;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP


/*
 * gps_time_printer.hpp
 * Copyright (C) 2012 Danny Havenith
 *
 * 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)
 *
 *  Created on: Jan 28, 2012
 *      Author: danny
 */

#ifndef GPS_TIME_PRINTER_HPP_
#define GPS_TIME_PRINTER_HPP_

#include "bytes_to_numbers.hpp"
#include "messages.hpp"

#include <iostream>
#include <iomanip>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <utility>

/**
 * This class prints the contents of gps time channel packets.
 * The packets are printed, one per line in the following format:
 * <last seen timestamp>\t<gps timestamp>\t<average timestamp clockticks per second since previous gps timestamp>
 * Additionally, if the timestamps-per-gps-second average is lower than 50 for more than 50 consecutive gps timestamps this
 * class will output the file name to another output stream (normally std::cerr). The file name will be sent only once.
 */
struct gps_timestamp_printer : public rtlogs::messages_definition
{
    gps_timestamp_printer( std::ostream &out, std::ostream &error, std::string filename)
        :out( out), error( error), filename(std::move(filename)), last_timestamp(0), last_gps_timestamp(0), last_timestamp_at_gps(0), slow_clock_count(0) {};

    /// do nothing with most messages.
    void handle( ...)
    {
    };

    /// remember the value of the last timestamp encountered.
    template< typename iterator>
    void handle( timestamp, iterator begin, iterator end)
    {
        ++begin;
        unsigned long result = *begin++;
        result = (result << 8) + * begin++;
        result = (result << 8) + * begin++;
        last_timestamp = result;
    }

    /// output the gps time contents to output stream, calculating and printing average clock ticks.
    template< typename iterator>
    void handle( gps_time_storage, iterator begin, iterator end)
    {
        using namespace boost::posix_time;
        using namespace boost::gregorian;
        using namespace bytes_to_numbers;

        ++begin; // skip header

        // extract gps timestamp (4-byte, big-endian, unsigned integer)
        const auto gps_timestamp = get_big_endian<uint32_t>( begin);
        std::advance( begin, 4);
        // gps_timestamp now contains the number of milliseconds since 00:00:00 on Sunday.

        static const date start( 2012, Jan, 1); //start with a known Sunday, 00:00:00.
        ptime timeval( start, milliseconds(gps_timestamp)); // calculate a day/time given the known Sunday offset.

        // calculate the effective clock rate of the logger clock.
        unsigned long gps_interval = gps_timestamp - last_gps_timestamp;
        unsigned long logger_interval = last_timestamp - last_timestamp_at_gps;
        double rate = (1000.0 * logger_interval) / gps_interval;

        // slow_clock_count <0 means we're not interested anymore in slow clocks
        if (slow_clock_count >= 0)
        {
            // we're interested, count the number of consecutive slow clocks
            if (rate < slow_clock_treshold)
            {
                if (++slow_clock_count > slow_clock_count_trigger)
                {
                    // print if we've reached a high enough dose of slow clocks.
                    error << filename << '\n';
                    slow_clock_count = -1;
                }
            }
            else
            {
                slow_clock_count = 0;
            }
        }

        // report to output.
        out << last_timestamp << '\t' << timeval.date().day_of_week() << ' ' << timeval.time_of_day() << '\t' << std::setprecision(5) << rate << '\n';

        last_gps_timestamp = gps_timestamp;
        last_timestamp_at_gps = last_timestamp;
    }

private:
    std::ostream    &out;
    std::ostream    &error;
    const std::string filename;

    ///
    int             slow_clock_count;
    unsigned long   last_timestamp;
    unsigned long   last_gps_timestamp;
    unsigned long   last_timestamp_at_gps;

    /// what ticks/s value is considered a slow clock
    static const int slow_clock_treshold = 50;

    /// how many slow clocks before we raise an alarm.
    static const int slow_clock_count_trigger = 10;
};

#endif /* GPS_TIME_PRINTER_HPP_ */


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2018-2020.
// Modifications copyright (c) 2018-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_VECTOR_CONCEPT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_VECTOR_CONCEPT_HPP

#include <type_traits>

#include <boost/concept_check.hpp>
#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/extensions/algebra/core/access.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_dimension.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_type.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_system.hpp>

namespace boost { namespace geometry { namespace concepts {

template <typename Geometry>
class Vector
{
#ifndef DOXYGEN_NO_CONCEPT_MEMBERS

    typedef typename coordinate_type<Geometry>::type ctype;
    typedef typename coordinate_system<Geometry>::type csystem;

    enum { ccount = dimension<Geometry>::value };

    template <typename V, std::size_t Dimension, std::size_t DimensionCount>
    struct dimension_checker
    {
        static void apply()
        {
            V* v = 0;
            geometry::set<Dimension>(*v, geometry::get<Dimension>(*v));
            dimension_checker<V, Dimension+1, DimensionCount>::apply();
        }
    };

    template <typename V, std::size_t DimensionCount>
    struct dimension_checker<V, DimensionCount, DimensionCount>
    {
        static void apply() {}
    };

public:

    /// BCCL macro to apply the Vector concept
    BOOST_CONCEPT_USAGE(Vector)
    {
        static const bool cs_check = std::is_same<csystem, cs::cartesian>::value;
        BOOST_GEOMETRY_STATIC_ASSERT(cs_check,
            "Not implemented for this coordinate system.",
            csystem);

        dimension_checker<Geometry, 0, ccount>::apply();
    }
#endif
};

template <typename Geometry>
class ConstVector
{
#ifndef DOXYGEN_NO_CONCEPT_MEMBERS

    typedef typename coordinate_type<Geometry>::type ctype;
    typedef typename coordinate_system<Geometry>::type csystem;

    enum { ccount = dimension<Geometry>::value };

    template <typename V, std::size_t Dimension, std::size_t DimensionCount>
    struct dimension_checker
    {
        static void apply()
        {
            const V* v = 0;
            ctype coord(geometry::get<Dimension>(*v));
            boost::ignore_unused(coord);
            dimension_checker<V, Dimension+1, DimensionCount>::apply();
        }
    };

    template <typename V, std::size_t DimensionCount>
    struct dimension_checker<V, DimensionCount, DimensionCount>
    {
        static void apply() {}
    };

public:

    /// BCCL macro to apply the ConstVector concept
    BOOST_CONCEPT_USAGE(ConstVector)
    {
        static const bool cs_check = std::is_same<csystem, cs::cartesian>::value;
        BOOST_GEOMETRY_STATIC_ASSERT(cs_check,
            "Not implemented for this coordinate system.",
            csystem);

        dimension_checker<Geometry, 0, ccount>::apply();
    }
#endif
};

}}} // namespace boost::geometry::concepts

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_VECTOR_CONCEPT_HPP


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

// Standard includes
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>

// Third party includes
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>

// VOTCA includes
#include <votca/tools/average.h>
#include <votca/tools/tokenizer.h>

// Local VOTCA includes
#include <votca/csg/cgengine.h>
#include <votca/csg/csgapplication.h>

using namespace std;
using namespace votca::csg;

class CGOrderParam : public CsgApplication {
 public:
  string ProgramName() override { return "sphericalorder"; }

  void HelpText(ostream &out) override {

    out << "!! EXPERIMENTAL !! Calculate spherical order parameter.\n"
           " Needs non-spherical beads in mapping.\n\n";
  }

  void Initialize() override {
    CsgApplication::Initialize();
    AddProgramOptions()(
        "filter",
        boost::program_options::value<string>(&filter_)->default_value("*"),
        "filter molecule names")(
        "radialcut",
        boost::program_options::value<double>(&radialcutoff_)
            ->default_value(0.0),
        "radial cutoff: distance from center where bead is considered")(
        "minrad",
        boost::program_options::value<double>(&minrad_)->default_value(0.0),
        "minimal distance a parcle has to be apart from center to be "
        "considerd")(
        "refmol",
        boost::program_options::value<string>(&refmol_)->default_value(""),
        "Reference molecule")(
        "rbinw",
        boost::program_options::value<double>(&rbinw_)->default_value(0),
        "Do multiple r_bins multiple histograms");
  }

  bool EvaluateOptions() override {
    CsgApplication::EvaluateOptions();
    // CheckRequired("radialcut");
    return true;
  }

  bool DoTrajectory() override { return true; }
  bool DoMapping() override { return true; }

  void BeginEvaluate(Topology *top, Topology *) override {

    string filter;

    filter = OptionsMap()["filter"].as<string>();

    minrad_ = 0;

    radialcutoff_ = OptionsMap()["radialcut"].as<double>();
    minrad_ = OptionsMap()["minrad"].as<double>();
    refmol_ = OptionsMap()["refmol"].as<string>();
    rbinw_ = OptionsMap()["rbinw"].as<double>();

    if (rbinw_ == 0 && radialcutoff_ <= 0) {
      throw runtime_error(" radialcut_ > 0 has to be specified");
    }

    setFilter(filter);

    file_u_.open("hist_u.xvg");
    if (!file_u_) {
      throw runtime_error("cannot open hist_u.xvg for output");
    }
    file_v_.open("hist_v.xvg");
    if (!file_v_) {
      throw runtime_error("cannot open hist_v.xvg for output");
    }
    file_w_.open("hist_w.xvg");
    if (!file_w_) {
      throw runtime_error("cannot open hist_w.xvg for output");
    }

    n_ = 0;

    Eigen::Matrix3d box = top->getBox();
    Eigen::Vector3d a = box.col(0);

    if (refmol_ == "") {

      ref_ = box.rowwise().sum() / 2;

      cout << "Refernce is center of box " << ref_ << endl;
    }

    boxl = a.norm() / 2;
    if (rbinw_ > 0) {
      rbins_ = (votca::Index)(boxl / rbinw_) + 1;
      cout << "radial bins " << rbins_ << endl;
    } else {
      rbins_ = 1;
      cout << "considering atoms between " << minrad_ << " and "
           << radialcutoff_ << endl;
    }

    nbin_ = 100;
    hist_u_ = Eigen::MatrixXd::Zero(rbins_, nbin_);
    hist_v_ = Eigen::MatrixXd::Zero(rbins_, nbin_);
    hist_w_ = Eigen::MatrixXd::Zero(rbins_, nbin_);
    nmol_ = Eigen::VectorXi::Zero(rbins_);
  }

  void EndEvaluate() override {

    cout << "Average number of molecules within cutoff " << endl;
    for (votca::Index i = 0; i < rbins_; i++) {
      cout << (double)i * rbinw_ << " " << (double)nmol_[i] / (double)n_
           << endl;
    }

    double exp_value = 1.0 / (double)nbin_;
    double orderparam = 0;

    for (votca::Index n = 0; n < nbin_; n++) {
      hist_u_(0, n) /= (double)nmol_[0];  // normalize to numberframes and avg.
                                          // number of molecules
      hist_v_(0, n) /= (double)nmol_[0];
      hist_w_(0, n) /= (double)nmol_[0];

      file_u_ << (double)n * 2 / double(nbin_ - 1) << " " << hist_u_(0, n)
              << endl;
      file_v_ << (double)n * 2 / double(nbin_ - 1) << " " << hist_v_(0, n)
              << endl;
      file_w_ << (double)n * 2 / double(nbin_ - 1) << " " << hist_w_(0, n)
              << endl;

      orderparam += (hist_u_(0, n) - exp_value) * (hist_u_(0, n) - exp_value);
    }

    orderparam = sqrt(orderparam / (double)nbin_);

    cout << "Orderparam " << radialcutoff_ << " " << orderparam << endl;

    file_u_.close();
    file_v_.close();
    file_w_.close();
  };

  void EvalConfiguration(Topology *conf, Topology * = nullptr) override {

    Eigen::Vector3d eR;
    votca::Index nu, nv, nw;
    Eigen::Vector3d u, v, w;

    if (refmol_ != "") {
      for (const auto &bead : conf->Beads()) {
        if (votca::tools::wildcmp(refmol_, bead.getName())) {
          ref_ = bead.getPos();
        }
      }
    }

    for (const auto &bead : conf->Beads()) {
      if (!votca::tools::wildcmp(filter_, bead.getName())) {
        continue;
      }
      if (votca::tools::wildcmp(refmol_, bead.getName())) {
        continue;
      }

      eR = bead.getPos() - ref_;
      if ((eR.norm() < radialcutoff_ && eR.norm() > minrad_) || rbins_ != 1) {
        // cout << eR << endl;
        votca::Index rb = 0;
        if (rbinw_ > 0) {
          rb = (votca::Index)((eR.norm()) / boxl * (double)rbins_);
        }
        if (rb >= rbins_) {
          continue;
        }

        eR.normalize();
        u = bead.getU();
        v = bead.getV();
        w = bead.getW();
        u.normalize();
        v.normalize();
        w.normalize();

        nu = (votca::Index)((eR.dot(u) + 1) / 2) * nbin_;
        nv = (votca::Index)((eR.dot(v) + 1) / 2) * nbin_;
        nw = (votca::Index)((eR.dot(w) + 1) / 2) * nbin_;

        hist_u_(rb, nu) += 1;
        hist_v_(rb, nv) += 1;
        hist_w_(rb, nw) += 1;
        nmol_[rb]++;
      }
    }

    n_++;
  }

  void setOut(const string &filename) { filename_ = filename; }

  void setFilter(const string &filter) { filter_ = filter; }

 protected:
  ofstream file_;
  string filename_;
  votca::Index n_;
  Eigen::Vector3d ref_;
  ofstream file_u_;
  ofstream file_v_;
  ofstream file_w_;
  Eigen::MatrixXd hist_u_;
  Eigen::MatrixXd hist_v_;
  Eigen::MatrixXd hist_w_;
  votca::Index nbin_;
  Eigen::VectorXi nmol_;
  double radialcutoff_;
  double minrad_;
  votca::Index rbins_;
  double rbinw_;
  double boxl;

  string filter_;
  string refmol_;
};

int main(int argc, char **argv) {
  CGOrderParam app;
  return app.Exec(argc, argv);
}