/**
 * Copyright (c) 2015
 * Jakob van Santen <jvansanten@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id$
 *
 * @file Axis.cxx
 * @version $LastChangedRevision$
 * @date $Date$
 * @author Jakob van Santen
 */

#include "clsim/tabulator/Axis.h"
#include "clsim/I3CLSimHelperToFloatString.h"

#include <boost/foreach.hpp>

#include <cmath>

namespace clsim {

namespace tabulator {

Axis::Axis(double min, double max, unsigned n_bins)
    : min_(min), max_(max), n_bins_(n_bins)
{}

Axis::~Axis()
{}

std::string
Axis::GetIndexCode(const std::string &var) const {

	std::ostringstream ss;
	double scale = n_bins_/(InverseTransform(max_)-InverseTransform(min_));
	double offset = scale*InverseTransform(min_);

	using I3CLSimHelper::ToFloatString;
	ss << "(clamp(convert_int_sat_rtn("
	    <<ToFloatString(scale)<<"*"<<GetInverseTransformCode(var)
	    <<" - "<<ToFloatString(offset)
	    <<"), -1, "<<(n_bins_)<<")+1)";

	return ss.str();
}

std::vector<double>
Axis::GetBinEdges() const
{
	std::vector<double> edges(GetNBins()+1);

	double imin = InverseTransform(GetMin());
	double imax = InverseTransform(GetMax());
	double istep = (imax-imin)/GetNBins();
	for (unsigned i = 0; i <= GetNBins(); i++)
		edges[i] = Transform(imin + i*istep);

	return edges;
}

double
Axis::GetBinEdge(unsigned i) const
{
	double imin = InverseTransform(GetMin());
	double imax = InverseTransform(GetMax());
	double istep = (imax-imin)/GetNBins();
	return Transform(imin + i*istep);
}

LinearAxis::LinearAxis(double min, double max, unsigned n_bins)
    : Axis(min, max, n_bins)
{}

LinearAxis::~LinearAxis()
{}

double
LinearAxis::Transform(double value) const
{
	return value;
}

double
LinearAxis::InverseTransform(double value) const
{
	return value;
}

std::string
LinearAxis::GetTransformCode(const std::string &var) const
{
	return var;
}

std::string
LinearAxis::GetInverseTransformCode(const std::string &var) const
{
	return var;
}

PowerAxis::PowerAxis(double min, double max, unsigned n_bins, unsigned power)
    : Axis(min, max, n_bins), power_(power)
{}

PowerAxis::~PowerAxis()
{}

double
PowerAxis::Transform(double value) const
{
	return std::pow(value, power_);
}

double
PowerAxis::InverseTransform(double value) const
{
	return std::pow(value, 1./power_);
}

std::string
PowerAxis::GetTransformCode(const std::string &var) const
{
	std::ostringstream ss;
	if (power_ == 0) {
		ss << 1;
	} else if (power_ < 5) {
		ss << var;
		for (unsigned i = 0; i+1 < power_; i++)
			ss << "*" << var;
	} else {
		ss << "pow("<<var<<", "<<power_<< ")";
	}
	return ss.str();
}

std::string
PowerAxis::GetInverseTransformCode(const std::string &var) const
{
	std::ostringstream ss;
	switch (power_) {
		case 0:
			ss << 1;
			break;
		case 1:
			ss << var;
			break;
		case 2:
			ss << "sqrt(" << var << ")";
			break;
		case 3:
			ss << "cbrt(" << var << ")";
			break;
		default:
			ss << "pow(" << var << "," << I3CLSimHelper::ToFloatString(1./power_) << ")";
	}
	return ss.str();
}

}

}


//
//  torus.cpp
//
//
//  Created by Protoss Probe on 2017/04/09.
//  Copyright © 2016-2017年 probe. All rights reserved.
//

#include "poly_run.hpp"
#include "poly_grav.hpp"
#include <boost/array.hpp>
#include <cmath>
#include <iostream>
#include <string>

using namespace std;

class Particle {
  public:
    Particle() = default;
    ~Particle() = default;
    Particle(pos position) : r(position[0]), z(position[1]) {}
    Particle(state_type5 state)
        : r(state[0]), z(state[1]), r_dot(state[2]), z_dot(state[3]),
          lam_dot(state[4]) {}
    Particle(state_type5 state, double time)
        : r(state[0]), z(state[1]), r_dot(state[2]), z_dot(state[3]),
          lam_dot(state[4]), t(time) {}

    double r = 0.0, z = 0.0, r_dot = 0.0, z_dot = 0.0, lam_dot = 0.0, t = 0.0;

    state_type5 convert2state() const { return {r, z, r_dot, z_dot, lam_dot}; }
    pos convert2pos() const { return {r, z}; }
    vel convert2vel() const { return {r_dot, z_dot, lam_dot}; }
};

int main(int argc, char *argv[]) {
    string filename(argv[1]);

    PolyGrav poly(filename);

    poly.init();
    cout << poly.vert_n << '\t' << poly.face_n << '\t' << poly.edge_n << endl;
    poly.principle_axes();
    poly.export_3d_txt("assets/" + filename + "_prin.txt", 'd');

    vec3 pos;
    double x = 1.1;
    // double real = -1 / x;
    // poly.co = -1 / (4. / 3. * M_PI) * 0.5;

    const clock_t start = clock();
    pos = {{x, 0, 0}};
    // for (double i = 1.1; i < 3.0; i = i + 0.1) {
    //     pos[0] = i;
    //     cout << endl;
    //     cout << setprecision(12) << -1 / i << endl;
    //     cout << poly.potential(pos) << endl;
    //     cout << (-1 / i - poly.potential(pos)) / (-1 / i) << endl;
    // }
    double value = poly.potential(pos);
    cout << endl
         << "Cpu Time: "
         << static_cast<double>(clock() - start) / CLOCKS_PER_SEC << endl;
    cout << endl << "------------" << endl;
    // cout << "Real : " << real << endl;
    cout << "Poly Value : " << value << endl;
    // cout << "Ratio : " << real / value << endl;
    cout << "------------" << endl;
}


/* boost random/extreme_value_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: extreme_value_distribution.hpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 */

#ifndef BOOST_RANDOM_EXTREME_VALUE_DISTRIBUTION_HPP
#define BOOST_RANDOM_EXTREME_VALUE_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <istream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

/**
 * The extreme value distribution is a real valued distribution with two
 * parameters a and b.
 *
 * It has \f$\displaystyle p(x) = \frac{1}{b}e^{\frac{a-x}{b} - e^\frac{a-x}{b}}\f$.
 */
template<class RealType = double>
class extreme_value_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef extreme_value_distribution distribution_type;

        /**
         * Constructs a @c param_type from the "a" and "b" parameters
         * of the distribution.
         *
         * Requires: b > 0
         */
        explicit param_type(RealType a_arg = 1.0, RealType b_arg = 1.0)
          : _a(a_arg), _b(b_arg)
        {}

        /** Returns the "a" parameter of the distribtuion. */
        RealType a() const { return _a; }
        /** Returns the "b" parameter of the distribution. */
        RealType b() const { return _b; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._a << ' ' << parm._b; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._a >> std::ws >> parm._b; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._a == rhs._a && lhs._b == rhs._b; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _a;
        RealType _b;
    };

    /**
     * Constructs an @c extreme_value_distribution from its "a" and "b" parameters.
     *
     * Requires: b > 0
     */
    explicit extreme_value_distribution(RealType a_arg = 1.0, RealType b_arg = 1.0)
      : _a(a_arg), _b(b_arg)
    {}
    /** Constructs an @c extreme_value_distribution from its parameters. */
    explicit extreme_value_distribution(const param_type& parm)
      : _a(parm.a()), _b(parm.b())
    {}

    /**
     * Returns a random variate distributed according to the
     * @c extreme_value_distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng) const
    {
        using std::log;
        return _a - log(-log(uniform_01<RealType>()(urng))) * _b;
    }

    /**
     * Returns a random variate distributed accordint to the extreme
     * value distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return extreme_value_distribution(parm)(urng);
    }

    /** Returns the "a" parameter of the distribution. */
    RealType a() const { return _a; }
    /** Returns the "b" parameter of the distribution. */
    RealType b() const { return _b; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return -std::numeric_limits<RealType>::infinity(); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return std::numeric_limits<RealType>::infinity(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_a, _b); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _a = parm.a();
        _b = parm.b();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Writes an @c extreme_value_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, extreme_value_distribution, wd)
    {
        os << wd.param();
        return os;
    }

    /** Reads an @c extreme_value_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, extreme_value_distribution, wd)
    {
        param_type parm;
        if(is >> parm) {
            wd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c extreme_value_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(extreme_value_distribution, lhs, rhs)
    { return lhs._a == rhs._a && lhs._b == rhs._b; }

    /**
     * Returns true if the two instances of @c extreme_value_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(extreme_value_distribution)

private:
    RealType _a;
    RealType _b;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_EXTREME_VALUE_DISTRIBUTION_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <Eigen/Geometry>

#include "Tudat/Astrodynamics/Gravitation/directTidalDissipationAcceleration.h"

namespace tudat
{

namespace gravitation
{

//! Function to compute the acceleration acting on a satellite due to tidal deformation caused by this satellite on host planet.
Eigen::Vector3d computeDirectTidalAccelerationDueToTideOnPlanet(
        const Eigen::Vector6d relativeStateOfBodyExertingTide, const Eigen::Vector3d planetAngularVelocityVector,
        const double currentTidalAccelerationMultiplier, const double timeLag,
        const bool includeDirectRadialComponent )
{
    Eigen::Vector3d relativePosition = relativeStateOfBodyExertingTide.segment( 0, 3 );
    Eigen::Vector3d relativeVelocity = relativeStateOfBodyExertingTide.segment( 3, 3 );

    double distance = relativePosition.norm( );
    double distanceSquared = distance * distance;
    double radialComponentMultiplier = ( includeDirectRadialComponent == true ) ? 1.0 : 0.0;

    return currentTidalAccelerationMultiplier * (
                 radialComponentMultiplier * relativePosition + timeLag * (
                    2.0 * ( relativePosition.dot( relativeVelocity ) * relativePosition / distanceSquared ) +
                    ( relativePosition.cross( planetAngularVelocityVector ) + relativeVelocity ) ) );

}

//! Function to compute the acceleration acting on a satellite due to tidal deformation caused in this satellite by host planet.
Eigen::Vector3d computeDirectTidalAccelerationDueToTideOnSatellite(
        const Eigen::Vector6d relativeStateOfBodyExertingTide,
        const double currentTidalAccelerationMultiplier,
        const double timeLag,const bool includeDirectRadialComponent )
{
    Eigen::Vector3d relativePosition = relativeStateOfBodyExertingTide.segment( 0, 3 );
    Eigen::Vector3d relativeVelocity = relativeStateOfBodyExertingTide.segment( 3, 3 );

    double distance = relativePosition.norm( );
    double distanceSquared = distance * distance;
    double radialComponentMultiplier = ( includeDirectRadialComponent == true ) ? 1.0 : 0.0;

    return currentTidalAccelerationMultiplier * (
                 2.0 * radialComponentMultiplier * relativePosition + timeLag * (
                   7.0 * relativePosition.dot( relativeVelocity ) * relativePosition / distanceSquared ) );

}

//! Function to retrieve all DirectTidalDissipationAcceleration from an AccelerationMap, for specific deformed/deforming bodies
std::vector< std::shared_ptr< DirectTidalDissipationAcceleration > > getTidalDissipationAccelerationModels(
        const basic_astrodynamics::AccelerationMap accelerationModelList, const std::string bodyBeingDeformed,
        const std::vector< std::string >& bodiesCausingDeformation )
{
    // Iterate over all bodies undergoing acceleration
    std::vector< std::shared_ptr< DirectTidalDissipationAcceleration > > selectedDissipationModels;
    for( basic_astrodynamics::AccelerationMap::const_iterator modelIterator1 = accelerationModelList.begin( );
         modelIterator1 != accelerationModelList.end( ); modelIterator1++ )
    {
        std::string bodyUndergoingAcceleration = modelIterator1->first;

        // Iterate over all bodies exerting acceleration
        basic_astrodynamics::SingleBodyAccelerationMap singleBodyAccelerationList = modelIterator1->second;
        for( basic_astrodynamics::SingleBodyAccelerationMap::const_iterator modelIterator2 = singleBodyAccelerationList.begin( );
             modelIterator2 != singleBodyAccelerationList.end( ); modelIterator2++ )
        {
            std::string bodyExertingAcceleration = modelIterator2->first;

            // Iterate over all accelerations being exerted by bodyExertingAcceleration on bodyUndergoingAcceleration
            for( unsigned int i = 0; i < modelIterator2->second.size( ); i++ )
            {
                // Check if acceleration model is due to tidal dissipations
                std::shared_ptr< DirectTidalDissipationAcceleration > currentDissipationAcceleration =
                    std::dynamic_pointer_cast< DirectTidalDissipationAcceleration >( modelIterator2->second.at( i ) );
                if( currentDissipationAcceleration != nullptr )
                {
                    // Check whether model correspionds to input requirements
                    if( currentDissipationAcceleration->getModelTideOnPlanet( ) &&
                            ( bodyExertingAcceleration == bodyBeingDeformed ) )
                    {
                        if( ( std::find( bodiesCausingDeformation.begin( ), bodiesCausingDeformation.end( ),
                                       bodyUndergoingAcceleration ) != bodiesCausingDeformation.end( ) ) ||
                                bodiesCausingDeformation.size( ) == 0 )
                        {
                            selectedDissipationModels.push_back( currentDissipationAcceleration );
                        }
                    }
                    else if( !currentDissipationAcceleration->getModelTideOnPlanet( ) &&
                             ( bodyUndergoingAcceleration == bodyBeingDeformed ) )
                    {
                        if( ( std::find( bodiesCausingDeformation.begin( ), bodiesCausingDeformation.end( ),
                                       bodyExertingAcceleration ) != bodiesCausingDeformation.end( ) ) ||
                                bodiesCausingDeformation.size( ) == 0 )
                        {
                            selectedDissipationModels.push_back( currentDissipationAcceleration );
                        }
                    }
                }
            }
        }
    }
    return selectedDissipationModels;
}

} // namespace gravitation

} // namespace tudat


#include "coord.h"

#include "adler32.h"
#include "composite-imp.h"
#include "float.h"

#include <boost/static_assert.hpp>

using namespace std;

class Coords {
public:
  typedef Coord T1;
  typedef Coord2 T2;
  typedef Coord4 T4;

  static Coord atan2(Coord a, Coord b) { return Coord(float(::atan2(a.toFloat(), b.toFloat()))); }
  static Coord sin(Coord a) { return cfsin(a); };
  static Coord cos(Coord a) { return cfcos(a); };

  static const Coord tPI;
};
const Coord Coords::tPI = COORDPI;

string Coord::rawstr() const {
  return StringPrintf("%08x%08x", (unsigned int)(d >> 32), (unsigned int)d);
}

Coord coordFromRawstr(const string &lhs) {
  if(lhs.size() != 16) {
    dprintf("%s\n", lhs.c_str());
    CHECK(lhs.size() == 16);
  }
  for(int i = 0; i < lhs.size(); i++)
    CHECK(isdigit(lhs[i]) || (lhs[i] >= 'a' && lhs[i] <= 'f'));
  long long dd = 0;
  for(int i = 0; i < 16; i++) {
    dd *= 16;
    if(isdigit(lhs[i]))
      dd += lhs[i] - '0';
    else
      dd += lhs[i] - 'a' + 10;
  }
  CHECK(coordExplicit(dd).rawstr() == lhs);
  return coordExplicit(dd);
}

Float2 Coord2::toFloat() const { return Float2(x.toFloat(), y.toFloat()); }
Coord2::Coord2(const Float2 &rhs) : x(rhs.x), y(rhs.y) { };
string Coord2::rawstr() const {
  return StringPrintf("%s %s", x.rawstr().c_str(), y.rawstr().c_str());
}

Float4 Coord4::toFloat() const { return Float4(sx.toFloat(), sy.toFloat(), ex.toFloat(), ey.toFloat()); }
string Coord4::rawstr() const {
  return StringPrintf("%s %s %s %s", sx.rawstr().c_str(), sy.rawstr().c_str(), ex.rawstr().c_str(), ey.rawstr().c_str());
}

Coord4::Coord4(const Float4 &rhs) : sx(rhs.sx), sy(rhs.sy), ex(rhs.ex), ey(rhs.ey) { }

/*************
 * Computational geometry
 */

Coord len(const Coord2 &in) { return Coord(len(in.toFloat())); };
Coord2 normalize(const Coord2 &in) { return imp_normalize<Coords>(in); };
Coord dot(const Coord2 &lhs, const Coord2 &rhs) { return lhs.x * rhs.x + lhs.y * rhs.y; };

Coord getAngle(const Coord2 &in) { return imp_getAngle<Coords>(in); };
Coord2 makeAngle(const Coord &in) { return imp_makeAngle<Coords>(in); };

int whichSide(const Coord4 &f4, const Coord2 &pta) { return imp_whichSide<Coords>(f4, pta); };

Coord distanceFromLine(const Coord4 &line, const Coord2 &pt) {
  Coord u = ((pt.x - line.sx) * (line.ex - line.sx) + (pt.y - line.sy) * (line.ey - line.sy)) / ((line.ex - line.sx) * (line.ex - line.sx) + (line.ey - line.sy) * (line.ey - line.sy));
  if(u < 0 || u > 1)
    return min(len(pt - Coord2(line.sx, line.sy)), len(pt - Coord2(line.ex, line.ey)));
  Coord2 ipt = Coord2(line.sx, line.sy) + Coord2(line.ex - line.sx, line.ey - line.sy) * u;
  return len(ipt - pt);
}

int inPath(const Coord2 &point, const vector<Coord2> &path) {
  return imp_inPath<Coords>(point, path);
};

bool roughInPath(const Coord2 &point, const vector<Coord2> &path, int goal) {
  int dx[] = {0, 0, 0, 1, -1, 1, 1, -1, -1};
  int dy[] = {0, 1, -1, 0, 0, 1, -1, 1, -1};
  for(int i = 0; i < 9; i++)
    if((bool)inPath(point + Coord2(dx[i], dy[i]) / 65536, path) == goal)
      return true;
  return false;
}

Coord2 getPointIn(const vector<Coord2> &path) {
  // TODO: find a point inside the polygon in a better fashion
  //GetDifferenceHandler CrashHandler(path, path);
  Coord2 pt;
  bool found = false;
  for(int j = 0; j < path.size() && !found; j++) {
    Coord2 pospt = (path[j] + path[(j + 1) % path.size()] + path[(j + 2) % path.size()]) / 3;
    if(inPath(pospt, path)) {
      pt = pospt;
      found = true;
    }
  }
  CHECK(found);
  return pt;
}

bool pathReversed(const vector<Coord2> &path) {
  return imp_pathReversed<Coords>(path);
}

int getPathRelation(const vector<Coord2> &lhs, const vector<Coord2> &rhs) {
  for(int i = 0; i < lhs.size(); i++) {
    int i2 = (i + 1) % lhs.size();
    for(int j = 0; j < rhs.size(); j++) {
      int j2 = (j + 1) % rhs.size();
      if(linelineintersect(Coord4(lhs[i], lhs[i2]), Coord4(rhs[j], rhs[j2])))
        return PR_INTERSECT;
    }
  }
  bool lir = inPath(getPointIn(lhs), rhs);
  bool ril = inPath(getPointIn(rhs), lhs);
  if(!lir && !ril) {
    return PR_SEPARATE;
  } else if(lir && !ril) {
    return PR_RHSENCLOSE;
  } else if(!lir && ril) {
    return PR_LHSENCLOSE;
  } else if(lir && ril && abs(getArea(lhs)) < abs(getArea(rhs))) {
    return PR_RHSENCLOSE;
  } else if(lir && ril && abs(getArea(lhs)) > abs(getArea(rhs))) {
    return PR_LHSENCLOSE;
  } else {
    // dammit, don't send the same two paths! we deny!
    CHECK(0);
  }
}

Coord getArea(const vector<Coord2> &are) { return imp_getArea<Coords>(are); }
Coord2 getCentroid(const vector<Coord2> &are) { return imp_getCentroid<Coords>(are); }
Coord getPerimeter(const vector<Coord2> &are) {
  Coord totperi = 0;
  for(int i = 0; i < are.size(); i++) {
    int j = (i + 1) % are.size();
    totperi += len(are[i] - are[j]);
  }
  return totperi;
}
Coord4 getBoundBox(const vector<Coord2> &are) {
  Coord4 bbox = startCBoundBox();
  for(int i = 0; i < are.size(); i++)
    addToBoundBox(&bbox, are[i]);
  CHECK(bbox.isNormalized());
  return bbox;
}

bool colinear(const Coord4 &line, const Coord2 &pt) {
  Coord koord = distanceFromLine(line, pt);
  return koord < Coord(0.00001f);
}

Coord2 reflect(const Coord2 &incoming, Coord normal) {
  return imp_reflect<Coords>(incoming, normal);
}
Coord reflect(Coord incoming, Coord normal) {
  return imp_reflect<Coords>(incoming, normal);
}

Coord ang_dist(Coord lhs, Coord rhs) {
  lhs = mod(lhs, COORDPI * 2);
  rhs = mod(rhs, COORDPI * 2);
  Coord v = abs(lhs - rhs);
  if(v > COORDPI)
    return COORDPI * 2 - v;
  else
    return v;
}
Coord ang_approach(const Coord &cur, const Coord &goal, const Coord &amount) {
  Coord jcur = mod(cur, COORDPI * 2);
  Coord jgoal = mod(goal, COORDPI * 2);
  if(abs(jcur - jgoal) > abs(jcur - (jgoal - COORDPI * 2))) {
    jgoal -= COORDPI * 2;
  } else if(abs(jcur - jgoal) > abs(jcur - (jgoal + COORDPI * 2))) {
    jgoal += COORDPI * 2;
  }
  CHECK(ang_dist(cur, goal) >= ang_dist(approach(jcur, jgoal, amount), goal));
  return approach(jcur, jgoal, amount);
}

/*************
 * Bounding box
 */

Coord4 startCBoundBox() { return imp_startBoundBox<Coords>(); };

void addToBoundBox(Coord4 *bbox, const Coord4 &rect) { return imp_addToBoundBox<Coords>(bbox, rect); };
void addToBoundBox(Coord4 *bbox, const vector<Coord2> &line) { return imp_addToBoundBox<Coords>(bbox, line); };

void expandBoundBox(Coord4 *bbox, Coord factor) { return imp_expandBoundBox<Coords>(bbox, factor); };

/*************
 * Math
 */

bool linelineintersect(const Coord4 &lhs, const Coord4 &rhs) { return imp_linelineintersect<Coords>(lhs, rhs); };
Coord linelineintersectpos(const Coord4 &lhs, const Coord4 &rhs) { return imp_linelineintersectpos<Coords>(lhs, rhs); };

Coord approach(Coord start, Coord target, Coord delta, Coord drag) {
  if(!(((start < 0) == (target < 0)) && start < target))
    delta += drag;
  return approach(start, target, delta);
}
Coord2 approach(Coord2 start, Coord2 target, Coord delta) {
  CHECK(delta >= 0);
  Coord2 diff = target - start;
  if(len(diff) <= delta)
    return target;
  return start + diff / len(diff) * delta;
}
Coord2 approach(Coord2 start, Coord2 target, Coord delta, Coord drag) {
  if(target == start)
    return target;
  if(len(start) == 0)
    return approach(start, target, delta);
  return approach(start, target, delta + drag * max(Coord(0), -dot(normalize(start), normalize(target - start))));
}

Coord2 rotate(const Coord2 &in, Coord ang) {
  return imp_rotate<Coords>(in, ang);
}

Coord mod(const Coord &a, const Coord &b) {
  if(a < Coord(0))
    return b - mod(-a, b);
  return coordExplicit(a.d % b.d);
}

Coord2 lerp(const Coord2 &lhs, const Coord2 &rhs, Coord dist) {
  return lhs + (rhs - lhs) * dist;
}
Coord2 lerp(const Coord4 &movement, Coord dist) {
  return lerp(movement.s(), movement.e(), dist);
}
Coord4 lerp(const Coord4 &lhs, const Coord4 &rhs, Coord dist) {
  return lhs + (rhs - lhs) * dist;
}

BOOST_STATIC_ASSERT(sizeof(Coord) == 8);
BOOST_STATIC_ASSERT(sizeof(Coord2) == 16);
BOOST_STATIC_ASSERT(sizeof(Coord4) == 32);

void adler(Adler32 *adl, const Coord &val) { adl->addBytes(&val, sizeof(val)); }
void adler(Adler32 *adl, const Coord2 &val) { adl->addBytes(&val, sizeof(val)); }
void adler(Adler32 *adl, const Coord4 &val) { adl->addBytes(&val, sizeof(val)); }
void adler(Adler32 *adl, const CPosInfo &val) {
  adler(adl, val.pos);
  adler(adl, val.d);
}


//
// Created by Hamza El-Kebir on 4/21/21.
//

#ifndef LODESTAR_DISCRETESYSTEM_HPP
#define LODESTAR_DISCRETESYSTEM_HPP

#include <Eigen/Dense>
#include "StateSpace.hpp"
#include "SystemStateful.hpp"
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

namespace ls {
    namespace systems {
        template<class SYS>
        class DiscreteSystem : public SystemStateful {
        public:
            typedef SYS TDSystem;
            DiscreteSystem() : system(new SYS), state(new Eigen::VectorXd),
                               input(
                                       nullptr), time(0)
            {}

            DiscreteSystem(SYS *sys) : system(sys), state(new Eigen::VectorXd),
                                       input(nullptr), time(0)
            {}

            void advance();

//            void advance(void *control);

            void advanceFree();

            void advanceForced();

//            void advanceForced(void *control);

            SYS *system;
            void *state;
            void *input;

            double time;
        };

        template <template <typename, const int, const int, const int> class TSystem, typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
        class DiscreteSystem<TSystem<TScalar, TStateDim, TInputDim, TOutputDim>>;

        template <typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
        class DiscreteSystem<StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>> {
        public:
            typedef StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> TDSystem;
            typedef Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC_VAL(TStateDim, 1)> TDStateVector;
            typedef Eigen::Matrix<TScalar, TInputDim, LS_STATIC_UNLESS_DYNAMIC_VAL(TInputDim, 1)> TDInputVector;

            DiscreteSystem() : system(new TDSystem), state(new TDStateVector),
                               input(nullptr), time(0)
            {}

            DiscreteSystem(TDSystem *sys) : system(sys), state(new TDStateVector),
                                       input(nullptr), time(0)
            {}

            IF_DYNAMIC_RETURN(TStateDim, TInputDim, TOutputDim, void)
            initialize() {
                state->conservativeResize(system->getA().rows());

                if (input == nullptr)
                    input = new Eigen::VectorXd;

                input->conservativeResize(system->getB().cols());
            }

            IF_STATIC_RETURN(TStateDim, TInputDim, TOutputDim, void)
            initialize() {
                // NOTE: No action since no memory may be allocated.
                return;
            }

            void advance();

            void advance(TDInputVector *control);

            void advanceFree();

            void advanceForced();

            void advanceForced(TDInputVector *control);

            TDSystem *system;
            TDStateVector *state;
            TDInputVector *input;

            double time;
        };
    }
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advance()
{
    if (input != nullptr) {
        *state = (*system->getA()) * (*state) + (*system->getB()) * (*input);
    } else {
        *state = (*system->getA()) * (*state);
    }

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advance(
        Eigen::Matrix<TScalar, TInputDim, Kmax2(-1, TInputDim, 1)> *control)
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*control);

    time = system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceFree()
{
    *state = (*system->getA()) * (*state);

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceForced()
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*input);

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceForced(
        Eigen::Matrix<TScalar, TInputDim, Kmax2(-1, TInputDim, 1)> *control)
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*control);

    time += system->getSamplingPeriod();
}

#endif //LODESTAR_DISCRETESYSTEM_HPP


#include "math-dsp.h"
#include <speex/speex_resampler.h>
#include <boost/scope_exit.hpp>
#include <algorithm>
#include <stdexcept>
#include <cmath>

double tukey_window(double a, double x) {
  if (x < a / 2) {
    return (1.0 + std::cos(M_PI * (2.0 * x / a - 1.0))) / 2.0;
  } else if (x > 1.0 - a / 2) {
    return (1.0 + std::cos(M_PI * (2.0 * (x - 1.0) / a + 1.0))) / 2.0;
  } else {
    return 1.0;
  }
}

void resample(const float *in_samples,
              unsigned in_sample_count,
              float *out_samples,
              unsigned out_sample_count) {
  if (in_sample_count == 0) {
    std::fill(out_samples, out_samples + out_sample_count, 0.0f);
    return;
  }

  int err {};

  SpeexResamplerState *resampler = speex_resampler_init(
    1, in_sample_count, out_sample_count, SPEEX_RESAMPLER_QUALITY_MAX, &err);

  if (!resampler) {
    const char *errmsg = speex_resampler_strerror(err);
    throw std::runtime_error(std::string("speex_resampler_init: ") + errmsg);
  }

  BOOST_SCOPE_EXIT(resampler) {
    speex_resampler_destroy(resampler);
  } BOOST_SCOPE_EXIT_END;

  speex_resampler_skip_zeros(resampler);

  while (out_sample_count > 0) {
    unsigned in_count = in_sample_count;
    unsigned out_count = out_sample_count;
    speex_resampler_process_float(
      resampler, 0, in_samples, &in_count, out_samples, &out_count);
    in_samples += in_count;
    in_sample_count -= in_count;
    out_samples += out_count;
    out_sample_count -= out_count;
    if (out_count == 0)
      break;
  }

  static const unsigned nzeros = 32;
  static const float zero[nzeros] {};

  while (out_sample_count > 0) {
    unsigned in_count = nzeros;
    unsigned out_count = out_sample_count;
    speex_resampler_process_float(
      resampler, 0, zero, &in_count, out_samples, &out_count);
    out_samples += out_count;
    out_sample_count -= out_count;
  }
}


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

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2006 François du Vignaud
 Copyright (C) 2006, 2008 Ferdinando Ametrano
 Copyright (C) 2015 Peter Caspers

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file swaptionvolmatrix.hpp
    \brief Swaption at-the-money volatility matrix
*/

#ifndef quantlib_swaption_volatility_matrix_hpp
#define quantlib_swaption_volatility_matrix_hpp

#include <ql/termstructures/volatility/swaption/swaptionvoldiscrete.hpp>
#include <ql/math/interpolations/interpolation2d.hpp>
#include <ql/math/matrix.hpp>
#include <boost/noncopyable.hpp>
#include <vector>

namespace QuantLib {

    class Quote;

    //! At-the-money swaption-volatility matrix
    /*! This class provides the at-the-money volatility for a given
        swaption by interpolating a volatility matrix whose elements
        are the market volatilities of a set of swaption with given
        option date and swapLength.

        The volatility matrix <tt>M</tt> must be defined so that:
        - the number of rows equals the number of option dates;
        - the number of columns equals the number of swap tenors;
        - <tt>M[i][j]</tt> contains the volatility corresponding
          to the <tt>i</tt>-th option and <tt>j</tt>-th tenor.
    */
    class SwaptionVolatilityMatrix : public SwaptionVolatilityDiscrete,
                                     private boost::noncopyable {
      public:
        //! floating reference date, floating market data
        SwaptionVolatilityMatrix(
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const std::vector<std::vector<Real> >& shifts
                    = std::vector<std::vector<Real> >());
        //! fixed reference date, floating market data
        SwaptionVolatilityMatrix(
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const std::vector<std::vector<Real> >& shifts
                    = std::vector<std::vector<Real> >());
        //! floating reference date, fixed market data
        SwaptionVolatilityMatrix(
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const Matrix& shifts = Matrix());
        //! fixed reference date, fixed market data
        SwaptionVolatilityMatrix(
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const Matrix& shifts = Matrix());
        // fixed reference date and fixed market data, option dates
        SwaptionVolatilityMatrix(const Date& referenceDate,
                                 const std::vector<Date>& optionDates,
                                 const std::vector<Period>& swapTenors,
                                 const Matrix& volatilities,
                                 const DayCounter& dayCounter,
                                 const bool flatExtrapolation = false,
                                 const VolatilityType type = ShiftedLognormal,
                                 const Matrix& shifts = Matrix());

        //! \name LazyObject interface
        //@{
        void performCalculations() const;
        //@}
        //! \name TermStructure interface
        //@{
        Date maxDate() const;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Rate minStrike() const;
        Rate maxStrike() const;
        //@}
        //! \name SwaptionVolatilityStructure interface
        //@{
        const Period& maxSwapTenor() const;
        //@}
        //! \name Other inspectors
        //@{
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(const Date& optionDate,
                                    const Period& swapTenor) const {
            return locate(timeFromReference(optionDate),
                          swapLength(swapTenor));
        }
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(Time optionTime,
                                    Time swapLength) const {
            return std::make_pair(interpolation_.locateY(optionTime),
                                  interpolation_.locateX(swapLength));
        }
        //@}
        VolatilityType volatilityType() const;
      protected:
        // defining the following method would break CMS test suite
        // to be further investigated
        //ext::shared_ptr<SmileSection> smileSectionImpl(const Date&,
        //                                                 const Period&) const;
        ext::shared_ptr<SmileSection> smileSectionImpl(Time,
                                                         Time) const;
        Volatility volatilityImpl(Time optionTime,
                                  Time swapLength,
                                  Rate strike) const;
        Real shiftImpl(Time optionTime, Time swapLength) const;
      private:
        void checkInputs(Size volRows,
                         Size volsColumns,
                         Size shiftRows,
                         Size shiftsColumns) const;
        void registerWithMarketData();
        std::vector<std::vector<Handle<Quote> > > volHandles_;
        std::vector<std::vector<Real> > shiftValues_;
        mutable Matrix volatilities_, shifts_;
        Interpolation2D interpolation_, interpolationShifts_;
        VolatilityType volatilityType_;
    };

    // inline definitions

    inline Date SwaptionVolatilityMatrix::maxDate() const {
        return optionDates_.back();
    }

    inline Rate SwaptionVolatilityMatrix::minStrike() const {
        return -QL_MAX_REAL;
    }

    inline Rate SwaptionVolatilityMatrix::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline const Period& SwaptionVolatilityMatrix::maxSwapTenor() const {
        return swapTenors_.back();
    }

    inline Volatility SwaptionVolatilityMatrix::volatilityImpl(Time optionTime,
                                                               Time swapLength,
                                                               Rate) const {
        calculate();
        return interpolation_(swapLength, optionTime, true);
    }

    inline VolatilityType SwaptionVolatilityMatrix::volatilityType() const {
        return volatilityType_;
    }

    inline Real SwaptionVolatilityMatrix::shiftImpl(Time optionTime,
                                                    Time swapLength) const {
        calculate();
        Real tmp = interpolationShifts_(swapLength, optionTime, true);
        return tmp;
    }
} // namespace QuantLib

#endif


#include "integrator.h"
#include "biorbd.h"

#include <boost/numeric/odeint.hpp>
#include "dynamics.h"

AcadoIntegrator::AcadoIntegrator(biorbd::Model& m) :
    biorbd::rigidbody::Integrator(m),
    m_isKinematicsComputed(false),
    m_nMus(m.nbMuscleTotal()),
    m_nTorque(m.nbGeneralizedTorque())
{
    m_lhs = new double[m_model->nbQ() + m_model->nbQdot()
            + m_nMus + m_nTorque];
    m_rhs = new double[m_model->nbQ() + m_model->nbQdot()];
}

void AcadoIntegrator::operator()(
        const state_type &x,
        state_type &dxdt,
        double)
{
    for (unsigned int i=0; i<*m_nQ + *m_nQdot; i++){
        m_lhs[i] = x[i];
    }
    for (unsigned int i=0; i<m_nMus + m_nTorque; ++i){
        m_lhs[i + *m_nQ + *m_nQdot] = (*m_u)(i);
    }

    // Équation différentielle : x/xdot => xdot/xddot
    forwardDynamics_contact(m_lhs, m_rhs);

    // Faire sortir xdot/xddot
    for (unsigned int i=0; i<*m_nQ + *m_nQdot; i++){
        dxdt[i] = m_rhs[i];
    }

}

void AcadoIntegrator::integrateKinematics(
        const biorbd::rigidbody::GeneralizedCoordinates &Q,
        const biorbd::rigidbody::GeneralizedCoordinates &QDot,
        const biorbd::utils::Vector &control,
        double t0,
        double tend,
        double timeStep)
{
    biorbd::utils::Vector v(static_cast<unsigned int>(Q.rows()+QDot.rows()));
    v << Q,QDot;
    integrate(v, control, t0, tend, timeStep); // vecteur, t0, tend, pas, effecteurs
    m_isKinematicsComputed = true;
}

void AcadoIntegrator::getIntegratedKinematics(
        unsigned int step,
        biorbd::rigidbody::GeneralizedCoordinates &Q,
        biorbd::rigidbody::GeneralizedCoordinates &QDot)
{
    // Si la cinématique n'a pas été mise à jour
    biorbd::utils::Error::check(
                m_isKinematicsComputed,
                "ComputeKinematics must be call before calling updateKinematics");
    const biorbd::utils::Vector& tp(getX(step));
    for (unsigned int i=0; i< static_cast<unsigned int>(tp.rows()/2); i++){
        Q(i) = tp(i);
        QDot(i) = tp(i+tp.rows()/2);
    }
}
unsigned int AcadoIntegrator::nbInterationStep() const
{
    return steps();
}

void AcadoIntegrator::launchIntegrate(
        state_type &x,
        double t0,
        double tend,
        double timeStep)
{
    // Choix de l'algorithme et intégration
    boost::numeric::odeint::runge_kutta4< state_type > stepper;
    *m_steps = static_cast<unsigned int>(
                boost::numeric::odeint::integrate_const(
                    stepper, *this, x, t0, tend, timeStep,
                    push_back_state_and_time( *m_x_vec , *m_times )));
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/reader_utils.h>
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>

#include <boost/program_options.hpp>

#include <string>

using namespace Gudhi;
using namespace Gudhi::persistent_cohomology;

typedef double Filtration_value;

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence);

int main(int argc, char * argv[]) {
  std::string simplex_tree_file;
  std::string output_file;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, simplex_tree_file, output_file, p, min_persistence);

  std::cout << "Simplex_tree from file=" << simplex_tree_file.c_str() << " - output_file=" << output_file.c_str()
      << std::endl;
  std::cout << "     - p=" << p << " - min_persistence=" << min_persistence << std::endl;

  // Read the list of simplices from a file.
  Simplex_tree<> simplex_tree;

  std::ifstream simplex_tree_stream(simplex_tree_file);
  simplex_tree_stream >> simplex_tree;

  std::cout << "The complex contains " << simplex_tree.num_simplices() << " simplices" << std::endl;
  std::cout << "   - dimension " << simplex_tree.dimension() << std::endl;

  /*
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for( auto f_simplex : simplex_tree.filtration_simplex_range() )
  { std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
  for( auto vertex : simplex_tree.simplex_vertex_range(f_simplex) )
  { std::cout << vertex << " "; }
  std::cout << std::endl;
  }*/

  // Sort the simplices in the order of the filtration
  simplex_tree.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology< Simplex_tree<>, Field_Zp > pcoh(simplex_tree);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(p);

  pcoh.compute_persistent_cohomology(min_persistence);

  // Output the diagram in output_file
  if (output_file.empty()) {
    pcoh.output_diagram();
  } else {
    std::ofstream out(output_file);
    pcoh.output_diagram(out);
    out.close();
  }

  return 0;
}

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()
      ("input-file", po::value<std::string>(&simplex_tree_file),
       "Name of file containing a simplex set. Format is one simplex per line (cf. reader_utils.h - read_simplex): Dim1 X11 X12 ... X1d Fil1  ");

  po::options_description visible("Allowed options", 100);
  visible.add_options()
      ("help,h", "produce help message")
      ("output-file,o", po::value<std::string>(&output_file)->default_value(std::string()),
       "Name of file in which the persistence diagram is written. Default print in std::cout")
      ("field-charac,p", po::value<int>(&p)->default_value(11),
       "Characteristic p of the coefficient field Z/pZ for computing homology.")
      ("min-persistence,m", po::value<Filtration_value>(&min_persistence),
       "Minimal lifetime of homology feature to be recorded. Default is 0");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
            options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::cout << std::endl;
    std::cout << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::cout << "of a Rips complex defined on a set of input points.\n \n";
    std::cout << "The output diagram contains one bar per line, written with the convention: \n";
    std::cout << "   p   dim b d \n";
    std::cout << "where dim is the dimension of the homological feature,\n";
    std::cout << "b and d are respectively the birth and death of the feature and \n";
    std::cout << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::cout << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::cout << visible << std::endl;
    exit(-1);
  }
}


/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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 "Delaunay.hpp"

#if defined(__cplusplus)
extern "C"
{
#include <libqhull_r/libqhull_r.h>
}
#endif

#include <boost/numeric/conversion/cast.hpp>

using rw::core::ownedPtr;
using namespace rw::math;
using namespace rw::geometry;

namespace {
int build (int dim, double* coords, int nrCoords, std::vector< int >& vertIdxs,
           std::vector< int >& faceIdxs)
{
    vertIdxs.clear ();
    faceIdxs.clear ();

    std::vector< int >& result = vertIdxs;

    int curlong, totlong; /* used !qh_NOmem */
    int exitcode;
    boolT ismalloc = false;
    qhT qh;
    qhT* qhT_pointer = &qh;    // allocate memory for qhull

    char flags[] = "qhull d Qbb Qt Qz";
    // d Qbb: delaunay with scaling of last coordinate (eq. to qdelaunay)
    // Qt: triangulated output
    // Qz: add point-at-infinity (for cocircular points like in rectangles)

    // According to doc. in libqhull_r/user_r.c, the qhull memory should be cleared:
    qh_zero (qhT_pointer, stderr);

    // Then the object is constructed.
    exitcode = qh_new_qhull (qhT_pointer, dim, nrCoords, coords, ismalloc, flags, NULL, stderr);

    // Loop through all vertices,
    vertexT* vertex;    //, **vertexp;
    for (vertex = qhT_pointer->vertex_list; vertex && vertex->next; vertex = vertex->next) {
        int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
        result.push_back (vertexIdx);
    }

    if (!exitcode) {
        // For all facets:
        for (facetT* facet = qhT_pointer->facet_list; facet && facet->next; facet = facet->next) {
            if (!facet->upperdelaunay) {
                int vertex_n, vertex_i;
                FOREACHvertex_i_ (qhT_pointer, facet->vertices)
                {
                    int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
                    faceIdxs.push_back (vertexIdx);
                }
            }
        }
    }
    if (qhT_pointer->VERIFYoutput && !qhT_pointer->FORCEoutput && !qhT_pointer->STOPpoint &&
        !qhT_pointer->STOPcone)
        qh_check_points (qhT_pointer);

    qh_freeqhull (qhT_pointer, False);
    qh_memfreeshort (qhT_pointer, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (
            stderr,
            "qdelaunay internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
            totlong,
            curlong);
    return exitcode;
}

template< typename S >
typename IndexedTriMeshN0< double, S >::Ptr makeMesh (const std::vector< Vector2D<> >& hullVertices,
                                                      const std::vector< int >& faceIdxs,
                                                      const std::vector< double >& values)
{
    typedef IndexedTriMeshN0< double, S > Mesh;
    typedef typename Mesh::VertexArray VertexArray;
    typedef typename Mesh::TriangleArray TriangleArray;

    // Add vertices
    const rw::core::Ptr< VertexArray > meshV = ownedPtr (new VertexArray (hullVertices.size ()));
    for (size_t i = 0; i < hullVertices.size (); i++) {
        (*meshV)[i] = Vector3D<> (hullVertices[i][0],
                                  hullVertices[i][1],
                                  (values.size () == hullVertices.size ()) ? values[i] : 0);
    }

    // Add triangles
    const rw::core::Ptr< TriangleArray > meshTri =
        ownedPtr (new TriangleArray (faceIdxs.size () / 3));
    for (size_t i = 0; i < faceIdxs.size () / 3; i++) {
        const typename Mesh::tri_type tempTri (
            faceIdxs[i * 3 + 0], faceIdxs[i * 3 + 1], faceIdxs[i * 3 + 2]);
        const Vector3D<>& v1 = (*meshV)[tempTri[0]];
        const Vector3D<>& v2 = (*meshV)[tempTri[1]];
        const Vector3D<>& v3 = (*meshV)[tempTri[2]];

        // Make sure the vertices order is correct according to right-hand rule. Otherwise flip v2
        // and v3.
        const double dotProduct = dot (cross (v2 - v1, v3 - v2), Vector3D<>::z ());
        if (dotProduct < 0.0) {
            (*meshTri)[i] = typename Mesh::tri_type (tempTri[0], tempTri[2], tempTri[1]);
        }
        else {
            (*meshTri)[i] = tempTri;
        }
    }

    const typename Mesh::Ptr mesh = ownedPtr (new Mesh (meshV, meshTri));
    return mesh;
}
}    // namespace

Delaunay::Delaunay ()
{}

Delaunay::~Delaunay ()
{}

IndexedTriMesh<>::Ptr Delaunay::triangulate (const std::vector< Vector2D<> >& vertices,
                                             const std::vector< double >& values)
{
    if (vertices.size () == 3) {
        std::vector< int > faceIdxs (3);
        faceIdxs[0] = 0;
        faceIdxs[1] = 1;
        faceIdxs[2] = 2;
        return makeMesh< uint8_t > (vertices, faceIdxs, values);
    }
    const int nrInputVertices = boost::numeric_cast< int > (vertices.size ());

    // convert the vertice array to an array of double
    double* vertArray = new double[vertices.size () * 2];
    // copy all data into the vertArray
    for (size_t i = 0; i < vertices.size (); i++) {
        const Vector2D<>& v  = vertices[i];
        vertArray[i * 2 + 0] = v[0];
        vertArray[i * 2 + 1] = v[1];
    }
    // Build delaunay triangulation
    std::vector< Vector2D<> > triangVertices;
    std::vector< int > vertiIdxs;
    std::vector< int > faceIdxs;

    const int exitcode = build (2, vertArray, nrInputVertices, vertiIdxs, faceIdxs);

    delete[] vertArray;
    if (exitcode)
        RW_THROW ("Delaunay triangulation returned with exit code " << exitcode);

    std::vector< int > vertIdxMap (vertices.size ());
    triangVertices.resize (vertiIdxs.size () - 1);    // Qz gives extra vertex
    int triangVerticesI = 0;
    for (size_t i = 0; i < vertiIdxs.size (); i++) {
        if (vertiIdxs[i] < nrInputVertices) {
            triangVertices[triangVerticesI] = vertices[vertiIdxs[i]];
            vertIdxMap[vertiIdxs[i]]        = triangVerticesI;
            triangVerticesI++;
        }
    }
    for (size_t i = 0; i < faceIdxs.size (); i++) {
        const int tmp = faceIdxs[i];
        faceIdxs[i]   = vertIdxMap[tmp];
    }

    // Create the mesh
    if (vertices.size () <= 256) {
        return makeMesh< uint8_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 65536) {
        return makeMesh< uint16_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 4294967296) {
        return makeMesh< uint32_t > (triangVertices, faceIdxs, values);
    }
    else {
        return makeMesh< uint64_t > (triangVertices, faceIdxs, values);
    }
}


#include "static_ic.h"
#include "../../cosmo_includes.h"
#include "../../cosmo_types.h"
#include "../../cosmo_globals.h"
#include "../../ICs/ICs.h"
#include "../../utils/math.h"

#include <boost/math/special_functions/spherical_harmonic.hpp>

#include <utility>
#include <random>

namespace cosmo
{

typedef std::pair<real_t, real_t> complex_t;
#define m_idx(l, m) ((l)+(m))

/**
 * @brief Gaussian random field ICs
 *
 * General steps:
 *  1) Generate the Newtonian gauge variables:
 *     phi = psi, \dot{phi} = 0, \delta_u
 *  2) Compute the synchronous gauge variables
 *  3) Fix \phi, \bar{gamma}_{ij}, K
 *  4) Solve for V, U in CTT decomposition -> obtain \bar{A}_{ij}^{\rm NL}
 *  5) Use the Hamiltonian constraint residual to solve for the density field
 */
void static_ic_set_random(BSSN * bssn, Static * stat, Lambda * lambda,
  Fourier * fourier, IOData * iodata)
{
  idx_t i, j, k;

  // Background cosmology, a_FRW = 1
  real_t rho_FRW = 3.0/PI/8.0;
  real_t Omega_L = std::stod(_config("Omega_L", "1.0e-6"));
  real_t p0 = std::stod(_config("p0", "7.0"));
  real_t P = H_LEN_FRAC*H_LEN_FRAC*1.0e-15*std::stod(_config("P", "1.0"));
  real_t p_cut = std::stod(_config("p_cut", "1.0"));
  real_t rho_L = Omega_L * rho_FRW;
  lambda->setLambda(rho_L);

  // final metric fields
  arr_t & DIFFphi_p = *bssn->fields["DIFFphi_p"];
  arr_t & DIFFK_p = *bssn->fields["DIFFK_p"];
  arr_t & A11_p = *bssn->fields["A11_p"];
  arr_t & A12_p = *bssn->fields["A12_p"];
  arr_t & A13_p = *bssn->fields["A13_p"];
  arr_t & A22_p = *bssn->fields["A22_p"];
  arr_t & A23_p = *bssn->fields["A23_p"];
  arr_t & A33_p = *bssn->fields["A33_p"];
  arr_t & DIFFD_a = *stat->fields["DIFFD_a"];

  // Extra / auxiliary fields for intermediate computations
  // (re-use _c register, it gets overwritten later anyways)
  arr_t & phi_N = *bssn->fields["DIFFphi_c"];
  arr_t & lap_phi_N = *bssn->fields["DIFFK_c"];
  arr_t & invlape6pd1K = *bssn->fields["DIFFgamma11_c"];
  arr_t & invlape6pd2K = *bssn->fields["DIFFgamma12_c"];
  arr_t & invlape6pd3K = *bssn->fields["DIFFgamma13_c"];
  arr_t & W1 = *bssn->fields["A22_c"];
  arr_t & W2 = *bssn->fields["A23_c"];
  arr_t & W3 = *bssn->fields["A33_c"];

  // 1.a) Synchronous-gauge Newtonian potential:
  set_gaussian_random_Phi_N(phi_N, fourier, P, p0, p_cut);

  // 1.b) Preliminary metric variables: phi, K
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    lap_phi_N[idx] = laplacian(i, j, k, phi_N);

    DIFFphi_p[idx] = log1p(-10.0/3.0*phi_N[idx])/4.0;
    DIFFK_p[idx] = -3.0*(1.0 + 2.0*phi_N[idx]) + 2.0/3.0*lap_phi_N[idx];
  }

  // 1.c) Vector contribution to the extrinsic curvature, W^i
  // compute invlapK
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k) {
    idx_t idx = NP_INDEX(i,j,k);
    real_t e6p = exp(6.0*DIFFphi_p[idx]);
    invlape6pd1K[idx] = e6p*derivative(i,j,k,1,DIFFK_p);
    invlape6pd2K[idx] = e6p*derivative(i,j,k,2,DIFFK_p);
    invlape6pd3K[idx] = e6p*derivative(i,j,k,3,DIFFK_p);
  }
  fourier->inverseLaplacian <idx_t, real_t> (invlape6pd1K._array);
  fourier->inverseLaplacian <idx_t, real_t> (invlape6pd2K._array);
  fourier->inverseLaplacian <idx_t, real_t> (invlape6pd3K._array);
  // compute W^i = W_i
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    W1[idx] = -derivative(i,j,k,1,phi_N)/3.0 + invlape6pd1K[idx]/2.0;
    W2[idx] = -derivative(i,j,k,2,phi_N)/3.0 + invlape6pd2K[idx]/2.0;
    W3[idx] = -derivative(i,j,k,3,phi_N)/3.0 + invlape6pd3K[idx]/2.0;
  }

  // 1.d) Aij components
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    // these are the CTT conformal Aij
    real_t CTT2BSSNAij = exp(-6.0*DIFFphi_p[idx]);
    real_t DkWk = derivative(i,j,k,1,W1) + derivative(i,j,k,2,W2) + derivative(i,j,k,3,W3);
    A11_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,1,W1) + derivative(i,j,k,1,W1) + 2.0/3.0*double_derivative(i,j,k,1,1,phi_N)
      - 2.0/3.0*DkWk - 2.0/9.0*lap_phi_N[idx] );
    A12_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,1,W2) + derivative(i,j,k,2,W1) + 2.0/3.0*double_derivative(i,j,k,1,2,phi_N) );
    A13_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,1,W3) + derivative(i,j,k,3,W1) + 2.0/3.0*double_derivative(i,j,k,1,3,phi_N) );
    A22_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,2,W2) + derivative(i,j,k,2,W2) + 2.0/3.0*double_derivative(i,j,k,2,2,phi_N)
      - 2.0/3.0*DkWk - 2.0/9.0*lap_phi_N[idx] );
    A23_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,2,W3) + derivative(i,j,k,3,W2) + 2.0/3.0*double_derivative(i,j,k,2,3,phi_N) );
    A33_p[idx] = CTT2BSSNAij * ( derivative(i,j,k,3,W3) + derivative(i,j,k,3,W3) + 2.0/3.0*double_derivative(i,j,k,3,3,phi_N)
      - 2.0/3.0*DkWk - 2.0/9.0*lap_phi_N[idx] );
  }

  // 1.e) Density field
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    real_t AijAij = A11_p[idx]*A11_p[idx] + A22_p[idx]*A22_p[idx] + A33_p[idx]*A33_p[idx]
      + 2.0*(A12_p[idx]*A12_p[idx] + A13_p[idx]*A13_p[idx] + A23_p[idx]*A23_p[idx]);
    real_t lap_e_p = exp(DIFFphi_p[idx]) * ( laplacian(i,j,k,DIFFphi_p)
      + pw2(derivative(i,j,k,1,DIFFphi_p)) + pw2(derivative(i,j,k,2,DIFFphi_p)) + pw2(derivative(i,j,k,3,DIFFphi_p)) );
    real_t rho_ADM = 1.0/2.0/PI * ( DIFFK_p[idx]*DIFFK_p[idx]/12.0 - AijAij/8.0 - exp(-5.0*DIFFphi_p[idx])*lap_e_p );
    real_t rho_0 = rho_ADM - rho_L;

    if(rho_0 < 0.0)
    {
      iodata->log("Error: negative density in some regions.");
      throw -1;
    }

    DIFFD_a[idx] = exp(6.0*DIFFphi_p[idx])*rho_0;
  }

  iodata->log( "Average fluctuation density: " + stringify(average(DIFFD_a)) );
  iodata->log( "Std.dev fluctuation density: " + stringify(standard_deviation(DIFFD_a)) );
}

void static_ic_set_sinusoid_3d(BSSN * bssn, Static * stat, Lambda * lambda, Fourier * fourier,
  IOData * iodata)
{

  idx_t i, j, k;

  arr_t & DIFFr_a = *bssn->fields["DIFFr_a"];
  arr_t & DIFFphi_p = *bssn->fields["DIFFphi_p"];
  arr_t & DIFFD_a = *stat->fields["DIFFD_a"];

  real_t rho_FRW = 3.0/PI/8.0;

  real_t Omega_L = std::stod(_config("Omega_L", "0.0"));
  real_t rho_m = (1.0 - Omega_L) * rho_FRW;
  real_t rho_L = Omega_L * rho_FRW;
  lambda->setLambda(rho_L);

  real_t A = H_LEN_FRAC*H_LEN_FRAC*std::stod(_config("peak_amplitude_frac", "0.001"));

  // the conformal factor in front of metric is the solution to
  // d^2 exp(\phi) = -2*pi exp(5\phi) * \delta_rho
  // generate random mode in \phi
  // delta_rho = -(lap e^\phi)/e^(4\phi)/2pi
  real_t phix = std::stod(_config("phix", "0.0"));
  real_t phiy = phix, phiz = phix;

  // grid values
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    real_t x_frac = ((real_t) i / (real_t) NX),
      y_frac = ((real_t) j / (real_t) NY), z_frac = ((real_t) k / (real_t) NZ);

    real_t phi = A*
      (sin(2.0*PI*x_frac + phix) + sin(2.0*PI*y_frac + phiy) + sin(2.0*PI*z_frac + phiz));

    real_t DIFFrho = -exp(-4.0*phi)/PI/2.0*(
      pw2(A*2.0*PI / H_LEN_FRAC * cos(2.0*PI*x_frac + phix))
      + pw2(A*2.0*PI / H_LEN_FRAC * cos(2.0*PI*y_frac + phiy))
      + pw2(A*2.0*PI / H_LEN_FRAC * cos(2.0*PI*z_frac + phiz))
      -  ( A * pw2(2.0*PI / H_LEN_FRAC) * sin(2.0*PI*x_frac + phix)
           + A * pw2(2.0*PI / H_LEN_FRAC) * sin(2.0*PI*y_frac + phiy)
           + A * pw2(2.0*PI / H_LEN_FRAC) * sin(2.0*PI*z_frac + phiz))
    );

    // These aren't difference vars
    DIFFphi_p[NP_INDEX(i,j,k)] = phi;
    DIFFr_a[idx] = DIFFrho;

// // debugging: throw away field
// DIFFD_a[idx] = phi;
  }
// std::cout << std::setprecision(17);
// std::cout << "field[0] = " << DIFFD_a[0] << "; ";
// fourier->inverseLaplacian <idx_t, real_t> (DIFFD_a._array);
// std::cout << "lap/lap field = " << laplacian(0,0,0,DIFFD_a) << "\n";

  // Make sure min density value > 0
  // Set conserved density variable field
  real_t min = rho_m;
  real_t max = min;
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    real_t DIFFr = DIFFr_a[idx];
    real_t rho = rho_m + DIFFr;
    // phi_FRW = 0
    real_t DIFFphi = DIFFphi_p[idx];
    // phi = DIFFphi
    // DIFFK = 0

    DIFFD_a[idx] =
      rho_m*expm1(6.0*DIFFphi) + exp(6.0*DIFFphi)*DIFFr;

    if(rho < min)
    {
      min = rho;
    }
    if(rho > max)
    {
      max = rho;
    }
    if(rho != rho)
    {
      iodata->log("Error: NaN energy density.");
      throw -1;
    }
  }

  iodata->log( "Minimum fluid density: " + stringify(min) );
  iodata->log( "Maximum fluid density: " + stringify(max) );
  iodata->log( "Average fluctuation density: " + stringify(average(DIFFD_a)) );
  iodata->log( "Std.dev fluctuation density: " + stringify(standard_deviation(DIFFD_a)) );
  if(min < 0.0)
  {
    iodata->log("Error: negative density in some regions.");
    throw -1;
  }

# if USE_REFERENCE_FRW
  // Set values in reference FRW integrator
  auto & frw = bssn->frw;
  real_t K_FRW = -sqrt(24.0*PI*rho_FRW);
  frw->set_phi(0.0);
  frw->set_K(K_FRW);
  frw->addFluid(rho_m, 0.0 /* w=0 */);
# else
  arr_t & DIFFK_p = *bssn->fields["DIFFK_p"];
  arr_t & DIFFK_a = *bssn->fields["DIFFK_a"];
  // add in FRW pieces to ICs
  // phi is unchanged
  // rho (D) and K get contribs
  // w=0 fluid only
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    real_t D_FRW = rho_m; // on initial slice

    DIFFr_a[idx] += rho_m;

    DIFFK_a[idx] = -sqrt(24.0*PI*rho_FRW);
    DIFFK_p[idx] = -sqrt(24.0*PI*rho_FRW);

    DIFFD_a[idx] += D_FRW;
  }
# endif

}

/**
 * @brief Sinusoidal mode ICs
 */
void static_ic_set_sinusoid(BSSN * bssn, Static * stat, Lambda * lambda, Fourier * fourier,
  IOData * iodata)
{
  idx_t i, j, k;

  arr_t & DIFFr_a = *bssn->fields["DIFFr_a"];
  arr_t & DIFFphi_p = *bssn->fields["DIFFphi_p"];
  arr_t & DIFFD_a = *stat->fields["DIFFD_a"];

  real_t rho_FRW = 3.0/PI/8.0;

  real_t Omega_L = std::stod(_config("Omega_L", "0.0"));
  real_t rho_m = (1.0 - Omega_L) * rho_FRW;
  real_t rho_L = Omega_L * rho_FRW;
  lambda->setLambda(rho_L);

  real_t A = H_LEN_FRAC*H_LEN_FRAC*std::stod(_config("peak_amplitude_frac", "0.001"));

  // the conformal factor in front of metric is the solution to
  // d^2 exp(\phi) = -2*pi exp(5\phi) * \delta_rho
  // generate random mode in \phi
  // delta_rho = -(lap e^\phi)/e^(4\phi)/2pi
  real_t phix = std::stod(_config("phix", "0.0"));
  real_t twopi_L = 2.0*PI/H_LEN_FRAC;
  real_t pw2_twopi_L = twopi_L*twopi_L;
  // grid values
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    real_t x = ((real_t) i / (real_t) NX);
    real_t phi = A*sin(2.0*PI*x + phix);
    real_t DIFFrho = -exp(-4.0*phi)/PI/2.0*(
        pw2(twopi_L*A*cos(2.0*PI*x + phix))
        - pw2_twopi_L*A*sin(2.0*PI*x + phix)
      );

    // These aren't difference vars
    DIFFphi_p[NP_INDEX(i,j,k)] = phi;
    DIFFr_a[idx] = DIFFrho;

  }

  // Make sure min density value > 0
  // Set conserved density variable field
  real_t min = rho_m;
  real_t max = min;
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    real_t DIFFr = DIFFr_a[idx];
    real_t rho = rho_m + DIFFr;
    // phi_FRW = 0
    real_t DIFFphi = DIFFphi_p[idx];
    // phi = DIFFphi
    // DIFFK = 0

    DIFFD_a[idx] =
      rho_m*expm1(6.0*DIFFphi) + exp(6.0*DIFFphi)*DIFFr;

    if(rho < min)
    {
      min = rho;
    }
    if(rho > max)
    {
      max = rho;
    }
    if(rho != rho)
    {
      iodata->log("Error: NaN energy density.");
      throw -1;
    }
  }

  iodata->log( "Minimum fluid density: " + stringify(min) );
  iodata->log( "Maximum fluid density: " + stringify(max) );
  iodata->log( "Average fluctuation density: " + stringify(average(DIFFD_a)) );
  iodata->log( "Std.dev fluctuation density: " + stringify(standard_deviation(DIFFD_a)) );
  if(min < 0.0)
  {
    iodata->log("Error: negative density in some regions.");
    throw -1;
  }

# if USE_REFERENCE_FRW
  // Set values in reference FRW integrator
  auto & frw = bssn->frw;
  real_t K_FRW = -sqrt(24.0*PI*rho_FRW);
  frw->set_phi(0.0);
  frw->set_K(K_FRW);
  frw->addFluid(rho_m, 0.0 /* w=0 */);
# else
  arr_t & DIFFK_p = *bssn->fields["DIFFK_p"];
  arr_t & DIFFK_a = *bssn->fields["DIFFK_a"];
  // add in FRW pieces to ICs
  // phi is unchanged
  // rho (D) and K get contribs
  // w=0 fluid only
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);

    real_t D_FRW = rho_m; // on initial slice

    DIFFr_a[idx] += rho_m;

    DIFFK_a[idx] = -sqrt(24.0*PI*rho_FRW);
    DIFFK_p[idx] = -sqrt(24.0*PI*rho_FRW);

    DIFFD_a[idx] += D_FRW;
  }
# endif
}

void static_ic_set_semianalytic(
  BSSN * bssn, Static * stat, Lambda * lambda, Fourier * fourier,
  IOData * iodata)
{
  idx_t i, j, k;

  arr_t & DIFFr_a = *bssn->fields["DIFFr_a"];
  arr_t & DIFFphi_p = *bssn->fields["DIFFphi_p"];
  arr_t & DIFFD_a = *stat->fields["DIFFD_a"];
  arr_t & DIFFK_p = *bssn->fields["DIFFK_p"];

  real_t rho_FRW = 3.0/PI/8.0;
  real_t K_FRW = -3.0;

  real_t Omega_L = std::stod(_config("Omega_L", "0.0"));
  real_t rho_m = (1.0 - Omega_L) * rho_FRW;
  real_t rho_L = Omega_L * rho_FRW;
  lambda->setLambda(rho_L);
  real_t L = H_LEN_FRAC;
  real_t A = std::stod(_config("peak_amplitude", "0.001"))*0.026699*L*L;
  // grid values
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    real_t x = ((real_t) i) * dx;
    DIFFphi_p[idx] = std::log1p( A*std::sin(2.0*PI*x/L) );
    DIFFr_a[idx] = rho_m + 2.0*A*PI*std::sin((2*PI*x)/L)
      / ( L*L*std::pow(1 + A*std::sin((2*PI*x)/L), 5) );
    DIFFK_p[idx] = K_FRW;
    DIFFD_a[idx] = exp(6.0*DIFFphi_p[idx])*DIFFr_a[idx];
  }

  // Make sure min density value > 0
  // Set conserved density variable field
  real_t min = rho_m;
  real_t max = min;
  LOOP3(i,j,k)
  {
    real_t rho = DIFFr_a[NP_INDEX(i,j,k)];
    if(rho < min)
    {
      min = rho;
    }
    if(rho > max)
    {
      max = rho;
    }
    if(rho != rho)
    {
      iodata->log("Error: NaN energy density.");
      throw -1;
    }
  }

  iodata->log( "Minimum fluid density: " + stringify(min) );
  iodata->log( "Maximum fluid density: " + stringify(max) );
  iodata->log( "Average fluctuation density: " + stringify(average(DIFFD_a)) );
  iodata->log( "Std.dev fluctuation density: " + stringify(standard_deviation(DIFFD_a)) );
  if(min < 0.0)
  {
    iodata->log("Error: negative density in some regions.");
    throw -1;
  }
}

/**
 * @brief Spherical "shell" of perturbations around an observer
 */
void static_ic_set_sphere(BSSN * bssn, Static * stat, IOData * iodata)
{
  idx_t i, j, k;

  arr_t & DIFFr_a = *bssn->fields["DIFFr_a"];
  arr_t & DIFFphi_p = *bssn->fields["DIFFphi_p"];
  arr_t & DIFFphi_a = *bssn->fields["DIFFphi_a"];
  arr_t & DIFFphi_f = *bssn->fields["DIFFphi_f"];

  arr_t & DIFFD_a = *stat->fields["DIFFD_a"];

  // shell amplitude
  const real_t A = stod(_config("shell_amplitude", "1e-5"));
  // Shell described by only one fixed l:
  const idx_t l = stoi(_config("shell_angular_scale_l", "1"));
  iodata->log( "Generating ICs with shell angular scale of l = " + stringify(l) );
  iodata->log( "Generating ICs with peak amp. = " + stringify(A) );

  // spherical shell of perturbations in phi0field

  // place shell around center of box
  real_t x0 = (NX-0.001)*dx/2.0;
  real_t y0 = (NY-0.001)*dx/2.0;
  real_t z0 = (NZ-0.001)*dx/2.0;

  // place spherical "shell" of fluctuations at r = NX/4, 1/2-way between observer and boundary
  real_t r_shell = NX*dx / 4.0;
  // shell width
  real_t shell_width = NX*dx / 40.0;

  // Angular fluctuations in shell described by spherical harmonic coeffs, a_lm's,
  complex_t * alms = new complex_t[m_idx(l,l)+1];
  const real_t seed = stod(_config("mt19937_seed", "7"));
  std::mt19937 gen(seed);
  std::normal_distribution<> normal_dist(0.0, 1.0);
  std::uniform_real_distribution<> uniform_dist(0.0, 2.0*PI);

  std::cout << "normal_dist(gen) = " << normal_dist(gen) << ", uniform_dist(gen) = " << uniform_dist(gen) << "\n";

  // zero mode:
  alms[m_idx(l, 0)].first = normal_dist(gen);
  alms[m_idx(l, 0)].second = 0;
  // positive modes:
  for(int m = 1; m <= l; m++)
  {
    real_t phase = uniform_dist(gen);
    real_t amp = normal_dist(gen);
    alms[m_idx(l,m)].first = amp*std::cos(phase);
    alms[m_idx(l,m)].second = amp*std::sin(phase);
  }
  // negative modes:
  for(int m = -l; m <= -1; m++)
  {
    real_t Condon_Shortley_phase = std::abs(m) % 2 ? -1.0 : 1.0; // 0 (false) if m even, 1 (true) if odd
    alms[m_idx(l,m)].first = Condon_Shortley_phase*alms[m_idx(l,std::abs(m))].first;
    alms[m_idx(l,m)].second = -Condon_Shortley_phase*alms[m_idx(l,std::abs(m))].second;
  }

  for(int m = -l; m <= l; m++)
  {
    std::cout << "Amp. of a_{" << l << "," << m << "} = " << alms[m_idx(l,m)].first
      << " + " << alms[m_idx(l,m)].second << "i\n";
  }

  LOOP3(i,j,k) {
    idx_t idx = NP_INDEX(i,j,k);

    real_t x = i*dx;
    real_t y = j*dx;
    real_t z = k*dx;

    real_t r = sqrt( pw2(x - x0) + pw2(y - y0) + pw2(z - z0) );
    real_t theta = acos((z - z0) / r); // "theta" (polar angle)
    real_t phi = atan2(y - y0, x - x0); // "phi" (azimuthal angle)

    real_t DIFFphi_r = 0.0;
    real_t DIFFphi_i = 0.0;
    for(int m=-l; m<=l; m++)
    {
      real_t Y_r = boost::math::spherical_harmonic_r(l, m, theta, phi);
      real_t Y_i = boost::math::spherical_harmonic_i(l, m, theta, phi);

      DIFFphi_r += alms[m_idx(l,m)].first*Y_r - alms[m_idx(l,m)].second*Y_i;
      DIFFphi_i += alms[m_idx(l,m)].first*Y_i + alms[m_idx(l,m)].second*Y_r;
    }
    if(std::abs(DIFFphi_i) > 1e-6)
    {
      iodata->log("Significant non-zero imaginary component of solution exists!");
      throw -1;
    }

    // gaussian profile shell of fluctuations
    real_t U_r = A*std::exp( -pw2((r - r_shell)/2.0/shell_width) );
    // cosine profile
    // real_t U_r = (r < r_shell-shell_width || r > r_shell+shell_width ) ? 0 : A*(1+std::cos(PI*(r-r_shell)/shell_width));

    DIFFphi_p[idx] = U_r*DIFFphi_r;

    if(i==NX/2 && j==NY/2 && k==NZ/2)
      std::cout << "At a grid point close to the middle, r = " << r
                << ", r_shell = " << r_shell
                << ", shell_width = " << shell_width
                << ", U_r = " << U_r
                << ", DIFFphi_p = " << DIFFphi_p[idx] << "\n";
  }
  // cleanup
  delete [] alms;

  // delta_rho = -lap(phi)/(1+xi)^5/2pi
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k) {
    DIFFr_a[NP_INDEX(i,j,k)] = -0.5/PI/(
      pow(1.0 + DIFFphi_p[NP_INDEX(i,j,k)], 5.0)
    )*(
      double_derivative(i, j, k, 1, 1, DIFFphi_p)
      + double_derivative(i, j, k, 2, 2, DIFFphi_p)
      + double_derivative(i, j, k, 3, 3, DIFFphi_p)
    );
  }

  // phi = ln(xi)
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k) {
    idx_t idx = NP_INDEX(i,j,k);
    DIFFphi_a[idx] = log1p(DIFFphi_p[idx]);
    DIFFphi_f[idx] = log1p(DIFFphi_p[idx]);
    DIFFphi_p[idx] = log1p(DIFFphi_p[idx]);
  }

  // Make sure min density value > 0
  // Set conserved density variable field
  real_t min = 3.0/PI/8.0;
  real_t max = min;
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    real_t rho_FRW = 3.0/PI/8.0;
    real_t DIFFr = DIFFr_a[idx];
    real_t rho = rho_FRW + DIFFr;
    // phi_FRW = 0
    real_t DIFFphi = DIFFphi_a[idx];
    // phi = DIFFphi
    // DIFFK = 0

    DIFFD_a[idx] =
      rho_FRW*expm1(6.0*DIFFphi) + exp(6.0*DIFFphi)*DIFFr;

    if(rho < min)
    {
      min = rho;
    }
    if(rho > max)
    {
      max = rho;
    }
    if(rho != rho)
    {
      iodata->log("Error: NaN energy density.");
      throw -1;
    }
  }

  iodata->log( "Minimum fluid density: " + stringify(min) );
  iodata->log( "Maximum fluid density: " + stringify(max) );
  iodata->log( "Average fluctuation density: " + stringify(average(DIFFD_a)) );
  iodata->log( "Std.dev fluctuation density: " + stringify(standard_deviation(DIFFD_a)) );
  if(min < 0.0)
  {
    iodata->log("Error: negative density in some regions.");
    throw -1;
  }

# if USE_REFERENCE_FRW
  // Set values in reference FRW integrator
  real_t rho_FRW = 3.0/PI/8.0;
  real_t K_frw = -sqrt(24.0*PI*rho_FRW);

  auto & frw = bssn->frw;
  frw->set_phi(0.0);
  frw->set_K(K_frw);
  frw->addFluid(rho_FRW, 0.0 /* w=0 */);
# else
  arr_t & DIFFK_p = *bssn->fields["DIFFK_p"];
  arr_t & DIFFK_a = *bssn->fields["DIFFK_a"];
  // add in FRW pieces to ICs
  // phi is unchanged
  // rho (D) and K get contribs
  // w=0 fluid only
# pragma omp parallel for default(shared) private(i,j,k)
  LOOP3(i,j,k)
  {
    idx_t idx = NP_INDEX(i,j,k);
    real_t rho_FRW = 3.0/PI/8.0;
    real_t D_FRW = rho_FRW; // on initial slice

    DIFFr_a[idx] += rho_FRW;

    DIFFK_a[idx] = -sqrt(24.0*PI*rho_FRW);
    DIFFK_p[idx] = -sqrt(24.0*PI*rho_FRW);

    DIFFD_a[idx] += D_FRW;
  }
# endif
}

}


#include <iostream>
#include <iomanip>
#include <NTL/GF2X.h>
#include <stdint.h>
#include "xsadd.h"
#include "xsadd.c" // to check static functions

using namespace NTL;
using namespace std;

static void uztostring16(char * str, uz * x)
{
    int p = 0;
    int first = 1;
    for (int i = UZ_ARRAY_SIZE - 1; i >= 0; i--) {
	if (first) {
	    if (x->ar[i] != 0) {
		first = 0;
		sprintf(str, "%x", x->ar[i]);
		p = strlen(str);
	    }
	} else {
	    sprintf(str + p, "%04x", x->ar[i]);
	    p = p + 4;
	}
    }
    if (first) {
	strcpy(str, "0");
    }
}

//static const char * const characteristic_polynomial
//= "100000000008101840085118000000001";

void xsadd_calculate_jump_polynomial_debug(char *jump_str,
					   uint32_t mul_step,
					   const char * base_step)
{
    f2_polynomial jump_poly;
    f2_polynomial charcteristic;
    f2_polynomial tee;
    uz base;
    uz mul;
    uz step;
    char buff[200];

    strtopolynomial(&charcteristic, characteristic_polynomial);
    clear(&tee);
    tee.ar[0] = 2;
    string16touz(&base, base_step);
    uint32touz(&mul, mul_step);

    uztostring16(buff, &mul);
    cout << "mul:" << buff << endl;
    uztostring16(buff, &base);
    cout << "base:" << buff << endl;
    uz_mul(&step, &mul, &base);
    uztostring16(buff, &step);
    cout << "step:" << buff << endl;
    polynomial_power_mod(&jump_poly, &tee, &step, &charcteristic);
    polynomialtostr(jump_str, &jump_poly);
}

void xsadd_jump_by_polynomial_debug(xsadd_t *xsadd, const char * jump_str)
{
    f2_polynomial jump_poly;
    xsadd_t work_z;
    xsadd_t * work = &work_z;
    cout << "xsadd_jump_by_polynomial_debug: step1" << endl;
    *work = *xsadd;
    for (int i = 0; i < 4; i++) {
        work->state[i] = 0;
    }
    strtopolynomial(&jump_poly, jump_str);
    cout << "xsadd_jump_by_polynomial_debug: step2" << endl;
    char buff[200];
    polynomialtostr(buff, &jump_poly);
    cout << "jump_poly:" << buff << endl;
    for (int i = 0; i < POLYNOMIAL_ARRAY_SIZE; i++) {
	for (int j = 0; j < 32; j++) {
	    //cout << "(i,j) = (" << dec << i << "," << j << ")" << endl;
	    uint32_t mask = 1 << j;
	    if ((jump_poly.ar[i] & mask) != 0) {
		xsadd_add(work, xsadd);
	    }
	    xsadd_uint32(xsadd);
	}
    }
    cout << "xsadd_jump_by_polynomial_debug: step3" << endl;
    *xsadd = *work;
}

void xsadd_jump_debug(xsadd_t *xsadd,
		      uint32_t mul_step,
		      const char * base_step)
{
    char jump_str[200];
    cout << "step1" << endl;
    xsadd_calculate_jump_polynomial_debug(jump_str, mul_step, base_step);
    cout << "jump_str:" << jump_str << endl;
    cout << "step2" << endl;
    xsadd_jump_by_polynomial_debug(xsadd, jump_str);
    cout << "step3" << endl;
}

void test_xsadd_jump_debug()
{
    const char * base_step = "0";
    uint32_t step = 139;
    uint32_t seed = 1791095845;
    xsadd_t xsadd;
    xsadd_init(&xsadd, seed);
    xsadd_jump_debug(&xsadd, step, base_step);
    cout << "debug end" << endl;
}

int main() {
    test_xsadd_jump_debug();
}


/*
 * main_optimize.cpp
 * Date: 2013-01-28
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include <gpcxx/tree/basic_tree.hpp>
#include <gpcxx/generate/uniform_symbol.hpp>
#include <gpcxx/generate/node_generator.hpp>
#include <gpcxx/generate/ramp.hpp>
#include <gpcxx/operator/mutation.hpp>
#include <gpcxx/operator/simple_mutation_strategy.hpp>
#include <gpcxx/operator/random_selector.hpp>
#include <gpcxx/operator/tournament_selector.hpp>
#include <gpcxx/operator/crossover.hpp>
#include <gpcxx/operator/one_point_crossover_strategy.hpp>
#include <gpcxx/operator/reproduce.hpp>
#include <gpcxx/eval/static_eval.hpp>
#include <gpcxx/eval/regression_fitness.hpp>
#include <gpcxx/evolve/static_pipeline.hpp>
#include <gpcxx/io/best_individuals.hpp>
#include <gpcxx/stat/population_statistics.hpp>
#include <gpcxx/app/timer.hpp>
#include <gpcxx/app/normalize.hpp>
#include <gpcxx/app/generate_evenly_spaced_test_data.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <fstream>
#include <random>
#include <vector>
#include <functional>

const std::string tab = "\t";

namespace fusion = boost::fusion;

typedef double value_type;
typedef gpcxx::regression_training_data< value_type , 3 > trainings_data_type;
typedef std::mt19937 rng_type ;
typedef char symbol_type;
typedef std::array< value_type , 3 > eval_context_type;
typedef std::vector< value_type > fitness_type;

namespace pl = std::placeholders;

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

    trainings_data_type c = gpcxx::generate_evenly_spaced_test_data< 3 >( -5.0 , 5.0 + 0.1 , 0.4 , []( double x1 , double x2 , double x3 ) {
                        return  1.0 / ( 1.0 + pow( x1 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x2 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x3 , -4.0 ) ); } );
    gpcxx::normalize( c.y );

    std::ofstream fout1( "testdata.dat" );
    for( size_t i=0 ; i<c.x[0].size() ; ++i )
        fout1 << c.y[i] << " " << c.x[0][i] << " " << c.x[1][i] << " " << c.x[2][i] << "\n";
    fout1.close();

    auto eval = gpcxx::make_static_eval< value_type , symbol_type , eval_context_type >(
        fusion::make_vector(
            fusion::make_vector( 'x' , []( eval_context_type const& t ) { return t[0]; } )
          , fusion::make_vector( 'y' , []( eval_context_type const& t ) { return t[1]; } )
          , fusion::make_vector( 'z' , []( eval_context_type const& t ) { return t[2]; } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
          , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
          , fusion::make_vector( 'e' , []( double v ) -> double { return std::exp( v ); } )
          , fusion::make_vector( 'l' , []( double v ) -> double { return ( std::abs( v ) < 1.0e-20 ) ? log( 1.0e-20 ) : std::log( std::abs( v ) ); } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( '+' , std::plus< double >() )
          , fusion::make_vector( '-' , std::minus< double >() )
          , fusion::make_vector( '*' , std::multiplies< double >() )
          , fusion::make_vector( '/' , std::divides< double >() )
          ) );
    typedef decltype( eval ) eval_type;
    typedef eval_type::node_attribute_type node_attribute_type;

    typedef gpcxx::basic_tree< node_attribute_type > tree_type;
    typedef std::vector< tree_type > population_type;
    typedef gpcxx::static_pipeline< population_type , fitness_type , rng_type > evolver_type;

    size_t population_size = 1000;
    size_t generation_size = 20;
    double number_elite = 1;
    double mutation_rate = 0.0;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 8 , max_tree_height = 8;
    size_t tournament_size = 15;

    // generators< rng_type > gen( rng );
    auto terminal_gen = eval.get_terminal_symbol_distribution();
    auto unary_gen = eval.get_unary_symbol_distribution();
    auto binary_gen = eval.get_binary_symbol_distribution();
    gpcxx::node_generator< node_attribute_type , rng_type , 3 > node_generator {
        { 2.0 * double( terminal_gen.num_symbols() ) , 0 , terminal_gen } ,
        { double( unary_gen.num_symbols() ) , 1 , unary_gen } ,
        { double( binary_gen.num_symbols() ) , 2 , binary_gen } };

    auto tree_generator = gpcxx::make_ramp( rng , node_generator , min_tree_height , max_tree_height , 0.5 );

    evolver_type evolver( number_elite , mutation_rate , crossover_rate , reproduction_rate , rng );
    std::vector< double > fitness( population_size , 0.0 );
    std::vector< tree_type > population( population_size );

    auto fitness_f = gpcxx::regression_fitness< eval_type >( eval );
    evolver.mutation_function() = gpcxx::make_mutation(
        gpcxx::make_simple_mutation_strategy( rng , node_generator ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.crossover_function() = gpcxx::make_crossover(
        gpcxx::make_one_point_crossover_strategy( rng , max_tree_height ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.reproduction_function() = gpcxx::make_reproduce( gpcxx::make_tournament_selector( rng , tournament_size ) );

    gpcxx::timer timer;

    // initialize population with random trees and evaluate fitness
    timer.restart();
    for( size_t i=0 ; i<population.size() ; ++i )
    {
        tree_generator( population[i] );
        fitness[i] = fitness_f( population[i] , c );
    }
    std::cout << gpcxx::indent( 0 ) << "Generation time " << timer.seconds() << std::endl;
    std::cout << gpcxx::indent( 1 ) << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 1 , 10 ) << std::endl;
    std::cout << gpcxx::indent( 1 ) << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl;
    std::cout << gpcxx::indent( 1 ) << std::endl << std::endl;

    timer.restart();
    for( size_t generation=1 ; generation<=generation_size ; ++generation )
    {
        gpcxx::timer iteration_timer;
        iteration_timer.restart();
        evolver.next_generation( population , fitness );
        double evolve_time = iteration_timer.seconds();
        iteration_timer.restart();
        std::transform( population.begin() , population.end() , fitness.begin() , [&]( tree_type const &t ) { return fitness_f( t , c ); } );
        double eval_time = iteration_timer.seconds();

        std::cout << gpcxx::indent( 0 ) << "Generation " << generation << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Evolve time " << evolve_time << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Eval time " << eval_time << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 2 , 10 ) << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl << std::endl;
    }
    std::cout << "Overall time : " << timer.seconds() << std::endl;

    return 0;
}