// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

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

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

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

               Lambert Conformal Conic Alternative
               -----------------------------------

    This is Gerald Evenden's 2003 implementation of an alternative
    "almost" LCC, which has been in use historically, but which
    should NOT be used for new projects - i.e: use this implementation
    if you need interoperability with old data represented in this
    projection, but not in any other case.

    The code was originally discussed on the PROJ.4 mailing list in
    a thread archived over at

    http://lists.maptools.org/pipermail/proj/2003-March/000644.html

    It was discussed again in the thread starting at

    http://lists.maptools.org/pipermail/proj/2017-October/007828.html
        and continuing at
    http://lists.maptools.org/pipermail/proj/2017-November/007831.html

    which prompted Clifford J. Mugnier to add these clarifying notes:

    The French Army Truncated Cubic Lambert (partially conformal) Conic
    projection is the Legal system for the projection in France between
    the late 1800s and 1948 when the French Legislature changed the law
    to recognize the fully conformal version.

    It was (might still be in one or two North African prior French
    Colonies) used in North Africa in Algeria, Tunisia, & Morocco, as
    well as in Syria during the Levant.

    Last time I have seen it used was about 30+ years ago in
    Algeria when it was used to define Lease Block boundaries for
    Petroleum Exploration & Production.

    (signed)

    Clifford J. Mugnier, c.p., c.m.s.
    Chief of Geodesy
    LSU Center for GeoInformatics
    Dept. of Civil Engineering
    LOUISIANA STATE UNIVERSITY

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace lcca
    {

            static const int max_iter = 10;
            static const double del_tol = 1e-12;

            template <typename T>
            struct par_lcca
            {
                detail::en<T> en;
                T    r0, l, M0;
                T    C;
            };

            template <typename T> /* func to compute dr */
            inline T fS(T const& S, T const& C)
            {
                return(S * ( 1. + S * S * C));
            }

            template <typename T> /* deriv of fs */
            inline T fSp(T const& S, T const& C)
            {
                return(1. + 3.* S * S * C);
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_lcca_ellipsoid
                : public base_t_fi<base_lcca_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_lcca<T> m_proj_parm;

                inline base_lcca_ellipsoid(const Parameters& par)
                    : base_t_fi<base_lcca_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T S, r, dr;

                    S = pj_mlfn(lp_lat, sin(lp_lat), cos(lp_lat), this->m_proj_parm.en) - this->m_proj_parm.M0;
                    dr = fS(S, this->m_proj_parm.C);
                    r = this->m_proj_parm.r0 - dr;
                    xy_x = this->m_par.k0 * (r * sin( lp_lon *= this->m_proj_parm.l ) );
                    xy_y = this->m_par.k0 * (this->m_proj_parm.r0 - r * cos(lp_lon) );
                }

                // INVERSE(e_inverse)  ellipsoid & spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    T theta, dr, S, dif;
                    int i;

                    xy_x /= this->m_par.k0;
                    xy_y /= this->m_par.k0;
                    theta = atan2(xy_x , this->m_proj_parm.r0 - xy_y);
                    dr = xy_y - xy_x * tan(0.5 * theta);
                    lp_lon = theta / this->m_proj_parm.l;
                    S = dr;
                    for (i = max_iter; i ; --i) {
                        S -= (dif = (fS(S, this->m_proj_parm.C) - dr) / fSp(S, this->m_proj_parm.C));
                        if (fabs(dif) < del_tol) break;
                    }
                    if (!i) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    lp_lat = pj_inv_mlfn(S + this->m_proj_parm.M0, this->m_par.es, this->m_proj_parm.en);
                }

                static inline std::string get_name()
                {
                    return "lcca_ellipsoid";
                }

            };

            // Lambert Conformal Conic Alternative
            template <typename Parameters, typename T>
            inline void setup_lcca(Parameters& par, par_lcca<T>& proj_parm)
            {
                T s2p0, N0, R0, tan0;

                proj_parm.en = pj_enfn<T>(par.es);

                if (par.phi0 == 0.) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_lat_0_is_zero) );
                }
                proj_parm.l = sin(par.phi0);
                proj_parm.M0 = pj_mlfn(par.phi0, proj_parm.l, cos(par.phi0), proj_parm.en);
                s2p0 = proj_parm.l * proj_parm.l;
                R0 = 1. / (1. - par.es * s2p0);
                N0 = sqrt(R0);
                R0 *= par.one_es * N0;
                tan0 = tan(par.phi0);
                proj_parm.r0 = N0 / tan0;
                proj_parm.C = 1. / (6. * R0 * N0);
            }

    }} // namespace detail::lcca
    #endif // doxygen

    /*!
        \brief Lambert Conformal Conic Alternative projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin
        \par Example
        \image html ex_lcca.gif
    */
    template <typename T, typename Parameters>
    struct lcca_ellipsoid : public detail::lcca::base_lcca_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline lcca_ellipsoid(Params const& , Parameters const& par)
            : detail::lcca::base_lcca_ellipsoid<T, Parameters>(par)
        {
            detail::lcca::setup_lcca(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_lcca, lcca_ellipsoid, lcca_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(lcca_entry, lcca_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(lcca_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(lcca, lcca_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP


// Persistent Homology code by Hubert Wagner based on an implementation by Chao Chen
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <list>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <deque>
#include <cstring>
#include <ctime>
#include <blitz/array.h>
#include <blitz/tinyvec-et.h>

using namespace std;

const int BIG_INT = 0x7FFFFFFF;	//be careful, could be too small compared to # of cubes
typedef int CellNrType; // it could be long for really large inputs...

#include "PersistenceIO.h"
#include "Debugging.h"
#include "GeneralFiltration.h"

#include "InputRunner.h"

#include "PersistentPair.h"
#include "DataReaders.h"

#include "PersistenceCalculator.h"
#include "PersistenceCalcRunner.h"

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

	time_t startTime1, startTime2, endTime;

	if (argc < 2)
	{
		std::cout << "usage: " << argv[0] << " input_file.ext" << std::endl;
		return 1;
	}

	string input_file = argv[1];
	double pers_thd = 0.0;

	string lfile = "log.txt";
	string efile = "error.txt";
	DebuggerClass::init( false, lfile, efile );

	InputFileInfo input_file_info(input_file);

	int max_pers_pts = BIG_INT; // the maximal number of persistence pairs recorded

	fstream filestr;
	filestr.open (lfile.c_str(), fstream::out | fstream::trunc);
	//	filestr.open (LOG_FILE, fstream::out | fstream::ate);
	filestr << "################################################" << endl;
	filestr << "start computing persistence" << endl;
	filestr << "Input file = \'" << input_file << "\'" << endl;
	// filestr << "Output file = \'" << output_file << "\'" << endl;
	filestr << "Maximal number of persistence points recorded = " << max_pers_pts << endl;
	filestr.close();

	time(&startTime1);

	switch(input_file_info.dimension)
	{
	case 1:
		InputRunner<1>::run(input_file_info, pers_thd);
		break;
	case 2:
		InputRunner<2>::run(input_file_info, pers_thd);
		break;
	case 3:
		InputRunner<3>::run(input_file_info, pers_thd);
		break;
	case 4:
		InputRunner<4>::run(input_file_info, pers_thd);
		break;
	case 5:
		InputRunner<5>::run(input_file_info, pers_thd);
		break;
	case 6:
		InputRunner<6>::run(input_file_info, pers_thd);
		break;
	case 7:
		InputRunner<7>::run(input_file_info, pers_thd);
		break;
	case 8:
		InputRunner<8>::run(input_file_info, pers_thd);
		break;
	}

	time(&endTime);
	double ellapsed1 = difftime (endTime,startTime1);

	cout << "All calculations computed in " << setprecision(2) << ellapsed1/60.0 << endl;
	DebuggerClass::finish();

	return 0;
}


#include "puppeteer.hpp"

#include "file_io/vtk_solution_reader.hpp"
#include "file_io/vtk_solution_writer.hpp"
#include "governing_equations/cahn_hilliard/cahn_hilliard_2d_fd.hpp"
#include "governing_equations/cahn_hilliard/cahn_hilliard_3d_fd.hpp"
#include "governing_equations/cahn_hilliard/cahn_hilliard_initial_conditions.hpp"
#include "governing_equations/cahn_hilliard/cahn_hilliard_parameters.hpp"
#include "governing_equations/cahn_hilliard/cahn_hilliard_solution_monitor.hpp"
#include "governing_equations/initial_conditions.hpp"
#include "governing_equations/initial_conditions_from_file.hpp"
#include "governing_equations/residual_calculator.hpp"
#include "logger/logger.hpp"
#include "spatial_discretization/discretization_2d_cart.hpp"
#include "spatial_discretization/discretization_3d_cart.hpp"
#include "time_stepping/time_integrator.hpp"
#include "time_stepping/time_integrator_options.hpp"
#include "utils/registry.hpp"

#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <iomanip>

Puppeteer::Puppeteer(const std::vector<std::string>& cmd_line)
{
  ProgramOptionsParser parser;
  parser.parseInputOptions(cmd_line);

  Logger::get().initLogFile(Options::get().log_file());
  Logger::get().InfoMessage(parser.getConfigurationFileTemplate());

  const auto init_cond_file = Options::get().initial_condition_file();
  if (init_cond_file != "none") {
    solution_reader_ = std::make_unique<VTKSolutionReader>(init_cond_file);
    Logger::get().InfoMessage("Using initial conditions file " + init_cond_file);
  }

  file_output_string_ = [&]() {
    if (Options::get().solution_output_file().empty()) {
      return Options::get().log_file();
    }
    return Options::get().solution_output_file();
  }();

  //
  // construct the primary simulation components
  //
  geom_               = geomFactory();
  rhs_                = rhsFactory(*geom_);
  initial_conditions_ = initialConditionsFactory();
  time_integrator_    = timeIntegratorFactory(*rhs_);

  //
  // construct and attach observers
  //
  attachResidualObservers(*time_integrator_, *rhs_);
  attachSolutionObservers(*time_integrator_, *geom_, *rhs_);
  attachTimeObservers(*time_integrator_);

  time_integrator_->setNotifyFrequency(Event::TimeStepComplete, Options::get().log_frequency());
  time_integrator_->setNotifyFrequency(Event::SolutionUpdate, Options::get().save_every());
}

Puppeteer::~Puppeteer() = default;

void
Puppeteer::run()
{
  profile();
  auto run_timer = Logger::get().timer("Starting solver");

  time_integrator_->solve(*rhs_, *initial_conditions_);

  write_solution_to_vtk(time_integrator_->getCurrentSolutionState(), *geom_, file_output_string_ + "_final");

  Logger::get().InfoMessage(Profiler::get().finalize());
}

namespace {
BCType
parseBC(const std::string bcstr)
{
  if (bcstr == "periodic") {
    return BCType::Periodic;
  }
  if (bcstr == "neumann") {
    return BCType::Neumann;
  }
  Logger::get().FatalMessage("BC type string " + bcstr + " not recognized.");
  abort();
}
}  // namespace

std::unique_ptr<SpatialDiscretization>
Puppeteer::geomFactory()
{
  CartesianDomainDefinition domain;

  if (solution_reader_) {
    domain = solution_reader_->getCartesianDomain();
  }
  else {
    domain.nx   = Options::get().domain_resolution();
    domain.ny   = domain.nx;
    domain.nz   = domain.nx;
    domain.xbeg = Options::get().domain_x_begin();
    domain.xend = Options::get().domain_x_end();
    domain.ybeg = Options::get().domain_x_begin();
    domain.zbeg = Options::get().domain_x_begin();
  }

  domain.nhalo = 2;  // should be dependent on equation and order / stencil size

  domain.xbc = parseBC(Options::get().bc_x());
  domain.ybc = parseBC(Options::get().bc_y());
  domain.zbc = parseBC(Options::get().bc_z());

  const int dim = Options::get().dimension();
  switch (dim) {
    case 2:
      return std::make_unique<Discretization2DCart>(domain);
    case 3:
      return std::make_unique<Discretization3DCart>(domain);
    default:
      Logger::get().FatalMessage("Only dimensions 2 and 3 supported.");
  }
  return nullptr;
}

std::unique_ptr<TimeIntegrableRHS>
Puppeteer::rhsFactory(SpatialDiscretization& geom)
{
  Logger::get().FatalAssert(
      Options::get().equation_type() == "cahn-hilliard", "Only equation_type=cahn-hilliard currently supported.");

  CahnHilliardParameters params;

  const int dim = Options::get().dimension();
  switch (dim) {
    case 2:
      return std::make_unique<CahnHilliard2DFD>(dynamic_cast<Discretization2DCart&>(geom), params);
      ;
    case 3:
      return std::make_unique<CahnHilliard3DFD>(dynamic_cast<Discretization3DCart&>(geom), params);
      ;
    default:
      Logger::get().FatalMessage("Only dimensions 2 and 3 supported.");
  }

  return nullptr;
}

std::unique_ptr<InitialConditions>
Puppeteer::initialConditionsFactory()
{
  Logger::get().FatalAssert(
      Options::get().equation_type() == "cahn-hilliard", "Only equation_type=cahn-hilliard currently supported.");

  if (solution_reader_) {
    return std::make_unique<InitialConditionsFromFile>(*solution_reader_);
  }
  else {
    CahnHilliardParameters params;
    return std::make_unique<CahnHilliardInitialConditions>(params);
  }
}

std::unique_ptr<TimeIntegrator>
Puppeteer::timeIntegratorFactory(TimeIntegrableRHS& rhs)
{
  auto time_opts = getTimeOptionsUsingGlobalOptions();
  return FactoryRegistry<TimeIntegrator>::get().lookup(Options::get().time_integrator())(rhs, time_opts);
}

void
Puppeteer::attachTimeObservers(TimeIntegrator& time_integrator)
{
  time_integrator.registerObserver(
      Event::TimeStepComplete, [&]() { Logger::get().setTimeMonitor(time_integrator.getIterationStatus()); });

  time_integrator.registerObserver(Event::TimeStepComplete, [&]() { Logger::get().updateLog(); });
}

void
Puppeteer::attachResidualObservers(TimeIntegrator& time_integrator, TimeIntegrableRHS& rhs)
{
  time_integrator.registerObserver(Event::TimeStepComplete, [&]() {
    Logger::get().setResidualMonitor(residual_calculator(rhs, time_integrator.getCurrentResidual()));
  });
}

void
Puppeteer::attachSolutionObservers(TimeIntegrator& time_integrator, SpatialDiscretization& geom, TimeIntegrableRHS& rhs)
{
  const int dim = Options::get().dimension();
  switch (dim) {
    case 2:
      time_integrator.registerObserver(Event::TimeStepComplete, [&]() {
        Logger::get().setSolutionMonitor(cahn_hilliard_solution_monitor(
            rhs, dynamic_cast<Discretization2DCart&>(geom), time_integrator.getCurrentSolutionState()));
      });
      break;
    case 3:
      time_integrator.registerObserver(Event::TimeStepComplete, [&]() {
        Logger::get().setSolutionMonitor(cahn_hilliard_solution_monitor(
            rhs, dynamic_cast<Discretization3DCart&>(geom), time_integrator.getCurrentSolutionState()));
      });
      break;
    default:
      Logger::get().FatalMessage("Only dimensions 2 and 3 supported.");
  }

  const int save_every = Options::get().save_every();

  if (save_every) {
    time_integrator.registerObserver(Event::SolutionUpdate, [&]() {
      const int   iterwidth  = int(log10(Options::get().max_time_steps()) + 1);
      std::string iterstring = boost::lexical_cast<std::string>(time_integrator.getCurrentStep());
      iterstring.erase(std::remove(iterstring.begin(), iterstring.end(), ','), iterstring.end());

      std::ostringstream ss;
      ss << std::setw(iterwidth) << std::setfill('0') << iterstring;
      write_solution_to_vtk(
          time_integrator_->getCurrentSolutionState(), *geom_, file_output_string_ + "_step_" + ss.str());
    });
  }
}


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

#include "base/pose.h"

#include <Eigen/Eigenvalues>

#include "base/projection.h"
#include "base/triangulation.h"

namespace colmap {

Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d& vector) {
  Eigen::Matrix3d matrix;
  matrix << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1),
      vector(0), 0;
  return matrix;
}

void RotationMatrixToEulerAngles(const Eigen::Matrix3d& R, double* rx,
                                 double* ry, double* rz) {
  *rx = std::atan2(-R(1, 2), R(2, 2));
  *ry = std::asin(R(0, 2));
  *rz = std::atan2(-R(0, 1), R(0, 0));

  *rx = IsNaN(*rx) ? 0 : *rx;
  *ry = IsNaN(*ry) ? 0 : *ry;
  *rz = IsNaN(*rz) ? 0 : *rz;
}

Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry,
                                            const double rz) {
  const Eigen::Matrix3d Rx =
      Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix();
  const Eigen::Matrix3d Ry =
      Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix();
  const Eigen::Matrix3d Rz =
      Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix();
  return Rz * Ry * Rx;
}

Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d& rot_mat) {
  const Eigen::Quaterniond quat(rot_mat);
  return EigenQuaternionToQuaternion(quat);
}

Eigen::Vector4d EigenQuaternionToQuaternion(const Eigen::Quaterniond& quat) {
  return Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}

Eigen::Quaterniond QuaternionToEigenQuaternion(const Eigen::Vector4d& quat) {
  return Eigen::Quaterniond(quat(0), quat(1), quat(2), quat(3));
}

Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d& qvec) {
  const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec);
  const Eigen::Quaterniond quat(normalized_qvec(0), normalized_qvec(1),
                                normalized_qvec(2), normalized_qvec(3));
  return quat.toRotationMatrix();
}

Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec) {
  const double norm = qvec.norm();
  if (norm == 0) {
    // We do not just use (1, 0, 0, 0) because that is a constant and when used
    // for automatic differentiation that would lead to a zero derivative.
    return Eigen::Vector4d(1.0, qvec(1), qvec(2), qvec(3));
  } else {
    return qvec / norm;
  }
}

Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d& qvec) {
  return Eigen::Vector4d(qvec(0), -qvec(1), -qvec(2), -qvec(3));
}

Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d& qvec1,
                                       const Eigen::Vector4d& qvec2) {
  const Eigen::Vector4d normalized_qvec1 = NormalizeQuaternion(qvec1);
  const Eigen::Vector4d normalized_qvec2 = NormalizeQuaternion(qvec2);
  const Eigen::Quaterniond quat1(normalized_qvec1(0), normalized_qvec1(1),
                                 normalized_qvec1(2), normalized_qvec1(3));
  const Eigen::Quaterniond quat2(normalized_qvec2(0), normalized_qvec2(1),
                                 normalized_qvec2(2), normalized_qvec2(3));
  const Eigen::Quaterniond cat_quat = quat2 * quat1;
  return NormalizeQuaternion(EigenQuaternionToQuaternion(cat_quat));
}

Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d& qvec,
                                      const Eigen::Vector3d& point) {
  const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec);
  const Eigen::Quaterniond quat(normalized_qvec(0), normalized_qvec(1),
                                normalized_qvec(2), normalized_qvec(3));
  return quat * point;
}

Eigen::Vector4d AverageQuaternions(const std::vector<Eigen::Vector4d>& qvecs,
                                   const std::vector<double>& weights) {
  CHECK_EQ(qvecs.size(), weights.size());
  CHECK_GT(qvecs.size(), 0);

  if (qvecs.size() == 1) {
    return qvecs[0];
  }

  Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
  double weight_sum = 0;

  for (size_t i = 0; i < qvecs.size(); ++i) {
    CHECK_GT(weights[i], 0);
    const Eigen::Vector4d qvec = NormalizeQuaternion(qvecs[i]);
    A += weights[i] * qvec * qvec.transpose();
    weight_sum += weights[i];
  }

  A.array() /= weight_sum;

  const Eigen::Matrix4d eigenvectors =
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d>(A).eigenvectors();

  return eigenvectors.col(3);
}

Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d& vector1,
                                        const Eigen::Vector3d& vector2) {
  const Eigen::Vector3d v1 = vector1.normalized();
  const Eigen::Vector3d v2 = vector2.normalized();
  const Eigen::Vector3d v = v1.cross(v2);
  const Eigen::Matrix3d v_x = CrossProductMatrix(v);
  const double c = v1.dot(v2);
  if (c == -1) {
    return Eigen::Matrix3d::Identity();
  } else {
    return Eigen::Matrix3d::Identity() + v_x + 1 / (1 + c) * (v_x * v_x);
  }
}

Eigen::Vector3d ProjectionCenterFromMatrix(
    const Eigen::Matrix3x4d& proj_matrix) {
  return -proj_matrix.leftCols<3>().transpose() * proj_matrix.rightCols<1>();
}

Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d& qvec,
                                         const Eigen::Vector3d& tvec) {
  // Inverse rotation as conjugate quaternion.
  const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec);
  const Eigen::Quaterniond quat(normalized_qvec(0), -normalized_qvec(1),
                                -normalized_qvec(2), -normalized_qvec(3));
  return quat * -tvec;
}

void ComputeRelativePose(const Eigen::Vector4d& qvec1,
                         const Eigen::Vector3d& tvec1,
                         const Eigen::Vector4d& qvec2,
                         const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12,
                         Eigen::Vector3d* tvec12) {
  const Eigen::Vector4d inv_qvec1 = InvertQuaternion(qvec1);
  *qvec12 = ConcatenateQuaternions(inv_qvec1, qvec2);
  *tvec12 = tvec2 - QuaternionRotatePoint(*qvec12, tvec1);
}

void ConcatenatePoses(const Eigen::Vector4d& qvec1,
                      const Eigen::Vector3d& tvec1,
                      const Eigen::Vector4d& qvec2,
                      const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12,
                      Eigen::Vector3d* tvec12) {
  *qvec12 = ConcatenateQuaternions(qvec1, qvec2);
  *tvec12 = tvec2 + QuaternionRotatePoint(qvec2, tvec1);
}

void InvertPose(const Eigen::Vector4d& qvec, const Eigen::Vector3d& tvec,
                Eigen::Vector4d* inv_qvec, Eigen::Vector3d* inv_tvec) {
  *inv_qvec = InvertQuaternion(qvec);
  *inv_tvec = -QuaternionRotatePoint(*inv_qvec, tvec);
}

void InterpolatePose(const Eigen::Vector4d& qvec1, const Eigen::Vector3d& tvec1,
                     const Eigen::Vector4d& qvec2, const Eigen::Vector3d& tvec2,
                     const double t, Eigen::Vector4d* qveci,
                     Eigen::Vector3d* tveci) {
  const Eigen::Vector4d normalized_qvec1 = NormalizeQuaternion(qvec1);
  const Eigen::Vector4d normalized_qvec2 = NormalizeQuaternion(qvec2);
  const Eigen::Quaterniond quat1(normalized_qvec1(0), normalized_qvec1(1),
                                 normalized_qvec1(2), normalized_qvec1(3));
  const Eigen::Quaterniond quat2(normalized_qvec2(0), normalized_qvec2(1),
                                 normalized_qvec2(2), normalized_qvec2(3));
  const Eigen::Vector3d tvec12 = tvec2 - tvec1;

  const Eigen::Quaterniond quati = quat1.slerp(t, quat2);

  *qveci = EigenQuaternionToQuaternion(quati);
  *tveci = tvec1 + tvec12 * t;
}

Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d& qvec1,
                                  const Eigen::Vector3d& tvec1,
                                  const Eigen::Vector4d& qvec2,
                                  const Eigen::Vector3d& tvec2) {
  const Eigen::Vector3d center1 = ProjectionCenterFromPose(qvec1, tvec1);
  const Eigen::Vector3d center2 = ProjectionCenterFromPose(qvec2, tvec2);
  return center2 - center1;
}

bool CheckCheirality(const Eigen::Matrix3d& R, const Eigen::Vector3d& t,
                     const std::vector<Eigen::Vector2d>& points1,
                     const std::vector<Eigen::Vector2d>& points2,
                     std::vector<Eigen::Vector3d>* points3D) {
  CHECK_EQ(points1.size(), points2.size());
  const Eigen::Matrix3x4d proj_matrix1 = Eigen::Matrix3x4d::Identity();
  const Eigen::Matrix3x4d proj_matrix2 = ComposeProjectionMatrix(R, t);
  const double kMinDepth = std::numeric_limits<double>::epsilon();
  const double max_depth = 1000.0f * (R.transpose() * t).norm();
  points3D->clear();
  for (size_t i = 0; i < points1.size(); ++i) {
    const Eigen::Vector3d point3D =
        TriangulatePoint(proj_matrix1, proj_matrix2, points1[i], points2[i]);
    const double depth1 = CalculateDepth(proj_matrix1, point3D);
    if (depth1 > kMinDepth && depth1 < max_depth) {
      const double depth2 = CalculateDepth(proj_matrix2, point3D);
      if (depth2 > kMinDepth && depth2 < max_depth) {
        points3D->push_back(point3D);
      }
    }
  }
  return !points3D->empty();
}

}  // namespace colmap


/*!
@file
Internal header to break cyclic dependencies.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_LOGICAL_DETAIL_LOGICAL_FWD_HPP
#define BOOST_HANA_LOGICAL_DETAIL_LOGICAL_FWD_HPP

#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/variadic/foldl.hpp>

namespace boost { namespace hana {
    namespace logical_detail { namespace operators { struct enable { }; }}

    //! @ingroup group-typeclasses
    //! The `Logical` type class is for data types acting like a boolean.
    //!
    //! @anchor Logical_terminology
    //! ### Terminology
    //! Let `x` be a `Logical`. Then, we say that `x` is true-valued if and
    //! only if `or_(x, y) == x` for all `Logical`s `y`. Conversely, we say
    //! that `x` is false-valued if and only if `and_(x, y) == y` for all
    //! `Logical`s `y`.
    //!
    //! Also, we say that `x` is a _compile-time_ `Logical` if and only if
    //! it can have values of different types in both branches of the `if_`
    //! method.
    //!
    //!
    //! @bug
    //! We don't short-circuit right now. Don't forget to change the examples
    //! and unit tests when that's implemented.
    //!
    //! @todo
    //! Consider making this a real boolean algebra.
    struct Logical {
        BOOST_HANA_TYPECLASS(Logical);
        struct mcd;
        using operators = logical_detail::operators::enable;
    };

    //! Conditionally return one of two values based on a condition.
    //! @relates Logical
    //!
    //! Specifically, `then_` is returned iff `logical` is true-valued, and
    //! `else_` is returned otherwise. Note that some `Logical` instances
    //! may allow values of different types while others may require both
    //! values to have the same type.
    //!
    //!
    //! @param logical
    //! The condition determining which of the two values is returned.
    //!
    //! @param then_
    //! The value returned when `logical` is true-valued.
    //!
    //! @param else_
    //! The value returned when `logical` is false-valued.
    //!
    //!
    //! ### Example
    //! @snippet example/logical/if.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto if_ = [](auto logical, auto then_, auto else_) {
        return Logical::instance<
            datatype_t<decltype(logical)>
        >::if_impl(logical, then_, else_);
    };

    //! Conditionally execute one of two branches based on a condition.
    //! @relates Logical
    //!
    //! The selected branch will be invoked with an identity function, wich
    //! allows making types and values dependent inside a lambda and achieve
    //! a lazy-like behavior. However, type instantiation laziness can only
    //! be achieved with `Integral` conditions or equivalent. The result of
    //! the `eval_if` is the result of the invoked branch.
    //!
    //!
    //! @param logical
    //! The condition determining which of the two branches is selected.
    //!
    //! @param then_branch
    //! A function called as `then_branch([](auto x) { return x; })` iff
    //! `logical` is true-valued.
    //!
    //! @param else_branch
    //! A function called as `else_branch([](auto x) { return x; })` iff
    //! `logical` is false-valued.
    //!
    //!
    //! ### Example (purely compile-time condition)
    //! @snippet example/logical/eval_if.cpp heterogeneous
    //!
    //! ### Example (runtime or `constexpr` condition)
    //! @snippet example/logical/eval_if.cpp homogeneous
    BOOST_HANA_CONSTEXPR_LAMBDA auto eval_if = [](auto logical, auto then_branch, auto else_branch) {
        return Logical::instance<
            datatype_t<decltype(logical)>
        >::eval_if_impl(logical, then_branch, else_branch);
    };

    //! Negates a `Logical`.
    //! @relates Logical
    //!
    //! Specifically, `not_(x)` returns a false-valued `Logical` iff `x`
    //! is a true-valued `Logical`, and a true-valued one otherwise.
    //!
    //! ### Example
    //! @snippet example/logical/not.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto not_ = [](auto logical) {
        return Logical::instance<
            datatype_t<decltype(logical)>
        >::not_impl(logical);
    };

    namespace logical_detail {
        BOOST_HANA_CONSTEXPR_LAMBDA auto and2 = [](auto x, auto y) {
            return Logical::instance<datatype_t<decltype(x)>>::and_impl(x, y);
        };

        BOOST_HANA_CONSTEXPR_LAMBDA auto or2 = [](auto x, auto y) {
            return Logical::instance<datatype_t<decltype(x)>>::or_impl(x, y);
        };
    }

    //! Return whether all the arguments are true-valued.
    //! @relates Logical
    //!
    //! `and_` can be called with one argument or more. When called with
    //! two arguments, `and_` dispatches to the type class implementation.
    //! Otherwise,
    //! @code
    //!     and_(x) == x
    //!     and_(x, y, ...z) == and_(and_(x, y), z...)
    //! @endcode
    //!
    //! ### Example
    //! @snippet example/logical/and.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto and_ = [](auto x, auto ...y) {
        return detail::variadic::foldl(logical_detail::and2, x, y...);
    };

    //! Return whether any of the arguments is true-valued.
    //! @relates Logical
    //!
    //! `or_` can be called with one argument or more. When called with
    //! two arguments, `or_` dispatches to the type class implementation.
    //! Otherwise,
    //! @code
    //!     or_(x) == x
    //!     or_(x, y, ...z) == or_(or_(x, y), z...)
    //! @endcode
    //!
    //! ### Example
    //! @snippet example/logical/or.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto or_ = [](auto x, auto ...y) {
        return detail::variadic::foldl(logical_detail::or2, x, y...);
    };

    namespace logical_detail { namespace operators {
        //! Equivalent to `and_`.
        //! @relates boost::hana::Logical
        template <typename X, typename Y>
        constexpr auto operator&&(X x, Y y)
        { return and_(x, y); }

        //! Equivalent to `or_`.
        //! @relates boost::hana::Logical
        template <typename X, typename Y>
        constexpr auto operator||(X x, Y y)
        { return or_(x, y); }

        //! Equivalent to `not_`.
        //! @relates boost::hana::Logical
        template <typename X>
        constexpr auto operator!(X x)
        { return not_(x); }
    }}
}} // end namespace boost::hana

#endif // !BOOST_HANA_LOGICAL_DETAIL_LOGICAL_FWD_HPP


/*
 * Copyright 2009-2020 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 <exception>
#include <fstream>

// Third party includes
#include <boost/format.hpp>
#include <boost/progress.hpp>

// VOTCA includes
#include <votca/tools/constants.h>
#include <votca/tools/property.h>

// Local VOTCA includes
#include "votca/xtp/eigen.h"
#include "votca/xtp/gnode.h"
#include "votca/xtp/qmstate.h"
#include "votca/xtp/topology.h"

// Local private VOTCA includes
#include "kmclifetime.h"

namespace votca {
namespace xtp {

void KMCLifetime::ParseSpecificOptions(const tools::Property& options) {

  insertions_ = options.get(".numberofinsertions").as<unsigned long>();
  lifetimefile_ = options.get(".lifetimefile").as<std::string>();

  probfile_ = options.ifExistsReturnElseReturnDefault<std::string>(
      ".decayprobfile", "");

  const tools::Property& carrier_options = options.get(".carrierenergy");
  do_carrierenergy_ = carrier_options.get(".run").as<bool>();
  energy_outputfile_ = carrier_options.get(".outputfile").as<std::string>();
  alpha_ = carrier_options.get(".alpha").as<double>();
  outputsteps_ = carrier_options.get(".outputsteps").as<unsigned long>();

  log_.setReportLevel(Log::current_level);
  log_.setCommonPreface("\n ...");

  carriertype_ = QMStateType(QMStateType::Singlet);
  XTP_LOG(Log::error, log_)
      << "carrier type:" << carriertype_.ToLongString() << std::flush;
  field_ = Eigen::Vector3d::Zero();
}

void KMCLifetime::WriteDecayProbability(std::string filename) {

  Eigen::VectorXd outrates = Eigen::VectorXd::Zero(nodes_.size());
  Eigen::VectorXd inrates = Eigen::VectorXd::Zero(nodes_.size());
  Eigen::VectorXd decayrates = Eigen::VectorXd::Ones(nodes_.size());

  for (unsigned i = 0; i < nodes_.size(); i++) {
    GNode& node = nodes_[i];
    if (node.canDecay()) {
      for (const GLink& event : node.Events()) {
        if (event.isDecayEvent()) {
          decayrates[i] = event.getRate();
        } else {
          inrates[event.getDestination()->getId()] += event.getRate();
          outrates[i] += event.getRate();
        }
      }
    }
  }
  outrates = decayrates.cwiseQuotient(outrates + decayrates);
  inrates = decayrates.cwiseQuotient(inrates + decayrates);

  std::fstream probs;
  probs.open(filename, std::fstream::out);
  if (!probs.is_open()) {
    std::string error_msg = "Unable to write to file " + filename;
    throw std::runtime_error(error_msg);
  }
  probs << "#SiteID, Relative Prob outgoing, Relative Prob ingoing\n";
  for (unsigned i = 0; i < nodes_.size(); i++) {
    probs << nodes_[i].getId() << " " << outrates[i] << " " << inrates[i]
          << std::endl;
  }
  probs.close();
  return;
}

void KMCLifetime::ReadLifetimeFile(std::string filename) {
  tools::Property xml;
  xml.LoadFromXML(filename);
  std::vector<tools::Property*> jobProps = xml.Select("lifetimes.site");
  if (jobProps.size() != nodes_.size()) {
    throw std::runtime_error(
        (boost::format("The number of sites in the topology: %i does not match "
                       "the number in the lifetimefile: %i") %
         nodes_.size() % jobProps.size())
            .str());
  }

  for (tools::Property* prop : jobProps) {
    Index site_id = prop->getAttribute<Index>("id");
    double lifetime = boost::lexical_cast<double>(prop->value());
    bool check = false;
    for (auto& node : nodes_) {
      if (node.getId() == site_id && !(node.canDecay())) {
        node.AddDecayEvent(1.0 / lifetime);
        check = true;
        break;
      } else if (node.getId() == site_id && node.canDecay()) {
        throw std::runtime_error(
            (boost::format("Node %i appears twice in your list") % site_id)
                .str());
      }
    }
    if (!check) {
      throw std::runtime_error(
          (boost::format("Site from file with id: %i not found in state file") %
           site_id)
              .str());
    }
  }
  for (auto& node : nodes_) {
    node.InitEscapeRate();
    node.MakeHuffTree();
  }
  return;
}

void KMCLifetime::WriteToTraj(std::fstream& traj, unsigned long insertioncount,
                              double simtime,
                              const Chargecarrier& affectedcarrier) const {
  const Eigen::Vector3d& dr_travelled = affectedcarrier.get_dRtravelled();
  traj << simtime << "\t" << insertioncount << "\t" << affectedcarrier.getId()
       << "\t" << affectedcarrier.getLifetime() << "\t"
       << affectedcarrier.getSteps() << "\t"
       << affectedcarrier.getCurrentNodeId() + 1 << "\t"
       << dr_travelled.x() * tools::conv::bohr2nm << "\t"
       << dr_travelled.y() * tools::conv::bohr2nm << "\t"
       << dr_travelled.z() * tools::conv::bohr2nm << std::endl;
}

void KMCLifetime::RunVSSM() {

  std::chrono::time_point<std::chrono::system_clock> realtime_start =
      std::chrono::system_clock::now();
  XTP_LOG(Log::error, log_)
      << "\nAlgorithm: VSSM for Multiple Charges with finite Lifetime\n"
         "number of charges: "
      << numberofcarriers_ << "\nnumber of nodes: " << nodes_.size()
      << std::flush;

  if (numberofcarriers_ > Index(nodes_.size())) {
    throw std::runtime_error(
        "ERROR in kmclifetime: specified number of charges is greater than the "
        "number of nodes. This conflicts with single occupation.");
  }

  std::fstream traj;
  std::fstream energyfile;

  XTP_LOG(Log::error, log_)
      << "Writing trajectory to " << trajectoryfile_ << "." << std::flush;

  traj.open(trajectoryfile_, std::fstream::out);
  if (!traj.is_open()) {
    std::string error_msg = "Unable to write to file " + trajectoryfile_;
    throw std::runtime_error(error_msg);
  }

  traj << "#Simtime [s]\t Insertion\t Carrier ID\t Lifetime[s]\tSteps\t Last "
          "Segment\t x_travelled[nm]\t y_travelled[nm]\t z_travelled[nm]\n";

  if (do_carrierenergy_) {

    XTP_LOG(Log::error, log_)
        << "Tracking the energy of one charge carrier and exponential average "
           "with alpha="
        << alpha_ << " to " << energy_outputfile_ << std::flush;
    energyfile.open(energy_outputfile_, std::fstream::out);
    if (!energyfile.is_open()) {
      std::string error_msg = "Unable to write to file " + energy_outputfile_;
      throw std::runtime_error(error_msg);
    }

    energyfile << "Simtime [s]\tSteps\tCarrier ID\tEnergy_a=" << alpha_
               << "[eV]\n";
  }

  // Injection
  XTP_LOG(Log::error, log_)
      << "\ninjection method: " << injectionmethod_ << std::flush;

  RandomlyCreateCharges();

  unsigned long insertioncount = 0;
  unsigned long step = 0;
  double simtime = 0.0;

  std::vector<GNode*> forbiddennodes;
  std::vector<GNode*> forbiddendests;

  time_t now = time(nullptr);
  tm* localtm = localtime(&now);
  XTP_LOG(Log::error, log_)
      << "Run started at " << asctime(localtm) << std::flush;

  double avlifetime = 0.0;
  double meanfreepath = 0.0;
  Eigen::Vector3d difflength_squared = Eigen::Vector3d::Zero();

  double avgenergy = carriers_[0].getCurrentEnergy();
  Index carrieridold = carriers_[0].getId();

  while (insertioncount < insertions_) {
    std::chrono::duration<double> elapsed_time =
        std::chrono::system_clock::now() - realtime_start;
    if (elapsed_time.count() > (maxrealtime_ * 60. * 60.)) {
      XTP_LOG(Log::error, log_)
          << "\nReal time limit of " << maxrealtime_ << " hours ("
          << Index(maxrealtime_ * 60 * 60 + 0.5)
          << " seconds) has been reached. Stopping here.\n"
          << std::flush;
      break;
    }

    double cumulated_rate = 0;

    for (const auto& carrier : carriers_) {
      cumulated_rate += carrier.getCurrentEscapeRate();
    }
    if (cumulated_rate == 0) {  // this should not happen: no possible jumps
                                // defined for a node
      throw std::runtime_error(
          "ERROR in kmclifetime: Incorrect rates in the database file. All the "
          "escape rates for the current setting are 0.");
    }
    // go forward in time
    double dt = Promotetime(cumulated_rate);

    if (do_carrierenergy_) {
      bool print = false;
      if (carriers_[0].getId() > carrieridold) {
        avgenergy = carriers_[0].getCurrentEnergy();
        print = true;
        carrieridold = carriers_[0].getId();
      } else if (step % outputsteps_ == 0) {
        avgenergy =
            alpha_ * carriers_[0].getCurrentEnergy() + (1 - alpha_) * avgenergy;
        print = true;
      }
      if (print) {
        energyfile << simtime << "\t" << step << "\t" << carriers_[0].getId()
                   << "\t" << avgenergy * tools::conv::hrt2ev << std::endl;
      }
    }

    simtime += dt;
    step++;
    for (auto& carrier : carriers_) {
      carrier.updateLifetime(dt);
      carrier.updateSteps(1);
      carrier.updateOccupationtime(dt);
    }

    ResetForbiddenlist(forbiddennodes);
    bool secondlevel = true;
    while (secondlevel) {

      // determine which carrier will escape
      GNode* newnode = nullptr;
      Chargecarrier* affectedcarrier = ChooseAffectedCarrier(cumulated_rate);

      if (CheckForbidden(affectedcarrier->getCurrentNode(), forbiddennodes)) {
        continue;
      }

      // determine where it will jump to
      ResetForbiddenlist(forbiddendests);
      boost::progress_display progress(insertions_);

      while (true) {
        // LEVEL 2

        newnode = nullptr;
        const GLink& event =
            ChooseHoppingDest(affectedcarrier->getCurrentNode());

        if (event.isDecayEvent()) {
          const Eigen::Vector3d& dr_travelled =
              affectedcarrier->get_dRtravelled();
          avlifetime += affectedcarrier->getLifetime();
          meanfreepath += dr_travelled.norm();
          difflength_squared += dr_travelled.cwiseAbs2();
          WriteToTraj(traj, insertioncount, simtime, *affectedcarrier);
          ++progress;
          RandomlyAssignCarriertoSite(*affectedcarrier);
          affectedcarrier->resetCarrier();
          insertioncount++;
          affectedcarrier->setId(numberofcarriers_ - 1 + insertioncount);
          secondlevel = false;
          break;
        } else {
          newnode = event.getDestination();
        }

        // check after the event if this was allowed
        if (CheckForbidden(*newnode, forbiddendests)) {
          continue;
        }

        // if the new segment is unoccupied: jump; if not: add to forbidden list
        // and choose new hopping destination
        if (newnode->isOccupied()) {
          if (CheckSurrounded(affectedcarrier->getCurrentNode(),
                              forbiddendests)) {
            AddtoForbiddenlist(affectedcarrier->getCurrentNode(),
                               forbiddennodes);
            break;  // select new escape node (ends level 2 but without setting
                    // level1step to 1)
          }
          AddtoForbiddenlist(*newnode, forbiddendests);
          continue;  // select new destination
        } else {
          affectedcarrier->jumpAccordingEvent(event);
          secondlevel = false;

          break;  // this ends LEVEL 2 , so that the time is updated and the
                  // next MC step started
        }

        // END LEVEL 2
      }
      // END LEVEL 1
    }
  }

  XTP_LOG(Log::error, log_)
      << "\nTotal runtime:\t\t\t\t\t" << simtime
      << " s\n"
         "Total KMC steps:\t\t\t\t"
      << step << "\nAverage lifetime:\t\t\t\t"
      << avlifetime / double(insertioncount) << " s\n"
      << "Mean freepath\t l=<|r_x-r_o|> :\t\t"
      << (meanfreepath * tools::conv::bohr2nm / double(insertioncount))
      << " nm\n"
      << "Average diffusionlength\t d=sqrt(<(r_x-r_o)^2>)\t"
      << std::sqrt(difflength_squared.norm() / double(insertioncount)) *
             tools::conv::bohr2nm
      << " nm\n"
      << std::flush;

  WriteOccupationtoFile(simtime, occfile_);
  traj.close();
  if (do_carrierenergy_) {
    energyfile.close();
  }
  return;
}

bool KMCLifetime::Evaluate(Topology& top) {
  XTP_LOG(Log::error, log_) << "\n-----------------------------------"
                               "\n      KMCLIFETIME started"
                               "\n-----------------------------------\n"
                            << std::flush;

  XTP_LOG(Log::info, log_) << "\nInitialising random number generator"
                           << std::flush;

  RandomVariable_.init(seed_);
  LoadGraph(top);
  ReadLifetimeFile(lifetimefile_);

  if (!probfile_.empty()) {
    WriteDecayProbability(probfile_);
  }
  RunVSSM();

  time_t now = time(nullptr);
  tm* localtm = localtime(&now);

  XTP_LOG(Log::info, log_) << " KMCLIFETIME finished at:" << asctime(localtm)
                           << std::flush;
  std::cout << log_;
  return true;
}

}  // namespace xtp
}  // namespace votca


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
#include <NTL/lzz_pXFactoring.h>
NTL_CLIENT
#include "FHE.h"
#include "EncryptedArray.h"

#include <cstdlib>
#include <cassert>
#include <list>
#include <sstream>

#if (__cplusplus>199711L)
#include <memory>
#else
#include <tr1/memory>
#endif

#include <NTL/vector.h>
#include "NumbTh.h"
#include "permutations.h"

static void
recursiveGeneralBenesInit(long n, long k, long d, long delta_j,
                          const Permut& perm, const Permut& iperm,
                          Vec< Vec<short> >& level, Vec< Vec<short> >& ilevel)
{
  long sz = perm.length();

  if (d == k-1) {
    // recursion stops

    if (sz == 1) {
      level[d][delta_j] = 0;
      ilevel[d][delta_j] = 0;
    }

    if (sz == 2) {
      // only two possibilities for perm: the identity perm or the swap perm

      if (perm[0] == 0) {
        // the identity perm

        level[d][delta_j] = 0;
        level[d][delta_j+1] = 0;
        ilevel[d][delta_j] = 0;
        ilevel[d][delta_j+1] = 0;
      }
      else {
        // the swap perm

        level[d][delta_j] = 1;
        level[d][delta_j+1] = -1;
        ilevel[d][delta_j] = 1;
        ilevel[d][delta_j+1] = -1;
      }
    }
    return;
  }

  long nlev = 2*k-1;

  long sz0 = GeneralBenesNetwork::shamt(n, k, d); // size of upper internal network
  long sz1 = sz - sz0;

  assert(labs(sz0-sz1) <= 1);

  // id_perm: the identity permutation on {0,...,sz-1}
  Permut id_perm;
  id_perm.SetLength(sz);
  for (long j = 0; j < sz; j++) id_perm[j] = j;

  // *xperm[0] is the perm on LHS of network
  // *xperm[1] is the perm on RHS of network
  // *xiperm[0] is the inv perm on LHS of network
  // *xiperm[1] is the inv perm on RHS
  const Permut *xperm[] = { &id_perm, &perm };
  const Permut *xiperm[] = { &id_perm, &iperm };

  // *first_level[0] is the first level when traversing left to right
  // *first_level[1] is the first level when traversing right to left
  // *ifirst_level[0] is the reversed first level when traversing left to right
  // *ifirst_level[1] is the reversed first level when traversing right to left
  Vec<short> *first_level[] = { &level[d], &ilevel[nlev-1-d] };
  Vec<short> *ifirst_level[] = { &ilevel[d], &level[nlev-1-d] };

  // *last_level[0] is the last level when traversing left to right
  // *last_level[1] is the last level when traversing right to left
  // *ilast_level[0] is the reversed last level when traversing left to right
  // *ilast_level[1] is the reversed last level when traversing right to left
  Vec<short> *last_level[] = { &level[nlev-1-d], &ilevel[d] };
  Vec<short> *ilast_level[] = { &ilevel[nlev-1-d], &level[d] };

  // inner_perm[0][0] upper internal perm
  // inner_perm[0][1] upper internal inv perm
  // inner_perm[1][0] lower internal perm
  // inner_perm[1][1] lower internal inv perm
  Permut inner_perm[2][2];

  inner_perm[0][0].SetLength(sz0);
  inner_perm[0][1].SetLength(sz0);
  inner_perm[1][0].SetLength(sz1);
  inner_perm[1][1].SetLength(sz1);

  // marked[0] indicates which nodes on left have been marked
  // marked[1] indicates which nodes on right have been marked
  Vec<bool> marked[2];
  marked[0].SetLength(sz);
  for (long j = 0; j < sz; j++) marked[0][j] = false;
  marked[1].SetLength(sz);
  for (long j = 0; j < sz; j++) marked[1][j] = false;

  // counter[0] is used to count through nodes on left side
  // counter[1] is used to count through nodes on right side
  long counter[2];
  counter[0] = 0;
  counter[1] = 0;

  long dir = 0; // direction, initially left to right
  long e = 0;  // current edge, initially straight

  long node; // current node
  long stop_node; // stopping node, initially undefined
  long stop_side;  // stopping side

  if (sz0 == sz1) {
    // an even split
    node = 0;
    stop_node = sz0;
    stop_side = 0;
  }
  else if (sz0 > sz1) {
    // an odd split, top larger
    node = sz0-1;
    stop_node = sz0-1;
    stop_side = 1;
  }
  else { // sz0 < sz1
    // an odd split, bottom larger
    node = sz-1;
    stop_node = sz-1;
    stop_side = 1;
  }

  for (;;) {
    marked[dir][node] = true; // mark current node

    // traverse path through graph

    long v = (*xperm[dir])[node]; // value associated with this node

    long net = long(node >= sz0)^labs(e);
       // net = 0 => upper internal network
       // net = 1 => lower internal network

    long node1 = node - sz0*long(node >= sz0);
      // node adjacent to node in the internal network

    long node3 = (*xiperm[1-dir])[v];
      // corresponding node of the other side of the external network

    long node2 = node3 - sz0*long(node3 >= sz0);
      // node adjacent to node3 in the internal network

    long e2 = (node3 - (node2 + net*sz0))/sz0;
      // edge type for edge connecting node2 to node3

    /* here is the picture:
     *
     *         e                       e2
     *  node ------ node1 ---- node2 ----- node3
     *               \           /
     *                \_________/
     *                     |
     *              internal network
     */

    // update external edges in level graph

    (*first_level[dir])[delta_j+node] = e;
    (*ifirst_level[dir])[delta_j+net*sz0+node1] = -e;

    (*last_level[dir])[delta_j+net*sz0+node2] = e2;
    (*ilast_level[dir])[delta_j+node3] = -e2;

    // update internal permutations

    inner_perm[net][dir][node2] = node1;
    inner_perm[net][1-dir][node1] = node2;

    // change direction
    dir = 1-dir;

    // mark node3 on new side
    marked[dir][node3] = true;

    // check for stop node
    if (node3 == stop_node && dir == stop_side) {
       // search for unmarked node

      while (counter[dir] < sz && marked[dir][counter[dir]]) counter[dir]++;
      if (counter[dir] >= sz) break; // we're done!!

      // update node, e, stop_node, stop_side
      node = counter[dir];
      e = 0;

      if (node < sz0)
        stop_node = node + sz0;
      else
        stop_node = node - sz0;

      stop_side = dir;
    }
    else {
       // calculate sibling node and continue
      if (node3 < sz0)
        node = node3 + sz0;
      else
        node = node3 - sz0;

      e = e2;
    }
  }

  // done constructing the external edges and internal permutations...
  // time to recurse

  // recurse on upper internal network:
  recursiveGeneralBenesInit(n, k, d+1, delta_j,
                            inner_perm[0][0], inner_perm[0][1], level, ilevel);

  // recurse on lower intrernal network
  recursiveGeneralBenesInit(n, k, d+1, delta_j + sz0,
                            inner_perm[1][0], inner_perm[1][1], level, ilevel);
}

GeneralBenesNetwork::GeneralBenesNetwork(const Permut& perm)
{
  n = perm.length();

  // check that n > 1
  assert(n > 1);

  // compute recursion depth k = least integer k s/t 2^k >= n
  k = GeneralBenesNetwork::depth(n);

  // construct the inverse perm, which is convenient in the recursive function.
  // As a byproduct, verify that perm is indeed a permutation on {0,...,n-1}.

  Permut iperm;
  iperm.SetLength(n);

  for (long j = 0; j < n; j++)
    iperm[j] = -1;

  for (long j = 0; j < n; j++) {
    long j1 = perm[j];
    assert(j1 >= 0 && j1 < n);
    iperm[j1] = j;
  }

  for (long j = 0; j < n; j++)
    assert(iperm[j] != -1);

  // allocate space for the levels graph

  level.SetLength(2*k-1);
  for (long i = 0; i < 2*k-1; i++)
    level[i].SetLength(n);

  // allocate space for the reverse levels graph...
  // makes the recursive construction more convenient

  Vec< Vec<short> > ilevel;
  ilevel.SetLength(2*k-1);
  for (long i = 0; i < 2*k-1; i++)
    ilevel[i].SetLength(n);

  // recursively construct the levels graph

  recursiveGeneralBenesInit(n, k, 0, 0, perm, iperm, level, ilevel);
}

bool GeneralBenesNetwork::testNetwork(const Permut& perm) const
{
  long sz = getSize();
  long nlev = getNumLevels();

  for (long j = 0; j < sz; j++) {
    // find correct position for j

    long j1 = j;
    for (long i = 0; i < nlev; i++) {
      const Vec<short>& lev = getLevel(i);
      j1 += shamt(i)*lev[j1];
    }
    if (perm[j1] != j) return false;
  }
  return true;
}


#ifndef HMLIB_ODEINT_INTERFERE_BREAKABLEINTEGRATECONST_INC
#define HMLIB_ODEINT_INTERFERE_BREAKABLEINTEGRATECONST_INC 100
#
#include <tuple>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include "../utility.hpp"
#include "../stepper_categories.hpp"
namespace hmLib {
	namespace odeint {
		namespace detail {
			namespace boost_odeint = boost::numeric::odeint;

			//breakable integrate const for simple stepper
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_const(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, stepper_tag
			){
				typename boost_odeint::unwrap_reference< Observer >::type& obs = observer;
				typename boost_odeint::unwrap_reference< Stepper >::type& st = stepper;

				// check breaker condition with the initial state
				bool IsBreak = breaker(start_state, start_time);

				Time time = start_time;
				int step = 0;

				// cast time+dt explicitely in case of expression templates (e.g. multiprecision)
				while (!IsBreak && boost_odeint::detail::less_eq_with_sign(static_cast<Time>(time + dt), end_time, dt))
				{
					obs(start_state, time);
					st.do_step(sys, start_state, time, dt);

					// direct computation of the time avoids error propagation happening when using time += dt
					// we need clumsy type analysis to get boost units working here
					++step;
					time = start_time + static_cast<typename boost_odeint::unit_value_type<Time>::type>(step) * dt;

					// check breaker condition
					IsBreak = breaker(start_state, time);
				}
				obs(start_state, time);

				return std::make_tuple(IsBreak, step, time);
			}

			// forward declaration for breakable integrate const for controlled_stepper
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_adaptive(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, controlled_stepper_tag
			);

			//breakable integrate const for controlled_stepper
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_const(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, controlled_stepper_tag
			)
			{
				typename boost_odeint::unwrap_reference< Observer >::type& obs = observer;

				Time time = start_time;
				const Time time_step = dt;
				int real_steps = 0;
				int step = 0;

				// check breaker condition with the initial state
				if (breaker(start_state, start_time)) {
					obs(start_state, start_time);
					return std::make_tuple(true, real_steps, start_time);
				}

				while (boost_odeint::detail::less_eq_with_sign(static_cast<Time>(time + time_step), end_time, dt)){
					obs(start_state, time);
					// integrate_adaptive_checked uses the given checker to throw if an overflow occurs
					auto ans = breakable_integrate_adaptive(stepper, sys, start_state,
						time, static_cast<Time>(time + time_step), dt, breaker,
						boost_odeint::null_observer(), controlled_stepper_tag());

					//if break is detected, finish immidiately
					if (std::get<0>(ans)) {
						return ans;
					}

					real_steps += std::get<1>(ans);
					// direct computation of the time avoids error propagation happening when using time += dt
					// we need clumsy type analysis to get boost units working here
					step++;
					time = start_time + static_cast<typename boost_odeint::unit_value_type<Time>::type>(step) * time_step;
				}
				obs(start_state, time);

				return std::make_tuple(false, real_steps, time);
			}

			//breakable integrate const for dense_output_stepper
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_const(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, dense_output_stepper_tag
			){
				typename boost_odeint::unwrap_reference< Observer >::type& obs = observer;
				typename boost_odeint::unwrap_reference< Stepper >::type& st = stepper;

				Time time = start_time;
				int obs_step(1);
				int real_step(0);

				st.initialize(start_state, time, dt);
				bool IsBreak = breaker(start_state, time);
				obs(start_state, time);
				if (IsBreak) {
					return std::make_tuple(true, real_step, time);
				}
				time += dt;
				while (boost_odeint::detail::less_eq_with_sign(static_cast<Time>(time + dt), end_time, dt)){
					while (boost_odeint::detail::less_eq_with_sign(time, st.current_time(), dt)){
						calc_state(st, sys, time, start_state);
						obs(start_state, time);
						++obs_step;
						// direct computation of the time avoids error propagation happening when using time += dt
						// we need clumsy type analysis to get boost units working here
						time = start_time + static_cast<typename boost_odeint::unit_value_type<Time>::type>(obs_step) * dt;
					}
					// we have not reached the end, do another real step
					if (boost_odeint::detail::less_with_sign(static_cast<Time>(st.current_time() + st.current_time_step()),
						end_time,
						st.current_time_step())) {
						while (!IsBreak && boost_odeint::detail::less_eq_with_sign(st.current_time(), time, dt))
						{
							st.do_step(sys);
							IsBreak = breaker(st.current_state(), st.current_time());
							++real_step;
						}
					} else if (boost_odeint::detail::less_with_sign(st.current_time(), end_time, st.current_time_step()))
					{ // do the last step ending exactly on the end point
						st.initialize(st.current_state(), st.current_time(), end_time - st.current_time());
						st.do_step(sys);
						IsBreak = breaker(st.current_state(), st.current_time());
						++real_step;
					}

					if (IsBreak) {
						//last observation before break
						while (boost_odeint::detail::less_eq_with_sign(time, st.current_time(), dt)) {
							calc_state(st, sys, time, start_state);
							obs(start_state, time);
							++obs_step;
							// direct computation of the time avoids error propagation happening when using time += dt
							// we need clumsy type analysis to get boost units working here
							time = start_time + static_cast<typename boost_odeint::unit_value_type<Time>::type>(obs_step) * dt;
						}
						return std::make_tuple(true, real_step, st.current_time());
					}
				}
				// last observation, if we are still in observation interval
				// might happen due to finite precision problems when computing the the time
				if (boost_odeint::detail::less_eq_with_sign(time, end_time, dt)){
					calc_state(st, sys, time, start_state);
					obs(start_state, time);
				}

				return std::make_tuple(false, real_step, st.current_time());
			}

			// forward declaration for breakable integrate const for adaptive_stepper_tag
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_adaptive(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, adaptive_stepper_tag
			);

			//breakable integrate const for dense_output_stepper
			template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
			std::tuple<bool, size_t, Time> breakable_integrate_const(
				Stepper stepper, System sys, State& start_state,
				Time start_time, Time end_time, Time dt, Breaker breaker,
				Observer observer, adaptive_stepper_tag
			) {
				typename boost_odeint::unwrap_reference< Observer >::type& obs = observer;

				Time time = start_time;
				const Time time_step = dt;
				int real_steps = 0;
				int step = 0;

				// check breaker condition with the initial state
				if (breaker(start_state, start_time)) {
					obs(start_state, start_time);
					return std::make_tuple(true, real_steps, start_time);
				}

				while (boost_odeint::detail::less_eq_with_sign(static_cast<Time>(time + time_step), end_time, dt)) {
					obs(start_state, time);
					// integrate_adaptive_checked uses the given checker to throw if an overflow occurs
					auto ans = breakable_integrate_adaptive(stepper, sys, start_state,
						time, static_cast<Time>(time + time_step), dt, breaker,
						boost_odeint::null_observer(), adaptive_stepper_tag());

					//if break is detected, finish immidiately
					if (std::get<0>(ans)) {
						return ans;
					}

					real_steps += std::get<1>(ans);
					// direct computation of the time avoids error propagation happening when using time += dt
					// we need clumsy type analysis to get boost units working here
					step++;
					time = start_time + static_cast<typename boost_odeint::unit_value_type<Time>::type>(step) * time_step;
				}
				obs(start_state, time);

				return std::make_tuple(false, real_steps, time);
			}
		}

		template< class Stepper, class System, class State, class Time, class Breaker, class Observer >
		std::tuple<bool, size_t, Time> breakable_integrate_const(
			Stepper stepper, System sys, State& start_state,
			Time start_time, Time end_time, Time dt,
			Breaker breaker, Observer observer) {
			using stepper_category = typename boost::numeric::odeint::unwrap_reference<Stepper>::type::stepper_category;
			return detail::breakable_integrate_const(stepper, sys, start_state, start_time, end_time, dt, breaker, observer, stepper_category());
		}

		template< class Stepper, class System, class State, class Time, class Breaker>
		std::tuple<bool, size_t, Time> breakable_integrate_const(
			Stepper stepper, System sys, State& start_state,
			Time start_time, Time end_time, Time dt,
			Breaker breaker) {
			using stepper_category = typename boost::numeric::odeint::unwrap_reference<Stepper>::type::stepper_category;
			return detail::breakable_integrate_const(stepper, sys, start_state, start_time, end_time, dt, breaker, boost::numeric::odeint::null_observer(), stepper_category());
		}

	}
}
#
#endif


#include "SpikeTrain.h"
#include "RanUtilsRandom.h"
#include <stdexcept>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

using namespace boost::python;

std::vector<double> * SpikeTrain::getTds(){
    std::vector<double> * a=new std::vector<double>(number_of_tds);
    for (int i=0 ; i<number_of_tds; ++i){
        (*a)[i]=tds[i]->timeBlock*timeBlockSize + tds[i]->time;
    }
    return a;
}
std::vector<double> * SpikeTrain::getTdsMinusEps(){
        std::vector<double> * a=new std::vector<double>(number_of_tds);
        for (int i=0 ; i<number_of_tds; ++i){
                (*a)[i]=tds_minus_eps[i]->timeBlock*timeBlockSize + tds_minus_eps[i]->time;
    }
    return a;
}

SpikeTrain * generatePoissonSpikeTrian(boost::python::object const & mean_rates,
                                       double T,int N,
                                       int N_noise){
    /*
     * Generates a spike train with N Poisson spike times with mean rate mean_rates[i]
     * For Duration T. In addition adds a single spike at a random time from each
     * noise afferent with N_noise noise afferents
     */

    static RU::PoissonProccess p;
    static RU::Random rnd;
    static double t;
    SpikeTrain * st_ptr = new SpikeTrain();
    boost::python::stl_input_iterator<double> x_i(mean_rates),x_end;
    int i = 0;
    while (x_i!=x_end){
        // generate poisson rates with mean rate x_i
        p.setTau(1./(*x_i));
        t=0.0;
        while (t<T){
            t += p.NextEvent();
            st_ptr->add_spike(i,t);
        }
        st_ptr->pop_back();
        ++i;
        ++x_i;
    }
    if (i!=N)
        throw std::runtime_error("Number of elements in iterable object is not N");//assert i==N
    //generate noise spikes (1 spike per afferent.)
    for(i=N; i<N+N_noise; ++i){
        t = rnd.nextFloat(T);
        st_ptr->add_spike(i,t);
    }
    //sorting according to spike time
    st_ptr->sort();

    return st_ptr;
}

SpikeTrain * generateNoisySpikeTrian(SpikeTrain * st, double sigma,
                                       double T,int N, int N_noise){
    static RU::Random rnd;
    NR::GausianDist g(0.0,sigma);
    SpikeTrain * st_ptr = new SpikeTrain();

    for (auto spike=st->begin(); spike!=st->end(); ++spike){
        st_ptr->add_spike(spike->affarent,spike->time+g());
    }

    //generate noise spikes (1 spike per afferent.)
    for(int i=N; i<N+N_noise; ++i){
        st_ptr->add_spike(i,rnd.nextFloat(T));
    }
    //sorting according to spike time
    st_ptr->sort();

    return st_ptr;
}

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(setTdMarkers_OL,setTdMarkers,1,2)

void export_SpikeTrain(){
    class_<SpikeTrain>("SpikeTrain")
        .def(vector_indexing_suite<SpikeTrain>())
        .def("getTds",&SpikeTrain::getTds,return_value_policy<manage_new_object>())
        .def("getTdsMinusEps",&SpikeTrain::getTdsMinusEps,return_value_policy<manage_new_object>())
        .def("sort",&SpikeTrain::sort)
        .def("setTdMarkers",&SpikeTrain::setTdMarkers,setTdMarkers_OL())
        .def_readwrite("timeBlockSize",&SpikeTrain::timeBlockSize)
        .def("applyTimeBlock",&SpikeTrain::applyTimeBlock)
        .def("calcExponents",&SpikeTrain::calcExponents)
        .def("_cpp_add_spike",&SpikeTrain::add_spike)
    ;
    def("CPPgeneratePoissonSpikeTrian",generatePoissonSpikeTrian,return_value_policy<manage_new_object>());
    def("CPPgenerateNoisySpikeTrian",generateNoisySpikeTrian,return_value_policy<manage_new_object>());
}


/*
The MIT License

Copyright (c) 2015-2016 Albert Murienne

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "network_vexcl.h"

#include "common/network_config.h"
#include "common/network_exception.h"
#include "common/network_random.h"

#include <boost/shared_array.hpp>

// vex::constant is not available for cuda backend,
// so we have to redefine a forwarding identity function:
// http://stackoverflow.com/questions/38353823/identity-function-with-perfect-forwarding
#ifdef VEXCL_BACKEND_CUDA
namespace vex {
    template<typename V>
    constexpr V&& constant(V&& v) { return std::forward<V>(v); }
};
#endif

namespace neurocl { namespace mlp {

VEX_CONSTANT(_zero, 0.f);
VEX_CONSTANT(_one, 1.f);

class my_vex_ctx
{
public:
    static my_vex_ctx& instance() { static my_vex_ctx _ctx; return _ctx; }
    vex::Context& get() { return m_ctx; }
protected:
	my_vex_ctx() : m_ctx( vex::Filter::GPU && vex::Filter::Count(1) ) {}
    virtual ~my_vex_ctx() {}
private:
    vex::Context m_ctx;
};

template<typename T>
const std::string dump_vec( const vex::vector<T>& vec, boost::optional<std::string> label = boost::none )
{
    std::string separator;
    std::stringstream ss;
    ss << ( label ? label.get() : "" ) << std::endl;
    for( typename vex::vector<T>::const_iterator it = vec.begin(); it != vec.end(); ++it )
    {
            ss << separator << *it;
            separator = " ";
    }
    ss << std::endl;
    return ss.str();
}

void random_normal_init( vex::vector<float>& container, const float stddev = 1.f )
{
    random::rand_gaussian_generator rgg( 0.f, stddev );

    boost::shared_array<float> arand( new float[container.size()] );

    for ( size_t i=0; i<container.size(); i++ )
        arand[i] = rgg();

    vex::copy( arand.get(), arand.get()+container.size(), container.begin() );
}

layer_vexcl::layer_vexcl()
{
}

// WARNING : size is the square side size
void layer_vexcl::populate( const layer_size& cur_layer_size, const layer_size& next_layer_size )
{
    //LOGGER(info) << "layer_vexcl::populate - populating layer of size " << cur_layer_size << " (next size is " << next_layer_size << ")" << std::endl;

    vex::Context& _ctx = my_vex_ctx::instance().get();

    if ( next_layer_size.size() ) // non-output layer
    {
        m_weights_size = std::make_pair( next_layer_size.size(), cur_layer_size.size() );
        m_output_weights = vex::vector<float>( _ctx, next_layer_size.size() * cur_layer_size.size() );
        // cf. http://neuralnetworksanddeeplearning.com/chap3.html#weight_initialization
        random_normal_init( m_output_weights, 1.f / std::sqrt( cur_layer_size.size() ) );
        m_deltas_weight = vex::vector<float>( _ctx, next_layer_size.size() * cur_layer_size.size() );
        m_deltas_weight = _zero();

        m_bias = vex::vector<float>( _ctx, next_layer_size.size() );
        random_normal_init( m_bias, 1.f );
        m_deltas_bias = vex::vector<float>( _ctx, next_layer_size.size() );
        m_deltas_bias = _zero();
    }

    m_activations = vex::vector<float>( _ctx, cur_layer_size.size() );
    m_activations = _zero();
    m_errors = vex::vector<float>( _ctx, cur_layer_size.size() );
    m_errors = _zero();
}

const std::string layer_vexcl::dump_weights() const
{
    return dump_vec( m_output_weights );
}

const std::string layer_vexcl::dump_bias() const
{
    return dump_vec( m_bias );
}

const std::string layer_vexcl::dump_activations() const
{
    return dump_vec( m_activations );
}

network_vexcl::network_vexcl() : m_training_samples( 0 ), m_learning_rate( 3.0f/*0.01f*/ ), m_weight_decay( 0.0f )
{
    const network_config& nc = network_config::instance();
    nc.update_optional( "learning_rate", m_learning_rate );

    vex::Context& _ctx = my_vex_ctx::instance().get();

    if ( !_ctx ) throw std::runtime_error( "No devices available." );

    // Print out list of selected devices:
    LOGGER(info) << "network_vexcl::network_vexcl - vexcl devices:" << std::endl;
    LOGGER(info) << _ctx << std::endl;
}

void network_vexcl::set_input(  const size_t& in_size, const float* in )
{
    if ( in_size > m_layers[0].activations().size() )
        throw network_exception( "sample size exceeds allocated layer size!" );

    //LOGGER(info) << "network_vexcl::set_input - input (" << in << ") size = " << in_size << std::endl;

    vex::copy( in, in + in_size, m_layers[0].activations().begin() );
}

void network_vexcl::set_output( const size_t& out_size, const float* out )
{
    if ( out_size > m_training_output.size() )
        throw network_exception( "output size exceeds allocated layer size!" );

    //LOGGER(info) << "network_vexcl::set_output - output (" << out << ") size = " << out_size << std::endl;

    vex::copy( out, out + out_size, m_training_output.begin() );
}

void network_vexcl::add_layers_2d( const std::vector<layer_size>& layer_sizes )
{
    m_layers.resize( layer_sizes.size() );

    // Last layer should be output layer
    const layer_size& _last_size = layer_sizes.back();
    m_layers.back().populate( _last_size, layer_size( 0, 0 ) );

    vex::Context& _ctx = my_vex_ctx::instance().get();

    // Initialize training output
    m_training_output = vex::vector<float>( _ctx, _last_size.size() );

    // Populate all but input layer
    for ( int idx=layer_sizes.size()-2; idx>=0; idx-- )
    {
        const layer_size& _size = layer_sizes[idx];
        const layer_size& _next_layer_size = layer_sizes[idx+1];
        m_layers[idx].populate( _size, _next_layer_size );
    }
}

const layer_ptr network_vexcl::get_layer_ptr( const size_t layer_idx )
{
    if ( layer_idx >= m_layers.size() )
    {
        LOGGER(error) << "network_vexcl::get_layer_ptr - cannot access layer " << layer_idx << std::endl;
        throw network_exception( "invalid layer index" );
    }

    vex::vector<float>& weights = m_layers[layer_idx].weights();
    vex::vector<float>& bias = m_layers[layer_idx].bias();

    layer_ptr l( weights.size(), bias.size() );
    vex::copy( weights.begin(), weights.end(), l.weights.get() );
    vex::copy( bias.begin(), bias.end(), l.bias.get() );

    return l;
}

void network_vexcl::set_layer_ptr( const size_t layer_idx, const layer_ptr& layer )
{
    if ( layer_idx >= m_layers.size() )
    {
        LOGGER(error) << "network_vexcl::set_layer_ptr - cannot access layer " << layer_idx << std::endl;
        throw network_exception( "invalid layer index" );
    }

    vex::vector<float>& weights = m_layers[layer_idx].weights();
    std::copy( layer.weights.get(), layer.weights.get() + layer.num_weights, weights.begin() );
    vex::vector<float>& bias = m_layers[layer_idx].bias();
    std::copy( layer.bias.get(), layer.bias.get() + layer.num_bias, bias.begin() );
}

const output_ptr network_vexcl::output()
{
    // very slow for a GPU backend!!!
    vex::vector<float>& output = m_layers.back().activations();
    output_ptr l( output.size() );
    vex::copy( output.begin(), output.end(), l.outputs.get() );

    return l;
}

void network_vexcl::feed_forward()
{
    //std::cout << m_layers.size() << " layers propagation" << std::endl;

    for ( size_t i=0; i<m_layers.size()-1; i++ )
    {
        const size_t n = m_layers[i].w_size().first;
        const size_t m = m_layers[i].w_size().second;

        m_layers[i+1].activations() = _one() / ( _one() + exp(
            -( vex::reduce<vex::SUM>(
                vex::extents[n][m],     // Shape of the expression to reduce,
                m_layers[i].weights()
                *
                vex::reshape(
                    m_layers[i].activations(),
                    vex::extents[n][m], // (We need an n x m matrix...
                    vex::extents[1]     // ... but we only have vector of size m).
                ),                      // the expression,
                1                       // and the dimension to reduce along.
            )
            + m_layers[i].bias() ) )
        );
    }
}

void network_vexcl::back_propagate()
{
    // PREREQUISITE : FEED FORWARD PASS

    // Output layer error vector
    layer_vexcl& output_layer = m_layers.back();
    output_layer.errors() = output_layer.activations() * ( _one() - output_layer.activations() )
        * ( output_layer.activations() - vex::constant( m_training_output ) );

    // Hidden layers error vectors
    for ( size_t i=m_layers.size()-2; i>0; i-- )
    {
        size_t n = m_layers[i].w_size().first;
        size_t m = m_layers[i].w_size().second;

        m_layers[i].errors() = m_layers[i].activations() * ( _one() - m_layers[i].activations() )
            *
            vex::reduce<vex::SUM>(
                vex::extents[m][n],     // Shape of the expression to reduce,
                vex::reshape(
                    m_layers[i].weights(),
                    vex::extents[m][n], // new shape
                    vex::extents[1][0]  // m_layers[i].weights is shaped as [n][m]
                )
                *
                vex::reshape(
                    m_layers[i+1].errors(),
                    vex::extents[m][n], // (We need an m x n matrix...
                    vex::extents[1]     // ... but we only have vector of size m).
                ),                      // the expression,
                1                       // and the dimension to reduce along.
            );
    }

    // Update gradients
    for ( size_t i=0; i<m_layers.size()-1; i++ )
    {
        size_t n = m_layers[i].w_size().first;
        size_t m = m_layers[i].w_size().second;

        m_layers[i].w_deltas() += vex::reduce<vex::SUM>(
            vex::extents[n][1][m],
        	vex::reshape( m_layers[i+1].errors(), vex::extents[n][1][m], vex::extents[0][1] )
            * vex::reshape( m_layers[i].activations(), vex::extents[n][1][m], vex::extents[1][2] ),
            1 );
        m_layers[i].b_deltas() += m_layers[i+1].errors();
    }

    ++m_training_samples;
}

void network_vexcl::gradient_descent()
{
    //LOGGER(info) << "network_bnu::gradient_descent - updating after " << m_training_samples << " backpropagations" << std::endl;

    float invm = 1.f / static_cast<float>( m_training_samples );

    for ( size_t i=0; i<m_layers.size()-1; i++ ) // avoid output layer
    {
        m_layers[i].weights() -= m_learning_rate * ( ( invm * m_layers[i].w_deltas() ) + ( m_weight_decay * m_layers[i].weights() ) );
        m_layers[i].bias() -= m_learning_rate * ( invm * m_layers[i].b_deltas() );
    }
}

void network_vexcl::clear_gradients()
{
    // Clear gradients
    for ( size_t i=0; i<m_layers.size()-1; i++ )
    {
        m_layers[i].w_deltas() = _zero();
        m_layers[i].b_deltas() = _zero();
    }

    m_training_samples = 0;
}

float network_vexcl::loss()
{
    vex::Reductor<float,vex::SUM> sum;

    layer_vexcl& output_layer = m_layers.back();
    float _loss = sum( ( output_layer.activations() - vex::constant( m_training_output ) )
        * ( output_layer.activations() - vex::constant( m_training_output ) ) );

    return 0.5f * _loss / static_cast<float>( m_training_output.size() );
}

const std::string network_vexcl::dump_weights()
{
    return "WEIGHTS DUMPING NOT IMPLEMENTED YET";
}

const std::string network_vexcl::dump_bias()
{
    std::stringstream ss;
    ss << "*************************************************" << std::endl;
    for( const auto& layer : m_layers )
    {
        ss << layer.dump_bias();
        ss << "-------------------------------------------------" << std::endl;
    }
    ss << "*************************************************" << std::endl;
    return ss.str();
}

const std::string network_vexcl::dump_activations()
{
    std::stringstream ss;
    ss << "*************************************************" << std::endl;
    for( const auto& layer : m_layers )
    {
        ss << layer.dump_activations();
        ss << "-------------------------------------------------" << std::endl;
    }
    ss << "*************************************************" << std::endl;
    return ss.str();
}

void network_vexcl::gradient_check( const output_ptr& out_ref )
{
    LOGGER(error) << "network_vexcl::gradient_check - not implemented yet for MLP" << std::endl;
}

} /*namespace neurocl*/ } /*namespace mlp*/


#include <boost/thread.hpp>
#include <seq2map/solve.hpp>

using namespace seq2map;

//==[ LeastSquaresProblem ]===================================================//

size_t LeastSquaresProblem::GetHardwareConcurrency()
{
    return boost::thread::hardware_concurrency();
}

VectorisableD::Vec LeastSquaresProblem::operator() (const VectorisableD& vec) const
{
    VectorisableD::Vec v;

    if (!vec.Store(v))
    {
        E_ERROR << "vectorisation failed";
        return VectorisableD::Vec();
    }

    return (*this)(v);
}

cv::Mat LeastSquaresProblem::ComputeJacobian(const VectorisableD::Vec& x, VectorisableD::Vec& y) const
{
    cv::Mat J = cv::Mat::zeros(static_cast<int>(m_conds), static_cast<int>(m_varIdx.size()), CV_64F);
    bool masking = !m_jacobianPattern.empty();

    boost::thread_group threads;

    y = y.empty() ? (*this)(x) : y;

    // make evaluation slices for multi-threaded numerical differentiation
    std::vector<JacobianSlices> slices(m_diffThreads);
    size_t i = 0;

    for (IndexList::const_iterator var = m_varIdx.begin(); var != m_varIdx.end(); var++)
    {
        JacobianSlice slice;
        size_t threadIdx = i % m_diffThreads;

        slice.x    = x;
        slice.y    = y;
        slice.var  = *var;
        slice.col  = J.col(static_cast<int>(i));
        slice.mask = masking ? m_jacobianPattern.col(static_cast<int>(*var)) : cv::Mat();

        slices[threadIdx].push_back(slice);

        i++;
    }

    // lunch the threads
    for (size_t k = 0; k < slices.size(); k++)
    {
        threads.add_thread(new boost::thread(
            LeastSquaresProblem::DiffThread,
            (const LeastSquaresProblem*) this,
            boost::ref(slices[k]))
        );
    }

    // wait for completions
    threads.join_all();

    return J;
}

void LeastSquaresProblem::DiffThread(const LeastSquaresProblem* lsq, JacobianSlices& slices)
{
    BOOST_FOREACH (JacobianSlice& slice, slices)
    {
        cv::Mat x = cv::Mat(slice.x).clone();
        cv::Mat y = cv::Mat(slice.y).clone();
        double dx = lsq->m_diffStep;

        x.at<double>(static_cast<int>(slice.var)) += dx;
        cv::Mat dy = cv::Mat((*lsq)(x)) - y;

        // Jk = (f(x+dx) - f(x)) / dx
        cv::divide(dy, dx, slice.col);
    }
}

bool LeastSquaresProblem::SetActiveVars(const IndexList& varIdx)
{
    BOOST_FOREACH(size_t var, varIdx)
    {
        if (var >= m_vars) return false;
    }

    m_varIdx = varIdx;

    return true;
}

cv::Mat LeastSquaresProblem::ApplyUpdate(const VectorisableD::Vec& x0, const VectorisableD::Vec& delta)
{
    assert(delta.size() <= x0.size());
    VectorisableD::Vec x = x0;
    size_t i = 0;

    BOOST_FOREACH(size_t var, m_varIdx)
    {
        x[var] += delta[i++];
    }

    return cv::Mat(x, true);
}

//==[ LeastSquaresSolver ]====================================================//

bool LeastSquaresSolver::Logger::operator() (const LeastSquaresSolver::State& state)
{
    if (state.updates == 0)
    {
        E_TRACE << std::setw(80) << std::setfill('=') << "";
        E_TRACE << std::setw( 6) << std::right << "Update"
                << std::setw(14) << std::right << "RMSE"
                << std::setw(20) << std::right << "Lambda"
                << std::setw(20) << std::right << "Rel. Step Size"
                << std::setw(20) << std::right << "Rel. Error Drop";
        E_TRACE << std::setw(80) << std::setfill('=') << "";
        E_TRACE << std::setw( 6) << std::right << state.updates
                << std::setw(14) << std::right << state.error
                << std::setw(20) << std::right << state.lambda
                << std::setw(20) << std::right << "-"
                << std::setw(20) << std::right << "-";
    }
    else
    {
        E_TRACE << std::setw(6)  << std::right << state.updates
                << std::setw(14) << std::right << state.error
                << std::setw(20) << std::right << state.lambda
                << std::setw(20) << std::right << state.relStep
                << std::setw(20) << std::right << state.relError;
    }

    return true;
}

//==[ LevenbergMarquardtAlgorithm ]===========================================//

bool LevenbergMarquardtAlgorithm::Solve(LeastSquaresProblem& f, State& state)
{
    assert(m_eta > 1.0f);

    if (!f.Initialise(state.x))
    {
        return false;
    }

    state.y = f(state.x);
    state.error = rms(cv::Mat(state.y, false));
    state.lambda = m_lambda;
    state.converged = false;
    state.updates = 0;

    if (m_verbose && m_updater)
    {
        (*m_updater)(state);
    }

    try
    {
        //
        // outer loop : goes until a possible minimum is approached, or termination criterion met
        //
        while (!state.converged)
        {
            cv::Mat J = f.ComputeJacobian(state.x, state.y); // Jacobian matrix
            cv::Mat H = J.t() * J;                           // Hessian matrix
            cv::Mat D = J.t() * cv::Mat(state.y);            // error gradient

            state.lambda = state.lambda < 0 ? cv::mean(H.diag())[0] : state.lambda;
            state.jacobian = J;
            state.hessian  = H;

            bool better = false;
            size_t trials = 0;

            //
            // inner loop : keep trying
            //
            while (!better && !state.converged)
            {
                // augmented normal equations
                cv::Mat A = H + state.lambda * cv::Mat::diag(H.diag()); // or A = N + lambda*eye(d);
                cv::Mat x_delta = A.inv() * -D; // x_delta =  A \ -D;

                VectorisableD::Vec x = f.ApplyUpdate(state.x, x_delta); // = cv::Mat(cv::Mat(x_best) + x_delta);
                VectorisableD::Vec y = f(x);

                const double e_try = rms(cv::Mat(y));
                const double de = state.error - e_try;

                better = de > 0;
                trials++;

                if (better) // accept the update
                {
                    state.lambda /= m_eta;

                    state.de.push_back(de);
                    state.x        = x;
                    state.y        = y;
                    state.error    = e_try;
                    state.relError = state.de.size() > 1 ? (state.de.rbegin()[0] / state.de.rbegin()[1]) : 1.0f;
                    state.relStep  = cv::norm(x_delta) / cv::norm(cv::Mat(x, false));
                    state.updates++;
                }
                else // reject the update
                {
                    state.lambda *= m_eta;
                }

                // convergence control
                state.converged |= (state.updates >= m_term.maxCount); // # iterations check
                state.converged |= (state.updates > 1) && (state.relError < m_term.epsilon); // error differential check
                state.converged |= (state.updates > 1) && (state.relStep  < m_term.epsilon); // step ratio check
                state.converged |= (!better && trials >= m_term.maxCount);

                cv::Mat icv = H.diag();

                bool ill = false;

                for (int d = 0; d < icv.total(); d++)
                {
                    if (icv.at<double>(d) == 0)
                    {
                        E_WARNING << "change of parameter " << d << " not responsive";
                        ill = true;
                    }
                }

                ill |= !isfinite(norm(x_delta));

                if (ill)
                {
                    PersistentMat(J).Store(Path("J.bin"));
                    PersistentMat(x_delta).Store(Path("x_delta.bin"));
                    PersistentMat(cv::Mat(x)).Store(Path("x.bin"));
                    PersistentMat(cv::Mat(y)).Store(Path("y.bin"));

                    E_ERROR << "problem ill-posed";

                    return false;
                }
            }

            if (m_verbose && m_updater)
            {
                if (!(*m_updater)(state))
                {
                    return true;
                }
            }
        }
    }
    catch (std::exception& ex)
    {
        E_ERROR << "exception caught in optimisation loop";
        E_ERROR << ex.what();

        return false;
    }

    if (!f.Finalise(state.x))
    {
        E_ERROR << "error setting solution";
        E_ERROR << mat2string(cv::Mat(state.x, false), "x_final");

        return false;
    }

    return true;
}