/*
MIT License

Copyright (c) 2020 Rik Baehnemann, ASL, ETH Zurich, Switzerland

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 "fm_trajectories/rectangle_trajectory.h"

#include <angles/angles.h>
#include <Eigen/Geometry>
#include <cmath>

namespace fm_trajectories {

std::vector<Eigen::Vector3d>
RectangleTrajectory::Settings::computeVerticesFromSettings(
    double A_east, double A_north, double delta_deg, double a, double b,
    double h_A_abs, double h_B_rel, double h_C_rel, double h_D_rel) const {
  // Express rectangle edges without changed heading.
  // Make height absolute.
  h_B_rel += h_A_abs;
  h_C_rel += h_A_abs;
  h_D_rel += h_A_abs;
  Eigen::Vector3d v_a(a, 0.0, h_B_rel - h_A_abs);
  Eigen::Vector3d v_b(0.0, b, h_C_rel - h_B_rel);
  Eigen::Vector3d v_c(-a, 0.0, h_D_rel - h_C_rel);

  // Change heading.
  double delta = angles::from_degrees(delta_deg);
  Eigen::Quaterniond q_heading(
      Eigen::AngleAxisd(delta, Eigen::Vector3d::UnitZ()));

  v_a = q_heading * v_a;
  v_b = q_heading * v_b;
  v_c = q_heading * v_c;

  std::vector<Eigen::Vector3d> vertices(4);
  vertices[0] << A_east, A_north, h_A_abs;
  vertices[1] = vertices[0] + v_a;
  vertices[2] = vertices[1] + v_b;
  vertices[3] = vertices[2] + v_c;

  return vertices;
}

bool RectangleTrajectory::toYaml(YAML::Node* node) {
  CHECK_NOTNULL(node);
  *node = YAML::Node();

  Eigen::Vector3d A_wgs84;
  if (!geotf_.convert(
          "enu",
          Eigen::Vector3d(
              std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
                  ->A_east,
              std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
                  ->A_north,
              std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
                  ->h_A_abs),
          "wgs84", &A_wgs84))
    return false;

  (*node)["A_lat"] = A_wgs84.x();
  (*node)["A_lon"] = A_wgs84.y();
  (*node)["delta_deg"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->delta_deg;
  (*node)["a"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)->a;
  (*node)["b"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)->b;
  (*node)["velocity"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->velocity;
  (*node)["h_A_abs"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->h_A_abs;
  (*node)["h_B_rel"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->h_B_rel;
  (*node)["h_C_rel"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->h_C_rel;
  (*node)["h_D_rel"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->h_D_rel;
  (*node)["offset_heading_deg"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->offset_heading_deg;
  (*node)["start_vertex"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->start_vertex;
  (*node)["num_edges"] =
      std::static_pointer_cast<RectangleTrajectory::Settings>(settings_)
          ->num_edges;
  (*node)["trajectory_type"] = static_cast<int>(TrajectoryType::Rectangle);

  return true;
}

}  // namespace fm_trajectories


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/trigonometric/include/functions/atand.hpp>

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

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( atand_real,  NT2_REAL_TYPES)
{
  using nt2::atand;
  using nt2::tag::atand_;

  typedef typename nt2::meta::call<atand_(T)>::type r_t;
  typedef T wished_r_t;

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

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(atand(nt2::Inf<T>()), 90, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::Minf<T>()), -90, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::Nan<T>()), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(atand(nt2::Half<T>()), 2.656505117707799e+01, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::Mhalf<T>()), -2.656505117707799e+01, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::Mone<T>()), -45, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::One<T>()), 45, 0.5);
  NT2_TEST_ULP_EQUAL(atand(nt2::Zero<T>()), nt2::Zero<r_t>(), 0.5);
}


//------------------------------------------------------------------------------
/// \file ModuloArithmetic_tests.cpp
/// \ref https://en.cppreference.com/w/cpp/language/operator_arithmetic
/// \brief Demonstrate Modulo arithmetic.
//------------------------------------------------------------------------------
#include "Utilities/SuperBitSet.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(Algebra)
BOOST_AUTO_TEST_SUITE(Groups)
BOOST_AUTO_TEST_SUITE(AbelianGroups)
BOOST_AUTO_TEST_SUITE(ModuloArithmetic_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IdentityReturnsZero)
{
  BOOST_TEST(true);
}

BOOST_AUTO_TEST_SUITE_END() // ModuloArithmetic_tests
BOOST_AUTO_TEST_SUITE_END() // AbelianGroups
BOOST_AUTO_TEST_SUITE_END() // Groups
BOOST_AUTO_TEST_SUITE_END() // Algebra


/************************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros
 * 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 Autonomous Systems Lab nor ETH Zurich
 *     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 OWNER 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.
 */

/*!
* @file    QuadraticProblemFormulation_test.cpp
* @author  Péter Fankhauser
* @date    Aug 16, 2013
*/

#include <gtest/gtest.h>
#include "ooqp_eigen_interface/QuadraticProblemFormulation.hpp"
#include "ooqp_eigen_interface/ooqpei_gtest_eigen.hpp"
#include <Eigen/Core>

using namespace Eigen;
using namespace ooqpei;
using namespace ooqpei::eigen;
using namespace std;

TEST(OOQPEITest, QuadraticProblemFormulation)
{
  Vector2d solution(1.030779872038734, -3.143697043057237);

  Eigen::SparseMatrix<double, Eigen::RowMajor> A, C, D;
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> S, W;
  Eigen::VectorXd b, c, d, f, x;

  A.resize(3, 2);
  A.insert(0, 0) = 1.0;
  A.insert(0, 1) = -1.0;
  A.insert(1, 0) = -4.0;
  A.insert(1, 1) = -3.0;
  A.insert(2, 0) = 2.0;
  A.insert(2, 1) = 0.0;
  S = Vector3d(3.0, 2.0, 0.69).asDiagonal();
  W = Vector2d(0.5, 0.2).asDiagonal();
  b.resize(3);
  b << 3.0, 6.0, 9.0;

  QuadraticProblemFormulation::solve(A, S, b, W, C, c, D, d, f, x);

  expectNear(x, solution, 1e-10, OOQPEI_SOURCE_FILE_POS);
}


/*
 * Copyright 2021 INRIA
 */

#ifndef __pycppad_fwd_hpp__
#define __pycppad_fwd_hpp__

#include "pycppad/config.hpp"
#include "pycppad/deprecated.hpp"
#include "pycppad/warning.hpp"

#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include <boost/python.hpp>
#include <boost/python/scope.hpp>

#undef BOOST_BIND_GLOBAL_PLACEHOLDERS

#include <cppad/cppad.hpp>
#include <Eigen/Core>

#ifndef PYCPPAD_EXCLUDE_EIGEN_NUMTRAITS_SPECIALIZATION

namespace Eigen
{
  template <class Base> struct NumTraits< CppAD::AD<Base> >
  {  // type that corresponds to the real part of an AD<Base> value
    typedef CppAD::AD<Base>   Real;
    // type for AD<Base> operations that result in non-integer values
    typedef CppAD::AD<Base>   NonInteger;
    //  type to use for numeric literals such as "2" or "0.5".
    typedef CppAD::AD<Base>   Literal;
    // type for nested value inside an AD<Base> expression tree
    typedef CppAD::AD<Base>   Nested;

    enum {
      // does not support complex Base types
      IsComplex             = 0 ,
      // does not support integer Base types
      IsInteger             = 0 ,
      // only support signed Base types
      IsSigned              = 1 ,
      // must initialize an AD<Base> object
      RequireInitialization = 1 ,
      // computational cost of the corresponding operations
      ReadCost              = 1 ,
      AddCost               = 2 ,
      MulCost               = 2
    };

    // machine epsilon with type of real part of x
    // (use assumption that Base is not complex)
    static CppAD::AD<Base> epsilon(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::epsilon(); }

    // relaxed version of machine epsilon for comparison of different
    // operations that should result in the same value
    static CppAD::AD<Base> dummy_precision(void)
    {  return 100. *
      CppAD::numeric_limits< CppAD::AD<Base> >::epsilon();
    }

    // minimum normalized positive value
    static CppAD::AD<Base> lowest(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::min(); }

    // maximum finite value
    static CppAD::AD<Base> highest(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::max(); }

    // number of decimal digits that can be represented without change.
    static int digits10(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::digits10; }
  };
} // namespace Eigen

#endif // ifndef PYCPPAD_EXCLUDE_EIGEN_NUMTRAITS_SPECIALIZATION

#endif // ifndef __pycppad_fwd_hpp__


#ifndef RAY_HPP
#define	RAY_HPP
#include <armadillo>

using namespace arma;

class Ray {

private:
    vec origem, d, inv_d;
    double tmin, tmax;
    int sign[3];

public:
    Ray();

    Ray(vec origem, vec d){

	this->origem = origem;
	this->d = d;
        this->inv_d = 1/d;
	this->tmin = std::numeric_limits<double>::min();
	this->tmax = std::numeric_limits<double>::max();
        this->sign[0] = (inv_d(0) < 0);
        this->sign[1] = (inv_d(1) < 0);
        this->sign[2] = (inv_d(2) < 0);
      }

    void SetTmax(double tmax) {
        this->tmax = tmax;
    }

    double GetTmax() const {
        return tmax;
    }

    void SetTmin(double tmin) {
        this->tmin = tmin;
    }

    double GetTmin() const {
        return tmin;
    }

    void SetInv_d(vec inv_d) {
        this->inv_d = inv_d;
    }

    vec GetInv_d() const {
        return inv_d;
    }

    void SetD(vec d) {
        this->d = d;
    }

    vec GetD() const {
        return d;
    }

    void SetOrigem(vec origem) {
        this->origem = origem;
    }

    vec GetOrigem() const {
        return origem;
    }

    void SetSign(int sign[3]) {
        this->sign[0] = sign[0];
        this->sign[1] = sign[1];
        this->sign[2] = sign[2];
    }

    int* GetSign() {
        return sign;
    }

};

#endif	/* RAY_HPP */


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <iostream>
#include <stdint.h>
#include <Eigen/Dense>

#include <dpMM/global.hpp>
#include <dpMM/clGMMData.hpp>

using namespace Eigen;
using std::cout;
using std::endl;

//TODO add to remaining classes. I added them to the ones I found.. RC 10/29/14.
enum baseMeasureType {BASE, NIW_SAMPLED, NIW_MARGINALIZED,
					  DIR_SAMPLED, NIW_TANGENT, NIW_SPHERE,
					  NIW_SPHERE_FULL, UNIF_SPHERE, MF_T};

template<typename T>
class BaseMeasure
{
public:
  BaseMeasure()
  {};
  virtual ~BaseMeasure()
  {};

  virtual baseMeasureType getBaseMeasureType() const {return(BASE); }

  virtual BaseMeasure<T>* copy() = 0;

  // log likelihood under the model sampled from this base measure
  virtual T logLikelihood(const Matrix<T,Dynamic,1>& x) const =0;
  virtual T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};

  //adding this here to let derived classes to work with SS rather than data
  virtual T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const
	  {assert(false); return(T(1e20));}
  virtual void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const VectorXu& z, uint32_t k)
	{assert(false);}
  virtual void posteriorFromSS(const Matrix<T,Dynamic,1> &x)
	{assert(false);}

  // sample new model from the possterior under this base measure
  virtual void posterior(const Matrix<T,Dynamic,Dynamic>&x, const VectorXu& z,
      uint32_t k) =0;
  virtual void posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const VectorXu& z,
      uint32_t k)
  {assert(false);};
  virtual void posterior(const boost::shared_ptr<ClGMMData<T> >& cldp, uint32_t k)
  {assert(false);};

  // log pdf value of the current model under the base measure
  virtual T logPdfUnderPrior() const =0;

  // log pdf value of the data point under the base measure (integrated over
  // model parameters - could use Monte carlo integration if not analytic)
  virtual T logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x) {return 0.;};
  //  log pdf value of the data point under the base measure using the
  //  sufficient statistics stored with the base measure
  virtual T logPdfUnderPriorMarginalized() const {return 0;};

  virtual void print() const =0;
  virtual uint32_t getDim() const =0; // {return(0);};

//  //TODO UGLY
//  virtual Matrix<T,Dynamic,Dynamic>& scatter()
//    {assert(false); return Matrix<T,Dynamic,Dynamic>::Zero(1,1);};
//  //TODO UGLY
//  virtual T& count()
//    {assert(false); return 0;};
};


/*
 *            Copyright 2009-2017 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.
 *
 */

#include <votca/xtp/gwbse.h>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <votca/xtp/aomatrix.h>
#include <votca/xtp/threecenters.h>
// #include <votca/xtp/logger.h>
#include <votca/xtp/qmpackagefactory.h>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <votca/xtp/aoshell.h>

using boost::format;
using namespace boost::filesystem;

namespace votca {
    namespace xtp {
        namespace ub = boost::numeric::ublas;

        void GWBSE::PPM_construct_parameters(const ub::matrix<double>& _overlap_cholesky_inverse) {

            // multiply with L-1^t from the right

            ub::matrix<double> _temp = ub::prod(_epsilon[0], ub::trans(_overlap_cholesky_inverse));
            // multiply with L-1 from the left

            _temp = ub::prod(_overlap_cholesky_inverse, _temp);

            // get eigenvalues and eigenvectors of this matrix
            ub::vector<double> _eigenvalues;
            ub::matrix<double> _eigenvectors;

            linalg_eigenvalues(_temp, _eigenvalues, _eigenvectors);
            _eigenvectors=ub::trans(_eigenvectors);
            // multiply eigenvectors with overlap_cholesky_inverse and store as eigenvalues of epsilon
            _ppm_phi = ub::prod(_eigenvectors, _overlap_cholesky_inverse);

            // store PPM weights from eigenvalues
            _ppm_weight.resize(_eigenvalues.size());
            for (unsigned _i = 0; _i < _eigenvalues.size(); _i++) {
                _ppm_weight(_i) = 1.0 - 1.0 / _eigenvalues(_i);
            }

            // determine PPM frequencies
            _ppm_freq.resize(_eigenvalues.size());
            // a) phi^t * epsilon(1) * phi

            _temp = ub::prod(_ppm_phi, _epsilon[1]);

            _eigenvectors = ub::prod(_temp, ub::trans(_ppm_phi));
            // b) invert

            linalg_invert(_eigenvectors, _temp);
            // c) PPM parameters -> diagonal elements

            #pragma omp parallel for
            for (unsigned _i = 0; _i < _eigenvalues.size(); _i++) {

                if (_screening_freq(1, 0) == 0.0) {
                    if (_ppm_weight(_i) < 1.e-5) {
                        _ppm_weight(_i) = 0.0;
                        _ppm_freq(_i) = 0.5;//Hartree
                        continue;
                    } else {
                        double _nom = _temp(_i, _i) - 1.0;
                        double _frac = -1.0 * _nom / (_nom + _ppm_weight(_i)) * _screening_freq(1, 1) * _screening_freq(1, 1);
                        _ppm_freq(_i) = sqrt(std::abs(_frac));
                    }

                } else {
                    // only purely imaginary frequency assumed
                    cerr << " mixed frequency! real part: " << _screening_freq(1, 0) << " imaginary part: " << _screening_freq(1, 1) << flush;
                    exit(1);
                }

            }

            // epsilon can be deleted
            _epsilon[0].resize(0, 0);
            _epsilon[1].resize(0, 0);

            return;
        }

        //imaginary
        ub::matrix<double> GWBSE::RPA_imaginary(const TCMatrix& _Mmn_RPA,const double screening_freq) {
            const int _size = _Mmn_RPA.get_beta(); // size of gwbasis
            const int index_n = _Mmn_RPA.get_nmin();
            const int index_m = _Mmn_RPA.get_mmin();
            const double screenf2=screening_freq * screening_freq;
            ub::matrix<double> result=ub::zero_matrix<double>(_size);

            const ub::vector<double>& qp_energies=   _qp_energies;

            #pragma omp parallel for
            for (int _m_level = 0; _m_level < _Mmn_RPA.get_mtot(); _m_level++) {
                const double _qp_energy_m=qp_energies(_m_level + index_m);
#if (GWBSE_DOUBLE)
                const ub::matrix<double>& Mmn_RPA = _Mmn_RPA[ _m_level ];
#else
                const ub::matrix<double> Mmn_RPA = _Mmn_RPA[ _m_level ];
#endif
                // a temporary matrix, that will get filled in empty levels loop
                ub::matrix<double> _temp = ub::matrix<double>(_Mmn_RPA.get_ntot(), _size);

                // loop over empty levels
                for (int _n_level = 0; _n_level < _Mmn_RPA.get_ntot(); _n_level++) {

                    const double _deltaE = qp_energies(_n_level + index_n) -_qp_energy_m ; // get indices and units right!!!

                    // this only works, if we have either purely real or purely imaginary frequencies

                    // purely imaginary
                    const double _energy_factor = 4.0 * _deltaE / (_deltaE * _deltaE + screenf2);//hartree
                    for (int _i_gw = 0; _i_gw < _size; _i_gw++) {
                        _temp(_n_level, _i_gw) = _energy_factor * Mmn_RPA(_i_gw, _n_level);
                    } // matrix size

                } // empty levels
                _temp=ub::prod(Mmn_RPA, _temp);
                // now multiply and add to epsilon
                #pragma omp critical
                {
                result+=_temp;
                }
            } // occupied levels

            return result;
        }
        //real

        ub::matrix<double> GWBSE::RPA_real(const TCMatrix& _Mmn_RPA, const double screening_freq) {
            const int _size = _Mmn_RPA.get_beta(); // size of gwbasis
            const int index_n = _Mmn_RPA.get_nmin();
            const int index_m = _Mmn_RPA.get_mmin();
            const ub::vector<double>& qp_energies=   _qp_energies;
            ub::matrix<double> result=ub::zero_matrix<double>(_size);

            #pragma omp parallel for
            for (int _m_level = 0; _m_level < _Mmn_RPA.get_mtot(); _m_level++) {
                const double _qp_energy_m=qp_energies(_m_level + index_m);

#if (GWBSE_DOUBLE)
                const ub::matrix<double>& Mmn_RPA = _Mmn_RPA[ _m_level ];
#else
                const ub::matrix<double> Mmn_RPA = _Mmn_RPA[ _m_level ];
#endif

                // a temporary matrix, that will get filled in empty levels loop
                ub::matrix<double> _temp = ub::matrix<double>(_Mmn_RPA.get_ntot(), _size);

                // loop over empty levels
                for (int _n_level = 0; _n_level < _Mmn_RPA.get_ntot(); _n_level++) {

                    const double _deltaE = qp_energies(_n_level + index_n) - _qp_energy_m; // get indices and units right!!!

                    // this only works, if we have either purely real or purely imaginary frequencies

                    // purely real
                    const double _energy_factor =2.0*  (1.0 / (_deltaE - screening_freq) + 1.0 / (_deltaE + screening_freq));//hartree

                    for (int _i_gw = 0; _i_gw < _size; _i_gw++) {
                        _temp(_n_level, _i_gw) = _energy_factor * Mmn_RPA(_i_gw, _n_level);
                    } // matrix size

                } // empty levels

                // now multiply and add to epsilon
               _temp=ub::prod(Mmn_RPA, _temp);
                #pragma omp critical
                {
                result+=_temp;
                }
            } // occupied levels

            return result;
        }

    void GWBSE::RPA_calculate_epsilon(const TCMatrix& _Mmn_RPA){

        // loop over frequencies
        for ( unsigned _i_freq = 0 ; _i_freq < _screening_freq.size1() ; _i_freq++ ){

             if ( _screening_freq( _i_freq, 0) == 0.0 ) {
                 _epsilon[ _i_freq ]+=RPA_imaginary(_Mmn_RPA, _screening_freq( _i_freq, 1));
             }

             else if ( _screening_freq( _i_freq, 1) == 0.0  ) {
                  // purely real
                 _epsilon[ _i_freq ]+= RPA_real(_Mmn_RPA, _screening_freq( _i_freq, 0));
                    }
             else {
                    // mixed -> FAIL
                    cerr << " mixed frequency! real part: " << _screening_freq( _i_freq, 0 ) << " imaginary part: "  << _screening_freq( _i_freq, 1 ) << flush;
                    exit(1);
                    }

        } // loop over frequencies

        return;
    }

    void GWBSE::RPA_prepare_threecenters(TCMatrix& _Mmn_RPA,const TCMatrix& _Mmn_full){

        ub::range full=ub::range(0, _Mmn_full.get_beta());
        ub::range RPA_cut=ub::range(_Mmn_RPA.get_nmin() - _Mmn_full.get_nmin(), _Mmn_RPA.get_nmax() - _Mmn_full.get_nmin() + 1);
            // loop over m-levels in _Mmn_RPA
            #pragma omp parallel for
            for (int _m_level = 0; _m_level < _Mmn_RPA.size(); _m_level++) {

                // copy to _Mmn_RPA
                _Mmn_RPA[ _m_level ] = ub::project(_Mmn_full[ _m_level ], full, RPA_cut);

            }// loop m-levels
     return;
    }

}};


#ifndef PYTHONIC_SCIPY_SPECIAL_BINOM_HPP
#define PYTHONIC_SCIPY_SPECIAL_BINOM_HPP

#include "pythonic/include/scipy/special/binom.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/binomial.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double binom(T0 n, T1 k)
      {
        static_assert(std::is_integral<T0>::value &&
                          std::is_integral<T1>::value,
                      "only support integer case of scipy.special.binom");
        using namespace boost::math::policies;
        return boost::math::binomial_coefficient<double>(
            n, k, make_policy(promote_double<true>()));
      }
    }

#define NUMPY_NARY_FUNC_NAME binom
#define NUMPY_NARY_FUNC_SYM details::binom
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2008-2012  Artyom Beilis (Tonkikh) <artyomtnk@yahoo.com>
//
//  See accompanying file COPYING.TXT file for licensing details.
//
///////////////////////////////////////////////////////////////////////////////
#define CPPCMS_SOURCE
#include <cppcms/crypto.h>
#include <cppcms/config.h>
#ifdef CPPCMS_HAVE_GCRYPT
#  include <gcrypt.h>
#endif
#ifdef CPPCMS_HAVE_OPENSSL
#  include <openssl/sha.h>
#endif
#include <vector>
#include <booster/backtrace.h>
#include <booster/nowide/cstdio.h>
#include "md5.h"
#include "sha1.h"

#include <string.h>

namespace cppcms {
namespace crypto {
	namespace {
		class md5_digets : public message_digest {
		public:
			md5_digets()
			{
				impl::md5_init(&state_);
			}
			virtual unsigned digest_size() const
			{
				return 16;
			}
			virtual unsigned block_size() const
			{
				return 64;
			}
			virtual void append(void const *ptr,size_t size)
			{
				impl::md5_append(&state_,reinterpret_cast<impl::md5_byte_t const *>(ptr),size);
			}
			virtual void readout(void *ptr)
			{
				impl::md5_finish(&state_,reinterpret_cast<impl::md5_byte_t *>(ptr));
				impl::md5_init(&state_);
			}
			virtual char const *name() const
			{
				return "md5";
			}
			virtual md5_digets *clone() const
			{
				return new md5_digets();
			}
			virtual ~md5_digets()
			{
				memset(&state_,0,sizeof(state_));
			}
		private:
			impl::md5_state_t state_;
		};
		class sha1_digets : public message_digest {
		public:
			sha1_digets()
			{
				state_.reset();
			}
			virtual unsigned digest_size() const
			{
				return 20;
			}
			virtual unsigned block_size() const
			{
				return 64;
			}
			virtual void append(void const *ptr,size_t size)
			{
				state_.process_bytes(ptr,size);
			}
			virtual void readout(void *ptr)
			{
				unsigned digets[5];
				state_.get_digest(digets);
				state_.reset();
				unsigned char *out = reinterpret_cast<unsigned char *>(ptr);
				for(unsigned i=0;i<5;i++) {
					unsigned block = digets[i];
					*out ++ = (block >> 24u) & 0xFFu;
					*out ++ = (block >> 16u) & 0xFFu;
					*out ++ = (block >>  8u) & 0xFFu;
					*out ++ = (block >>  0u) & 0xFFu;
				}
			}
			virtual char const *name() const
			{
				return "sha1";
			}
			virtual sha1_digets *clone() const
			{
				return new sha1_digets();
			}
			virtual ~sha1_digets()
			{
				memset(&state_,0,sizeof(state_));
			}
		private:
			impl::sha1 state_;
		};

		#ifdef CPPCMS_HAVE_GCRYPT
		class gcrypt_digets : public message_digest {
		public:
			gcrypt_digets(int algorithm)
			{
				if(gcry_md_open(&state_,algorithm,0)!=0)
					throw booster::invalid_argument("gcrypt_digets - faled to create md");
				algo_ = algorithm;
			}
			virtual unsigned digest_size() const
			{
				return gcry_md_get_algo_dlen(algo_);
			}
			virtual unsigned block_size() const
			{
				if(algo_ == GCRY_MD_SHA384 || algo_ == GCRY_MD_SHA512)
					return 128;
				else
					return 64;
			}
			virtual void append(void const *ptr,size_t size)
			{
				gcry_md_write(state_,ptr,size);
			}
			virtual void readout(void *ptr)
			{
				memcpy(ptr,gcry_md_read(state_,0),digest_size());
				gcry_md_reset(state_);
			}
			virtual char const *name() const
			{
				switch(algo_) {
				case GCRY_MD_SHA224:
					return "sha224";
				case GCRY_MD_SHA256:
					return "sha256";
				case GCRY_MD_SHA384:
					return "sha384";
				case GCRY_MD_SHA512:
					return "sha512";
				default:
					return "unknown";
				}
			}
			virtual gcrypt_digets *clone() const
			{
				return new gcrypt_digets(algo_);
			}
			virtual ~gcrypt_digets()
			{
				gcry_md_close(state_);
				state_ = 0;
			}
		private:
			gcry_md_hd_t state_;
			int algo_;
		};

		#endif

		#ifdef CPPCMS_HAVE_OPENSSL

		#define CPPCMS_SSL_MD(len,base_len)				\
		class ssl_sha##len : public message_digest {			\
		public:								\
			ssl_sha##len() 						\
			{							\
				SHA ## len ## _Init(&state_);			\
			}							\
			virtual message_digest *clone() const			\
			{							\
				return new ssl_sha##len();			\
			}							\
			virtual ~ssl_sha##len()					\
			{							\
				memset(&state_,0,sizeof(state_));		\
			}							\
			virtual void append(void const *ptr,size_t size) 	\
			{							\
				SHA ## len ## _Update(&state_,ptr,size);	\
			}							\
			virtual void readout(void *ptr)				\
			{							\
				SHA ## len ## _Final(				\
					(unsigned char *)(ptr),			\
					&state_					\
					);					\
				SHA ## len ## _Init(&state_);			\
			}							\
			virtual char const *name() const			\
			{							\
				return "sha" #len;				\
			}							\
			virtual unsigned digest_size() const			\
			{							\
				return len / 8;					\
			}							\
			virtual unsigned block_size() const			\
			{							\
				if (len >= 384)					\
					return 128;				\
				else						\
					return 64;				\
			}							\
		private:							\
			SHA ## base_len ##	_CTX state_;			\
		};

		CPPCMS_SSL_MD(224,256)
		CPPCMS_SSL_MD(256,256)
		CPPCMS_SSL_MD(384,512)
		CPPCMS_SSL_MD(512,512)

		#endif

	} // anon

	std::auto_ptr<message_digest> message_digest::md5()
	{
		std::auto_ptr<message_digest> d(new md5_digets());
		return d;
	}

	std::auto_ptr<message_digest> message_digest::sha1()
	{
		std::auto_ptr<message_digest> d(new sha1_digets());
		return d;
	}

	std::auto_ptr<message_digest> message_digest::create_by_name(std::string const &namein)
	{
		std::auto_ptr<message_digest> d;
		std::string name = namein;
		for(unsigned i=0;i<name.size();i++)
			if('A' <= name[i] && name[i]<='Z')
				name[i] = name[i] - 'A' + 'a';
		if(name=="md5")
			d = md5();
		else if(name == "sha1")
			d = sha1();
		#ifdef CPPCMS_HAVE_GCRYPT
		else if(name == "sha224")
			d.reset(new gcrypt_digets(GCRY_MD_SHA224));
		else if(name == "sha256")
			d.reset(new gcrypt_digets(GCRY_MD_SHA256));
		else if(name == "sha384")
			d.reset(new gcrypt_digets(GCRY_MD_SHA384));
		else if(name == "sha512")
			d.reset(new gcrypt_digets(GCRY_MD_SHA512));
		#endif

		#ifdef CPPCMS_HAVE_OPENSSL
		else if(name == "sha224")
			d.reset(new ssl_sha224());
		else if(name == "sha256")
			d.reset(new ssl_sha256());
		else if(name == "sha384")
			d.reset(new ssl_sha384());
		else if(name == "sha512")
			d.reset(new ssl_sha512());
		#endif

		return d;
	}

	key::key() : data_(0), size_(0)
	{
	}

	key::key(key const &other) : data_(0),size_(0)
	{
		set(other.data(),other.size());
	}

	key const &key::operator=(key const &other)
	{
		if(this != &other) {
			set(other.data_,other.size_);
		}
		return *this;
	}

	key::key(void const *ptr,size_t len) : data_(0), size_(0)
	{
		set(ptr,len);
	}

	key::key(char const *s) : data_(0), size_(0)
	{
		if(s!=0)
			set_hex(s,strlen(s));
	}

	key::key(std::string const &s) : data_(0), size_(0)
	{
		set_hex(s.c_str(),s.size());
	}

	unsigned key::from_hex(char c)
	{
		if('0' <= c && c<='9')
			return c-'0';
		if('a' <= c && c<='f')
			return c-'a' + 10;
		if('A' <= c && c<='F')
			return c-'A' + 10;
		return 0;
	}
	void key::read_from_file(std::string const &file_name)
	{
		reset();

		FILE *f = 0;
		char *buf = 0;
		size_t buf_size = 0;

		try {
			f = booster::nowide::fopen(file_name.c_str(),"rb");
			if(!f) {
				throw booster::runtime_error("cppcms::crypto::key Failed to open file:" + file_name);
			}
			setbuf(f,0);
			fseek(f,0,SEEK_END);
			long size = ftell(f);
			if(size < 0) {
				throw booster::runtime_error("cppcms::crypto::key failed to get file size:" + file_name);
			}
			if(size == 0) {
				throw booster::runtime_error("cppcms::crypto::key file " + file_name + " is empty");
			}
			fseek(f,0,SEEK_SET);
			buf = new char[size]();
			buf_size = size;
			if(fread(buf,1,buf_size,f)!=buf_size) {
				throw booster::runtime_error("cppcms::crypto::key failed reading file " + file_name);
			}
			fclose(f);
			f=0;
			int i;
			for(i=buf_size-1;i>=0;i--) {
				if(buf[i]==' ' || buf[i]=='\n' || buf[i]=='\r' || buf[i]=='\t')
					continue;
				break;
			}
			size_t real_size = i + 1;
			set_hex(buf,real_size);
		}
		catch(...) {
			if(f) {
				fclose(f);
				f=0;
			}
			if(buf) {
				memset(buf,0,buf_size);
				delete [] buf;
				buf = 0;
			}
			throw;
		}
		if(f) {
			fclose(f);
			f=0;
		}
		if(buf) {
			memset(buf,0,buf_size);
			delete [] buf;
			buf = 0;
		}
	}

	void key::set_hex(char const *ptr,size_t len)
	{
		reset();
		if(len == 0)
			return;
		if(len % 2 != 0) {
			throw booster::invalid_argument("cppcms::crypto::key: the hexadecimal key length is not multiple of 2");
		}
		for(unsigned i=0;i<len;i++) {
			char c=ptr[i];
			if(	('0'<=c && c<='9')
				|| ('a' <= c && c<='f')
				|| ('A' <= c && c<='F')
			  )
			{
				continue;
			}
			throw booster::invalid_argument("cppcms::crypto::key: the hexadecimal key has invalid characters");
		}
		size_ = len / 2;
		data_ = new char[size_];
		for(unsigned h=0,b=0;h<len;h+=2,b++) {
			data_[b] = (from_hex(ptr[h]) << 4) + from_hex(ptr[h+1]);
		}
	}

	void key::set(void const *ptr,size_t len)
	{
		reset();
		if(ptr) {
			data_ = new char[len];
			size_ = len;
			memcpy(data_,ptr,len);
		}
	}
	void key::reset()
	{
		if(data_) {
			memset(data_,0,size_);
			delete [] data_;
			data_ = 0;
			size_ = 0;
		}
	}
	key::~key()
	{
		reset();
	}

	char const *key::data() const
	{
		if(data_ == 0)
			return "";
		return data_;
	}
	size_t key::size() const
	{
		return size_;
	}

	struct hmac::data_ {};

	hmac::hmac(std::auto_ptr<message_digest> digest,key const &k) : key_(k)
	{
		if(!digest.get())
			throw booster::invalid_argument("Has algorithm is not provided");
		md_ = digest;
		md_opad_.reset(md_->clone());
		init();
	}
	hmac::hmac(std::string const &name,key const &k) : key_(k)
	{
		md_ = message_digest::create_by_name(name);
		if(!md_.get()) {
			throw booster::invalid_argument("Invalid or unsupported hash function:" + name);
		}
		md_opad_.reset(md_->clone());
		init();
	}
	hmac::~hmac()
	{
	}
	void hmac::init()
	{
		unsigned const block_size = md_->block_size();
		std::vector<unsigned char> ipad(block_size,0);
		std::vector<unsigned char> opad(block_size,0);
		if(key_.size() > block_size) {
			md_->append(key_.data(),key_.size());
			md_->readout(&ipad.front());
			memcpy(&opad.front(),&ipad.front(),md_->digest_size());
		}
		else {
			memcpy(&ipad.front(),key_.data(),key_.size());
			memcpy(&opad.front(),key_.data(),key_.size());
		}
		for(unsigned i=0;i<block_size;i++) {
			ipad[i]^=0x36 ;
			opad[i]^=0x5c ;
		}
		md_opad_->append(&opad.front(),block_size);
		md_->append(&ipad.front(),block_size);
		ipad.assign(block_size,0);
		opad.assign(block_size,0);
	}
	unsigned hmac::digest_size() const
	{
		if(!md_.get()){
			throw booster::runtime_error("Hmac can be used only once");
		}
		return md_->digest_size();
	}
	void hmac::append(void const *ptr,size_t size)
	{
		if(!md_.get()){
			throw booster::runtime_error("Hmac can be used only once");
		}
		md_->append(ptr,size);
	}
	void hmac::readout(void *ptr)
	{
		std::vector<unsigned char> digest(md_->digest_size(),0);
		md_->readout(&digest.front()); // md_ is reset
		md_opad_->append(&digest.front(),md_->digest_size());
		md_opad_->readout(ptr); // md_opad_ is reset now
		digest.assign(md_->digest_size(),0);
		init();
	}

} // crypto
} // cppcms


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/lstm.h"
#include "dynet/dict.h"
#include "dynet/expr.h"
#include "getpid.h"

#include <iostream>
#include <fstream>
#include <sstream>

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

using namespace std;
using namespace dynet;
using namespace dynet::expr;

//parameters
unsigned LAYERS = 3;
unsigned INPUT_DIM = 500;
unsigned HIDDEN_DIM = 500;
unsigned INPUT_VOCAB_SIZE = 0;
unsigned OUTPUT_VOCAB_SIZE = 0;

dynet::Dict d, devd;
int kSOS;
int kEOS;

template <class Builder>
struct EncoderDecoder {
  LookupParameter p_c;
  LookupParameter p_ec;  // map input to embedding (used in fwd and rev models)
  Parameter p_ie2h;
  Parameter p_bie;
  Parameter p_h2oe;
  Parameter p_boe;
  Parameter p_R;
  Parameter p_bias;
  Builder dec_builder;
  Builder rev_enc_builder;
  Builder fwd_enc_builder;

  EncoderDecoder() {}

  explicit EncoderDecoder(Model& model) :
      dec_builder(LAYERS, INPUT_DIM, HIDDEN_DIM, &model),
      rev_enc_builder(LAYERS, INPUT_DIM, HIDDEN_DIM, &model),
      fwd_enc_builder(LAYERS, INPUT_DIM, HIDDEN_DIM, &model) {

    p_ie2h = model.add_parameters({unsigned(HIDDEN_DIM * LAYERS * 1.5), unsigned(HIDDEN_DIM * LAYERS * 2)});
    p_bie = model.add_parameters({unsigned(HIDDEN_DIM * LAYERS * 1.5)});
    p_h2oe = model.add_parameters({unsigned(HIDDEN_DIM * LAYERS), unsigned(HIDDEN_DIM * LAYERS * 1.5)});
    p_boe = model.add_parameters({unsigned(HIDDEN_DIM * LAYERS)});
    p_c = model.add_lookup_parameters(INPUT_VOCAB_SIZE, {INPUT_DIM});
    p_ec = model.add_lookup_parameters(INPUT_VOCAB_SIZE, {INPUT_DIM});
    p_R = model.add_parameters({OUTPUT_VOCAB_SIZE, HIDDEN_DIM});
    p_bias = model.add_parameters({OUTPUT_VOCAB_SIZE});
  }

  // build graph and return Expression for total loss
  Expression BuildGraph(const vector<int>& insent, const vector<int>& osent, ComputationGraph& cg) {
    // forward encoder
    fwd_enc_builder.new_graph(cg);
    fwd_enc_builder.start_new_sequence();
    for (unsigned t = 0; t < insent.size(); ++t) {
    	Expression i_x_t = lookup(cg,p_ec,insent[t]);
      fwd_enc_builder.add_input(i_x_t);
    }
    // backward encoder
    rev_enc_builder.new_graph(cg);
    rev_enc_builder.start_new_sequence();
    for (int t = insent.size() - 1; t >= 0; --t) {
      Expression i_x_t = lookup(cg, p_ec, insent[t]);
      rev_enc_builder.add_input(i_x_t);
    }

    // encoder -> decoder transformation
    vector<Expression> to;
    for (auto h_l : fwd_enc_builder.final_h()) to.push_back(h_l);
    for (auto h_l : rev_enc_builder.final_h()) to.push_back(h_l);

    Expression i_combined = concatenate(to);
    Expression i_ie2h = parameter(cg, p_ie2h);
    Expression i_bie = parameter(cg, p_bie);
    Expression i_t = i_bie + i_ie2h * i_combined;

    Expression i_h = rectify(i_t);
    Expression i_h2oe = parameter(cg,p_h2oe);
    Expression i_boe = parameter(cg,p_boe);
    Expression i_nc = i_boe + i_h2oe * i_h;

    vector<Expression> oein1, oein2, oein;
    for (unsigned i = 0; i < LAYERS; ++i) {
      oein1.push_back(pickrange(i_nc, i * HIDDEN_DIM, (i + 1) * HIDDEN_DIM));
      oein2.push_back(tanh(oein1[i]));
    }
    for (unsigned i = 0; i < LAYERS; ++i) oein.push_back(oein1[i]);
    for (unsigned i = 0; i < LAYERS; ++i) oein.push_back(oein2[i]);

    dec_builder.new_graph(cg);
    dec_builder.start_new_sequence(oein);

    // decoder
    Expression i_R = parameter(cg,p_R);
    Expression i_bias = parameter(cg,p_bias);
    vector<Expression> errs;

    const unsigned oslen = osent.size() - 1;
    for (unsigned t = 0; t < oslen; ++t) {
    	Expression i_x_t = lookup(cg, p_c, osent[t]);
    	Expression i_y_t = dec_builder.add_input(i_x_t);
    	Expression i_r_t = i_bias + i_R * i_y_t;
    	Expression i_ydist = log_softmax(i_r_t);
    	errs.push_back(pick(i_ydist,osent[t+1]));
    }
    Expression i_nerr = sum(errs);
    return -i_nerr;
  }

private:
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar & p_c & p_ec;
    ar & p_ie2h & p_bie & p_h2oe & p_boe & p_R & p_bias;
    ar & dec_builder & rev_enc_builder & fwd_enc_builder;
  }
};

int main(int argc, char** argv) {
  dynet::initialize(argc, argv);
  if (argc != 3 && argc != 4) {
    cerr << "Usage: " << argv[0] << " corpus.txt dev.txt [model.params]\n";
    return 1;
  }
  kSOS = d.convert("<s>");
  kEOS = d.convert("</s>");
  vector<vector<int>> training, dev;
  string line;
  int tlc = 0;
  int ttoks = 0;
  cerr << "Reading training data from " << argv[1] << "...\n";
  {
    ifstream in(argv[1]);
    assert(in);
    while(getline(in, line)) {
      ++tlc;
      training.push_back(read_sentence(line, &d));
      ttoks += training.back().size();
      if (training.back().front() != kSOS && training.back().back() != kEOS) {
	cerr << "Training sentence in " << argv[1] << ":" << tlc << " didn't start or end with <s>, </s>\n";
	abort();
      }
    }
    cerr << tlc << " lines, " << ttoks << " tokens, " << d.size() << " types\n";
  }
  d.freeze(); // no new word types allowed
  INPUT_VOCAB_SIZE = d.size();
  OUTPUT_VOCAB_SIZE = d.size();

  int dlc = 0;
  int dtoks = 0;
  cerr << "Reading dev data from " << argv[2] << "...\n";
  {
    ifstream in(argv[2]);
    assert(in);
    while(getline(in, line)) {
      ++dlc;
      dev.push_back(read_sentence(line, &devd));
      dtoks += dev.back().size();
      if (dev.back().front() != kSOS && dev.back().back() != kEOS) {
	cerr << "Dev sentence in " << argv[2] << ":" << dlc << " didn't start or end with <s>, </s>\n";
	abort();
      }
    }
    cerr << dlc << " lines, " << dtoks << " tokens\n";
  }

  ostringstream os;
  os << "bilm"
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "Parameters will be written to: " << fname << endl;
  double best = 9e+99;

  Model model;
  bool use_momentum = false;
  Trainer* sgd = nullptr;
  if (use_momentum)
    sgd = new MomentumSGDTrainer(&model);
  else
    sgd = new SimpleSGDTrainer(&model);

  //RNNBuilder rnn(LAYERS, INPUT_DIM, HIDDEN_DIM, &model);
  //EncoderDecoder<SimpleRNNBuilder> lm(model);
  EncoderDecoder<LSTMBuilder> lm;
  if (argc == 4) {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }
  else {
    lm = EncoderDecoder<LSTMBuilder>(model);
  }

  unsigned report_every_i = 50;
  unsigned dev_every_i_reports = 10;
  unsigned si = training.size();
  vector<unsigned> order(training.size());
  for (unsigned i = 0; i < order.size(); ++i) order[i] = i;
  bool first = true;
  int report = 0;
  unsigned lines = 0;
  while(1) {
    Timer iteration("completed in");
    double loss = 0;
    unsigned chars = 0;
    for (unsigned i = 0; i < report_every_i; ++i) {
        if (si == training.size()) {
            si = 0;
            if (first) { first = false; } else { sgd->update_epoch(); }
            cerr << "**SHUFFLE\n";
            random_shuffle(order.begin(), order.end());
        }

        // build graph for this instance
        ComputationGraph cg;
        auto& sent = training[order[si]];
        chars += sent.size() - 1;
        ++si;
        Expression loss_expr = lm.BuildGraph(sent, sent, cg);
        //cg.print_graphviz();
        loss += as_scalar(cg.forward(loss_expr));
        cg.backward(loss_expr);
        sgd->update();
        ++lines;
    }
    sgd->status();
    cerr << " E = " << (loss / chars) << " ppl=" << exp(loss / chars) << ' ';

#if 0
    lm.RandomSample();
#endif

    // show score on dev data?
    report++;
    if (report % dev_every_i_reports == 0) {
      double dloss = 0;
      int dchars = 0;
      for (auto& sent : dev) {
	ComputationGraph cg;
	Expression loss_expr = lm.BuildGraph(sent, sent, cg);
	dloss += as_scalar(cg.forward(loss_expr));
	dchars += sent.size() - 1;
      }
      if (dloss < best) {
	best = dloss;
	ofstream out(fname);
	boost::archive::text_oarchive oa(out);
	oa << model << lm;
      }
      cerr << "\n***DEV [epoch=" << (lines / (double)training.size()) << "] E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
    }
  }
  delete sgd;
}


#define BOOST_TEST_MODULE "test_recutangular_box_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/external/RectangularBoxInteraction.hpp>
#include <mjolnir/forcefield/external/ExcludedVolumeWallPotential.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(PositionRestraint_Harmonic)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger(
            "test_recutangular_box_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::ExcludedVolumeWallPotential<real_type>;
    using interaction_type = mjolnir::RectangularBoxInteraction<traits_type, potential_type>;

    coordinate_type lower( 0.0,  0.0,  0.0);
    coordinate_type upper(10.0, 10.0, 10.0);

    interaction_type interaction(lower, upper, /*margin = */0.5,
        potential_type(/*epsilon = */1.0, /* cutoff = */2.0, {
            {0, 1.0}, {1, 1.0}
        }));

    system_type sys(2, boundary_type{});

    std::mt19937 mt(123456789);

    for(int i = 0; i < 10000; ++i)
    {
        test::clear_everything(sys);
        sys.position(0) = coordinate_type( 1.0,  9.0,  1.0);
        sys.position(1) = coordinate_type( 9.0,  1.0,  9.0);

        test::apply_random_perturbation(sys, mt, 0.01);

        // compare between numerical diff and force implementation
        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr, false);
        test::check_force_and_energy(sys, interaction, tol);
    }
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python.hpp>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>

#include <mmtbx/tls/decompose.h>

namespace mmtbx { namespace tls { namespace decompose {
  namespace bp = boost::python;
  namespace af = scitbx::af;

namespace {

  boost::python::tuple get_v_amplitudes(const decompose_tls_matrices& a)
  { return boost::python::tuple(a.v_amplitudes); }

  boost::python::tuple get_l_amplitudes(const decompose_tls_matrices& a)
  { return boost::python::tuple(a.l_amplitudes); }

  boost::python::tuple get_s_amplitudes(const decompose_tls_matrices& a)
  { return boost::python::tuple(a.s_amplitudes); }

  af::shared<scitbx::vec3<double> > get_v_axis_directions(const decompose_tls_matrices& a)
  {
    af::shared<scitbx::vec3<double> > s(3);
    s[0] = a.v1_M;
    s[1] = a.v2_M;
    s[2] = a.v3_M;
    return s;
  }

  af::shared<scitbx::vec3<double> > get_l_axis_directions(const decompose_tls_matrices& a)
  {
    af::shared<scitbx::vec3<double> > s(3);
    s[0] = a.l1_M;
    s[1] = a.l2_M;
    s[2] = a.l3_M;
    return s;
  }

  af::shared<scitbx::vec3<double> > get_l_axis_intersections(const decompose_tls_matrices& a)
  {
    af::shared<scitbx::vec3<double> > s(3);
    s[0] = a.w1_M;
    s[1] = a.w2_M;
    s[2] = a.w3_M;
    return s;
  }

  boost::python::tuple get_R_ML(const decompose_tls_matrices& a)
  { return boost::python::tuple(a.R_ML); }

  boost::python::tuple get_R_MV(const decompose_tls_matrices& a)
  { return boost::python::tuple(a.R_MV); }

  void init_module()
  {
    using namespace boost::python;
    using boost::python::arg;

    class_<decompose_tls_matrices>(
        "decompose_tls_matrices",
        init< scitbx::sym_mat3<double> const&,
          scitbx::sym_mat3<double> const&,
          scitbx::mat3<double> const&,
          bool, bool,
          double, double,
          std::string, double>(
            (arg("T"),
             arg("L"),
             arg("S"),
             arg("l_and_s_in_degrees")=true,
             arg("verbose")=false,
             arg("tol")=1.e-6,
             arg("eps")=1.e-8,
             arg("t_S_formula")="11",
             arg("t_S_value")=0.0)))

      .def("is_valid", &decompose_tls_matrices::is_valid)
      .def("error", &decompose_tls_matrices::error)

      .add_property("v_amplitudes", get_v_amplitudes)
      .add_property("l_amplitudes", get_l_amplitudes)
      .add_property("s_amplitudes", get_s_amplitudes)

      .add_property("v_axis_directions",    get_v_axis_directions)
      .add_property("l_axis_directions",    get_l_axis_directions)
      .add_property("l_axis_intersections", get_l_axis_intersections)

      .add_property("R_ML", get_R_ML)
      .add_property("R_MV", get_R_MV)

    ;

  }

} // namespace <anonymous>
}}} // namespace mmtbx::tls::decompose

BOOST_PYTHON_MODULE(mmtbx_tls_decompose_ext)
{
  mmtbx::tls::decompose::init_module();
}


#pragma once

#include <atomic>
#include <bitset>
#include <functional>
#include <numeric>
#include <stdexcept>
#include <vector>

#include <dtl/dtl.hpp>
#include <dtl/bits.hpp>
#include <dtl/math.hpp>
#include <dtl/simd.hpp>
#include <dtl/filter/blocked_bloomfilter/vector_helper.hpp>

#include "immintrin.h"

#include <boost/integer/static_min_max.hpp>

namespace dtl {

//namespace {

//===----------------------------------------------------------------------===//
// Recursive template to compute a search mask with k bits set.
// Used in case of sector count >= word_cnt, which allows to set/test multiple
// bits in a word in one go.
//===----------------------------------------------------------------------===//
template<
    typename key_t,               // the key type
    typename word_t,              // the word type
    u32 s,                        // the numbers of sectors (must be a power of two)
    u32 k,                        // the number of bits to set
    template<typename Ty, u32 i> class hasher,      // the hash function family to use
    typename hash_value_t,        // the hash value type

    u32 hash_fn_idx,              // current hash function index (used for recursion)
    u32 remaining_hash_bit_cnt,   // the number of remaining hash bits (used for recursion)
    u32 remaining_k_cnt           // current k (used for recursion)

>
struct word_block {

  //===----------------------------------------------------------------------===//
  // Static part
  //===----------------------------------------------------------------------===//
  static_assert(dtl::is_power_of_two(s), "Parameter 's' must be a power of two.");

  static constexpr u32 word_bitlength = sizeof(word_t) * 8;
  static constexpr u32 word_bitlength_mask = word_bitlength - 1;

  static constexpr u32 hash_value_bitlength = sizeof(hash_value_t) * 8;

  static constexpr u32 sector_bitlength = word_bitlength / s;
  static constexpr u32 sector_bitlength_log2 = dtl::ct::log_2_u32<sector_bitlength>::value;
  __forceinline__ __host__ __device__
  static constexpr word_t sector_mask() { return static_cast<word_t>(sector_bitlength) - 1; }

  static_assert(sector_bitlength >= 8, "A sector must be at least one byte in size.");

  static constexpr u32 hash_bit_cnt_per_k = sector_bitlength_log2;
  static constexpr u32 k_cnt_per_hash_value = ((sizeof(hash_value_t) * 8) / hash_bit_cnt_per_k) ; // consider -1 to respect hash fn weakness in the low order bits
  static constexpr u32 k_cnt_per_sector = k / s;

  static constexpr u32 current_k = k - remaining_k_cnt;

  static constexpr u1 rehash = remaining_hash_bit_cnt < hash_bit_cnt_per_k;
  static constexpr u32 remaining_hash_bit_cnt_after_rehash = rehash ? hash_value_bitlength : remaining_hash_bit_cnt;
  //===----------------------------------------------------------------------===//

  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const key_t key, hash_value_t& hash_val, word_t& word) noexcept {

    hash_val = rehash ? hasher<key_t, hash_fn_idx>::hash(key) : hash_val;

    // Set one bit in the given word; rehash if necessary
    constexpr u32 sector_idx = current_k / k_cnt_per_sector;
    constexpr u32 shift = remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k;
    $u32 bit_idx = ((hash_val >> shift) & sector_mask()) + (sector_idx * sector_bitlength);
    word |= word_t(1) << bit_idx;

    // Recurse
    word_block<key_t, word_t, s, k,
        hasher, hash_value_t,
        (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
        remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
        remaining_k_cnt - 1> // decrement the remaining k counter
      ::which_bits(key, hash_val, word);
  }
  //===----------------------------------------------------------------------===//

  template<u64 n>
  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const vec<key_t, n>& keys,
             vec<hash_value_t, n>& hash_vals,
             vec<word_t, n>& words) noexcept {

    // Typedef vector types
    using key_vt = vec<key_t, n>;
    using hash_value_vt = vec<hash_value_t, n>;
    using word_vt = vec<word_t, n>;

    hash_vals = rehash ? hasher<key_vt, hash_fn_idx>::hash(keys) : hash_vals;
    const hash_value_t remaining_hash_bit_cnt_after_rehash = rehash ? hash_value_bitlength : remaining_hash_bit_cnt;

    // Set one bit in the given word; rehash if necessary
    constexpr u32 sector_idx = current_k / k_cnt_per_sector;
    constexpr u32 shift = remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k;
    hash_value_vt bit_idxs = ((hash_vals >> shift) & sector_mask()) + (sector_idx * sector_bitlength);
    words |= word_vt(1) << internal::vector_convert<hash_value_t, word_t, n>::convert(bit_idxs);

    // Recurse
    word_block<key_t, word_t, s, k,
        hasher, hash_value_t,
        (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
        remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
        remaining_k_cnt - 1> // decrement the remaining k counter
      ::which_bits(keys, hash_vals, words);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The number of required hash functions.
  //===----------------------------------------------------------------------===//
  static constexpr u32 hash_fn_idx_end =
  word_block<key_t, word_t, s, k,
      hasher, hash_value_t,
      (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
      remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
      remaining_k_cnt - 1> // decrement the remaining k counter
    ::hash_fn_idx_end;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The number of required hash bits.
  //===----------------------------------------------------------------------===//
  static constexpr u32 remaining_hash_bits =
  word_block<key_t, word_t, s, k,
      hasher, hash_value_t,
      (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
      remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
      remaining_k_cnt - 1> // decrement the remaining k counter
    ::remaining_hash_bits;
  //===----------------------------------------------------------------------===//

};

template<
    typename key_t,               // the key type
    typename word_t,              // the word type
    u32 s,                        // the numbers of sectors (must be a power of two)
    u32 k,                        // the number of bits to set
    template<typename Ty, u32 i> class hasher,      // the hash function family to use
    typename hash_value_t,        // the hash value type to use

    u32 hash_fn_idx,              // current hash function index (used for recursion)
    u32 remaining_hash_bit_cnt    // the number of remaining hash bits (used for recursion)
>
struct word_block<key_t, word_t, s, k, hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt, 0 /* no remaining k's */> {

  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const key_t key, hash_value_t& hash_value, word_t& word) noexcept {
    // End of recursion
  }
  //===----------------------------------------------------------------------===//

  template<u64 n>
  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const vec<key_t, n>& keys,
             vec<hash_value_t, n>& hash_values,
             vec<word_t, n>& words) noexcept {
    // End of recursion
  }
  //===----------------------------------------------------------------------===//

  static constexpr u32 hash_value_bitlength = sizeof(hash_value_t) * 8;

  static constexpr u32 hash_fn_idx_end = hash_fn_idx;

  static constexpr u32 remaining_hash_bits = remaining_hash_bit_cnt;

};
//===----------------------------------------------------------------------===//

//} // anonymous namespace
} // namespace dtl

#include "blocked_bloomfilter_block_logic_sgew_specialization.hpp"

namespace dtl {

//===----------------------------------------------------------------------===//
// Recursive template to work with multi-word blocks.
//
// Used in case of sector count >= word count, which allows to sequentially
// iterate over the words of a block (sequential block access pattern).
//===----------------------------------------------------------------------===//
template<
    typename _key_t,              // the key type
    typename _word_t,             // the word type
    u32 _word_cnt,                // the number of words per block
    u32 _s,                       // the numbers of sectors (must be a power of two and greater or equal to word_cnt))
    u32 _k,                       // the number of bits to set/test
    template<typename Ty, u32 i> class hasher,      // the hash function family to use
    typename hash_value_t,        // the hash value type to use

    u32 hash_fn_idx,              // current hash function index (used for recursion)
    u32 remaining_hash_bit_cnt,   // the number of remaining hash bits (used for recursion)
    u32 remaining_word_cnt,       // the remaining number of words to process in the block (used for recursion)

    u1 early_out = false          // allows for branching out during lookups (before the next word is loaded)
>
struct multiword_block {

  //===----------------------------------------------------------------------===//
  // Static part
  //===----------------------------------------------------------------------===//
  using key_t = _key_t;
  using word_t = _word_t;

  static constexpr u32 k = _k;
  static constexpr u32 word_cnt = _word_cnt;
  static constexpr u32 word_cnt_log2 = dtl::ct::log_2<word_cnt>::value;
  static_assert(dtl::is_power_of_two(word_cnt), "Parameter 'word_cnt' must be a power of two.");
  static_assert(k >= word_cnt, "Parameter 'k' must be greater or equal to 'word_cnt'.");
  static_assert(k % word_cnt == 0, "Parameter 'k' must be dividable by 'word_cnt'.");

  static constexpr u32 sector_cnt = _s;
  static_assert(dtl::is_power_of_two(sector_cnt), "Parameter 'sector_cnt' must be a power of two.");

  static constexpr u32 sector_cnt_per_word = sector_cnt / word_cnt;
  static_assert(sector_cnt_per_word > 0, "The number of sectors must be at least 'word_cnt'.");

  static constexpr u32 k_cnt_per_word = k / word_cnt;

  static constexpr u32 current_word_idx() { return word_cnt - remaining_word_cnt; }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Insert
  //===----------------------------------------------------------------------===//
  __forceinline__ __unroll_loops__
  static void
  insert(word_t* __restrict block_ptr, const key_t key) noexcept {

    hash_value_t hash_val = 0;

    // Call the recursive function
    static constexpr u32 remaining_hash_bits = 0;
    multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bits, remaining_word_cnt, early_out>
//        ::insert(block_ptr, key, hash_val);
        ::insert_atomic(block_ptr, key, hash_val);
  }
  //===----------------------------------------------------------------------===//

  __forceinline__ __unroll_loops__
  static void
  insert(word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val) noexcept {

    // Load the word of interest
    word_t word = block_ptr[current_word_idx()];

    word_t bit_mask = 0;

    using word_block_t =
      word_block<key_t, word_t, sector_cnt_per_word, k_cnt_per_word,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt,
        k_cnt_per_word>;
    word_block_t::which_bits(key, hash_val, bit_mask);

    // Update the bit vector
    word |= bit_mask;
    block_ptr[current_word_idx()] = word;

    // Process remaining words recursively, if any
    multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, word_block_t::hash_fn_idx_end, word_block_t::remaining_hash_bits, remaining_word_cnt - 1, early_out>
        ::insert(block_ptr, key, hash_val);
  }
  //===----------------------------------------------------------------------===//

  __forceinline__ __unroll_loops__
  static void
  insert_atomic(word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val) noexcept {

    word_t bit_mask = 0;

    using word_block_t =
      word_block<key_t, word_t, sector_cnt_per_word, k_cnt_per_word,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt,
        k_cnt_per_word>;
    word_block_t::which_bits(key, hash_val, bit_mask);

    word_t* word_ptr = &block_ptr[current_word_idx()];
    std::atomic<word_t>* atomic_word_ptr = reinterpret_cast<std::atomic<word_t>*>(word_ptr);
    $u1 success = false;
    do {
      // Load the word of interest
      word_t word = atomic_word_ptr->load();
      // Update the bit vector (atomically)
      word_t updated_word = word | bit_mask;
      success = atomic_word_ptr->compare_exchange_weak(word, updated_word);
    } while (!success);

    // Process remaining words recursively, if any
    multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, word_block_t::hash_fn_idx_end, word_block_t::remaining_hash_bits, remaining_word_cnt - 1, early_out>
        ::insert_atomic(block_ptr, key, hash_val);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains
  //===----------------------------------------------------------------------===//
  __forceinline__ __unroll_loops__ __host__ __device__
  static u1
  contains(const word_t* __restrict block_ptr, const key_t key) noexcept {

    hash_value_t hash_val = 0;

    // Call the recursive function
    static constexpr u32 remaining_hash_bits = 0;
    return multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bits, remaining_word_cnt, early_out>
        ::contains(block_ptr, key, hash_val, true);
  }
  //===----------------------------------------------------------------------===//

  __forceinline__ __unroll_loops__ __host__ __device__
  static u1
  contains(const word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val, u1 is_contained_in_block) noexcept {

    // Load the word of interest
    word_t word = block_ptr[current_word_idx()];

    word_t bit_mask = 0;

    // Compute the search mask
    using word_block_t =
      word_block<key_t, word_t, sector_cnt_per_word, k_cnt_per_word,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt,
        k_cnt_per_word>;
    word_block_t::which_bits(key, hash_val, bit_mask);

    // Bit testing
    u1 found_in_word = (word & bit_mask) == bit_mask;

    // Early out
    if (early_out) {
      if (likely(!found_in_word)) return false;
    }

    // Process remaining words recursively, if any
    return multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, word_block_t::hash_fn_idx_end, word_block_t::remaining_hash_bits, remaining_word_cnt - 1, early_out>
        ::contains(block_ptr, key, hash_val, found_in_word & is_contained_in_block);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains (SIMD)
  //===----------------------------------------------------------------------===//
  template<u64 n>
  __forceinline__ __unroll_loops__
  static typename vec<word_t,n>::mask
  contains(const vec<key_t,n>& keys,
           const word_t* __restrict bitvector_base_address,
           const vec<key_t,n>& block_start_word_idxs) noexcept {

    vec<hash_value_t, n> hash_vals(0);
    auto is_contained_in_block_mask = vec<word_t,n>::mask::make_all_mask();

    // Call recursive function
    static constexpr u32 remaining_hash_bits = 0;
    return multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
                           hasher, hash_value_t, hash_fn_idx, remaining_hash_bits,
                           remaining_word_cnt,
                           early_out>
        ::contains(keys, hash_vals, bitvector_base_address, block_start_word_idxs, is_contained_in_block_mask);
  }
  //===----------------------------------------------------------------------===//

  template<u64 n>
  __forceinline__ __unroll_loops__
  static auto
  contains(const vec<key_t,n>& keys,
           vec<hash_value_t,n>& hash_vals,
           const word_t* __restrict bitvector_base_address,
           const vec<key_t,n>& block_start_word_idxs,
           const typename vec<word_t,n>::mask& is_contained_in_block_mask) noexcept {

    // Typedef the vector types
    using word_vt = vec<word_t, n>;

    // Load the words of interest
    auto word_idxs = block_start_word_idxs + current_word_idx();
    const word_vt words = internal::vector_gather<word_t, hash_value_t, n>::gather(bitvector_base_address, word_idxs);

    // Compute the search mask
    word_vt bit_masks = 0;
    using word_block_t =
      word_block<key_t, word_t, sector_cnt_per_word, k_cnt_per_word,
        hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt,
        k_cnt_per_word>;
    word_block_t::which_bits(keys, hash_vals, bit_masks);

    // Bit testing
    auto found_in_word = (words & bit_masks) == bit_masks;

    // Early out
    if (early_out) {
      if (likely(found_in_word.none())) return found_in_word;
    }

    // Process remaining words recursively, if any
    return multiword_block<key_t, word_t, word_cnt, sector_cnt, k,
        hasher, hash_value_t, word_block_t::hash_fn_idx_end, word_block_t::remaining_hash_bits, remaining_word_cnt - 1,
        early_out>
        ::contains(keys, hash_vals, bitvector_base_address, block_start_word_idxs, found_in_word & is_contained_in_block_mask);
  }
  //===----------------------------------------------------------------------===//

};

template<
    typename key_t,               // the word type
    typename word_t,              // the word type
    u32 word_cnt,                 // the number of words per block
    u32 s,                        // the numbers of sectors (must be a power of two and greater or equal to word_cnt))
    u32 k,                        // the number of bits to set/test
    template<typename Ty, u32 i> class hasher,      // the hash function family to use
    typename hash_value_t,        // the hash value type to use

    u32 hash_fn_idx,              // current hash function index (used for recursion)
    u32 remaining_hash_bit_cnt,   // the number of remaining hash bits (used for recursion)

    u1 early_out                  // allows for branching out during lookups (before the next sector is tested)
>
struct multiword_block<key_t, word_t, word_cnt, s, k,
    hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt,
    0 /* no more words remaining */, early_out> {

  //===----------------------------------------------------------------------===//
  // Insert
  //===----------------------------------------------------------------------===//
  __forceinline__ __unroll_loops__
  static void
  insert(word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val) noexcept {
    // End of recursion
  }
  __forceinline__ __unroll_loops__
  static void
  insert_atomic(word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val) noexcept {
    // End of recursion
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains
  //===----------------------------------------------------------------------===//
  __forceinline__ __unroll_loops__ __host__ __device__
  static u1
  contains(const word_t* __restrict block_ptr, const key_t key, hash_value_t& hash_val, u1 is_contained) noexcept {
    // End of recursion
    return is_contained;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Contains (SIMD)
  //===----------------------------------------------------------------------===//
  template<u64 n>
  __forceinline__ __unroll_loops__
  static auto
  contains(const vec<key_t,n>& keys,
           vec<hash_value_t,n>& hash_vals,
           const word_t* __restrict bitvector_base_address,
           const vec<key_t,n>& block_idxs,
           const typename vec<word_t,n>::mask& is_contained_mask) noexcept {
    // End of recursion
    return is_contained_mask;
  }
  //===----------------------------------------------------------------------===//

};
//===----------------------------------------------------------------------===//

} // namespace dtl


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/sgtbx/tensor_rank_2.h>
#include <scitbx/array_family/versa_matrix.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace cctbx { namespace sgtbx { namespace boost_python {

namespace {

  struct tensor_rank_2_constraints_wrappers
  {
    typedef tensor_rank_2::constraints<> w_t;

    static
    af::versa<int, af::c_grid<2> >
    row_echelon_form_as_versa(w_t const& self)
    {
      return af::mat_const_ref_as_versa(self.row_echelon_form());
    }

    static
    af::versa<double, af::c_grid<2> >
    gradient_sum_matrix_as_versa(w_t const& self)
    {
      return af::mat_const_ref_as_versa(self.gradient_sum_matrix());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("tensor_rank_2_constraints", no_init)
        .def(init<sgtbx::space_group const&, bool>((
          arg("space_group"),
          arg("reciprocal_space"))))
        .def(init<af::shared<rt_mx> const&, std::size_t, bool>((
          arg("symmetry_matrices"),
          arg("i_first_matrix_to_use"),
          arg("reciprocal_space"))))
        .def("row_echelon_form", row_echelon_form_as_versa)
        .add_property("independent_indices",
          make_getter(&w_t::independent_indices, rbv()))
        .def("gradient_sum_matrix", gradient_sum_matrix_as_versa)
        .def("n_independent_params", &w_t::n_independent_params)
        .def("n_dependent_params", &w_t::n_dependent_params)
        .def("independent_params", &w_t::independent_params,
          (arg("all_params")))
        .def("all_params", &w_t::all_params, (arg("independent_params")))
        .def("independent_gradients", &w_t::independent_gradients,
          (arg("all_gradients")))
        .def("independent_curvatures", &w_t::independent_curvatures,
          (arg("all_curvatures")))
      ;
    }
  };

  struct tensor_rank_2_cartesian_constraints_wrapper
  {
    typedef tensor_rank_2::cartesian_constraints<> wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt>("tensor_rank_2_cartesian_constraints", no_init)
        .def(init<uctbx::unit_cell const&, sgtbx::space_group const&>((
          arg("unit_cell"), arg("space_group"))))
        .def("n_independent_params", &wt::n_independent_params)
        .def("all_params", &wt::all_params,
          arg("independent_params"))
        .def("independent_params", &wt::independent_params,
             arg("all_params"))
        .def("independent_gradients", &wt::independent_gradients,
          arg("all_gradients"))
        .def("jacobian", &wt::jacobian)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_tensor_rank_2()
  {
    tensor_rank_2_constraints_wrappers::wrap();
    tensor_rank_2_cartesian_constraints_wrapper::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


#ifndef GAME_HPP
#define GAME_HPP

#include <iostream>
#include <sstream>
#include <cmath>
#include <functional>
#include <iomanip>
#include <icecream.hpp>
#include <Eigen/Dense>
#include "Strategy.hpp"

class Game {
  public:
  using v3d_t = std::array<double,3>;
  Game(double mu_e, double mu_a, const ReputationDynamics& rd, const ActionRule& ar) : mu_e(mu_e), mu_a(mu_a), strategy(rd, ar) {
    resident_h_star_ready = false;
  }
  Game(double mu_e, double mu_a, uint64_t id) : mu_e(mu_e), mu_a(mu_a), strategy(id) {
    resident_h_star_ready = false;
  }
  Game(double mu_e, double mu_a, uint64_t id, double coop_prob, const std::array<double,3>& h_star) : mu_e(mu_e), mu_a(mu_a), strategy(id) {
    resident_coop_prob = coop_prob;
    resident_h_star = h_star;
    resident_h_star_ready = true;
  }
  std::string Inspect() const {
    std::stringstream ss;
    const auto& h = resident_h_star;
    ss << "GameID: " << ID() << std::endl
       << "(mu_e, mu_a): (" << mu_e << ", " << mu_a << ")" << std::endl
       << "(RD_id, AR_id): (" << strategy.rd.ID() << ", " << strategy.ar.ID() << ")" << std::endl
       << "--- Transition of residents" << std::endl;
    for (int i = 0; i < 9; i++) {
      Reputation X = static_cast<Reputation>(i/3);
      Reputation Y = static_cast<Reputation>(i%3);
      auto p = At(X, Y);
      ss << "(" << X << "->" << Y << "): " << std::get<0>(p) << std::get<1>(p) << ':' << std::get<2>(p);
      ss << ((i % 3 == 2) ? "\n" : "\t");
    }
    if (resident_h_star_ready) {
      ss << "(c_prob,h0,h1,h2): " << ResidentCoopProb() << ' ' << h[0] << ' ' << h[1] << ' ' << h[2] << std::endl;
    }
    return ss.str();
  }
  std::string Inspect() {
    ResidentEqReputation();
    const Game* pg = this;
    return pg->Inspect();
  }
  std::string InspectMD() const {
    std::stringstream ss;
    ss << "- GameID: " << ID() << "\n"
       << "  - RD_id, AR_id: " << strategy.rd.ID() << ", " << strategy.ar.ID() << "\n"
       << "  - mu_a, mu_e: " << mu_a << ' ' << mu_e << "\n"
       << "- Prescriptions:\n\n"
       << "  | | | |\n"
       << "  |-|-|-|\n";
    for (int i = 0; i < 9; i++) {
      Reputation X = static_cast<Reputation>(i/3);
      Reputation Y = static_cast<Reputation>(i%3);
      auto p = At(X, Y);
      ss << "  | (" << X << "->" << Y << ") " << std::get<0>(p) << std::get<1>(p) << ':' << std::get<2>(p) << " ";
      ss << ((i % 3 == 2) ? " |\n" : "");
    }
    ss << "\n" << std::fixed << std::setprecision(4);
    ss << "- Cooperation Probability: " << ResidentCoopProb() << "\n"
       << "- Population:" << "\n"
       << "  - h_B: " << resident_h_star[0] << "\n"
       << "  - h_N: " << resident_h_star[1] << "\n"
       << "  - h_G: " << resident_h_star[2] << "\n";

    auto cont_payoff = ContinuationPayoff(0.5, 2.0, 1.0, mu_e);
    ss << "- Continuation payoff (w=0.5, b/c=2.0):" << "\n"
       << "  - v_B: " << cont_payoff[0] << "\n"
       << "  - v_N: " << cont_payoff[1] << "\n"
       << "  - v_G: " << cont_payoff[2] << "\n";
    return ss.str();
  }
  uint64_t ID() const {
    return strategy.ID();
  }
  const double mu_e, mu_a;
  const Strategy strategy;
  std::tuple<Action,Reputation,Reputation> At(Reputation donor, Reputation recipient) const {
    return strategy.At(donor, recipient);
  }
  Game NormalizedGame() const {
    auto h = ResidentEqReputation();

    int Gi = 0;
    if (h[0] >= h[1] && h[0] >= h[2]) { Gi = 0; }
    else if (h[1] >= h[0] && h[1] >= h[2]) { Gi = 1; }
    else if (h[2] >= h[0] && h[2] >= h[1]) { Gi = 2; }
    else { IC(h); throw std::runtime_error("must not happen"); }

    Reputation Gn = static_cast<Reputation>(Gi);
    Reputation Bn = strategy.rd.RepAt(Gn, Gn, Action::D);
    if (Gn == Bn) { IC(Inspect()); throw std::runtime_error("must not happen"); }
    int Bi = static_cast<int>(Bn);
    int Ni = 3 - Bi - Gi;

    std::array<int,3> map = {0,0,0};
    map[Gi] = 2;
    map[Ni] = 1;
    map[Bi] = 0;

    ReputationDynamics new_repd = strategy.rd.Permute(map);
    ActionRule new_ar = strategy.ar.Permute(map);
    uint64_t ar_id = new_ar.ID();
    uint64_t rd_id = new_repd.ID();
    uint64_t new_g_id = (rd_id << 9) + ar_id;

    std::array<double,3> new_h = {h[Bi], h[Ni], h[Gi]};

    return Game(mu_e, mu_a, new_g_id, ResidentCoopProb(), new_h);
  }
  bool IsESS(double benefit, double cost) const {
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    auto p = FindNegativePayoffDiff(benefit, cost);
    // IC(p.first, p.second.ID());
    return p.first > 0.0;
  }
  std::pair<double,ActionRule> FindNegativePayoffDiff(double benefit, double cost) const { // an index of evolutionary stability
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    double res_payoff = MutantPayoff(strategy.ar, benefit, cost);
    double min = std::numeric_limits<double>::max();
    ActionRule highest_mut = strategy.ar;
    for (int i = 0; i < 512; i++) {
      if (i == strategy.ar.ID()) continue;
      ActionRule mut_ar(i);
      double d = res_payoff - MutantPayoff(mut_ar, benefit, cost);
      if (d < min) { min = d; highest_mut = mut_ar; }
      if (min < 0.0) break;
    }
    return std::make_pair(min, highest_mut);
  }
  std::array<double,2> ESS_Benefit_Range(double lower_bound_max = std::numeric_limits<double>::max(), double upper_bound_min = std::numeric_limits<double>::min()) const {
    // stop calculating b_range when lower_bound exceeds lower_bound_max
    // or upper_bound go below upper_bound_min
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    double res_res_coop = ResidentCoopProb();
    // IC(res_res_coop, MutantCoopProbs(strategy.ar));

    double b_lower_bound = std::numeric_limits<double>::min();
    double b_upper_bound = std::numeric_limits<double>::max();

    for (int i = 0; i < 512; i++) {
      if (i == strategy.ar.ID()) continue;
      ActionRule mut_ar(i);
      auto p = MutantCoopProbs(mut_ar);
      double mut_res_coop = p.first;
      double res_mut_coop = p.second;

      if (res_res_coop > res_mut_coop) {
        double b_lower = (res_res_coop - mut_res_coop) / (res_res_coop - res_mut_coop);
        if (b_lower > b_lower_bound) { b_lower_bound = b_lower; }
      }
      else if (res_res_coop < res_mut_coop) {
        double b_upper = (res_res_coop - mut_res_coop) / (res_res_coop - res_mut_coop);
        if (b_upper < b_upper_bound) { b_upper_bound = b_upper; }  // update the upper bound of b
      }
      else {  // res_coop_prob == mut_res_coop
        if (mut_res_coop <= res_res_coop) {  // cannot be ESS
          b_lower_bound = std::numeric_limits<double>::max();
          b_upper_bound = std::numeric_limits<double>::min();
        }
      }

      if (b_lower_bound > b_upper_bound || b_lower_bound >= lower_bound_max || b_upper_bound <= upper_bound_min) {
        // no ESS range is found between [upper_bound_min, lower_bound_max]
        b_lower_bound = std::numeric_limits<double>::max();
        b_upper_bound = std::numeric_limits<double>::min();
        break;
      }
    }

    return std::array<double,2>({b_lower_bound, b_upper_bound});
  }
  v3d_t ResidentEqReputation() { CalcHStarResident(); return resident_h_star; } // equilibrium reputation of resident species
  v3d_t ResidentEqReputation() const {
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    return resident_h_star;
  }
  double ResidentCoopProb() { CalcHStarResident(); return resident_coop_prob; } // cooperation probability between resident species without implementation error
  double ResidentCoopProb() const {
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    return resident_coop_prob;
  }
  v3d_t HStarMutant(const ActionRule& mutant_action_rule) const {
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    std::function<std::array<double,3>(std::array<double,3>)> func = [this,&mutant_action_rule](std::array<double,3> x) {
      return HdotMutant(x, mutant_action_rule);
    };
    return SolveByRungeKutta(func);
  }
  std::pair<double,double> MutantCoopProbs(const ActionRule& mutant) const {
    if (!resident_h_star_ready) throw std::runtime_error("cache is not ready");
    v3d_t mut_rep = HStarMutant(mutant);
    // cooperation probability of mutant against resident
    double mut_res_coop = CooperationProb(mutant, mut_rep, resident_h_star); // cooperation prob of mutant for resident
    double res_mut_coop = CooperationProb(strategy.ar, resident_h_star, mut_rep); // cooperation prob of resident for mutant
    return std::make_pair(mut_res_coop, res_mut_coop);
  }
  double MutantPayoff(const ActionRule& mutant, double benefit, double cost) const {
    auto p = MutantCoopProbs(mutant);
    double mut_res_coop = p.first;
    double res_mut_coop = p.second;
    return benefit * res_mut_coop - cost * mut_res_coop;
  }
  std::pair<std::array<Reputation,3>,std::array<Action,3>> TraceReputationAndAction(const Reputation& init, const Reputation& resident_rep) const {
    std::array<Reputation,3> rep_hist;
    std::array<Action,3> act_hist;
    rep_hist[0] = init;
    act_hist[0] = strategy.ar.ActAt(rep_hist[0], resident_rep);
    rep_hist[1] = strategy.rd.RepAt(rep_hist[0], resident_rep, act_hist[0]);
    act_hist[1] = strategy.ar.ActAt(rep_hist[1], resident_rep);
    rep_hist[2] = strategy.rd.RepAt(rep_hist[1], resident_rep, act_hist[1]);
    act_hist[2] = strategy.ar.ActAt(rep_hist[2], resident_rep);
    return std::make_pair(rep_hist, act_hist);
  }
  std::array<double,3> ContinuationPayoff(double w, double benefit, double cost, double mu_e) const {  // calculate continuation payoff for each reputation taking into account implementation error
    auto h = ResidentEqReputation();
    Eigen::Matrix3d A;
    A << 0,0,0, 0,0,0, 0,0,0;
    for (int x = 0; x < 3; x++) {
      Reputation X = static_cast<Reputation>(x);
      A(x, x) += 1.0;
      for (int y = 0; y < 3; y++) {
        Reputation Y = static_cast<Reputation>(y);
        auto next = At(X, Y);
        Reputation Z = std::get<1>(next);
        int z = static_cast<int>(Z);
        A(x, z) -= w * h[y] * (1.0 - mu_e);
        Reputation Z_not = std::get<2>(next);
        int z_not = static_cast<int>(Z_not);
        A(x, z_not) -= w * h[y] * mu_e;
      }
    }

    Eigen::Vector3d V;
    V << 0,0,0;
    for (int x = 0; x < 3; x++) {
      Reputation X = static_cast<Reputation>(x);
      for (int y = 0; y < 3; y++) {
        Reputation Y = static_cast<Reputation>(y);
        if (strategy.ar.ActAt(Y, X) == Action::C) { V(x) += benefit * h[y] * (1.0-mu_e); }
        if (strategy.ar.ActAt(X, Y) == Action::C) { V(x) -= cost * h[y] * (1.0-mu_e); }
      }
    }

    Eigen::Vector3d ans_e = A.colPivHouseholderQr().solve(V);
    ans_e *= (1.0 - w);  // normalize
    return {ans_e(0), ans_e(1), ans_e(2)};
  }
  v3d_t CalcHStarFromInitialPoint(const v3d_t & init) {
    std::function<std::array<double,3>(std::array<double,3>)> func = [this](std::array<double,3> x) {
      return HdotResident(x);
    };
    auto ans = SolveByRungeKutta(func, init);
    return ans;
  }

  private:
  v3d_t resident_h_star; // equilibrium reputation of resident species
  double resident_coop_prob;  // cooperation probability of resident species
  bool resident_h_star_ready; // if true, resident_h_star and resident_coop_prob are ready
  void CalcHStarResident() {
    if (resident_h_star_ready) return;
    std::function<std::array<double,3>(std::array<double,3>)> func = [this](std::array<double,3> x) {
      return HdotResident(x);
    };
    resident_h_star = SolveByRungeKutta(func);
    resident_coop_prob = CooperationProb(strategy.ar, resident_h_star, resident_h_star);
    resident_h_star_ready = true;
  }
  v3d_t HdotResident(const std::array<double,3>& ht) const {
    v3d_t ht_dot = {-ht[0], -ht[1], -ht[2]};
    for (int i = 0; i < 3; i++) {
      Reputation X = static_cast<Reputation>(i);
      for (int j = 0; j < 3; j++) {
        Reputation Y = static_cast<Reputation>(j);
        for (int k = 0; k < 3; k++) {
          Reputation Z = static_cast<Reputation>(k);
          int b1 = (strategy.rd.RepAt(X, Y, strategy.ar.ActAt(X, Y)) == Z) ? 1 : 0;
          int b2 = (strategy.rd.RepAt(X, Y, Action::D) == Z) ? 1 : 0;
          ht_dot[k] += ht[i] * ht[j] * ( (1.0-1.5*mu_a)*((1.0-mu_e)*b1+ mu_e*b2) + 0.5*mu_a);
        }
      }
    }
    return ht_dot;
  }
  v3d_t HdotMutant(const std::array<double,3>& ht, const ActionRule& mutant_action_rule) const {
    v3d_t ht_dot = {-ht[0], -ht[1], -ht[2]};
    for (int i = 0; i < 3; i++) {
      Reputation X = static_cast<Reputation>(i);
      for (int j = 0; j < 3; j++) {
        Reputation Y = static_cast<Reputation>(j);
        for (int k = 0; k < 3; k++) {
          Reputation Z = static_cast<Reputation>(k);
          int b1 = (strategy.rd.RepAt(X, Y, mutant_action_rule.ActAt(X, Y)) == Z) ? 1 : 0;
          int b2 = (strategy.rd.RepAt(X, Y, Action::D) == Z) ? 1 : 0;
          ht_dot[k] += ht[i] * resident_h_star[j] * ((1.0 - 1.5 * mu_a) * ((1.0 - mu_e) * b1 + mu_e * b2) + 0.5 * mu_a);
        }
      }
    }
    return ht_dot;
  }
  v3d_t SolveByRungeKutta(std::function<v3d_t (v3d_t)>& func, const v3d_t& init = {1.0/3.0,1.0/3.0,1.0/3.0}) const {
    v3d_t ht = init;
    const size_t N_ITER = 10'000'000;
    double dt = 0.01;
    const double conv_tolerance = 1.0e-6 * dt;
    for (size_t t = 0; t < N_ITER; t++) {
#ifdef DEBUG
      if (t % 10000 == 9999) {
        std::cerr << t << ' ' << ht[0] << ' ' << ht[1] << ' ' << ht[2] << std::endl;
      }
#endif
      v3d_t k1 = func(ht);
      v3d_t arg2;
      for(int i = 0; i < 3; i++) {
        k1[i] *= dt;
        arg2[i] = ht[i] + 0.5 * k1[i];
      }
      v3d_t k2 = func(arg2);
      v3d_t arg3;
      for(int i = 0; i < 3; i++) {
        k2[i] *= dt;
        arg3[i] = ht[i] + 0.5 * k2[i];
      }
      v3d_t k3 = func(arg3);
      v3d_t arg4;
      for(int i = 0; i < 3; i++) {
        k3[i] *= dt;
        arg4[i] = ht[i] + k3[i];
      }
      v3d_t k4 = func(arg4);
      for(int i = 0; i < 3; i++) {
        k4[i] *= dt;
      }
      v3d_t delta;
      double sum = 0.0;
      for (int i = 0; i < 3; i++) {
        delta[i] = (k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i]) / 6.0;
        ht[i] += delta[i];
        sum += ht[i];
      }
      // normalize ht
      double sum_inv = 1.0 / sum;
      for (int i = 0; i < 3; i++) { ht[i] *= sum_inv; }

      if (std::abs(delta[0]) < conv_tolerance &&
          std::abs(delta[1]) < conv_tolerance &&
          std::abs(delta[2]) < conv_tolerance) {
        break;
      }
      if (t == N_ITER-1) {
        IC(Inspect(), delta, ht);
        throw std::runtime_error("does not converge");
      }
    }
    return ht;
  }
  double CooperationProb(const ActionRule& donor_action, const std::array<double,3>& donor_reputation, const std::array<double,3>& recip_reputation) const {
    double sum = 0.0;
    for (int i = 0; i < 3; i++) {
      Reputation X = static_cast<Reputation>(i);
      for (int j = 0; j < 3; j++) {
        Reputation Y = static_cast<Reputation>(j);
        if (donor_action.ActAt(X, Y) == Action::C) {
          sum += donor_reputation[i] * recip_reputation[j];
        }
      }
    }
    return sum;
  }
};

#endif


#include <Eigen/Geometry>
#include <actionlib/client/simple_action_client.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <cmath>
#include <memory>

using MoveBaseClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;

ros::Subscriber ar_track_sub;
std_msgs::String state;
ros::Publisher state_pub;
move_base_msgs::MoveBaseGoal goal;
std::unique_ptr<MoveBaseClient> ac;

tf2_ros::Buffer buffer_;

enum class State { UNKNOWN , FOLLOW, STOP };

void pose_marker_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg) {
  State new_state = State::UNKNOWN;
  geometry_msgs::PoseStamped new_goal;

  for (const auto& marker : msg->markers) {
    switch (marker.id) {
      case 1:
        if (new_state == State::UNKNOWN) {
          new_state = State::FOLLOW;
        }
        new_goal = buffer_.transform(marker.pose, "base_link");
        break;
      case 2:
        new_state = State::STOP;
        break;
      default:
        // N/A
        break;
    }
  }

  // Offset pose by 2m in the direction of the base_link
  const auto mag = std::sqrt(new_goal.pose.position.x * new_goal.pose.position.x +
                             new_goal.pose.position.y * new_goal.pose.position.y);
  new_goal.pose.position.x *= (mag - 2) / mag;
  new_goal.pose.position.y *= (mag - 2) / mag;

  // Orientation based on heading to target
  const Eigen::Vector3d cur_heading(1,0,0);
  const Eigen::Vector3d to_goal(new_goal.pose.position.x, new_goal.pose.position.y, new_goal.pose.position.z);
  const auto quat = Eigen::Quaternion<double>::FromTwoVectors(cur_heading, to_goal);
  new_goal.pose.orientation.x = quat.x();
  new_goal.pose.orientation.y = quat.y();
  new_goal.pose.orientation.z = quat.z();
  new_goal.pose.orientation.w = quat.w();

  switch (new_state) {
    case State::FOLLOW:
      goal.target_pose.header.frame_id = "base_link";
      goal.target_pose.header.stamp = ros::Time::now();
      goal.target_pose.pose.position = new_goal.pose.position;
      goal.target_pose.pose.orientation.w = 1.0;
      ac->sendGoal(goal);
      // Don't wait for result lol
      // ac->waitForResult();
      // ac->getState();
      break;
    case State::STOP:
      ac->cancelAllGoals();
      break;
    default:
      // N/A
      break;
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "person_tracker");
  ros::NodeHandle nh;

  // Start a TF2 listener
  tf2_ros::TransformListener listener(buffer_);

  // Tell the action client that we want to spin a thread by default
  ac = std::make_unique<MoveBaseClient>("move_base", true);

  // Wait for the action server to come up
  while (!ac->waitForServer(ros::Duration(5.0))) {
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  ar_track_sub = nh.subscribe("/zed/ar_pose_marker", 100, pose_marker_callback);
  state_pub = nh.advertise<std_msgs::String>("state", 100);

  ros::spin();
  return 0;
}