//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/ieee/include/functions/frexp.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/halfeps.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/functions/ldexp.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>
#include <boost/simd/sdk/config.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>

NT2_TEST_CASE_TPL( frexp0, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::frexp;
  using boost::simd::tag::frexp_;

  typedef typename boost::dispatch::meta::as_integer<T,signed>::type iT;

  {
    iT e;
    T  m;
    T  a = boost::simd::Valmax<T>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::One<T>()-boost::simd::Halfeps<T>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Limitexponent<T>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
  }

  #ifndef BOOST_SIMD_NO_DENORMALS
  {
    iT e;
    T  m;
    T a = boost::simd::Mindenormal<T>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<T>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<T>()-boost::simd::Nbmantissabits<T>()+boost::simd::One<T>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
  }

  {
    iT e;
    T  m;
    T a = boost::simd::Smallestposval<T>()/2;
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<T>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<T>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
  }

  {
    iT e;
    T  m;
    T a = boost::simd::Smallestposval<T>()/4;
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<T>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<T>()-boost::simd::One<T>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
  }
  #endif

}

NT2_TEST_CASE_TPL( frexp, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::frexp;
  using boost::simd::tag::frexp_;

  typedef typename boost::dispatch::meta::as_integer<T,signed>::type iT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<frexp_(T)>::type)
                  , (std::pair<T,iT>)
                  );

  {
    iT e;
    T  m;

    frexp(boost::simd::One<T>(), m, e);
    NT2_TEST_EQUAL(m, boost::simd::Half<T>());
    NT2_TEST_EQUAL(e, boost::simd::One<iT>());
  }

  {
    iT e;
    T  m;

    m = frexp(boost::simd::One<T>(), e);
    NT2_TEST_EQUAL(m, boost::simd::Half<T>());
    NT2_TEST_EQUAL(e, boost::simd::One<iT>());
  }

  {
    iT e;
    T  m;

    boost::fusion::vector_tie(m,e) = frexp(boost::simd::One<T>());
    NT2_TEST_EQUAL(m, boost::simd::Half<T>());
    NT2_TEST_EQUAL(e, boost::simd::One<iT>());
  }

  {
    std::pair<T,iT> p;

    p = frexp(boost::simd::One<T>());
    NT2_TEST_EQUAL(p.first  , boost::simd::Half<T>());
    NT2_TEST_EQUAL(p.second , boost::simd::One<iT>());
  }
}


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

#include <boost/math/concepts/std_real_concept.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/multiprecision/cpp_int.hpp>

template <class T>
bool check_concepts()
{
   boost::math::tools::polynomial<T> a(2), b(3), c(4);

   a += b;
   a -= b;
   a *= b;
   a /= b;
   a %= b;
   a = c;
   a += b + c;
   a += b - c;
   a += b * c;
   a += b / c;
   a += b % c;

   int i = 4;

   a += i;
   a -= i;
   a *= i;
   a /= i;
   a %= i;
   a += b + i;
   a += i + b;
   a += b - i;
   a += i - b;
   a += b * i;
   a += i * b;
   a += b / i;
   a += b % i;

   bool bb = false;
   bb |= a == b;
   bb |= a != b;

   return bb;
}

int main()
{
   check_concepts<int>();
   check_concepts<boost::multiprecision::cpp_int>();
   return 0;
}


/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Mark Moll */

#include "omplapp/apps/DynamicCarPlanning.h"
#include <boost/math/constants/constants.hpp>

ompl::base::ScopedState<> ompl::app::DynamicCarPlanning::getDefaultStartState() const
{
    base::ScopedState<base::CompoundStateSpace> s(getStateSpace());
    base::SE2StateSpace::StateType& pose = *s->as<base::SE2StateSpace::StateType>(0);
    base::RealVectorStateSpace::StateType& vel = *s->as<base::RealVectorStateSpace::StateType>(1);

    aiVector3D c = getRobotCenter(0);
    pose.setX(c.x);
    pose.setY(c.y);
    pose.setYaw(0.);
    vel.values[0] = 0.;
    vel.values[1] = 0.;
    return s;
}

void ompl::app::DynamicCarPlanning::ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
{
    // Retrieving control inputs
    const double *u = ctrl->as<control::RealVectorControlSpace::ControlType>()->values;

    // zero out qdot
    qdot.resize (q.size (), 0);

    qdot[0] = q[3] * cos(q[2]);
    qdot[1] = q[3] * sin(q[2]);
    qdot[2] = q[3] * mass_ * lengthInv_ * tan(q[4]);

    qdot[3] = u[0];
    qdot[4] = u[1];
}

void ompl::app::DynamicCarPlanning::postPropagate(const base::State* /*state*/, const control::Control* /*control*/, const double /*duration*/, base::State* result)
{
    // Normalize orientation value between 0 and 2*pi
    const base::SO2StateSpace* SO2 = getStateSpace()->as<base::CompoundStateSpace>()
        ->as<base::SE2StateSpace>(0)->as<base::SO2StateSpace>(1);
    auto* so2 = result->as<base::CompoundStateSpace::StateType>()
        ->as<base::SE2StateSpace::StateType>(0)->as<base::SO2StateSpace::StateType>(1);
    SO2->enforceBounds(so2);
}

void ompl::app::DynamicCarPlanning::setDefaultBounds()
{
    base::RealVectorBounds bounds(2);
    bounds.low[0] = -1.;
    bounds.high[0] = 1.;
    bounds.low[1] = -boost::math::constants::pi<double>() * 30. / 180.;
    bounds.high[1] = boost::math::constants::pi<double>() * 30. / 180.;
    getStateSpace()->as<base::CompoundStateSpace>()->as<base::RealVectorStateSpace>(1)->setBounds(bounds);
    bounds.low[0] = -.5;
    bounds.high[0] = .5;
    bounds.low[1] = -boost::math::constants::pi<double>() * 2. / 180.;
    bounds.high[1] = boost::math::constants::pi<double>() * 2. / 180.;
    getControlSpace()->as<control::RealVectorControlSpace>()->setBounds(bounds);
}


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>
#include <limits>
#include <boost/detail/lightweight_test.hpp>

#include <libs/chrono/test/rep.h>

int main()
{
    BOOST_TEST((boost::chrono::duration_values<int>::min)() ==
           (std::numeric_limits<int>::min)());
    BOOST_TEST((boost::chrono::duration_values<double>::min)() ==
           -(std::numeric_limits<double>::max)());
    BOOST_TEST((boost::chrono::duration_values<Rep>::min)() ==
           (std::numeric_limits<Rep>::min)());

    BOOST_TEST((boost::chrono::duration_values<int>::max)() ==
           (std::numeric_limits<int>::max)());
    BOOST_TEST((boost::chrono::duration_values<double>::max)() ==
           (std::numeric_limits<double>::max)());
    BOOST_TEST((boost::chrono::duration_values<Rep>::max)() ==
           (std::numeric_limits<Rep>::max)());

    BOOST_TEST(boost::chrono::duration_values<int>::zero() == 0);
    BOOST_TEST(boost::chrono::duration_values<Rep>::zero() == 0);

    return boost::report_errors();
}


#pragma once

#include <boost/bimap.hpp>
#include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>

namespace libMultiRobotPlanning {

/*!
  \example assignment.cpp example that takes cost mappings from a file
*/

/*! \brief Find optimal (lowest total cost) assignment

This class can find the lowest sum-of-cost assignment
for given agents and tasks. The costs must be integers, the agents and
tasks can be of any user-specified type.

This method is based on maximum flow formulation.

\tparam Agent Type of the agent. Needs to be copy'able and comparable
\tparam Task Type of task. Needs to be copy'able and comparable
*/
template <typename Agent, typename Task>
class Assignment {
 public:
  Assignment()
      : m_agents(), m_tasks(), m_graph(), m_sourceVertex(), m_sinkVertex() {
    m_sourceVertex = boost::add_vertex(m_graph);
    m_sinkVertex = boost::add_vertex(m_graph);
  }

  void clear() {
    // std::cout << "Asg: clear" << std::endl;
    std::set<edge_t> edgesToRemove;
    for (const auto& agent : m_agents) {
      auto es = boost::out_edges(agent.right, m_graph);
      for (auto eit = es.first; eit != es.second; ++eit) {
        if (!m_graph[*eit].isReverseEdge) {
          edgesToRemove.insert(*eit);
          edgesToRemove.insert(m_graph[*eit].reverseEdge);
        }
      }
    }

    for (const auto& e : edgesToRemove) {
      boost::remove_edge(e, m_graph);
    }
  }

  void setCost(const Agent& agent, const Task& task, long cost) {
    // std::cout << "setCost: " << agent << "->" << task << " cost: " << cost <<
    // std::endl;
    // Lazily create vertex for agent
    auto agentIter = m_agents.left.find(agent);
    vertex_t agentVertex;
    if (agentIter == m_agents.left.end()) {
      agentVertex = boost::add_vertex(m_graph);
      addOrUpdateEdge(m_sourceVertex, agentVertex, 0);
      m_agents.insert(agentsMapEntry_t(agent, agentVertex));
    } else {
      agentVertex = agentIter->second;
    }

    // Lazily create vertex for task
    auto taskIter = m_tasks.left.find(task);
    vertex_t taskVertex;
    if (taskIter == m_tasks.left.end()) {
      taskVertex = boost::add_vertex(m_graph);
      addOrUpdateEdge(taskVertex, m_sinkVertex, 0);
      m_tasks.insert(tasksMapEntry_t(task, taskVertex));
    } else {
      taskVertex = taskIter->second;
    }

    addOrUpdateEdge(agentVertex, taskVertex, cost);
  }

  // find first (optimal) solution with minimal cost
  long solve(std::map<Agent, Task>& solution) {
    using namespace boost;

    successive_shortest_path_nonnegative_weights(
        m_graph, m_sourceVertex, m_sinkVertex,
        boost::capacity_map(get(&Edge::capacity, m_graph))
            .residual_capacity_map(get(&Edge::residualCapacity, m_graph))
            .weight_map(get(&Edge::cost, m_graph))
            .reverse_edge_map(get(&Edge::reverseEdge, m_graph)));

    // long cost = find_flow_cost(
    //   m_graph,
    //   boost::capacity_map(get(&Edge::capacity, m_graph))
    //   .residual_capacity_map(get(&Edge::residualCapacity, m_graph))
    //   .weight_map(get(&Edge::cost, m_graph)));
    long cost = 0;

    // find solution
    solution.clear();
    auto es = out_edges(m_sourceVertex, m_graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
      vertex_t agentVertex = target(*eit, m_graph);
      auto es2 = out_edges(agentVertex, m_graph);
      for (auto eit2 = es2.first; eit2 != es2.second; ++eit2) {
        if (!m_graph[*eit2].isReverseEdge) {
          vertex_t taskVertex = target(*eit2, m_graph);
          if (m_graph[*eit2].residualCapacity == 0) {
            solution[m_agents.right.at(agentVertex)] =
                m_tasks.right.at(taskVertex);
            cost += m_graph[edge(agentVertex, taskVertex, m_graph).first].cost;
            break;
          }
        }
      }
    }

    return cost;
  }

 protected:
  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
                                       boost::bidirectionalS>
      graphTraits_t;
  typedef graphTraits_t::vertex_descriptor vertex_t;
  typedef graphTraits_t::edge_descriptor edge_t;

  struct Vertex {
    // boost::default_color_type color;
    // edge_t predecessor;
  };

  struct Edge {
    Edge()
        : cost(0),
          capacity(0),
          residualCapacity(0),
          reverseEdge(),
          isReverseEdge(false) {}

    long cost;
    long capacity;
    long residualCapacity;
    edge_t reverseEdge;
    bool isReverseEdge;
  };

  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
                                Vertex, Edge>
      graph_t;

 protected:
  void addOrUpdateEdge(vertex_t from, vertex_t to, long cost) {
    auto e = boost::edge(from, to, m_graph);
    if (e.second) {
      m_graph[e.first].cost = cost;
      m_graph[m_graph[e.first].reverseEdge].cost = -cost;
    } else {
      auto e1 = boost::add_edge(from, to, m_graph);
      m_graph[e1.first].cost = cost;
      m_graph[e1.first].capacity = 1;
      auto e2 = boost::add_edge(to, from, m_graph);
      m_graph[e2.first].isReverseEdge = true;
      m_graph[e2.first].cost = -cost;
      m_graph[e2.first].capacity = 0;
      m_graph[e1.first].reverseEdge = e2.first;
      m_graph[e2.first].reverseEdge = e1.first;
    }
  }

 private:
  typedef boost::bimap<Agent, vertex_t> agentsMap_t;
  typedef typename agentsMap_t::value_type agentsMapEntry_t;
  typedef boost::bimap<Task, vertex_t> tasksMap_t;
  typedef typename tasksMap_t::value_type tasksMapEntry_t;

  agentsMap_t m_agents;
  tasksMap_t m_tasks;

  graph_t m_graph;
  vertex_t m_sourceVertex;
  vertex_t m_sinkVertex;
};

}  // namespace libMultiRobotPlanning


#include "clstm.h"
#include <assert.h>
#include <iostream>
#include <vector>
#include <string>
#include <memory>
#include <math.h>
#include <Eigen/Dense>

#include "multidim.h"
#include "h5multi.h"
#include "pymulti.h"
#include "extras.h"

using namespace std;
using namespace Eigen;
using namespace ocropus;
using namespace h5multi;
using namespace pymulti;

struct SeqDataset {
  string iname = "inputs";
  string oname = "outputs";
  HDF5 h5;
  int nsamples = -1;
  int nin = -1;
  int nout = -1;

  SeqDataset(const string &h5file) {
    H5::Exception::dontPrint();
    h5.open(h5file);
    mdarray<int> idims, odims;
    h5.shape(idims, iname);
    h5.shape(odims, oname);
    assert(idims(0) == odims(0));
    nsamples = idims(0);
    print("got", nsamples, "training samples");
    mdarray<float> a;
    h5.getdrow(a, 0, iname);
    assert(a.rank() == 2);
    nin = a.dim(1);
    h5.getdrow(a, 0, oname);
    assert(a.rank() == 2);
    nout = a.dim(1);
  }
  void input(mdarray<float> &a, int index) {
    h5.getdrow(a, index, iname);
    assert(a.rank() == 2);
    assert(a.dim(1) == nin);
  }
  void output(mdarray<float> &a, int index) {
    h5.getdrow(a, index, oname);
    assert(a.rank() == 2);
    assert(a.dim(1) == nout);
  }
};

inline void getslice(mdarray<float> &a, Sequence &seq, int i, int b = 0) {
  a.resize(int(seq.size()));
  for (int t = 0; t < seq.size(); t++) a(t) = seq[t](i, b);
}

inline void getslice(mdarray<float> &a, mdarray<float> &seq, int i, int b = 0) {
  if (a.rank() == 2) {
    a.resize(seq.dim(0));
    for (int t = 0; t < seq.size(); t++) a(t) = seq(t, i);
  } else if (a.rank() == 3) {
    a.resize(seq.dim(0));
    for (int t = 0; t < seq.size(); t++) a(t) = seq(t, i, b);
  } else {
    THROW("bad rank");
  }
}

void run_eval(INetwork *net, SeqDataset *dataset, int testmod, int at = -1,
              int ntest = 1000000) {
  mdarray<float> inputs, outputs, targets;
  double total_mse = 0.0;
  double total_mse0 = 0.0;
  double count = 0;
  for (int i = 0; i < dataset->nsamples; i += testmod) {
    if (count >= ntest) break;
    dataset->input(inputs, i);
    dataset->output(targets, i);
    set_inputs(net, inputs);
    net->forward();
    assign(outputs, net->outputs);
    count += 1;
    {
      double err = 0.0, err0 = 0.0;
      for (int i = 0; i < outputs.dim(0); i++) {
        for (int j = 0; j < outputs.dim(1); j++) {
          err += pow(outputs(i, j) - targets(i, j), 2);
          err0 += pow(targets(i, j), 2);
        }
      }
      err /= outputs.dim(0);
      err0 /= outputs.dim(0);
      total_mse += err;
      total_mse0 += err0;
    }
  }
  print("TESTERR", total_mse / count, total_mse / total_mse0, "N", count, "at",
        at);
}

double mse(mdarray<float> &a, mdarray<float> &b) {
  assert(a.size() == b.size());
  double total = 0.0;
  for (int i = 0; i < a.size(); i++) total += pow(a[i] - b[i], 2);
  total /= a.size();
  return total;
}

int main_seq(int argc, char **argv) {
  float lr = getdenv("lrate", 1e-4);
  int display_every = getienv("display_every", 0);
  int report_every = getienv("report_every", 100);
  int ntrain = getienv("ntrain", 10000000);

  int testmod = getienv("testmod", 10);
  int test_every = getienv("test_every", 1000);
  int ntest = getienv("ntest", 1000000);

  PyServer py;
  if (display_every > 0) py.open();

  assert(argc > 1);
  SeqDataset dataset(argv[1]);
  print("ninput", dataset.nin, "noutput", dataset.nout);
  Network net;
  string net_type = getoneof("lstm", "BIDILSTM");
  string lstm_type = getoneof("lstm_type", "LSTM");
  string output_type = getoneof("output_type", "LinearLayer");
  int nhidden = getrenv("nhidden", getrenv("hidden", 100));
  int nhidden2 = getrenv("nhidden2", getrenv("hidden2", -1));
  net = make_net(net_type, {
                            {"ninput", dataset.nin},
                            {"noutput", dataset.nout},
                            {"nhidden", nhidden},
                            {"nhidden2", nhidden2},
                           });

  double lrate = getdenv("lrate", 1e-4);
  net->setLearningRate(lrate, 0.9);

  if (getienv("debug_nets")) {
    net->networks("net", [](string name, INetwork *net) {
      print("net", name, net->learning_rate, net->momentum);
    });
  }
  if (getienv("debug_states")) {
    net->states("net", [](string name, Sequence *seq) {
      print("state", name, seq->size());
    });
  }

  mdarray<float> input, output, target;
  Classes classes;
  int next_test = test_every;
  for (int trial = 0; trial < ntrain; trial++) {
    if (test_every > 0 && trial >= next_test) {
      run_eval(net.get(), &dataset, testmod, trial, ntest);
      next_test += test_every;
      continue;
    }
    int sample;
    do {
      sample = irandom() % dataset.nsamples;
    } while (sample % testmod == 0);
    mdarray<float> input, target;
    dataset.input(input, sample);
    dataset.output(target, sample);
    set_inputs(net.get(), input);
    net->forward();
    assign(output, net->outputs);
    set_targets(net.get(), target);
    net->backward();
    net->update();
    if (report_every > 0 && trial % report_every == 0) {
      print(trial, mse(output, target));
    }
    if (display_every > 0 && trial % display_every == 0) {
      py.eval("clf()");
      py.eval("subplot(121)");
      py.eval("ylim(-.1,1.1)");
      mdarray<float> a;
      getslice(a, input, 0);
      py.plot(a, "color='y',linewidth=5");
      getslice(a, target, 1);
      py.plot(a, "color='b',linewidth=2");
      getslice(a, output, 0);
      py.plot(a, "color='r',ls='--'");
      Sequence *state = 0;
      state = net->getState(".lstm1.0.lstm.state");
      if (state) {
        mdarray<float> b;
        getslice(a, *state, 0);
        getslice(b, *state, 1);
        py.eval("subplot(122)");
        py.plot2(a, b);
      }
      py.eval("ginput(1,0.005)");
    }
  }
  run_eval(net.get(), &dataset, testmod, ntrain);
}

const char *usage = /*program+*/
    "data.h5\n\n"
    "data.h5 is an HDF5 file containing:\n\n"
    "float inputs(N,*): input sequences\n"
    "int inputs_dims(N,2): shape of input sequences\n"
    "float outputs(N,*): output sequences\n"
    "int outputs_dims(N,2): shape of output sequences\n";

int main(int argc, char **argv) {
  if (argc < 2) {
    print(string(argv[0]) + " " + usage);
    exit(1);
  }
  try {
    return main_seq(argc, argv);
  } catch (const char *msg) {
    print("EXCEPTION", msg);
  } catch (...) {
    print("UNKNOWN EXCEPTION");
  }
}


/* 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.
 */

#include <iostream>

#include <boost/program_options.hpp>

#include <dpMM/dirNaiveBayes.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/typedef.h>
#include <dpMM/timer.hpp>

namespace po = boost::program_options;

int main(int argc, char **argv){

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
	("K,K", po::value<int>(), "number of initial clusters ")
	("T,T", po::value<int>(), "iterations")
	("v,v", po::value<bool>(), "verbose output")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different "
      "datapoints)")
    ;

    po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	uint K=2;
	uint T=100;
	uint N=100;
	uint D=2;
	uint M=2;
	uint NumObs = 1;
	bool verbose = false;
	vector<uint> Mword;
	if (vm.count("K"))
		K = vm["K"].as<int>();
	if (vm.count("T"))
		T = vm["T"].as<int>();
	if (vm.count("v"))
		verbose = vm["v"].as<bool>();

	string pathIn ="";
	string pathOut ="";
	if(vm.count("input"))
		pathIn = vm["input"].as<string>();
	if(vm.count("output"))
		pathOut= vm["output"].as<string>();

	vector< Matrix<double, Dynamic, Dynamic> > x;
	x.reserve(N);
	if (!pathIn.compare(""))
	{
		cout<<"making some data up " <<endl;
		uint Ndoc=M;
		uint Nword=int(N/M);
		for(uint i=0; i<Ndoc; ++i) {
			MatrixXd  xdoc(D,Nword);
			for(uint w=0; w<Nword; ++w) {
				if(i<Ndoc/2)
					xdoc.col(w) <<  VectorXd::Zero(D);
				else
					xdoc.col(w) <<  2.0*VectorXd::Ones(D);
			}

			x.push_back(xdoc);
		}
	}else{

		cout<<"loading data from "<<pathIn<<endl;
		std::ifstream fin(pathIn.data(),std::ifstream::in);

		//read parameters from file (tired of passing them in)
		fin>>NumObs;
		fin>>N;
		fin>>M;
		fin>>D;

		MatrixXd data(D,N);
		VectorXu words(M);

		for (uint j=0; j<M; ++j)
			fin>>words(j);

		for (uint j=1; j<(D+1); ++j)
			for (uint i=0; i<N; ++i)
				fin>>data(j-1,i);

		uint count = 0;
		for (uint j=0; j<M; ++j)
		{
			x.push_back(data.middleCols(count,words[j]));
			count+=words[j];
		}
		fin.close();
	}

	double nu = D+1;
	double kappa = D+1;
	MatrixXd Delta = 0.1*MatrixXd::Identity(D,D);
	Delta *= nu;
	VectorXd theta = VectorXd::Zero(D);
	VectorXd alpha = 10.0*VectorXd::Ones(K);

	boost::mt19937 rndGen(9191);
	NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

	Dir<Catd,double> dir(alpha,&rndGen);

	//cout<<"------ marginalized ---- NIW "<<endl;
	//DirNaiveBayes<double> naive_marg(dir,niwMargBase);
	//naive_marg.initialize(x);
	//cout<<naive_marg.labels().transpose()<<endl;
	//for(uint t=0; t<30; ++t)
	//{
	//naive_marg.sampleLabels();
	//naive_marg.sampleParameters();
	//cout<<naive_marg.labels().transpose()
		//<<" logJoint="<<naive_marg.logJoint()<<endl;
	//}
	Timer tlocal;
	tlocal.tic();

	boost::shared_ptr<NiwSampled<double> > niwSampled( new NiwSampled<double>(niw));
	DirNaiveBayes<double> naive_samp(dir,niwSampled);

	cout << "naiveBayesian Clustering:" << endl;
	cout << "Ndocs=" << M << endl;
	cout << "Ndata=" << N << endl;
	cout << "dim=" << D << endl;
	cout << "Num Cluster = " << K << ", (" << T << " iterations)." << endl;

	naive_samp.initialize( (const vector< Matrix<double, Dynamic, Dynamic> >) x );
	naive_samp.inferAll(T,verbose);

	if (pathOut.compare(""))
	{
		std::ofstream fout(pathOut.data(),std::ofstream::out);

		std::streambuf *coutbuf = std::cout.rdbuf(); //save old cout buffer
		cout.rdbuf(fout.rdbuf()); //redirect std::cout to fout1 buffer

			naive_samp.dump(fout,fout);

		std::cout.rdbuf(coutbuf); //reset to standard output again

		fout.close();
	}

	tlocal.displayElapsedTimeAuto();
	return(0);

};


/**
 * @file       sorting_algorithm.inst.hpp
 * @brief
 * @date       2020-09-19
 * @author     Peter
 * @copyright
 *      Peter of [ThinkSpirit Laboratory](http://thinkspirit.org/)
 *   of [Nanjing University of Information Science & Technology](http://www.nuist.edu.cn/)
 *   all rights reserved
 */

#ifndef VISUAL_SORT_SORTING_ALGORITHM_INST_HPP
#define VISUAL_SORT_SORTING_ALGORITHM_INST_HPP

#include <kerbal/algorithm/sort.hpp>
#include <algorithm>

#ifdef _MSC_VER
#	include <ciso646>
#endif

#if BOOST_SORT_SUPPORT

#include <boost/sort/block_indirect_sort/block_indirect_sort.hpp>
#include <boost/sort/flat_stable_sort/flat_stable_sort.hpp>
#include <boost/sort/heap_sort/heap_sort.hpp>
#include <boost/sort/pdqsort/pdqsort.hpp>
#include <boost/sort/sample_sort/sample_sort.hpp>
#include <boost/sort/spinsort/spinsort.hpp>
#include <boost/sort/spreadsort/integer_sort.hpp>
#include <boost/sort/spreadsort/spreadsort.hpp>

#endif

#include "gfx/timsort.hpp"

#include "sorting_algorithm_united_interface.hpp"

extern template void std::sort(Iter first, Iter last, BinaryPredict cmp);
extern template void std::stable_sort(Iter first, Iter last, BinaryPredict cmp);
void std_parallel_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);
void std_parallel_stable_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);
void cqsort_wrapper(Iter first, Iter last, BinaryPredict cmp);

extern template void kerbal::algorithm::bubble_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::selection_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::heap_sort(Iter first, Iter last, BinaryPredict cmp);
void std_make_heap_then_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);

extern template void kerbal::algorithm::merge_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::inplace_merge_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::stable_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::insertion_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::directly_insertion_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::quick_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::nonrecursive_qsort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::intro_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void kerbal::algorithm::nonrecursive_intro_sort(Iter first, Iter last, BinaryPredict cmp);

void shell_sort_hibbard_sequence_wrapper(Iter first, Iter last, BinaryPredict cmp);
void shell_sort_minimun_limit_hibbard_sequence_wrapper(Iter first, Iter last, BinaryPredict cmp);
void shell_sort_reduce_by_half_wrapper(Iter first, Iter last, BinaryPredict cmp);
void shell_sort_q_wrapper(Iter first, Iter last, BinaryPredict cmp);

void radix_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);

void omp_quick_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);
void omp_shell_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);

#if BOOST_SORT_SUPPORT

extern template void boost::sort::block_indirect_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void boost::sort::flat_stable_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void boost::sort::heap_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void boost::sort::pdqsort(Iter first, Iter last, BinaryPredict cmp);
extern template void boost::sort::sample_sort(Iter first, Iter last, BinaryPredict cmp);
extern template void boost::sort::spinsort(Iter first, Iter last, BinaryPredict cmp);

void boost_spreadsort_wrapper(Iter first, Iter last, BinaryPredict);
void boost_parallel_stable_sort_wrapper(Iter first, Iter last, BinaryPredict cmp);

#endif

extern template void gfx::timsort(Iter first, Iter last, BinaryPredict cmp);

void freedom_implement(Iter first, Iter last, BinaryPredict cmp);

#endif // VISUAL_SORT_SORTING_ALGORITHM_INST_HPP


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

/**
 * \file
 * \brief Implementation of \link maptk::rotation_ rotation_<T> \endlink
 *        for \c T = { \c float, \c double }
 */

#include "rotation.h"
#include "eigen_io.h"

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

namespace maptk
{

/// Constructor - from a Rodrigues vector
template <typename T>
rotation_<T>
::rotation_(const Eigen::Matrix<T,3,1>& rvec)
{
  T mag = rvec.norm();
  if (mag == T(0))
  {
    // identity rotation is a special case
    q_.setIdentity();
  }
  else
  {
    q_ = Eigen::Quaternion<T>(Eigen::AngleAxis<T>(mag, rvec/mag));
  }
}

/// Constructor - from rotation angle and axis
template <typename T>
rotation_<T>
::rotation_(T angle, const Eigen::Matrix<T,3,1>& axis)
  : q_(Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis.normalized())))
{
}

/// Constructor - from yaw, pitch, and roll
template <typename T>
rotation_<T>
::rotation_(const T& yaw, const T& pitch, const T& roll)
{
  using std::sin;
  using std::cos;
  // compute the rotation from North-East-Down (NED) coordinates to
  // East-North-Up coordinates (ENU). It is a 180 degree rotation about
  // the axis [1/sqrt(2), 1/sqrt(2), 0]
  const double root_two = boost::math::constants::root_two<double>();
  const T inv_root_two = static_cast<T>(1.0/root_two);
  const rotation_<T> Rned2enu(Eigen::Quaternion<T>(0, inv_root_two, inv_root_two, 0));
  const double half_x = 0.5 * static_cast<double>(-roll);
  const double half_y = 0.5 * static_cast<double>(-pitch);
  const double half_z = 0.5 * static_cast<double>(-yaw);
  rotation_<T> Rx(Eigen::Quaternion<T>(T(cos(half_x)), T(sin(half_x)), 0, 0));
  rotation_<T> Ry(Eigen::Quaternion<T>(T(cos(half_y)), 0, T(sin(half_y)), 0));
  rotation_<T> Rz(Eigen::Quaternion<T>(T(cos(half_z)), 0, 0, T(sin(half_z))));
  *this = Rx * Ry * Rz * Rned2enu;
}

/// Constructor - from a matrix
/**
 * requires orthonormal matrix with +1 determinant
 */
template <typename T>
rotation_<T>
::rotation_(const Eigen::Matrix<T,3,3>& rot)
{
  q_ = Eigen::Quaternion<T>(rot);
}

/// Convert to a 3x3 matrix
template <typename T>
rotation_<T>
::operator Eigen::Matrix<T,3,3>() const
{
  return q_.toRotationMatrix();
}

/// Returns the axis of rotation
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::axis() const
{
  Eigen::Matrix<T,3,1> dir(q_.x(), q_.y(), q_.z());
  T mag = dir.norm();
  if (mag == T(0))
  {
    return Eigen::Matrix<T,3,1>(0,0,1);
  }
  return dir / mag;
}

/// Returns the angle of the rotation in radians about the axis
template <typename T>
T
rotation_<T>
::angle() const
{
  const double i = Eigen::Matrix<T,3,1>(q_.x(), q_.y(), q_.z()).norm();
  const double r = q_.w();
  T a = static_cast<T>(2.0 * std::atan2(i, r));
  const T pi = boost::math::constants::pi<T>();
  const T two_pi = static_cast<T>(2) * boost::math::constants::pi<T>();
  // make sure computed angle lies within a sensible range,
  // i.e. -pi/2 < a < pi/2
  if (a >= pi)
  {
    a -= two_pi;
  }
  if (a <= -pi)
  {
    a += two_pi;
  }
  return a;
}

/// Return the rotation as a Rodrigues vector
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::rodrigues() const
{
  T angle = this->angle();
  if (angle == 0.0)
  {
    return Eigen::Matrix<T,3,1>(0,0,0);
  }
  return this->axis() * angle;
}

/// Convert to yaw, pitch, and roll
template <typename T>
void
rotation_<T>
::get_yaw_pitch_roll(T& yaw, T& pitch, T& roll) const
{
  Eigen::Matrix<T,3,3> rotM(*this);
  T cos_p = T(std::sqrt(double(rotM(1,2)*rotM(1,2)) + rotM(2,2)*rotM(2,2)));
  yaw   = T(std::atan2(double(rotM(0,0)),double(rotM(0,1))));
  pitch = T(std::atan2(double(rotM(0,2)),double(cos_p)));
  roll  = T(std::atan2(double(-rotM(1,2)),double(-rotM(2,2))));
}

/// Compose two rotations
template <typename T>
rotation_<T>
rotation_<T>
::operator*(const rotation_<T>& rhs) const
{
  return q_ * rhs.q_;
}

/// Rotate a vector
/**
 * \note for a large number of vectors, it is more efficient to
 * create a rotation matrix and use matrix multiplcation
 */
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::operator*(const Eigen::Matrix<T,3,1>& rhs) const
{
  return q_ * rhs;
}

/// output stream operator for a rotation
template <typename T>
std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r)
{
  s << r.quaternion().coeffs();
  return s;
}

/// input stream operator for a rotation
template <typename T>
std::istream&  operator>>(std::istream& s, rotation_<T>& r)
{
  Eigen::Matrix<T,4,1> q;
  s >> q;
  r = rotation_<T>(q);
  return s;
}

/// Generate a rotation vector that, when applied to A N times, produces B.
template <typename T>
rotation_<T>
interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f)
{
  // rotation from A -> B
  rotation_<T> C = A.inverse() * B;
  // Reduce the angle of rotation by the fraction provided
  return A * rotation_<T>(C.angle() * f, C.axis());
}

/// Generate N evenly interpolated rotations inbetween \c A and \c B.
template <typename T>
void
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)
{
  interp_rots.reserve(interp_rots.capacity() + n);
  size_t denom = n + 1;
  for (size_t i=1; i<denom; ++i)
  {
    interp_rots.push_back(interpolate_rotation<T>(A, B, static_cast<T>(i) / denom));
  }
}

/// \cond DoxygenSuppress
#define INSTANTIATE_ROTATION(T) \
template class MAPTK_LIB_EXPORT rotation_<T>; \
template MAPTK_LIB_EXPORT std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r); \
template MAPTK_LIB_EXPORT std::istream&  operator>>(std::istream& s, rotation_<T>& r); \
template MAPTK_LIB_EXPORT rotation_<T> interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f); \
template MAPTK_LIB_EXPORT void \
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)

INSTANTIATE_ROTATION(double);
INSTANTIATE_ROTATION(float);

#undef INSTANTIATE_ROTATION
/// \endcond

} // end namespace maptk


#include <boost/test/unit_test.hpp>
#include <ayla/geometry/axis_aligned_box.hpp>
#include <ayla/geometry/vector.hpp>

#include <iostream>
#include <stdio.h>
#include <fstream>

BOOST_AUTO_TEST_SUITE( ayla )
BOOST_AUTO_TEST_SUITE( axis_aligned_box )

BOOST_AUTO_TEST_CASE( intersectsTest ) {
	const AxisAlignedBox a(glm::vec3(5,5,5), 2,2,2);
	AxisAlignedBox b(glm::vec3(5,4,5), 1,1,1);

	BOOST_CHECK(a.intersects(b));
	b.translate(glm::vec3(0, -2, 0));
	BOOST_CHECK(a.intersects(b));
	b.translate(glm::vec3(0,-1,0));
	BOOST_CHECK(!a.intersects(b));
	b.translate(glm::vec3(1,1,0));
	BOOST_CHECK(a.intersects(b));
}

BOOST_AUTO_TEST_CASE( volumeTest ) {
	const AxisAlignedBox a(glm::vec3(134,431,2), 1,2,3);

	BOOST_CHECK_EQUAL(a.getVolume(), 2*4*6);
}

BOOST_AUTO_TEST_CASE( extremePointsTest ) {
	const glm::vec3 aabCenter(23,43,123);
	const glm::vec3 hExts(10,15,20);
	const glm::vec3 maxPt = aabCenter + hExts;
	const glm::vec3 minPt = aabCenter - hExts;

	const AxisAlignedBox a(aabCenter, hExts.x, hExts.y, hExts.z);

	BOOST_CHECK(a.getMaxPoint() == maxPt);
	BOOST_CHECK(a.getMinPoint() == minPt);

	glm::vec3 posPt, negPt;
	a.getDirExtremePts(glm::normalize(glm::vec3(1,1,0)), posPt, negPt);

	BOOST_CHECK(negPt == minPt);
	BOOST_CHECK(posPt == maxPt);

	a.getDirExtremePts(glm::normalize(glm::vec3(1,-1,-1)), posPt, negPt);

	BOOST_CHECK(negPt == (aabCenter+glm::vec3(-hExts.x, hExts.y,hExts.z)));
	BOOST_CHECK(posPt == (aabCenter+glm::vec3(hExts.x,-hExts.y,-hExts.z)));
}

BOOST_AUTO_TEST_CASE( dimensionsTest ) {
	const glm::vec3 minPt(20,30,40);
	const glm::vec3 maxPt(25,35,45);

	const AxisAlignedBox a(minPt, maxPt);

	BOOST_CHECK(a.getCenter() == (minPt+maxPt)*0.5f);

	BOOST_CHECK_EQUAL(a.getWidth(), maxPt.x-minPt.x);
	BOOST_CHECK_EQUAL(a.getHeight(), maxPt.y-minPt.y);
	BOOST_CHECK_EQUAL(a.getDepth(), maxPt.z-minPt.z);

	BOOST_CHECK_EQUAL(a.getHalfWidth(), 0.5f*(maxPt.x-minPt.x));
	BOOST_CHECK_EQUAL(a.getHalfHeight(), 0.5f*(maxPt.y-minPt.y));
	BOOST_CHECK_EQUAL(a.getHalfDepth(), 0.5f*(maxPt.z-minPt.z));
}

BOOST_AUTO_TEST_CASE( constructionTest ) {
	const glm::vec3 center(-20,120,-100);
	const glm::vec3 hExts(2,4,5);

	const glm::vec3 minPt = center-hExts;
	const glm::vec3 maxPt = center+hExts;

	AxisAlignedBox a(center, hExts.x, hExts.y, hExts.z);
	const AxisAlignedBox b(minPt, maxPt);

	BOOST_CHECK(a.getCenter() == b.getCenter());
	BOOST_CHECK(a.getHalfExtensions() == b.getHalfExtensions());
	BOOST_CHECK(a.getVolume() == b.getVolume());

	BOOST_CHECK(a.getCenter() == center);
	a.setCenter(glm::vec3(0));

	BOOST_CHECK(a.getCenter() == glm::vec3(0));
	BOOST_CHECK(a.getHalfExtensions() == b.getHalfExtensions());

	a.setCenter(center);

	BOOST_CHECK(a.getCenter() == b.getCenter());
	BOOST_CHECK(a.getHalfExtensions() == b.getHalfExtensions());
}

BOOST_AUTO_TEST_CASE( clip_line_segment ) {
	AxisAlignedBox aab( glm::vec3( 0.0f, 0.0f, 0.0f ), 10.0f, 10.0f, 10.0f );
	LineSegment clipped;

	LineSegment outside( glm::vec3( 20.0f, 0.0f, 0.0f ), glm::vec3( 22.0f, 30.0f, 5.0f )  );
	LineSegment parallelOutside( glm::vec3( 20.0f, 0.0f, 0.0f ), glm::vec3( 20.0f, 10.0f, 0.0f ) );

	BOOST_CHECK( ! aab.clipLineSegment( outside, clipped ) );
	BOOST_CHECK( ! aab.clipLineSegment( parallelOutside, clipped ) );

	LineSegment contained( glm::vec3( -5.0f, 0.0f, 0.0f ), glm::vec3( 5.0f, 0.0f, 0.0f ) );

	BOOST_CHECK( aab.clipLineSegment( contained, clipped ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getP(), glm::vec3( -5.0f, 0.0f, 0.0f ) ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getQ(), glm::vec3( 5.0f, 0.0f, 0.0f ) ) );

	LineSegment onFace( glm::vec3( 0.0f, 0.0f, 0.0f ), glm::vec3( 15.0f, 0.0f, 0.0f ) );

	BOOST_CHECK( aab.clipLineSegment( onFace, clipped ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getP(), glm::vec3( 0.0f, 0.0f, 0.0f ) ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getQ(), glm::vec3( 10.0f, 0.0f, 0.0f ) ) );

	LineSegment onEdge( glm::vec3( 0.0f, 0.0f, 0.0f ), glm::vec3( 14.0f, -14.0f, 0.0f ) );

	BOOST_CHECK( aab.clipLineSegment( onEdge, clipped ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getP(), glm::vec3( 0.0f, 0.0f, 0.0f ) ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getQ(), glm::vec3( 10.0f, -10.0f, 0.0f ) ) );

	LineSegment onVertex( glm::vec3( 0.0f, 0.0f, 0.0f ), glm::vec3( -16.0f, 16.0f, -16.0f ) );

	BOOST_CHECK( aab.clipLineSegment( onVertex, clipped ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getP(), glm::vec3( 0.0f, 0.0f, 0.0f ) ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getQ(), glm::vec3( -10.0f, 10.0f, -10.0f ) ) );

	LineSegment diagonal( glm::vec3( -50.0f, -50.0f, -50.0f ), glm::vec3( 50.0f, 50.0f, 50.0f ) );

	BOOST_CHECK( aab.clipLineSegment( diagonal, clipped ) );
	BOOST_CHECK( ayla::epsilonEqual( clipped.getP(), glm::vec3( -10.0f, -10.0f, -10.0f ), 0.000001f ) );
	BOOST_CHECK(ayla::epsilonEqual( clipped.getQ(), glm::vec3(10.0f, 10.0f, 10.0f), 0.000001f ) );
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <utility>
#include <fastad_bits/util/type_traits.hpp>
#include <Eigen/Dense>

namespace ad {
namespace util {

template <class T>
constexpr inline
std::enable_if_t<!util::is_eigen_v<std::decay_t<T>>, T&&>
to_array(T&& x)
{ return std::forward<T>(x); }

template <class T>
constexpr inline
auto to_array(const Eigen::MatrixBase<T>& x)
{ return x.array(); }

template <class T>
constexpr inline
auto to_array(Eigen::MatrixBase<T>& x)
{ return x.array(); }

template <class T, class XType>
constexpr inline
auto cast_to(const XType& x)
{
    if constexpr (util::is_eigen_v<XType>) {
        return x.template cast<T>();
    } else {
        return x;
    }
};

template <class T>
constexpr inline void ones(T& x)
{
    using x_t = std::decay_t<T>;
    if constexpr (util::is_eigen_v<x_t>) {
        x.setOnes();
    } else {
        x = 1;
    }
};

} // namespace util
} // namespace ad


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file substitute_test.cpp
/// \brief Test for the pbes rewriters.

#include <iostream>
#include <boost/test/minimal.hpp>
#include "mcrl2/data/parse.h"
#include "mcrl2/data/substitutions/mutable_map_substitution.h"
#include "mcrl2/data/substitutions/variable_assignment.h"
#include "mcrl2/pbes/parse.h"
#include "mcrl2/pbes/substitutions.h"
#include "mcrl2/pbes/replace.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

void test_substitution()
{
  std::string text1 =
    "pbes nu X(n: Nat) = X(n + 1);\n"
    "init X(0);                   \n"
    ;
  pbes p1 = txt2pbes(text1);

  std::string text2 =
    "pbes nu X(n: Nat) = X(4 + 1);\n"
    "init X(0);                   \n"
    ;
  pbes p2 = txt2pbes(text2);

  data::mutable_map_substitution<> sigma;
  data::variable n("n", data::sort_nat::nat());
  sigma[n] = data::parse_data_expression("4");

  pbes p = p1;
  pbes_system::replace_free_variables(p.equations(), sigma);
  BOOST_CHECK(p == p1);

  std::cout << "--- p =\n" << pbes_system::pp(p) << std::endl;
  std::cout << "--- p1 =\n" << pbes_system::pp(p1) << std::endl;

  pbes_system::replace_variables(p, sigma);

  std::cout << "--- p =\n" << pbes_system::pp(p) << std::endl;
  std::cout << "--- p2 =\n" << pbes_system::pp(p2) << std::endl;

  // compare textual representations, to avoid conflicts between types
  BOOST_CHECK(pbes_system::pp(p) == pbes_system::pp(p2));
}

void test_propositional_variable_substitution()
{
  std::string text1 =
    "pbes                     \n"
    "nu X(m: Nat) = Y(m + 1); \n"
    "nu Y(n: Nat) = X(n + 2); \n"
    "init X(0);               \n"
    ;
  pbes p1 = txt2pbes(text1);

  std::string text2 =
    "pbes                                     \n"
    "nu X(m: Nat) = Y(m + 1);                 \n"
    "nu Y(n: Nat) = X(n + 2 + 1) && Y(n + 2); \n"
    "init X(0);                               \n"
    ;
  pbes p2 = txt2pbes(text2);

  pbes p = p1;
  propositional_variable X = p.equations().front().variable();
  pbes_expression phi = parse_pbes_expression("X(m + 1) && Y(m)", "datavar m: Nat; \npredvar X: Nat; Y: Nat");
  propositional_variable_substitution sigma(X, phi);
  pbes_system::replace_propositional_variables(p, sigma);
  std::cout << pbes_system::pp(p) << std::endl;
  std::cout << pbes_system::pp(p2) << std::endl;

  // compare textual representations, to avoid conflicts between types
  BOOST_CHECK(pbes_system::pp(p) == pbes_system::pp(p2));

}

inline
pbes_expression sigma(const pbes_expression& x)
{
  std::string var_decl = "datavar b: Bool; \npredvar X: Bool;";
  pbes_expression x1 = parse_pbes_expression("X(false)", var_decl);
  pbes_expression x2 = parse_pbes_expression("X(true) || true", var_decl);
  return x == x1 ? x2 : x;
}

void test_replace_pbes_expressions()
{
  std::string var_decl = "datavar b: Bool; \npredvar X: Bool;";
  pbes_expression x = parse_pbes_expression("X(b) || X(false)", var_decl);
  pbes_expression result = replace_pbes_expressions(x, sigma);
  pbes_expression expected_result = parse_pbes_expression("X(b) || X(true) || true", var_decl);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

void test_replace_variables()
{
  pbes_expression x = parse_pbes_expression("forall n: Nat. exists m: Nat. val(m > n)");
  pbes_expression expected_result = parse_pbes_expression("forall n: Nat. exists m: Nat. val(n > n)");
  data::mutable_map_substitution<> sigma;
  data::variable m("m", data::sort_nat::nat());
  data::variable n("n", data::sort_nat::nat());
  sigma[m] = n;
  pbes_expression result = pbes_system::replace_variables(x, sigma);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

void test_variable_assignment()
{
  pbes_expression x = parse_pbes_expression("forall n: Nat. exists m: Nat. val(m > n)");
  pbes_expression expected_result = parse_pbes_expression("forall n: Nat. exists m: Nat. val(n > n)");
  data::variable m("m", data::sort_nat::nat());
  data::variable n("n", data::sort_nat::nat());
  data::variable_assignment sigma(m, n);
  pbes_expression result = pbes_system::replace_variables(x, sigma);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

int test_main(int argc, char* argv[])
{
  test_substitution();
  test_propositional_variable_substitution();
  test_replace_pbes_expressions();
  test_replace_variables();
  test_variable_assignment();

  return 0;
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file single_minded_auctions_test.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-2-5
 */

#include "test_utils/auctions.hpp"

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/single_minded_auctions.hpp"
#include "paal/data_structures/fraction.hpp"
#include "paal/utils/functors.hpp"

#include <boost/none.hpp>
#include <boost/test/unit_test.hpp>

#include <iterator>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>

BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(testSingleMindedAuction, 1);

BOOST_AUTO_TEST_CASE(testSingleMindedAuction)
{
   using Bidder = std::string;
   using Bidders = std::unordered_set<Bidder>;
   using Item = std::string;
   using Items = std::unordered_set<Item>;
   using Value = double;
   using Bid = std::pair<Items, Value>;
   namespace pu = paal::utils;
   namespace pa = paal::auctions;

   // input data
   Bidders bidders {"John", "Bob", "Steve"};
   std::unordered_map<Bidder, Bid> bids {
      {"John", {Items{}, 0.0}},
      {"Bob", {{"apple", "orange", "knife"}, 4.5}},
      {"Steve", {{"apple", "ball"}, 10.1}}
   };
   auto get_value = [&](const Bidder& bidder) { return bids.at(bidder).second; };
   auto get_items = [&](const Bidder& bidder) -> const Items& { return bids.at(bidder).first; };
   auto get_copies_num = [](const Item& item) { return item == "apple" ? 2 : 1; };

   Items items;
   pa::extract_items_from_single_minded(bidders, get_items,
         std::inserter(items, items.begin()));

   Items want_items = {"apple", "ball", "knife", "orange"};

   // value query auction
   {
      auto auction = pa::make_single_minded_to_value_query_auction(
         bidders, items, get_value, get_items, get_copies_num
      );
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("apple"), 2);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("knife"), 1);
      auto const tests = {
         std::make_tuple("John", Items{}, 0.),
         std::make_tuple("John", Items{"apple", "knife"}, 0.),
         std::make_tuple("Bob", Items{"apple", "ball", "orange"}, 0.),
         std::make_tuple("Bob", Items{"apple", "knife", "orange"}, 4.5),
         std::make_tuple("Steve", Items{"apple", "orange", "knife"}, 0.),
         std::make_tuple("Steve", Items{"apple", "orange", "knife", "ball"}, 10.1),
      };
      for (auto t: tests)
         BOOST_CHECK_EQUAL(auction.call<pa::value_query>(std::get<0>(t),
                  std::get<1>(t)), std::get<2>(t));
   }

   // demand query auction
   {
      auto auction = paal::auctions::make_single_minded_to_demand_query_auction(
         bidders, items, get_value, get_items, get_copies_num
      );
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<paal::auctions::get_copies_num>("apple"), 2);
      BOOST_CHECK_EQUAL(auction.call<paal::auctions::get_copies_num>("ball"), 1);
      auto test = [&](Bidder bidder, Value want_util, Items want_items = Items{})
      {
         return check_demand_query<Items>(
            auction, bidder, pu::make_dynamic_return_constant_functor(1.5), {want_items, want_util}
         );
      };
      BOOST_CHECK(test("John", 0));
      BOOST_CHECK(test("Bob", 0));
      BOOST_CHECK(test("Steve", 10.1 - 2 * 1.5, {"apple", "ball"}));
   }

   // gamma oracle auction
   {
      using Frac = paal::data_structures::fraction<Value, Value>;

      auto auction = pa::make_single_minded_to_gamma_oracle_auction(
         bidders, items, get_value, get_items, get_copies_num
      );
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("apple"), 2);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("orange"), 1);
      BOOST_CHECK_EQUAL(auction.get<pa::gamma>(), 1);
      auto test = [&](
         Bidder bidder,
         Value threshold,
         boost::optional<Frac>want_frac,
         Items want_items = Items{}
      ) {
         boost::optional<std::pair<Items, Frac>> want;
         if (want_frac) want = std::make_pair(want_items, *want_frac);
         return check_gamma_oracle<Items>(auction, bidder,
               pu::return_one_functor(), threshold, want);
      };
      BOOST_CHECK(test("John", 1., boost::none));
      BOOST_CHECK(test("Bob", 4.5, boost::none));
      BOOST_CHECK(test("Bob", 4., Frac(3.0, 0.5), Items{"apple", "orange", "knife"}));
      BOOST_CHECK(test("Steve", 10.1, boost::none));
      BOOST_CHECK(test("Steve", 10., Frac(2.0, 0.1), Items{"apple", "ball"}));
   }
}


#pragma once
#include <Eigen/Sparse>

namespace edp
{
namespace internal
{
    inline uint32_t ipow(uint32_t base, uint32_t exp)
    {
        uint32_t result = 1U;
        while(exp != 0)
        {
            if((exp & 1U) != 0)
            {
                result *= base;
            }
            exp >>= 1U;
            base *= base;
        }

        return result;
    }
}

template<typename T> struct TwoSiteTerm
{
    std::pair<uint32_t, uint32_t> sites;
    Eigen::SparseMatrix<T> m;

    TwoSiteTerm(std::pair<uint32_t, uint32_t> p1, const Eigen::SparseMatrix<T>& p2)
        : sites(std::move(p1)), m(p2)
    { }
    TwoSiteTerm(std::pair<uint32_t, uint32_t>&& p1, Eigen::SparseMatrix<T>&& p2) : sites(p1), m(p2)
    { }
};

template<typename T> struct OneSiteTerm
{
    uint32_t site;
    Eigen::SparseMatrix<T> m;

    OneSiteTerm(uint32_t p1, const Eigen::SparseMatrix<T>& p2) : site(p1), m(p2) { }

    OneSiteTerm(uint32_t p1, Eigen::SparseMatrix<T>&& p2) : site(p1), m(p2) { }
};

template<typename T> class LocalHamiltonian
{
private:
    uint32_t numSites_;
    uint64_t d_; // local Hibert dimension

    std::vector<TwoSiteTerm<T>> twoSiteTerms_;
    std::vector<OneSiteTerm<T>> oneSiteTerms_;

    [[nodiscard]] uint32_t swapBaseD(uint32_t idx, uint32_t pos, uint32_t val) const
    {
        uint32_t b = internal::ipow(d_, pos);
        uint32_t upper = (idx / (b * d_)) * d_ + val;
        return upper * b + (idx % b);
    }

public:
    LocalHamiltonian(uint32_t numSites, uint32_t d) : numSites_{numSites}, d_{d} { }

    void clearTerms()
    {
        std::vector<TwoSiteTerm<T>>().swap(twoSiteTerms_);
        std::vector<OneSiteTerm<T>>().swap(oneSiteTerms_);
    }
    [[nodiscard]] uint32_t getNumSites() const { return numSites_; }

    [[nodiscard]] std::map<uint32_t, T> getCol(uint32_t n) const;
    [[nodiscard]] inline std::map<uint32_t, T> operator()(uint32_t n) const { return getCol(n); }

    void addTwoSiteTerm(const std::pair<int, int>& site, Eigen::SparseMatrix<T> m)
    {
        m.makeCompressed();
        twoSiteTerms_.emplace_back(site, std::move(m));
    }
    void addOneSiteTerm(uint32_t site, Eigen::SparseMatrix<T> m)
    {
        m.makeCompressed();
        oneSiteTerms_.emplace_back(site, std::move(m));
    }
};
} // namespace edp

template<typename T> std::map<uint32_t, T> edp::LocalHamiltonian<T>::getCol(uint32_t n) const
{
    using Eigen::SparseMatrix;
    using internal::ipow;

    std::map<uint32_t, T> m;

    for(auto& twoSiteTerm : twoSiteTerms_)
    {
        auto a = (n / ipow(d_, twoSiteTerm.sites.first)) % d_;
        auto b = (n / ipow(d_, twoSiteTerm.sites.second)) % d_;
        // auto col = twoSiteTerm.m.col(a*d_ + b);
        auto col = b * d_ + a;
        for(typename SparseMatrix<T>::InnerIterator it(twoSiteTerm.m, col); it; ++it)
        {
            uint32_t r = it.row();
            uint32_t t = n;
            t = swapBaseD(t, twoSiteTerm.sites.first, r % d_);
            t = swapBaseD(t, twoSiteTerm.sites.second, r / d_);
            m[t] += it.value();
        }
    }
    for(auto& oneSiteTerm : oneSiteTerms_)
    {
        uint32_t a = (n / ipow(d_, oneSiteTerm.site)) % d_;
        // auto col = oneSiteTerm.m.col(a);
        auto col = a;

        for(typename SparseMatrix<T>::InnerIterator it(oneSiteTerm.m, col); it; ++it)
        {
            uint32_t r = it.row();
            uint32_t t = n;
            t = swapBaseD(t, oneSiteTerm.site, r);
            m[t] += it.value();
        }
    }
    return m;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REM_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REM_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/rem.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/divfix.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::rem_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return selsub(is_nez(a1), a0,idivfix(a0,a1)*a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::rem_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      //      return b_or(is_invalid(a1), selsub(is_nez(a1), a0, tofloat(idivfix(a0,a1))*a1));
      return if_nan_else(is_invalid(a1),
                      selsub(is_nez(a1), a0, divfix(a0,a1)*a1));

    }
  };
} } }

#endif


/**
 * \file
 * \author Thomas Fischer
 * \date   2011-03-17
 * \brief  Implementation of the AngleSkewMetric class.
 *
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AngleSkewMetric.h"

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

#include "MeshLib/Node.h"

#include "MathLib/MathTools.h"

using namespace boost::math::double_constants;

namespace MeshLib
{
AngleSkewMetric::AngleSkewMetric(Mesh const& mesh) :
    ElementQualityMetric(mesh)
{}

void AngleSkewMetric::calculateQuality ()
{
    const std::vector<MeshLib::Element*>& elements(_mesh.getElements());
    const std::size_t nElements (_mesh.getNumberOfElements());

    for (std::size_t k(0); k < nElements; k++)
    {
        Element const& elem (*elements[k]);
        switch (elem.getGeomType())
        {
        case MeshElemType::LINE:
            _element_quality_metric[k] = -1.0;
            break;
        case MeshElemType::TRIANGLE:
            _element_quality_metric[k] = checkTriangle (elem);
            break;
        case MeshElemType::QUAD:
            _element_quality_metric[k] = checkQuad (elem);
            break;
        case MeshElemType::TETRAHEDRON:
            _element_quality_metric[k] = checkTetrahedron (elem);
            break;
        case MeshElemType::HEXAHEDRON:
            _element_quality_metric[k] = checkHexahedron (elem);
            break;
        case MeshElemType::PRISM:
            _element_quality_metric[k] = checkPrism (elem);
            break;
        default:
            break;
        }
    }
}

double AngleSkewMetric::checkTriangle (Element const& elem) const
{
    double const* const node0 (elem.getNode(0)->getCoords());
    double const* const node1 (elem.getNode(1)->getCoords());
    double const* const node2 (elem.getNode(2)->getCoords());

    double min_angle(two_pi);
    double max_angle(0.0);
    getMinMaxAngleFromTriangle (node0, node1, node2, min_angle, max_angle);

    return 1.0 -
           std::max((max_angle - third_pi) / two_thirds_pi,
                    (third_pi - min_angle) / third_pi);
}

double AngleSkewMetric::checkQuad (Element const& elem) const
{
    double const* const node0 (elem.getNode(0)->getCoords());
    double const* const node1 (elem.getNode(1)->getCoords());
    double const* const node2 (elem.getNode(2)->getCoords());
    double const* const node3 (elem.getNode(3)->getCoords());

    double min_angle (two_pi);
    double max_angle (0.0);

    getMinMaxAngleFromQuad (node0, node1, node2, node3, min_angle, max_angle);

    return 1.0 -
           std::max((max_angle - two_pi) / (-pi), (two_pi - min_angle) / (two_pi));
}

double AngleSkewMetric::checkTetrahedron (Element const& elem) const
{
    double const* const node0 (elem.getNode(0)->getCoords());
    double const* const node1 (elem.getNode(1)->getCoords());
    double const* const node2 (elem.getNode(2)->getCoords());
    double const* const node3 (elem.getNode(3)->getCoords());

    double min_angle (two_pi);
    double max_angle (0.0);

    // first triangle (0,1,2)
    getMinMaxAngleFromTriangle(node0, node1, node2, min_angle, max_angle);
    // second triangle (0,1,3)
    getMinMaxAngleFromTriangle(node0, node1, node3, min_angle, max_angle);
    // third triangle (0,2,3)
    getMinMaxAngleFromTriangle(node0, node2, node3, min_angle, max_angle);
    // fourth triangle (1,2,3)
    getMinMaxAngleFromTriangle(node1, node2, node3, min_angle, max_angle);

    return 1.0 - std::max((max_angle - two_pi) / two_thirds_pi,
                          (third_pi - min_angle) / third_pi);
}

double AngleSkewMetric::checkHexahedron (Element const& elem) const
{
    double const* const node0 (elem.getNode(0)->getCoords());
    double const* const node1 (elem.getNode(1)->getCoords());
    double const* const node2 (elem.getNode(2)->getCoords());
    double const* const node3 (elem.getNode(3)->getCoords());
    double const* const node4 (elem.getNode(4)->getCoords());
    double const* const node5 (elem.getNode(5)->getCoords());
    double const* const node6 (elem.getNode(6)->getCoords());
    double const* const node7 (elem.getNode(7)->getCoords());

    double min_angle (two_pi);
    double max_angle (0.0);

    // first surface (0,1,2,3)
    getMinMaxAngleFromQuad (node0, node1, node2, node3, min_angle, max_angle);
    // second surface (0,3,7,4)
    getMinMaxAngleFromQuad (node0, node3, node7, node4, min_angle, max_angle);
    // third surface (4,5,6,7)
    getMinMaxAngleFromQuad (node4, node5, node6, node7, min_angle, max_angle);
    // fourth surface (5,1,2,6)
    getMinMaxAngleFromQuad (node5, node1, node2, node6, min_angle, max_angle);
    // fifth surface (5,1,0,4)
    getMinMaxAngleFromQuad (node5, node1, node0, node4, min_angle, max_angle);
    // sixth surface (6,2,3,7)
    getMinMaxAngleFromQuad (node6, node2, node3, node7, min_angle, max_angle);

    return 1.0 -
           std::max((max_angle - two_pi) / (-pi), (two_pi - min_angle) / two_pi);
}

double AngleSkewMetric::checkPrism (Element const& elem) const
{
    double const* const node0 (elem.getNode(0)->getCoords());
    double const* const node1 (elem.getNode(1)->getCoords());
    double const* const node2 (elem.getNode(2)->getCoords());
    double const* const node3 (elem.getNode(3)->getCoords());
    double const* const node4 (elem.getNode(4)->getCoords());
    double const* const node5 (elem.getNode(5)->getCoords());

    double min_angle_tri (two_pi);
    double max_angle_tri (0.0);

    // first triangle (0,1,2)
    getMinMaxAngleFromTriangle (node0, node1, node2, min_angle_tri, max_angle_tri);
    // second surface (3,4,5)
    getMinMaxAngleFromTriangle (node3, node4, node5, min_angle_tri, max_angle_tri);

    double tri_criterion (1.0 - std::max((max_angle_tri - two_pi) / two_thirds_pi,
                                         (third_pi - min_angle_tri) / third_pi));

    double min_angle_quad (two_pi);
    double max_angle_quad (0.0);
    // surface (0,3,4,1)
    getMinMaxAngleFromQuad (node0, node3, node4, node1, min_angle_quad, max_angle_quad);
    // surface (2,5,3,0)
    getMinMaxAngleFromQuad (node2, node5, node3, node0, min_angle_quad, max_angle_quad);
    // surface (1,2,5,4)
    getMinMaxAngleFromQuad (node1, node2, node5, node4, min_angle_quad, max_angle_quad);

    double quad_criterion (1.0 - std::max((max_angle_quad - two_pi) / (-pi),
                                          (two_pi - min_angle_quad) / two_pi));

    return std::min (tri_criterion, quad_criterion);
}

void AngleSkewMetric::getMinMaxAngleFromQuad (
        double const* const n0, double const* const n1,
        double const* const n2, double const* const n3,
        double &min_angle, double &max_angle) const
{
    const double* nodes[4] = {n0, n1, n2, n3};
    for (unsigned i=0; i<4; ++i)
    {
        const double angle (MathLib::getAngle (nodes[i], nodes[(i+1)%4], nodes[(i+2)%4]));
        min_angle = std::min(angle, min_angle);
        max_angle = std::max(angle, max_angle);
    }
}

void AngleSkewMetric::getMinMaxAngleFromTriangle(double const* const n0,
                                                          double const* const n1,
                                                          double const* const n2,
                                                          double &min_angle,
                                                          double &max_angle) const
{
    const double* nodes[3] = {n0, n1, n2};
    for (unsigned i=0; i<3; ++i)
    {
        const double angle (MathLib::getAngle (nodes[i], nodes[(i+1)%3], nodes[(i+2)%3]));
        min_angle = std::min(angle, min_angle);
        max_angle = std::max(angle, max_angle);
    }
}

} // end namespace MeshLib


// Copyright (c)  Mingcheng Zuo, Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution (GCL-DE) derived from
// "A case learning-based differential evolution algorithm for global optimization of interplanetary trajectory design,
//  Mingcheng Zuo, Guangming Dai, Lei Peng, Maocai Wang, Zhengquan Liu", https://doi.org/10.1016/j.asoc.2020.106451

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"
#include "call_java.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef void (*callback_parallel)(int, int, double[], double[]);

namespace cl_differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);
static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static double normreal(pcg64 *rs, double mu, double sdev) {
    return gauss_01(*rs) * sdev + mu;
}

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static vec constant(int n, double val) {
    return Eigen::MatrixXd::Constant(n, 1, val);
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dx, dy, [&]() {
        return distr_01(rs);
    });
}

struct IndexVal {
    int index;
    double val;
};

static bool compareIndexVal(IndexVal i1, IndexVal i2) {
    return (i1.val < i2.val);
}

static ivec sort_index(const vec &x) {
    int size = x.size();
    IndexVal ivals[size];
    for (int i = 0; i < size; i++) {
        ivals[i].index = i;
        ivals[i].val = x[i];
    }
    std::sort(ivals, ivals + size, compareIndexVal);
    return Eigen::MatrixXi::NullaryExpr(size, 1, [&ivals](int i) {
        return ivals[i].index;
    });
}

static ivec sort_index(const vec &y, int *indices, int size) {
    IndexVal ivals[size];
    for (int i = 0; i < size; i++) {
        ivals[i].index = indices[i];
        ivals[i].val = y[indices[i]];
    }
    std::sort(ivals, ivals + size, compareIndexVal);
    return Eigen::MatrixXi::NullaryExpr(size, 1, [&ivals](int i) {
        return ivals[i].index;
    });
}

// wrapper around the fitness function, scales according to boundaries

class Fitness {

public:

    Fitness(CallJava *func_par_, const vec &lower_limit,
            const vec &upper_limit) {
        func_par = func_par_;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    vec norm(vec &X) {
        return (X - lower).array() / scale.array();
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(upper).cwiseMax(lower);
        }
        return X;
    }

    void values(const mat &popX, vec &ys) {
        int popsize = popX.cols();
        int n = popX.rows();
        double pargs[popsize * n];
        double res[popsize];
        for (int p = 0; p < popX.cols(); p++) {
            for (int i = 0; i < n; i++)
                pargs[p * n + i] = popX(i, p);
        }
        func_par->evalJava(popsize, n, pargs, res);
        for (int p = 0; p < popX.cols(); p++)
            ys[p] = res[p];
        evaluationCounter += popsize;
    }

    bool feasible(int i, double x) {
        return x >= lower[i] && x <= upper[i];
    }

    vec uniformX(pcg64 &rs) {
        vec rv = uniformVec(lower.size(), rs);
        return (rv.array() * scale.array()).matrix() + lower;
    }

    double uniformXi(int i, pcg64 &rs) {
        return lower[i] + scale[i] * distr_01(rs);
    }

    int getEvaluations() {
        return evaluationCounter;
    }

    vec lower;
    vec upper;

private:
    CallJava *func_par;
    long evaluationCounter;
    vec scale;
};

class ClDeOptimizer {

public:

    ClDeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double pbest_,
            double stopfitness_, double K1_, double K2_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize0 = popsize_ > 0 ? popsize_ : int(dim * 8.5 + 150);
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // use low value 0 < pbest <= 1 to narrow search.
        pbest0 = pbest_;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        K1 = K1_;
        K2 = K2_;
        // stop criteria
        stop = 0;
        rs = new pcg64(seed_);
        init();
    }

    ~ClDeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    void doOptimize() {

        double CR, F;
        vector<vec> sp;

        double stage = 0.5;

        int popsize = popsize0;
        double pbest = pbest0;

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1;; iterations++) {
            // sort population
            ivec sindex = sort_index(nextY);
            popY = nextY(sindex, Eigen::all);
            popX = nextX(Eigen::all, sindex);

            bestX = popX.col(0);
            bestY = popY[0];

            if (isfinite(stopfitness) && bestY < stopfitness) {
                stop = 1;
                return;
            }

            if (fitfun->getEvaluations() >= maxEvaluations)
                return;

            double evals = float(fitfun->getEvaluations()) / maxEvaluations;
            if (evals > stage)
                pbest = pbest0;
            else {
                for (double per = 1.0; per >= 0; per = per - 0.05)
                    if (evals * evals < per && evals * evals >= per - 0.05)
                        pbest = 1 - per;
            }
            popsize = min(popsize0,
                    max(7 * dim, int(popsize0 - (popsize0 - 50) * evals)));

            mat local_upper(dim, popsize);
            mat local_lower(dim, popsize);
            double bound_info[popsize];
            for (int p = 0; p < popsize; p++) {
                vec X = popX.col(p);
                local_upper.col(p) = fitfun->upper.cwiseMin(X);
                local_lower.col(p) = fitfun->lower.cwiseMax(X);
                vec norm = fitfun->norm(X);
                double avnorm = norm.sum() / popsize;
                bound_info[p] = 0;
                for (int j = 0; j < dim; j++)
                    bound_info[p] += abs(norm[j] - avnorm);
                bound_info[p] /= popsize;
            }

            for (int p = 0; p < popsize; p++) {
                int r1, r2, r3;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p);
                do {
                    r2 = rndInt(int(popsize * pbest));
                } while (r2 == p || r2 == r1);
                do {
                    r3 = rndInt(popsize + sp.size());
                } while (r3 == p || r3 == r2 || r3 == r1);
                int jr = rndInt(dim);

                if (iterations % 2 == 1)
//					CR = 1;
                    CR = normreal(rs, 0.95, 0.01);
                else
//					CR = 0;
                    CR = normreal(rs, 0.0, 0.01);

                mat mutationX;
                vec mutationY;
                int mutationI[3];
                if (evals > stage) {
                    mutationI[0] = r1;
                    mutationI[1] = r2;
                    mutationI[2] = r3 < popsize ? r3 : r3 - popsize;
                    ivec mindex = sort_index(nextY, mutationI, 3);
                    mutationY = nextY(mindex, Eigen::all);
                    mutationX = nextX(Eigen::all, mindex);
                    F = 2 * (mutationY[1] - mutationY[0])
                            / (mutationY[2] - mutationY[0]);
                } else {
                    if (rnd01() < 0.5)
                        F = normreal(rs, 0.1, 0.04);
                    else
                        F = normreal(rs, 1.0, 1.0);
                    if (F < 0 || F > 1)
                        F = rnd01();
                }
                vec ui = popX.col(p);
                vec ub = local_upper.col(p);
                vec lb = local_lower.col(p);
                for (int j = 0; j < dim; j++) {
                    if (j == jr || rnd01() < CR) {
                        if (bound_info[j] > 0.1 && evals < K1 && rnd01() > K2)
                            ui[j] = ub[j] + lb[j] - popX(j, r1);
                        else {
                            if (evals > stage) {
                                ui[j] = mutationX(j, 0)
                                        + F
                                                * (mutationX(j, 1)
                                                        - mutationX(j, 2));
                            } else {
                                if (r3 < popsize)
                                    ui[j] = popX(j, r1)
                                            + F * (popX(j, r2) - popX(j, r3));
                                else
                                    ui[j] =
                                            popX(j, r1)
                                                    + F
                                                            * ((popX)(j, r2)
                                                                    - sp[r3
                                                                            - popsize][j]);
                            }
                        }
                        if (!fitfun->feasible(j, ui[j]))
                            ui[j] = fitfun->uniformXi(j, *rs);
                    }
                }
                nextX.col(p) = ui;
            }
            fitfun->values(nextX, nextY);
            for (int p = 0; p < popsize; p++) {
                if (nextY[p] < popY[p]) {
                    if (sp.size() < popsize)
                        sp.push_back(popX.col(p));
                    else
                        sp[rndInt(popsize)] = popX.col(p);
                } else {    // no improvement, copy from parent
                    nextX.col(p) = popX.col(p);
                    nextY[p] = popY[p];
                }
            }
        }
    }

    void init() {
        popCR = zeros(popsize0);
        popF = zeros(popsize0);
        nextX = mat(dim, popsize0);
        for (int p = 0; p < popsize0; p++)
            nextX.col(p) = fitfun->uniformX(*rs);
        nextY = vec(popsize0);
        fitfun->values(nextX, nextY);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize0; // population size
    int dim;
    int maxEvaluations;
    double pbest0;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int stop;
    double K1;
    double K2;
    pcg64 *rs;
    mat popX;
    vec popY;
    mat nextX;
    vec nextY;
    vec popCR;
    vec popF;
};
}

using namespace cl_differential_evolution;

/*
 * Class:     fcmaes_core_Jni
 * Method:    optimizeCLDE
 * Signature: (Lfcmaes/core/Fitness;[D[D[DIDIDDDJI)I
 */
JNIEXPORT jint JNICALL Java_fcmaes_core_Jni_optimizeCLDE(JNIEnv *env,
        jclass cls, jobject func, jdoubleArray jlower, jdoubleArray jupper,
        jdoubleArray jinit, jint maxEvals, jdouble stopfitness, jint popsize,
        jdouble pbest, jdouble K1, jdouble K2, jlong seed, jint runid) {
    // init java function callback
    double *init = env->GetDoubleArrayElements(jinit, JNI_FALSE);
    double *lower = env->GetDoubleArrayElements(jlower, JNI_FALSE);
    double *upper = env->GetDoubleArrayElements(jupper, JNI_FALSE);
    int dim = env->GetArrayLength(jinit);

    vec lower_limit(dim), upper_limit(dim);
    bool useLimit = false;
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    CallJava callJava(func, env);
    Fitness fitfun(&callJava, lower_limit, upper_limit);
    ClDeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, pbest,
            stopfitness, K1, K2);
    try {
        opt.doOptimize();
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();

        for (int i = 0; i < dim; i++)
            init[i] = bestX[i];

        env->SetDoubleArrayRegion(jinit, 0, dim, (jdouble*) init);
        env->ReleaseDoubleArrayElements(jinit, init, 0);
        env->ReleaseDoubleArrayElements(jupper, upper, 0);
        env->ReleaseDoubleArrayElements(jlower, lower, 0);
        return fitfun.getEvaluations();

    } catch (std::exception &e) {
        cout << e.what() << endl;
        return fitfun.getEvaluations();
    }
    return 0;
}


/*
 * Copyright 2009-2021 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE aopotential_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/aopotential.h"
#include "votca/xtp/orbitals.h"
#include "votca/xtp/orbreorder.h"
#include <libint2/initialize.h>
#include <votca/tools/eigenio_matrixmarket.h>
using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(aopotential_test)

BOOST_AUTO_TEST_CASE(aopotentials_test) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/aopotential/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  AOMultipole esp;
  esp.FillPotential(aobasis, orbitals.QMAtoms());
  Eigen::MatrixXd esp_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/esp_ref.mm");
  bool check_esp = esp.Matrix().isApprox(esp_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_esp, 1);
  if (!check_esp) {
    std::cout << "esp Ref" << endl;
    std::cout << esp_ref << endl;
    std::cout << "esp" << endl;
    std::cout << esp.Matrix() << endl;
  }

  ECPBasisSet ecps;
  ecps.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/ecp.xml");
  ECPAOBasis ecpbasis;
  ecpbasis.Fill(ecps, orbitals.QMAtoms());
  AOECP ecp;
  ecp.FillPotential(aobasis, ecpbasis);
  Eigen::MatrixXd ecp_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/ecp_ref.mm");

  bool check_ecp = ecp.Matrix().isApprox(ecp_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_ecp, 1);
  if (!check_ecp) {
    std::cout << "ecp Ref" << endl;
    std::cout << ecp_ref << endl;
    std::cout << "ecp" << endl;
    std::cout << ecp.Matrix() << endl;
  }

  StaticSegment seg("", 0);
  seg.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/aopotential/polarsite.mps");

  std::vector<std::unique_ptr<StaticSite> > externalsites;
  for (const StaticSite& site : seg) {
    externalsites.push_back(std::unique_ptr<StaticSite>(new StaticSite(site)));
  }
  AOMultipole dip;
  dip.FillPotential(aobasis, externalsites);

  Eigen::MatrixXd dip_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/dip_ref.mm");
  bool dip_check = dip_ref.isApprox(dip.Matrix(), 1e-4);
  BOOST_CHECK_EQUAL(dip_check, 1);
  if (!dip_check) {
    std::cout << "dip Ref" << endl;
    std::cout << dip_ref << endl;
    std::cout << "Dip" << endl;
    std::cout << dip.Matrix() << endl;
  }

  StaticSegment seg2("", 0);
  seg2.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                    "/aopotential/polarsite2.mps");

  std::vector<std::unique_ptr<StaticSite> > externalsites2;
  for (const StaticSite& site : seg2) {
    externalsites2.push_back(std::unique_ptr<StaticSite>(new StaticSite(site)));
  }

  AOMultipole quad;
  quad.FillPotential(aobasis, externalsites2);

  Eigen::MatrixXd quad_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/quad_ref.mm");
  bool quad_check = quad_ref.isApprox(quad.Matrix(), 1e-4);
  BOOST_CHECK_EQUAL(quad_check, 1);
  if (!quad_check) {
    std::cout << "Quad Ref" << endl;
    std::cout << quad_ref << endl;
    std::cout << "Quad" << endl;
    std::cout << quad.Matrix() << endl;
  }

  AOPlanewave planewave;
  std::vector<Eigen::Vector3d> kpoints = {
      {1, 1, 1}, {2, 1, 1}, {-1, -1, -1}, {-2, -1, -1}};
  Eigen::MatrixXd planewave_ref = Eigen::MatrixXd::Zero(17, 17);
  planewave_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/planewave_ref.mm");

  planewave.FillPotential(aobasis, kpoints);
  Eigen::MatrixXd planewave_real = planewave.Matrix().real();
  bool planewave_check = planewave_ref.isApprox(planewave_real, 1e-4);

  BOOST_CHECK_EQUAL(planewave_check, 1);
  if (!planewave_check) {
    std::cout << "planewave Ref real" << endl;
    std::cout << planewave_ref << endl;
    std::cout << "planewave real" << endl;
    std::cout << planewave.Matrix().real() << endl;
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(aomultipole_comparison) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/aopotential/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  {
    StaticSegment seg("", 0);
    seg.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                     "/aopotential/polarsite_ao.mps");

    std::vector<std::unique_ptr<StaticSite> > externalsites;
    for (const StaticSite& site : seg) {
      externalsites.push_back(
          std::unique_ptr<StaticSite>(new StaticSite(site)));
    }
    AOMultipole dip;
    dip.FillPotential(aobasis, externalsites);

    Eigen::MatrixXd dip_ref = Eigen::MatrixXd::Zero(17, 17);

    double a = 0.1;                            // this is in a0
    double mag_d = seg[0].getDipole().norm();  // this is in e * a0
    const Eigen::Vector3d dir_d = seg[0].getDipole().normalized();
    const Eigen::Vector3d A = seg[0].getPos() + 0.5 * a * dir_d;
    const Eigen::Vector3d B = seg[0].getPos() - 0.5 * a * dir_d;
    double qA = mag_d / a;
    double qB = -qA;
    StaticSite site1 = StaticSite(0, "", A);
    site1.setCharge(qA);
    StaticSite site2 = StaticSite(1, "", B);
    site2.setCharge(qB);
    std::vector<std::unique_ptr<StaticSite> > externalsites_mono;
    externalsites_mono.push_back(
        std::unique_ptr<StaticSite>(new StaticSite(site1)));
    externalsites_mono.push_back(
        std::unique_ptr<StaticSite>(new StaticSite(site2)));
    AOMultipole mono2;
    mono2.FillPotential(aobasis, externalsites_mono);

    bool dip_check = mono2.Matrix().isApprox(dip.Matrix(), 1e-4);
    BOOST_CHECK_EQUAL(dip_check, 1);
    if (!dip_check) {
      std::cout << "mono2 Ref" << endl;
      std::cout << mono2.Matrix() << endl;
      std::cout << "Dip" << endl;
      std::cout << dip.Matrix() << endl;
    }
  }

  {

    StaticSegment seg2("", 0);
    seg2.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                      "/aopotential/polarsite_ao2.mps");

    std::vector<std::unique_ptr<StaticSite> > externalsites2;
    for (const StaticSite& site : seg2) {
      externalsites2.push_back(
          std::unique_ptr<StaticSite>(new StaticSite(site)));
    }
    AOMultipole quad;
    quad.FillPotential(aobasis, externalsites2);

    std::vector<std::unique_ptr<StaticSite> > externalsites_mono6;
    const Eigen::Matrix3d components = seg2[0].CalculateCartesianMultipole();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
    es.computeDirect(components);
    double a = 2 * 0.01;
    for (Index i = 0; i < 3; i++) {
      double q = es.eigenvalues()[i] / (a * a);
      const Eigen::Vector3d vec1 =
          seg2[0].getPos() + 0.5 * a * es.eigenvectors().col(i);
      const Eigen::Vector3d vec2 =
          seg2[0].getPos() - 0.5 * a * es.eigenvectors().col(i);
      StaticSite site1 = StaticSite(0, "", vec1);
      site1.setCharge(q);
      StaticSite site2 = StaticSite(1, "", vec2);
      site2.setCharge(q);
      externalsites_mono6.push_back(
          std::unique_ptr<StaticSite>(new StaticSite(site1)));
      externalsites_mono6.push_back(
          std::unique_ptr<StaticSite>(new StaticSite(site2)));
    }

    AOMultipole mono6;
    mono6.FillPotential(aobasis, externalsites_mono6);

    bool quad_check = mono6.Matrix().isApprox(quad.Matrix(), 1e-4);
    BOOST_CHECK_EQUAL(quad_check, 1);
    if (!quad_check) {
      std::cout << "mono6 Ref" << endl;
      std::cout << mono6.Matrix() << endl;
      std::cout << "Quad" << endl;
      std::cout << quad.Matrix() << endl;
      std::cout << "diff" << endl;
      std::cout << mono6.Matrix().cwiseQuotient(quad.Matrix()) << endl;
    }
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(large_l_test) {
  libint2::initialize();
  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/C2.xyz");

  BasisSet basisset;
  basisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/G.xml");
  AOBasis dftbasis;
  dftbasis.Fill(basisset, mol);

  Index dftbasissize = 18;

  StaticSegment seg2("", 0);
  seg2.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                    "/aopotential/polarsite_all.mps");

  std::vector<std::unique_ptr<StaticSite> > externalsites2;
  for (const StaticSite& site : seg2) {
    externalsites2.push_back(std::make_unique<StaticSite>(site));
  }

  AOMultipole esp;
  esp.FillPotential(dftbasis, externalsites2);
  Eigen::MatrixXd esp_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/esp_ref_l.mm");
  bool check_esp = esp.Matrix().isApprox(esp_ref, 0.00001);

  BOOST_CHECK_EQUAL(check_esp, 1);
  if (!check_esp) {
    cout << "esp ref" << endl;
    cout << esp_ref << endl;
    cout << "esp result" << endl;
    cout << esp.Matrix() << endl;
  }

  ECPBasisSet ecps;
  ecps.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/ecp.xml");
  ECPAOBasis ecpbasis;
  ecpbasis.Fill(ecps, mol);
  AOECP ecp;
  ecp.FillPotential(dftbasis, ecpbasis);

  Eigen::MatrixXd ecp_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/ecp_ref_l.mm");
  bool check_ecp = ecp.Matrix().isApprox(ecp_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_ecp, 1);
  if (!check_ecp) {
    cout << "ecp ref" << endl;
    cout << ecp_ref << endl;
    cout << "ecp result" << endl;
    cout << ecp.Matrix() << endl;
  }

  AOPlanewave planewave;
  std::vector<Eigen::Vector3d> kpoints = {
      {1, 1, 1}, {2, 1, 1}, {-1, -1, -1}, {-2, -1, -1}};
  Eigen::MatrixXd planewave_ref =
      Eigen::MatrixXd::Zero(dftbasissize, dftbasissize);
  planewave_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aopotential/planewave_ref_l.mm");

  planewave.FillPotential(dftbasis, kpoints);
  bool planewave_check =
      planewave_ref.isApprox(planewave.Matrix().real(), 1e-4);
  BOOST_CHECK_EQUAL(planewave_check, 1);
  if (!planewave_check) {
    std::cout << "planewave Ref real" << endl;
    std::cout << planewave_ref << endl;
    std::cout << "planewave real" << endl;
    std::cout << planewave.Matrix().real() << endl;
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_SUITE_END()