#include <Eigen/Core>
#include <Eigen/Geometry>

#include <libshm/c/shm.h>

#include "passive.hpp"
#include "world.hpp"
#include "engine.hpp"
#include "force.hpp"

namespace cuauv {
namespace fishbowl {

double clamp(double x, double a, double b) {
    return std::min(std::max(x, a), b);
}

Eigen::Vector3d clamp(const Eigen::Vector3d& x, const Eigen::Vector3d& a, const Eigen::Vector3d& b)
{
    return Eigen::Vector3d(clamp(x[0], a[0], b[0]), clamp(x[1], a[1], b[1]), clamp(x[2], a[2], b[2]));
}

gravity::gravity(world& w)
    : w(w)
{
}

screw gravity::on(entity_id id)
{
    return screw(Eigen::Vector3d(0, 0, w.get_entity(id).get_m() * 9.81), Eigen::Vector3d(0, 0, 0));
}

void gravity::step(double delta)
{
}

turbulence::turbulence(const Eigen::Vector3d& fa, const Eigen::Vector3d& fb, std::uniform_real_distribution<double> fwd, const Eigen::Vector3d& ta, const Eigen::Vector3d& tb, std::uniform_real_distribution<double> twd)
    : fa(fa)
    , fb(fb)
    , fwd(fwd)
    , ta(ta)
    , tb(tb)
    , twd(twd)
    , f(0, 0, 0)
    , t(0, 0, 0)
{
}

screw turbulence::on(entity_id id)
{
    return screw(f, t);
}

void turbulence::step(double delta)
{
    f += Eigen::Vector3d(fwd(gen), fwd(gen), fwd(gen)) * delta;
    t += Eigen::Vector3d(twd(gen), twd(gen), twd(gen)) * delta;
    f = clamp(f, fa, fb);
    t = clamp(t, ta, tb);
}

buoyancy::buoyancy(const Eigen::Quaterniond& q, double s, const Eigen::Vector3d& x)
    : q(q)
    , s(s)
    , x(x)
{
}

screw buoyancy::on()
{
    const Eigen::Vector3d f = q.conjugate() * Eigen::Vector3d(0, 0, -s);
    const Eigen::Vector3d t(x.cross(f));

    return screw(f, t);
}

void buoyancy::step(double delta)
{
}

drag::drag(const Eigen::Quaterniond& q, const Eigen::Vector3d& v, const Eigen::Vector3d& w, const Eigen::Vector3d& x, const Eigen::Vector3d& n, double c, double a)
    : q(q)
    , v(v)
    , w(w)
    , x(x)
    , n(n)
    , t(x.cross(n))
    , c(c)
    , a(a)
{
}

screw drag::on()
{
    const double vs = (q.conjugate() * v + w.cross(x)).dot(n);
    // Fd = .5 * p * v^2 * c * a
    // assume p (mass density) of pool water is 1 g/cm^3
    // though who knows what might be in there...
    // the force should be opposite the direction of velocity
    double f = (vs > 0 ? -1 : 1) * 0.5 * 1000 * pow(vs, 2) * c * a;
    return screw(n * f, t * f);
}

void drag::step(double delta)
{
}

} // namespace fishbowl
} // namespace cuauv


/**
 * @file sh_astar.cpp
 * @author Licheng Wen (wenlc@zju.edu.cn)
 * @brief The implement of Spatiotemporal Hybrid-State Astar for single_agent
 * @date 2020-11-12
 *
 * @copyright Copyright (c) 2020
 *
 */
#include <math.h>
#include <ompl/base/State.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <boost/functional/hash.hpp>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <iostream>
typedef ompl::base::SE2StateSpace::StateType OmplState;

#include "hybrid_astar.hpp"
#include "timer.hpp"

using libMultiRobotPlanning::HybridAStar;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;
using namespace libMultiRobotPlanning;

namespace Constants {
// [m] --- The minimum turning radius of the vehicle
static const float r = 3;
static const float deltat = 6.75 / 180.0 * M_PI;
// [#] --- A movement cost penalty for turning (choosing non straight motion
// primitives)
static const float penaltyTurning = 1.3;
// [#] --- A movement cost penalty for reversing (choosing motion primitives >
// 2)
static const float penaltyReversing = 2.0;
// [#] --- A movement cost penalty for change of direction (changing from
// primitives < 3 to primitives > 2)
static const float penaltyCOD = 2.0;
// map resolution
static const float mapResolution = 2.0;
static const float xyResolution = r * deltat;
static const float yawResolution = deltat;

// width of car
static const float carWidth = 2.0;
// distance from rear to vehicle front end
static const float LF = 2.0;
// distance from rear to vehicle back end
static const float LB = 1.0;
// obstacle default radius
static const float obsRadius = 1;

// R = 3, 6.75 DEG
const double dx[] = {r * deltat, r* sin(deltat),  r* sin(deltat),
                     -r* deltat, -r* sin(deltat), -r* sin(deltat)};
const double dy[] = {0, -r*(1 - cos(deltat)), r*(1 - cos(deltat)),
                     0, -r*(1 - cos(deltat)), r*(1 - cos(deltat))};
const double dyaw[] = {0, deltat, -deltat, 0, -deltat, deltat};

static inline float normalizeHeadingRad(float t) {
  if (t < 0) {
    t = t - 2.f * M_PI * (int)(t / (2.f * M_PI));
    return 2.f * M_PI + t;
  }

  return t - 2.f * M_PI * (int)(t / (2.f * M_PI));
}
}  // namespace Constants

struct State {
  State(double x, double y, double yaw) : x(x), y(y), yaw(yaw) {}
  State(const State&) = default;
  State(State&&) = default;
  State& operator=(const State&) = default;
  State& operator=(State&&) = default;

  bool operator==(const State& other) const {
    return std::tie(x, y, yaw) == std::tie(other.x, other.y, other.yaw);
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ":" << s.yaw << ")";
  }

  double x;
  double y;
  double yaw;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.yaw);
    return seed;
  }
};
}  // namespace std

using Action = int;  // Action < 6

class Environment {
 public:
  Environment(size_t maxx, size_t maxy, std::unordered_set<State> obstacles,
              State goal)
      : m_obstacles(std::move(obstacles)),
        m_goal(goal)  // NOLINT
  {
    m_dimx = (int)maxx / Constants::mapResolution;
    m_dimy = (int)maxy / Constants::mapResolution;
    // std::cout << "env build " << m_dimx << " " << m_dimy << " "
    //           << m_obstacles.size() << std::endl;
    holonomic_cost_map = std::vector<std::vector<double>>(
        m_dimx, std::vector<double>(m_dimy, 0));
    m_goal = State(goal.x, goal.y, Constants::normalizeHeadingRad(goal.yaw));
    updateCostmap();
  }

  struct compare_node {
    bool operator()(const std::pair<State, double>& n1,
                    const std::pair<State, double>& n2) const {
      return (n1.second > n2.second);
    }
  };

  uint64_t calcIndex(const State& s) {
    return (uint64_t)(Constants::normalizeHeadingRad(s.yaw) /
                      Constants::yawResolution) *
               (m_dimx * Constants::mapResolution / Constants::xyResolution) *
               (m_dimy * Constants::mapResolution / Constants::xyResolution) +
           (uint64_t)(s.y / Constants::xyResolution) *
               (m_dimx * Constants::mapResolution / Constants::xyResolution) +
           (uint64_t)(s.x / Constants::xyResolution);
  }

  double admissibleHeuristic(const State& s) {
    // non-holonomic-without-obstacles heuristic: use a Reeds-Shepp
    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);
    OmplState* rsStart = (OmplState*)reedsSheppPath.allocState();
    OmplState* rsEnd = (OmplState*)reedsSheppPath.allocState();
    rsStart->setXY(s.x, s.y);
    rsStart->setYaw(s.yaw);
    rsEnd->setXY(m_goal.x, m_goal.y);
    rsEnd->setYaw(m_goal.yaw);
    double reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);
    // std::cout << "ReedsShepps cost:" << reedsSheppCost << std::endl;
    // Euclidean distance
    double euclideanCost =
        sqrt(pow(m_goal.x - s.x, 2) + pow(m_goal.y - s.y, 2));
    // std::cout << "Euclidean cost:" << euclideanCost << std::endl;
    // holonomic-with-obstacles heuristic
    double twoDoffset =
        sqrt(pow((s.x - (int)s.x) - (m_goal.x - (int)m_goal.x), 2) +
             pow((s.y - (int)s.y) - (m_goal.y - (int)m_goal.y), 2));
    double twoDCost = holonomic_cost_map[(int)s.x / Constants::mapResolution]
                                        [(int)s.y / Constants::mapResolution] -
                      twoDoffset;
    // std::cout << "holonomic cost:" << twoDCost << std::endl;

    return std::max({reedsSheppCost, euclideanCost, twoDCost});
  }

  bool isSolution(
      const State& state, double gscore,
      std::unordered_map<State, std::tuple<State, Action, double, double>,
                         std::hash<State>>& _camefrom) {
    double goal_distance =
        sqrt(pow(state.x - m_goal.x, 2) + pow(state.y - m_goal.y, 2));
    if (goal_distance > 2 * (Constants::LB + Constants::LF)) return false;

    ompl::base::ReedsSheppStateSpace reedsSheppSpace(Constants::r);
    OmplState* rsStart = (OmplState*)reedsSheppSpace.allocState();
    OmplState* rsEnd = (OmplState*)reedsSheppSpace.allocState();
    rsStart->setXY(state.x, state.y);
    rsStart->setYaw(-state.yaw);
    rsEnd->setXY(m_goal.x, m_goal.y);
    rsEnd->setYaw(-m_goal.yaw);
    ompl::base::ReedsSheppStateSpace::ReedsSheppPath reedsShepppath =
        reedsSheppSpace.reedsShepp(rsStart, rsEnd);

    std::vector<State> path;
    std::unordered_map<State, std::tuple<State, Action, double, double>,
                       std::hash<State>>
        cameFrom;
    cameFrom.clear();
    path.emplace_back(state);
    for (auto pathidx = 0; pathidx < 5; pathidx++) {
      if (fabs(reedsShepppath.length_[pathidx]) < 1e-6) continue;
      double deltat, dx, act, cost;
      switch (reedsShepppath.type_[pathidx]) {
        case 0:  // RS_NOP
          continue;
          break;
        case 1:  // RS_LEFT
          deltat = -reedsShepppath.length_[pathidx];
          dx = Constants::r * sin(-deltat);
          // dy = Constants::r * (1 - cos(-deltat));
          act = 2;
          cost = reedsShepppath.length_[pathidx] * Constants::r *
                 Constants::penaltyTurning;
          break;
        case 2:  // RS_STRAIGHT
          deltat = 0;
          dx = reedsShepppath.length_[pathidx] * Constants::r;
          // dy = 0;
          act = 0;
          cost = dx;
          break;
        case 3:  // RS_RIGHT
          deltat = reedsShepppath.length_[pathidx];
          dx = Constants::r * sin(deltat);
          // dy = -Constants::r * (1 - cos(deltat));
          act = 1;
          cost = reedsShepppath.length_[pathidx] * Constants::r *
                 Constants::penaltyTurning;
          break;
        default:
          std::cout << "\033[1m\033[31m"
                    << "Warning: Receive unknown ReedsSheppPath type"
                    << "\033[0m\n";
          break;
      }
      if (cost < 0) {
        cost = -cost * Constants::penaltyReversing;
        act = act + 3;
      }
      State s = path.back();
      std::vector<std::pair<State, double>> next_path =
          generatePath(s, act, deltat, dx);
      // State next_s(s.x + dx * cos(-s.yaw) - dy * sin(-s.yaw),
      //              s.y + dx * sin(-s.yaw) + dy * cos(-s.yaw),
      //              Constants::normalizeHeadingRad(s.yaw + deltat));
      for (auto iter = next_path.begin(); iter != next_path.end(); iter++) {
        State next_s = iter->first;
        if (!stateValid(next_s))
          return false;
        else {
          gscore += iter->second;
          if (!(next_s == path.back())) {
            cameFrom.insert(std::make_pair<>(
                next_s,
                std::make_tuple<>(path.back(), act, iter->second, gscore)));
          }
          path.emplace_back(next_s);
        }
      }
    }

    m_goal = path.back();
    // auto iter = cameFrom.find(getGoal());
    // do {
    //   std::cout << " From " << std::get<0>(iter->second)
    //             << " to Node:" << iter->first
    //             << " with ACTION: " << std::get<1>(iter->second) << " cost "
    //             << std::get<2>(iter->second) << " g_score "
    //             << std::get<3>(iter->second) << std::endl;
    //   iter = cameFrom.find(std::get<0>(iter->second));
    // } while (calcIndex(std::get<0>(iter->second)) != calcIndex(state));
    // std::cout << " From " << std::get<0>(iter->second)
    //           << " to Node:" << iter->first
    //           << " with ACTION: " << std::get<1>(iter->second) << " cost "
    //           << std::get<2>(iter->second) << " g_score "
    //           << std::get<3>(iter->second) << std::endl;

    _camefrom.insert(cameFrom.begin(), cameFrom.end());
    return true;
  }

  void getNeighbors(const State& s, Action action,
                    std::vector<Neighbor<State, Action, double>>& neighbors) {
    neighbors.clear();
    for (Action act = 0; act < 6; act++) {  // has 6 directions for Reeds-Shepp
      double xSucc, ySucc, yawSucc;
      double g = Constants::dx[0];
      xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
              Constants::dy[act] * sin(-s.yaw);
      ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
              Constants::dy[act] * cos(-s.yaw);
      yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
      if (act != action) {  // penalize turning
        g = g * Constants::penaltyTurning;
        if (act >= 3)  // penalize change of direction
          g = g * Constants::penaltyCOD;
      }
      if (act > 3) {  // backwards
        g = g * Constants::penaltyReversing;
      }
      State tempState(xSucc, ySucc, yawSucc);
      if (stateValid(tempState)) {
        neighbors.emplace_back(
            Neighbor<State, Action, double>(tempState, act, g));
      }
    }
  }

  void onExpandNode(const State& s, int /*fScore*/, int /*gScore*/) {
    Ecount++;
    // std::cout << "Expand " << Ecount << " new Node:" << s << std::endl;
  }

  void onDiscover(const State& s, double fScore, double gScore) {
    Dcount++;
    // std::cout << "Discover " << Dcount << "  Node:" << s << " f:" << fScore
    //           << " g:" << gScore << std::endl;
  }

 public:
  State getGoal() { return m_goal; }
  int Ecount = 0;
  int Dcount = 0;

 private:
  bool stateValid(const State& s) {
    double x_ind = s.x / Constants::mapResolution;
    double y_ind = s.y / Constants::mapResolution;
    if (x_ind < 0 || x_ind >= m_dimx || y_ind < 0 || y_ind >= m_dimy)
      return false;

    Eigen::Matrix2f rot;
    rot << cos(-s.yaw), -sin(-s.yaw), sin(-s.yaw), cos(-s.yaw);
    for (auto it = m_obstacles.begin(); it != m_obstacles.end(); it++) {
      Eigen::Matrix<float, 1, 2> obs;
      obs << it->x - s.x, it->y - s.y;
      auto rotated_obs = obs * rot;
      if (rotated_obs(0) > -Constants::LB - Constants::obsRadius &&
          rotated_obs(0) < Constants::LF + Constants::obsRadius &&
          rotated_obs(1) > -Constants::carWidth / 2.0 - Constants::obsRadius &&
          rotated_obs(1) < Constants::carWidth / 2.0 + Constants::obsRadius)
        return false;
    }
    return true;
    // Eigen::Matrix2f rot;
    // double yaw = M_PI / 2;
    // rot << cos(yaw), -sin(yaw), sin(yaw), cos(yaw);
    // Eigen::Matrix<float, 1, 2> temp;
    // temp << 1, 2;
    // auto ro = temp * rot;
    // std::cout << ro(0) << ro(1) << std::endl;
  }

  void updateCostmap() {
    boost::heap::fibonacci_heap<std::pair<State, double>,
                                boost::heap::compare<compare_node>>
        heap;
    heap.clear();

    std::set<std::pair<int, int>> temp_obs_set;
    for (auto it = m_obstacles.begin(); it != m_obstacles.end(); it++) {
      temp_obs_set.insert(
          std::make_pair((int)it->x / Constants::mapResolution,
                         (int)it->y / Constants::mapResolution));
    }

    int goal_x = (int)m_goal.x / Constants::mapResolution;
    int goal_y = (int)m_goal.y / Constants::mapResolution;
    heap.push(std::make_pair(State(goal_x, goal_y, 0), 0));

    while (!heap.empty()) {
      std::pair<State, double> node = heap.top();
      heap.pop();

      int x = node.first.x;
      int y = node.first.y;
      for (int dx = -1; dx <= 1; dx++)
        for (int dy = -1; dy <= 1; dy++) {
          if (dx == 0 && dy == 0) continue;
          int new_x = x + dx;
          int new_y = y + dy;
          if (new_x == goal_x && new_y == goal_y) continue;
          if (new_x >= 0 && new_x < m_dimx && new_y >= 0 && new_y < m_dimy &&
              holonomic_cost_map[new_x][new_y] == 0 &&
              temp_obs_set.find(std::make_pair(new_x, new_y)) ==
                  temp_obs_set.end()) {
            holonomic_cost_map[new_x][new_y] =
                holonomic_cost_map[x][y] +
                sqrt(pow(dx * Constants::mapResolution, 2) +
                     pow(dy * Constants::mapResolution, 2));
            heap.push(std::make_pair(State(new_x, new_y, 0),
                                     holonomic_cost_map[new_x][new_y]));
          }
        }
    }

    // for (size_t i = 0; i < m_dimx; i++) {
    //   for (size_t j = 0; j < m_dimy; j++)
    //     std::cout << holonomic_cost_map[i][j] << "\t";
    //   std::cout << std::endl;
    // }
  }

  std::vector<std::pair<State, double>> generatePath(State startState, int act,
                                                     double deltaSteer,
                                                     double deltaLength) {
    std::vector<std::pair<State, double>> result;
    double xSucc, ySucc, yawSucc, dx, dy, dyaw, ratio;
    result.emplace_back(std::make_pair<>(startState, 0));
    if (act == 0 || act == 3) {
      for (size_t i = 0; i < (size_t)(deltaLength / Constants::dx[act]); i++) {
        State s = result.back().first;
        xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
                Constants::dy[act] * sin(-s.yaw);
        ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
                Constants::dy[act] * cos(-s.yaw);
        yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
        result.emplace_back(
            std::make_pair<>(State(xSucc, ySucc, yawSucc), Constants::dx[0]));
      }
      ratio = (deltaLength -
               (int)(deltaLength / Constants::dx[act]) * Constants::dx[act]) /
              Constants::dx[act];
      dyaw = 0;
      dx = ratio * Constants::dx[act];
      dy = 0;
    } else {
      for (size_t i = 0; i < (size_t)(deltaSteer / Constants::dyaw[act]); i++) {
        State s = result.back().first;
        xSucc = s.x + Constants::dx[act] * cos(-s.yaw) -
                Constants::dy[act] * sin(-s.yaw);
        ySucc = s.y + Constants::dx[act] * sin(-s.yaw) +
                Constants::dy[act] * cos(-s.yaw);
        yawSucc = Constants::normalizeHeadingRad(s.yaw + Constants::dyaw[act]);
        result.emplace_back(
            std::make_pair<>(State(xSucc, ySucc, yawSucc),
                             Constants::dx[0] * Constants::penaltyTurning));
      }
      ratio =
          (deltaSteer -
           (int)(deltaSteer / Constants::dyaw[act]) * Constants::dyaw[act]) /
          Constants::dyaw[act];
      dyaw = ratio * Constants::dyaw[act];
      dx = Constants::r * sin(dyaw);
      dy = -Constants::r * (1 - cos(dyaw));
      if (act == 2 || act == 5) {
        dx = -dx;
        dy = -dy;
      }
    }
    State s = result.back().first;
    xSucc = s.x + dx * cos(-s.yaw) - dy * sin(-s.yaw);
    ySucc = s.y + dx * sin(-s.yaw) + dy * cos(-s.yaw);
    yawSucc = Constants::normalizeHeadingRad(s.yaw + dyaw);
    result.emplace_back(std::make_pair<>(State(xSucc, ySucc, yawSucc),
                                         ratio * Constants::dx[0]));
    // std::cout << "Have generate " << result.size() << " path segments:\n\t";
    // for (auto iter = result.begin(); iter != result.end(); iter++)
    //   std::cout << iter->first << ":" << iter->second << "->";
    // std::cout << std::endl;

    return result;
  }

  int m_dimx;
  int m_dimy;
  std::unordered_set<State> m_obstacles;
  std::vector<std::vector<double>> holonomic_cost_map;
  State m_goal;
};

int main() {
  // TODO: read map info from yaml
  std::unordered_set<State> obs;
  obs.insert(State(6.66799, 9.66868, 0));
  obs.insert(State(6.86099, 6.20241, 0));
  obs.insert(State(6.2493, 3.45836, 0));
  obs.insert(State(6.93504, 0.747862, 0));
  obs.insert(State(6.81566, 4.75936, 0));
  State goal(13, 10, -M_PI);
  State start(2, 2, 0);
  Environment env(16, 16, obs, goal);
  HybridAStar<State, Action, double, Environment> hybridAStar(env);
  PlanResult<State, Action, double> solution;
  Timer timer;
  bool searchSuccess = hybridAStar.search(start, solution);
  timer.stop();

  std::string outputFile = "output_h.yaml";
  std::ofstream out(outputFile);
  if (searchSuccess) {
    std::cout << "\033[1m\033[32m Succesfully find a path! \033[0m\n";
    std::cout << "Solution get " << solution.states.size() << " states "
              << solution.actions.size() << " moves\n"
              << "Runtime: " << timer.elapsedSeconds() << std::endl;

    out << "schedule:" << std::endl;
    out << "  agent1:" << std::endl;
    for (size_t i = 0; i < solution.states.size(); ++i) {
      out << "    - x: " << solution.states[i].first.x << std::endl
          << "      y: " << solution.states[i].first.y << std::endl
          << "      yaw: " << solution.states[i].first.yaw << std::endl
          << "      t: " << i << std::endl;
    }
    // for (auto iter = solution.states.begin(); iter != solution.states.end();
    //      iter++)
    //   std::cout << iter->first << ":" << iter->second << "->";
    // std::cout << std::endl;
    // for (auto iter = solution.actions.begin(); iter !=
    // solution.actions.end();
    //      iter++)
    //   std::cout << iter->first << ":" << iter->second << "->";
    // std::cout << std::endl;
    std::cout << "Solution: gscore/cost:" << solution.cost
              << "\t fmin:" << solution.fmin << "\n\rDiscover " << env.Dcount
              << " Nodes and Expand " << env.Ecount << " Nodes." << std::endl;
  } else {
    std::cout << "\033[1m\033[31m Fail to find a path \033[0m\n";
  }
}


#include <iostream>
#include <vector>
#include <boost/ref.hpp>
#include <boost/utility.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/def.hpp>
#include <boost/python/pure_virtual.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/operators.hpp>
#include <boost/python/numpy.hpp>
#include <boost/python/list.hpp>
#include <exception>
#define private public

using namespace boost;
using namespace boost::python;
using namespace boost::python::numpy;

#include "cwt1d_wavelets.hpp"
using namespace std;
using namespace cwt1d;

typedef dog<double> pydog;
typedef morlet<double> pymorlet;
typedef paul<double> pypaul;
typedef wavelet_func<double> wf;

namespace
{
  class initializer{
  public:
    initializer()
    {
      //boost::python::numeric::array::set_module_and_type("numpy","ndarray");
      Py_Initialize();
      numpy::initialize();
    }
  }_init;
}

boost::python::numpy::ndarray pycwt(const boost::python::numpy::ndarray& x,const boost::python::numpy::ndarray& s,const wavelet_func<double>& wf)
{
  boost::python::object shape(x.attr("shape"));
  int ndim=extract<int>(shape.attr("__len__")());
  if(ndim!=1)
    {
      throw std::exception();
    }
  int ndata=extract<int>(shape[0]);

  blitz::Array<double,1> x1(ndata);
  for(int i=0;i<ndata;++i)
    {
      x1(i)=extract<double>(x[i]);
    }
  shape=s.attr("shape");
  ndim=extract<int>(shape.attr("__len__")());
  if(ndim!=1)
    {
      throw std::exception();
    }
  int nscales=extract<int>(shape[0]);
  blitz::Array<double,1> s1(nscales);

  for(int i=0;i<nscales;++i)
    {
      s1(i)=extract<double>(s[i]);
    }

  blitz::Array<complex<double>,2> y(cwt(x1,s1,wf));
  boost::python::list l;
  for(int i=0;i<nscales;++i)
    {
      boost::python::list l1;
      for(int j=0;j<ndata;++j)
	{
	  l1.append(y(i,j));
	}
      l.append(l1);
    }

  return boost::python::numpy::array(l);

}

boost::python::numpy::ndarray pyicwt(const boost::python::numpy::ndarray& x,const boost::python::numpy::ndarray& s,const wavelet_func<double>& wf)
{
  boost::python::object shape(x.attr("shape"));
  int ndim=extract<int>(shape.attr("__len__")());
  if(ndim!=2)
    {
      throw std::exception();
    }
  int ndata=extract<int>(shape[1]);
  int nscales=extract<int>(shape[0]);

  blitz::Array<complex<double>,2> x1(nscales,ndata);
  for(int i=0;i<nscales;++i)
    {
      for(int j=0;j<ndata;++j)
	{
	  x1(i,j)=extract<complex<double> >(x[boost::python::make_tuple(i,j)]);
	}
    }
  shape=s.attr("shape");
  ndim=extract<int>(shape.attr("__len__")());
  if(ndim!=1)
    {
      throw std::exception();
    }
  if(nscales!=extract<int>(shape[0]))
    {
      throw std::exception();
    }
  blitz::Array<double,1> s1(nscales);

  for(int i=0;i<nscales;++i)
    {
      s1(i)=extract<double>(s[i]);
    }
  blitz::Array<double,1> result1(icwt(x1,s1,wf));
  boost::python::list l;
  for(int i=0;i<ndata;++i)
    {
      l.append(result1(i));
    }
  return boost::python::numpy::array(l);
  //return result;
}

boost::python::numpy::ndarray generate_log_scales(double min_scale,double max_scale,int num_scales)
{
  boost::python::list l;
  double lmin_scale=log(min_scale);
  double lmax_scale=log(max_scale);
  for(int i=0;i<num_scales;++i)
    {
      double s=exp(lmin_scale+(lmax_scale-lmin_scale)/(num_scales-1)*i);
      l.append(s);
    }
  return boost::python::numpy::array(l);
  //  return result;
}

double pycalc_norm(int dl,const boost::python::numpy::ndarray& s,const wavelet_func<double>& wf)
{
  boost::python::object shape(s.attr("shape"));
  int ndim=extract<int>(shape.attr("__len__")());
  if(ndim!=1)
    {
      throw std::exception();
    }
  int nscales=extract<int>(shape[0]);
  blitz::Array<double,1> s1(nscales);
  for(int i=0;i<nscales;++i)
    {
      s1(i)=extract<double>(s[i]);
    }
  return calc_norm(dl,s1,wf);
}

BOOST_PYTHON_MODULE(cwtcore)
{
  class_<wf,boost::noncopyable>("wf",no_init)
    .def("wavelet_f",&wf::wavelet_f);

  class_<pydog,bases<wf> >("dog")
    .def(init<>())
    .def(init<int>());

  class_<pymorlet,bases<wf> >("morlet")
    .def(init<>())
    .def(init<double>());

  class_<pypaul,bases<wf> >("paul")
    .def(init<>())
    .def(init<double>());

  def("cwt",pycwt);
  def("icwt",pyicwt);
  def("calc_norm",pycalc_norm);
  def("generate_log_scales",generate_log_scales);
}


#pragma once
#include "shared.h"
#include <boost/numeric/odeint.hpp>

using namespace boost::numeric::odeint;
typedef std::vector<double> state_type;
typedef runge_kutta_cash_karp54< state_type > error_stepper_type;

class Elevator
{
private:
  bool Initialize = false;
  bool FixedEarthBoundary = false;

  const uint32_t
    Dimensions = 3;

  uint32_t
    ChainSize = 0,
    TotalSteps = 0,
    SavedSteps = 0;

  double
    L0 = 0.0,
    GM = 0.0,
    T0 = 0.0,
    Tf = 0.0,
    Dt = 0.0,
    WPlanet = 0.0,
    RSurface = 0.0,
    FrictA = 0.0,
    FrictB = 0.0,
    FrictC = 0.0,
    * AnchorAngles = nullptr,
    * SprK = nullptr,
    * RotK = nullptr,
    * Inertia = nullptr,
    * Mass = nullptr;

  state_type * State = nullptr;

  std::vector<double*> StoredStates;

  void __RHS(state_type &y, state_type &dy, double t);
  double __Distance(const state_type &y, uint32_t i, uint32_t j);
  double __Modulus2(const state_type &y, uint32_t i);
  double __Alpha(const state_type &y, uint32_t i);

  void __EarthFixedBoundary(state_type &y, state_type &dy, const double t);
  void __EarthFreeBoundary(state_type &y, state_type &dy, const double t);
  void __SpaceFreeBoundary(state_type &y, state_type &dy, const double t);

  void __StoreState(const state_type &y, const double t);

public:
  Elevator() {}
  ~Elevator() {
    delete[] SprK;
    delete[] RotK;
    delete[] Inertia;
    delete[] Mass;
    delete[] AnchorAngles;
    delete State;
  }

  virtual void InitializeFromFile(char const * filename);
  void LoadSystem(char const * filename);
  void SaveSystem(char const * filename);

  void Integrate();
};


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2012 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if defined(USING_NUMARRAY)
#	define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandle
#	define NO_IMPORT_ARRAY
#endif

#include <numeric>
#include <functional>
#include <cmath>
#include <boost/lambda/lambda.hpp>

#include "tree_length_distribution.hpp"
#include "dirichlet_distribution.hpp"

#include <fstream>  //temporary!
#include <iterator>  //temporary!

namespace phycas
{

/*----------------------------------------------------------------------------------------------------------------------
|	By default, let the tree length distribution be exponential with mean 10 and the edge length distribution
|   conditional on tree length be flat.
*/
TreeLengthDistribution::TreeLengthDistribution()
  : _alphaT(1.0), _betaT(0.1), _alpha(1.0), _c(1.0), _lot(&_myLot), _num_internal_edges(0), _num_external_edges(0)
	{
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a TreeLengthDistribution based on the parameter values supplied.
*/
TreeLengthDistribution::TreeLengthDistribution(
  double alphaT,     /**< is the shape parameter of the gamma distribution of tree length (mean = shape/scale) */
  double betaT,      /**< is the scale parameter of the gamma distribution of tree length (mean = shape/scale) */
  double alpha,      /**< is the Dirichlet parameter governing external edge length */
  double c)          /**< is the Dirichlet parameter governing internal edge length */
  :  _alphaT(alphaT), _betaT(betaT), _alpha(alpha), _c(c), _lot(&_myLot), _num_internal_edges(0), _num_external_edges(0)
	{
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Initializes parameters to the values contained in their counterparts in `other'.
*/
TreeLengthDistribution::TreeLengthDistribution(
  const TreeLengthDistribution & other)	/* the tree length distribution to clone */
  : _alphaT(other._alphaT), _betaT(other._betaT), _alpha(other._alpha), _c(other._c), _lot(other._lot), _num_internal_edges(0), _num_external_edges(0)
	{
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns the string "TreeLengthDistribution".
*/
std::string TreeLengthDistribution::GetDistributionName() const
	{
	return "TreeLengthDistribution";
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns a string similar to "TreeLengthDistribution(1.0,0.1,1.0,1.0)".
*/
std::string TreeLengthDistribution::GetDistributionDescription() const
	{
	return boost::str(boost::format("TreeLengthDistribution(%#.5f,%#.5f,%#.5f,%#.5f)") % _alphaT % _betaT % _alpha % _c);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates two probability distributions that can be used to sample from this tree length distribution. The first is
|   a Gamma distribution stored in the data member `_tldist' and the second is a Dirichlet distribution stored in the
|   data member `_eldist'. The edge length distribution depends on the number of internal and external edges, so this
|   function must be called whenever either of these numbers changes. The number of internal and external edges last
|   used is stored in `_num_internal_edges' and `_num_external_edges', respectively, to avoid unnecessary
|   construction/destruction.
*/
void TreeLengthDistribution::SetupSamplingDistributions(
  unsigned num_external, /**< is the number of external edges */
  unsigned num_internal) /**< is the number of internal edges */
	{
    unsigned num_total = num_external + num_internal;
    PHYCAS_ASSERT(num_total > 0);
    if (num_external == _num_external_edges && num_internal == _num_internal_edges && _tldist && _eldist)
        {
        return;
        }

    //std::cerr << "Creating _tldist and _eldist for " << num_internal << " internal and " << num_external << " external edges" << std::endl;

    double_vect_t dirichlet_params(num_total, _alpha);

    //std::cerr << "\n\n***** dirichlet_params before *****" << std::endl;
    //std::copy(dirichlet_params.begin(), dirichlet_params.end(), std::ostream_iterator<double>(std::cerr, "|"));
    //std::cerr << "\n\n" << std::endl;

    std::transform(dirichlet_params.begin()+num_external, dirichlet_params.end(), dirichlet_params.begin()+num_external, boost::lambda::_1*_c);

    //std::cerr << "\n\n***** dirichlet_params after *****" << std::endl;
    //std::copy(dirichlet_params.begin(), dirichlet_params.end(), std::ostream_iterator<double>(std::cerr, "|"));
    //std::cerr << "\n\n" << std::endl;

    _tldist.reset(new GammaDistribution(_alphaT, 1.0/_betaT));   // Rannala-Zhu-Yang paper defines Gamma mean = shape/scale, which contrasts with other parts of phycas where Gamma mean = shape*scale
    _tldist->SetLot(_lot);

    _eldist .reset(new DirichletDistribution(dirichlet_params));
    _eldist->SetLot(_lot);

    _num_internal_edges = num_internal;
    _num_external_edges = num_external;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Samples and returns a point from this distribution. In the returned point, the `num_external' external edge lengths
|   will be first, followed by the `num_internal' internal edge lengths.
*/
double_vect_t TreeLengthDistribution::Sample(
  unsigned num_external, /**< is the number of external edges */
  unsigned num_internal) /**< is the number of internal edges */
	{
    SetupSamplingDistributions(num_external, num_internal);

    double tree_length = _tldist->Sample();
    double_vect_t edge_lengths = _eldist->Sample();
    std::transform(edge_lengths.begin(), edge_lengths.end(), edge_lengths.begin(), boost::lambda::_1*tree_length);

	return edge_lengths;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the natural logarithm of the probability density function evaluated for the edge lengths in the supplied
|   tree `t'. Assumes supplied tree `t' is unrooted. See Rannala, Zhu and Yang (2012) for details.
*/
double TreeLengthDistribution::GetLnPDF(
  TreeShPtr t) const 	/**< is the tree for which the density is to be evaulated */
	{
    PHYCAS_ASSERT(!t->IsRooted());

    double tree_length = 0.0;
    double sum_internal = 0.0;
    double sum_external = 0.0;
    unsigned num_total = 0;
    unsigned num_internal = 0;
    unsigned num_external = 0;
    for (preorder_iterator it = t->begin(); it != t->end(); ++it)
        {
        if (!it->IsAnyRoot())
            {
            double edge_length = it->GetEdgeLen();
            tree_length += edge_length;
            num_total++;
            if (it->IsTip() || it->IsSubroot())
                {
                num_external++;
                sum_external += (_alpha - 1.0)*log(edge_length);
                }
            else
                {
                num_internal++;
                sum_internal += (_alpha*_c - 1.0)*log(edge_length);
                }
            }
        }

    PHYCAS_ASSERT(num_total == num_internal + num_external);

    // logterm1 is first line of equation 36 in Rannala, Zhu, and Yang (2012)
    double logterm1 = _alphaT*log(_betaT) - _cdf.LnGamma(_alphaT) - _betaT*tree_length + (_alphaT - 1.0)*log(tree_length);

    // logterm2 is second line of equation 36 in Rannala, Zhu, and Yang (2012)
    double beta_term = _cdf.LnGamma(_alpha)*num_external + _cdf.LnGamma(_alpha*_c)*num_internal - _cdf.LnGamma(_alpha*num_external + _alpha*_c*num_internal);
    double logterm2 = sum_internal + sum_external - beta_term;

    // logterm3 is third line of equation 36 in Rannala, Zhu, and Yang (2012)
    double logterm3 = (1.0 - _alpha*num_external - _alpha*_c*num_internal)*log(tree_length);

    double lnPDF = logterm1 + logterm2 + logterm3;
    return lnPDF;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the natural logarithm of the probability density function evaluated for the edge lengths in the supplied
|   tree `t'. Assumes supplied tree `t' is unrooted. See Rannala, Zhu and Yang (2012) for details.
*/
double TreeLengthDistribution::GetRelativeLnPDF(
  TreeShPtr t) const  	/**< is the tree for which the density is to be evaulated */
	{
	return GetLnPDF(t);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a new object that is a clone of this object, calls the new object's SetLot member function (passing the
|	supplied Lot object `other'), and returns a pointer to it. The caller is expected to manage the new object.
*/
TreeLengthDistribution * TreeLengthDistribution::cloneAndSetLot(Lot * other) const
	{
    TreeLengthDistribution * clone = new TreeLengthDistribution(*this);
	clone->SetLot(other);
	return clone;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a new object that is a clone of this object and returns a pointer to it. Caller is expected to manage the
|   new object.
*/
TreeLengthDistribution * TreeLengthDistribution::Clone() const
	{
    return new TreeLengthDistribution(*this);
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Replaces the random number generator used with the TreeLengthDistribution::Sample member function. The original
|	random number generator	(data member `myLot') can be replaced by calling the ProbabilityDistribution::ResetLot
|	function. Note that this object does not take ownership of the Lot object whose pointer is specified as `other'.
|	It is assumed that `other' is non-NULL.
*/
void TreeLengthDistribution::SetLot(
	Lot * other) /**< is a pointer to the random number generator object to be used subsequently by Sample */
	{
	if (other == NULL)
		throw XProbDist("attempt made to install a non-existent pseudorandom number generator");
	_lot = other;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets the random number seed for the `myLot' data member. Note that if TreeLengthDistribution::SetLot has been
|	called, calling TreeLengthDistribution::SetSeed is pointless because you will not be setting the seed for the
|	correct random number generator!
*/
void TreeLengthDistribution::SetSeed(
  unsigned rnseed)	/**< is the new seed value */
	{
	_myLot.SetSeed(rnseed);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Makes the data member `_lot' (which is used as the random number generator by the member function
|	TreeLengthDistribution::Sample) point to the local data member _myLot. This function only needs to be called if
|	TreeLengthDistribution::SetLot has been called previously to replace the random number generator used by
|	TreeLengthDistribution::Sample.
*/
void TreeLengthDistribution::ResetLot()
	{
	_lot = &_myLot;
	}

} // namespace phycas


#ifndef _NCTX_PY_CNTRL_
#define _NCTX_PY_CNTRL_

#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/page_rank.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>

namespace nctx { namespace python {
  template<bool d>
  class CentralityDummy{};

  template <bool directed>
  inline void wrap_centralities(){
    using PMap = PropertyMapValueHolder<double_t,directed>;
    using GraphC = GraphContainer<directed>;
    using Vertex = typename GraphC::vertex;
    using Edge = typename GraphC::edge;

    py::class_<CentralityDummy<directed>>("AlgCentralities", py::no_init)
      //~ .def("page_rank_ctx", +[](GraphC& gc, py::object fct_decision, PMap& rank_map, double_t damping = 0.85, int n_iter = 20) {
        //~ std::function<double_t (Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<double_t, Vertex, Vertex>( fct_decision );
        //~ auto g = gc.get_graph();
        //~ page_rank_ctx(
          //~ g,
          //~ rank_map.get_map(),
          //~ lmbd_decision,
          //~ boost::graph::n_iterations(n_iter),
          //~ damping);
      //~ })
      //~ .def("page_rank_ctx", +[](GraphC& gc, py::object fct_decision, double_t damping = 0.85, int n_iter = 20) -> PMap {
        //~ std::function<double_t (Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<double_t, Vertex, Vertex>( fct_decision );
        //~ auto g = gc.get_graph();
        //~ PMap rank_map(gc, "pagerank_constraint");
        //~ page_rank_ctx(
          //~ g,
          //~ rank_map.get_map(),
          //~ lmbd_decision,
          //~ boost::graph::n_iterations(n_iter),
          //~ damping);
        //~ return rank_map;
      //~ })
      //~ .def("page_rank", +[](GraphC& gc, PMap& rank_map, double_t damping = 0.85, int n_iter = 20) {
        //~ auto g = gc.get_graph();
        //~ page_rank(
          //~ g,
          //~ rank_map.get_map(),
          //~ boost::graph::n_iterations(n_iter),
          //~ damping);
      //~ })
      //~ .def("page_rank", +[](GraphC& gc, double_t damping = 0.85, int n_iter = 20) -> PMap {
        //~ auto g = gc.get_graph();
        //~ PMap rank_map(gc, "pagerank");
        //~ page_rank(
          //~ g,
          //~ rank_map.get_map(),
          //~ boost::graph::n_iterations(n_iter),
          //~ damping);
        //~ return rank_map;
      //~ })
      .def("betweenness_ctx", +[](GraphC& gc, py::object fct_decision, PMap& centr_map) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        brandes_betweenness_centrality_ctx(
          g,
          centrality_map(centr_map.get_map())
          .vertex_index_map(gc.make_index_map())
          .weight_map(weight_map),
          lmbd_decision);
      },(py::arg("g"), py::arg("betw_decision_fct"), py::arg("outmap")), "Betweenness centrality with dynamic contextual constraints.\n\n\n\nUsing this function allows obtaining betweenness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the betweenness calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.\n\nObtaining betweenness centrality is based on Brandes' efficient algorithm. At the current stage, the implementation allows for single-core execution only.\n\n\n\nArgs:\n    g The graph object\n    betw_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.\n    outmap (PropertyMapDouble): map in which centrality values will be written\n\nExample:\n    >>> outmap = PropertyMapDouble(g, 'betw_ctx')\n    >>> AlgCentralities.betweenness_ctx(g,lambda _start,_current,_next: (True),outmap)\n\n\n")
      .def("betweenness_ctx", +[](GraphC& gc, py::object fct_decision) -> PMap {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        auto g = gc.get_graph();
        PMap centr_map(gc, "betweenness_constraint");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        brandes_betweenness_centrality_ctx(
          g,
          centrality_map(centr_map.get_map())
          .vertex_index_map(gc.make_index_map())
          .weight_map(weight_map),
          lmbd_decision);
        return centr_map;
      },(py::arg("g"), py::arg("betw_decision_fct")), "Betweenness centrality with dynamic contextual constraints.\n\n\n\nUsing this function allows obtaining betweenness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the betweenness calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.\n\nObtaining betweenness centrality is based on Brandes' efficient algorithm. At the current stage, the implementation allows for single-core execution only.\n\n\n\nArgs:\n    g The graph object\n    betw_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.\n\n\nReturns:\n    PropertyMapDouble: list of centrality values named betweenness_constraint\n\nExample:\n    >>> outmap = AlgCentralities.betweenness_ctx(g,lambda _start,_current,_next: (True))\n\n\n")
      .def("closeness_ctx", +[](GraphC& gc, py::object fct_decision, PMap& centr_map) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1);

        closeness_centrality_ctx(g,
            b::weight_map(weight_map)
            .centrality_map(centr_map.get_map())
            .vertex_index_map(gc.make_index_map()),
            lmbd_decision);
      },(py::arg("g"), py::arg("clsn_decision_fct") ,py::arg("outmap")), "Closeness centrality with dynamic contextual constraints.\n\n\n\nUsing this function allows obtaining closeness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the centrality calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.\n\n\n\nArgs:\n    g The graph object\n    clsn_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.\n\n    outmap (PropertyMapDouble): map in which centrality values will be written\n\nExample:\n    >>> outmap = PropertyMapDouble(g, 'clsn_ctx')\n    >>> AlgCentralities.closeness_ctx(g,lambda _start,_current,_next: (True),outmap)\n\n\n")
      .def("closeness_ctx", +[](GraphC& gc, py::object fct_decision) -> PMap {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        auto g = gc.get_graph();
        PMap centr_map(gc, "closeness_constraint");
        auto weight_map = boost::make_constant_property<Edge>(1);

        closeness_centrality_ctx(g,
            b::weight_map(weight_map)
            .centrality_map(centr_map.get_map())
            .vertex_index_map(gc.make_index_map()),
            lmbd_decision);
        return centr_map;
      },(py::arg("g"), py::arg("clsn_decision_fct")), "Closeness centrality with dynamic contextual constraints.\n\n\n\nUsing this function allows obtaining closeness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the centrality calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.\n\n\n\nArgs:\n    g The graph object\n    clsn_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.\n\n\nReturns:\n    PropertyMapDouble: list of centrality values named closeness_constraint\n\nExample:\n    >>> outmap = AlgCentralities.closeness_ctx(g,lambda _start,_current,_next: (True))\n\n\n")
      .def("betweenness", +[](GraphC& gc, PMap& centr_map) {
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        brandes_betweenness_centrality(
          g,
          centrality_map(centr_map.get_map())
          .vertex_index_map(gc.make_index_map())
          .weight_map(weight_map));
      }, (py::arg("g"), py::arg("outmap")), "Obtain betweenness centrality for a graph using Brandes' efficient algorithm.\n\nArgs:\n    g (Graph): Graph object\n    outmap (PropertyMapDouble): map in which centrality values will be written\n\nExample:\n    >>> outmap = PropertyMapDouble(g, 'betw')\n    >>> AlgCentralities.betweenness(g, outmap)\n\n\n")
      .def("betweenness", +[](GraphC& gc) -> PMap {
        auto g = gc.get_graph();
        PMap centr_map(gc, "betweenness");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        brandes_betweenness_centrality(
          g,
          centrality_map(centr_map.get_map())
          .vertex_index_map(gc.make_index_map())
          .weight_map(weight_map));
        return centr_map;
      }, (py::arg("g")), "Obtain betweenness centrality for a graph using Brandes' efficient algorithm.\n\nArgs:\n    g (Graph): Graph object\n\nReturns:\n    PropertyMapDouble: list of centrality values named betweenness\n\nExample:\n    >>> outmap = AlgCentralities.betweenness(g)\n\n\n")
      .def("closeness", +[](GraphC& gc) -> PMap {
        auto g = gc.get_graph();
        PMap centr_map(gc, "closeness");
        auto weight_map = boost::make_constant_property<Edge>(1);
        std::function<bool (Vertex, Vertex, Vertex)> dummy_decision = [](Vertex s, Vertex u, Vertex v){return true;};

        closeness_centrality_ctx(g,
            b::weight_map(weight_map)
            .centrality_map(centr_map.get_map())
            .vertex_index_map(gc.make_index_map()),
            dummy_decision);

        return centr_map;
      }, (py::arg("g")), "Obtain closeness centrality for a graph.\n\nArgs:\n    g (Graph): Graph object\n\nReturns:\n    PropertyMapDouble: list of centrality values named betweenness\n\nExample:\n    >>> outmap = AlgCentralities.closeness(g)\n\n\n")
      .def("closeness", +[](GraphC& gc, PMap& centr_map) {
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1);
        std::function<bool (Vertex, Vertex, Vertex)> dummy_decision = [](Vertex s, Vertex u, Vertex v){return true;};

        closeness_centrality_ctx(g,
            b::weight_map(weight_map)
            .centrality_map(centr_map.get_map())
            .vertex_index_map(gc.make_index_map()),
            dummy_decision);
      }, (py::arg("g"), py::arg("outmap")), "Obtain closeness centrality for a graph.\n\nArgs:\n    g (Graph): Graph object\n    outmap (PropertyMapDouble): map in which centrality values will be written\n\nExample:\n    >>> outmap = PropertyMapDouble(g, 'clsn')\n    >>> AlgCentralities.closeness(g, outmap)\n\n\n")
      ;
  }

}} //nproc::python

#endif


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file quasi_cauchy_rrd.hpp
///
#ifndef MXPFIT_QUASI_CAUCHY_RRD_HPP
#define MXPFIT_QUASI_CAUCHY_RRD_HPP

#include <cassert>

#include <Eigen/Core>

namespace mxpfit
{
namespace detail
{

//
// Compute exponential z minus one, exp(z) -1
//
// --- for real
template <typename T>
inline auto expm1(T x) -> decltype(std::expm1(x))
{
    return std::expm1(x);
}
// --- for complex
template <typename T>
std::complex<T> expm1(const std::complex<T>& z)
{
    constexpr const T inf = std::numeric_limits<T>::infinity();
    constexpr const T nan = std::numeric_limits<T>::quiet_NaN();

    const T x = std::real(z);
    const T y = std::imag(z);

    if (std::isnan(x))
    {
        return {x, y == T() ? y : x};
    }
    else if (!std::isfinite(y))
    {
        if (x == inf)
        {
            return {-x, nan};
        }
        else if (x == -inf)
        {
            return {-T(1), std::copysign(T(), y)};
        }
        else
        {
            return {nan, nan};
        }
    }
    else
    {
        const auto u = std::expm1(x);
        if (y == T())
        {
            return {u, y};
        }
        else
        {
            const auto v = u + T(1);
            const auto w = std::sin(y / 2);
            const auto re =
                std::isfinite(v) ? u - 2 * v * w * w : v * std::cos(y);
            return {re, v * std::sin(y)};
        }
    }
}

} // namespace: detail

///
/// ### DefaultQuasiCauchyRRDFunctor
///
/// Default Functor class for QuasiCauchyRRD.
///
/// \tparam T Scalar type of matrix elements
///
/// This functor is for a quasi-Cauchy matrix whose element is expressed as
/// \f$C_{ij}=\frac{a_{i}b_{j}}{x_{i}+y_{j}}\f$
///
template <typename T>
struct DefaultQuasiCauchyRRDFunctor
{
    constexpr static T matrix_element(T ai, T bj, T xi, T yj)
    {
        return ai * bj / (xi + yj);
    }

    constexpr static T update_coeff(T xi, T xj, T yj)
    {
        return (xi - xj) / (xi + yj);
    }
};

///
/// ### QuasiCauchyRRDFunctorLogPole
///
/// Functor class for a rank-revealing Cholesky decomposition of Cauchy-like
/// matrix appearing in parameter reduction of exponential sum.
///
/// \tparam T Scalar type of matrix elements
///
/// This functor is for a Cauchy-like matrix whose element is expressed as
/// \f[
///    C_{ij}=\frac{a_{i}b_{j}}{exp(p_{i}) - exp(q_{j})}
/// \f]
///
template <typename T>
struct QuasiCauchyRRDFunctorLogPole
{
    constexpr static T matrix_element(T ai, T bj, T pi, T qj)
    {
        using Eigen::numext::exp;
        return ai * bj / (exp(qj) * detail::expm1(pi - qj));
    }

    constexpr static T update_coeff(T pi, T pj, T qj)
    {
        using Eigen::numext::exp;
        // (1 - exp(pj - pi)) / (1 - exp(qj - pi));
        return detail::expm1(pj - pi) / detail::expm1(qj - pi);
    }
};

namespace detail
{

// Apply row permutation
template <typename IPiv, typename MatX, typename VecWork>
void apply_row_permutation(const Eigen::DenseBase<IPiv>& ipiv,
                           Eigen::DenseBase<MatX>& matX,
                           Eigen::DenseBase<VecWork>& work)
{
    using Index = Eigen::Index;
    for (Index j = 0; j < matX.cols(); ++j)
    {
        for (Index i = 0; i < matX.rows(); ++i)
        {
            work(ipiv(i)) = matX(i, j);
        }

        matX.col(j) = work;
    }
}

//
// Functor class for making dense matrix expression of a quasi-Cauchy matrix
//
template <typename VecA, typename VecB, typename VecX, typename VecY,
          typename FunctorBody>
struct quasi_cauchy_functor
{
    using Index = Eigen::Index;

    quasi_cauchy_functor(const VecA& a, const VecB& b, const VecX& x,
                         const VecY& y)
        : m_a(a), m_b(b), m_x(x), m_y(y)
    {
    }

    auto operator()(Index i, Index j) const
        -> decltype(FunctorBody::matrix_element(typename VecA::Scalar(),
                                                typename VecB::Scalar(),
                                                typename VecX::Scalar(),
                                                typename VecY::Scalar()))
    {
        // m_a(i) * m_b(j) / (m_x(i) + m_y(j));
        return FunctorBody::matrix_element(m_a(i), m_b(j), m_x(i), m_y(j));
    }

private:
    const VecA& m_a;
    const VecB& m_b;
    const VecX& m_x;
    const VecY& m_y;
};

} // namespace detail

///
/// ### QuasiCauchyRRD
///
/// \brief Compute the rank-revealing Cholesky decomposition of a self-adjoint
///   quasi-Cauchy matrix in high relative accuracy.
///
/// \tparam T the scalar type of matrix to be decomposed
/// \tparam Functor Functor class that provides static functions for calculating
///   matrix elements and Cholesky factors, such that,
/// ``` c++
///   /* Compute `ai * bj / (xi + yj)
///   static T matrix_element(T ai, T bj, T xi, T yj);
///   /* Compute `(xi - xj) / (xi + yj)
///   static T update_coeff(T xi, T xj, T yj);
/// ```
///
/// For given arrays of $a_{i},b_{i},x_{i},y_{i} \, (i=1,2,\dots,n)$ a
/// quasi-Cauchy matrix \f$ C \f$ is defined as
///
///   \f[
///     C_{ij} = \frac{a_{i}^{} b_{j}^{}}{x_{i}^{} + y_{j}^{}} \quad
///     (i,j=1,2,\dots,n).
///   \f]
///
/// We assume that the matrix \f$ C \f$ is self-adjoint and positive
/// definite. A quasi-Cauchy matrix is usually rank deficient. Let \f$ m =
/// \text{rank}(C) \f$ then, matrix `C` can have partial Cholesky decomposition
/// of the form
///
///   \f[ C = (PL)D^2(PL)^{\ast}, \f]
///
/// where \f$ L \f$ is \f$ n \times m \f$ unit triangular (trapezoidal) matrix,
/// \f$ D \f$ is \f$ m \times m \f$ diagonal matrix, and \f$ P \f$ is a \f$ m
/// \times n \f$ permutation matrix.
///
///
/// #### Algorithm
///
/// The factorization is made using the modified Gaussian elimination of
/// complete pivoting (GECP) proposed in Ref. [1], which is also described in
/// Algorithm 2 and 3 in Ref.[2]. Following the algorithms described in Ref.
/// [2], we stop the factorization when the diagonal element \f$ D_{mm} \f$
/// becomes smaller than the threshold value, \f$ D_{mm} \leq \delta^2 \epsilon
/// \f$, at certain \f$ m \f$ where \f$ \delta \f$ is mimimal target size of
/// singular values of the systems to be kept in the later step, and \f$
/// \epsilon \f$ is the machine epsilon. Note that the diagonal elements \f$
/// D_{ii} \f$ are sorted in non-increasing order by complete pivoting.
///
///
/// #### Complexity
///
/// The complexity of this algorithm is \f$
/// \mathcal{O}(n(\log(\delta\epsilon)^{-1})^2) \f$.
///
///
/// #### References
///
/// 1. J. Demmel, "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED
///    MATRICES", SIAM J. Matrix Anal. Appl. **21** (1999) 562-580.
///    [DOI: https://doi.org/10.1137/S0895479897328716]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
///

// --- Forward declaration
template <typename T, typename Functor = DefaultQuasiCauchyRRDFunctor<T>>
class QuasiCauchyRRD;
//
// --- Implementation
//
template <typename T, typename Functor>
class QuasiCauchyRRD
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using StorageIndex  = Eigen::Index;
    using Index         = Eigen::Index;
    using MatrixType    = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVectorType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using IndicesType    = Eigen::Matrix<Index, Eigen::Dynamic, 1>;

    ///
    /// Default constructor
    ///
    QuasiCauchyRRD()
        : m_a(),
          m_b(),
          m_x(),
          m_y(),
          m_work(),
          m_ipiv(),
          m_matPL(),
          m_vecD(),
          m_threshold(Eigen::NumTraits<RealScalar>::epsilon()),
          m_is_initialized()
    {
    }

    ///
    /// Default destructor
    ///
    ~QuasiCauchyRRD() = default;

    ///
    /// Compute RRD of self-adjoint quasi-Cauchy matrix.
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] b  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    /// \param[in] y  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecB, typename VecX, typename VecY>
    void
    compute(const Eigen::EigenBase<VecA>& a, const Eigen::EigenBase<VecB>& b,
            const Eigen::EigenBase<VecX>& x, const Eigen::EigenBase<VecY>& y)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecB);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecY);

        m_a = a.derived();
        m_b = b.derived();
        m_x = x.derived();
        m_y = y.derived();

        m_work.resize(a.derived().size());
        m_ipiv.resize(a.derived().size());

        const Index rank = pivot_order();
        m_matPL.resize(m_a.derived().size(), rank);
        m_vecD.resize(rank);
        factorize();

        // apply row permutations
        detail::apply_row_permutation(m_ipiv, m_matPL, m_work);

        m_is_initialized = true;
    }

    ///
    /// Set threshold value for GECP termination.
    ///
    /// We stop GECP step as soon as the diagonal element of Cholesky factor
    /// \f$D_{mm}\f$ becomes smaller than this value.
    ///
    QuasiCauchyRRD& setThreshold(RealScalar threshold)
    {
        m_threshold = threshold;
        return *this;
    }

    ///
    /// \return the rank of matrix revealed.
    ///
    Index rank() const
    {
        return m_vecD.size();
    }

    ///
    /// \return const reference to the rank revealing factor \f$ X=PL \f$
    ///
    const MatrixType& matrixPL() const
    {
        assert(m_is_initialized && "QuasiCauchyRRD is not initialized");
        return m_matPL;
    }

    ///
    /// \return const reference to the rank revealing factor \f$ D \f$
    ///
    const RealVectorType& vectorD() const
    {
        assert(m_is_initialized && "QuasiCauchyRRD is not initialized");
        return m_vecD;
    }

    ///
    /// Create dense matrix expression of quasi-Cauchy matrix
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] b  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    /// \param[in] y  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecB, typename VecX, typename VecY>
    static Eigen::CwiseNullaryOp<
        detail::quasi_cauchy_functor<VecA, VecB, VecX, VecY, Functor>,
        MatrixType>
    makeDenseExpr(const Eigen::EigenBase<VecA>& a,
                  const Eigen::EigenBase<VecB>& b,
                  const Eigen::EigenBase<VecX>& x,
                  const Eigen::EigenBase<VecY>& y)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecB);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecY);

        assert(b.size() == a.size());
        assert(x.size() == a.size());
        assert(y.size() == a.size());

        using functor_t =
            detail::quasi_cauchy_functor<VecA, VecB, VecX, VecY, Functor>;

        return MatrixType::NullaryExpr(
            a.size(), a.size(),
            functor_t(a.derived(), b.derived(), x.derived(), y.derived()));
    }

protected:
    VectorType m_a;
    VectorType m_b;
    VectorType m_x;
    VectorType m_y;
    VectorType m_work;
    IndicesType m_ipiv;
    MatrixType m_matPL;
    RealVectorType m_vecD;

    RealScalar m_threshold;
    bool m_is_initialized;

    Index pivot_order();
    void factorize();
};

template <typename T, typename Functor>
typename QuasiCauchyRRD<T, Functor>::Index
QuasiCauchyRRD<T, Functor>::pivot_order()
{
    using Eigen::numext::abs;

    const Index n = m_a.size();

    assert(m_b.size() == n);
    assert(m_x.size() == n);
    assert(m_y.size() == n);
    assert(m_work.size() == n);
    assert(m_ipiv.size() == n);

    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    VectorType& g = m_work;

    for (Index i = 0; i < n; ++i)
    {
        g[i] = Functor::matrix_element(m_a[i], m_b[i], m_x[i], m_y[i]);
    }

    // Initialize rows transposition matrix
    for (Index i = 0; i < n; ++i)
    {
        m_ipiv[i] = i;
    }

    // GECP iteration
    Index m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        Index l;
        auto max_diag = RealScalar();
        for (Index k = m; k < n; ++k)
        {
            const auto abs_gk = abs(g[k]);
            if (abs_gk > max_diag)
            {
                max_diag = abs_gk;
                l        = k;
            }
        }

        if (max_diag < m_threshold)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(m_ipiv[l], m_ipiv[m]);
            std::swap(g[l], g[m]);
            std::swap(m_a[l], m_a[m]);
            std::swap(m_b[l], m_b[m]);
            std::swap(m_x[l], m_x[m]);
            std::swap(m_y[l], m_y[m]);
        }

        // Update diagonal of Schur complement
        const auto xm = m_x[m];
        const auto ym = m_y[m];

        for (Index k = m + 1; k < n; ++k)
        {
            // g[k] *= (m_x[k] - xm) / (m_x[k] + ym);
            g[k] *= Functor::update_coeff(m_x[k], xm, ym);
            // g[k] *= (m_y[k] - ym) / (m_y[k] + xm);
            g[k] *= Functor::update_coeff(m_y[k], ym, xm);
        }
        ++m;
    }
    //
    // Returns the rank of input matrix
    //
    return m;
}

template <typename T, typename Functor>
void QuasiCauchyRRD<T, Functor>::factorize()
{
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    const auto n = m_matPL.rows();
    const auto m = m_matPL.cols();

    m_matPL.setZero();
    const auto b0 = m_b[0];
    const auto y0 = m_y[0];

    for (Index l = 0; l < n; ++l)
    {
        // m_matPL(l, 0) = m_a[l] * b0 / (m_x[l] + y0);
        m_matPL(l, 0) = Functor::matrix_element(m_a[l], b0, m_x[l], y0);
    }

    for (Index k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = m_x[k - 1];
        const auto ykm1 = m_y[k - 1];
        for (Index l = k; l < n; ++l)
        {
            // m_a[l] *= (m_x[l] - xkm1) / (m_x[l] + ykm1);
            m_a[l] *= Functor::update_coeff(m_x[l], xkm1, ykm1);
        }
        for (Index l = k; l < n; ++l)
        {
            // m_b[l] *= (m_y[l] - ykm1) / (m_y[l] + xkm1);
            m_b[l] *= Functor::update_coeff(m_y[l], ykm1, xkm1);
        }
        // Extract k-th column for Cholesky factors
        const auto bk = m_b[k];
        const auto yk = m_y[k];
        for (Index l = k; l < n; ++l)
        {
            m_matPL(l, k) = Functor::matrix_element(m_a[l], bk, m_x[l], yk);
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (Index j = 0; j < m; ++j)
    {
        const auto djj   = real(m_matPL(j, j));
        const auto scale = RealScalar(1) / djj;

        m_matPL(j, j) = RealScalar(1);
        m_vecD[j] = sqrt(djj);
        for (Index i = j + 1; i < n; ++i)
        {
            m_matPL(i, j) *= scale;
        }
    }

    return;
}

namespace detail
{

template <typename VecA, typename VecX>
struct self_adjoint_quasi_cauchy_helper
{
    using Scalar =
        typename Eigen::ScalarBinaryOpTraits<typename VecA::Scalar,
                                             typename VecX::Scalar>::ReturnType;
    using MatrixType =
        Eigen::Matrix<Scalar, VecA::SizeAtCompileTime, VecA::SizeAtCompileTime,
                      Eigen::ColMajor, VecA::MaxSizeAtCompileTime,
                      VecA::MaxSizeAtCompileTime>;
};

template <typename VecA, typename VecX>
struct self_adjoint_quasi_cauchy_functor
{
    using Scalar =
        typename Eigen::ScalarBinaryOpTraits<typename VecA::Scalar,
                                             typename VecX::Scalar>::ReturnType;
    using Index = Eigen::Index;
    using MatrixType =
        Eigen::Matrix<Scalar, VecA::SizeAtCompileTime, VecA::SizeAtCompileTime,
                      Eigen::ColMajor, VecA::MaxSizeAtCompileTime,
                      VecA::MaxSizeAtCompileTime>;

    self_adjoint_quasi_cauchy_functor(const VecA& a, const VecX& x)
        : m_a(a), m_x(x)
    {
    }

    Scalar operator()(Index i, Index j) const
    {
        using Eigen::numext::conj;
        return m_a(i) * conj(m_a(j)) / (m_x(i) + conj(m_x(j)));
    }

private:
    const VecA& m_a;
    const VecX& m_x;
};

} // namespace detail

///
/// ### SelfAdjointQuasiCauchyRRD
///
/// \brief  Compute the rank-revealing decomposition (RRD) of a self-adjoint
///   quasi-Cauchy matrix in high relative accuracy.
///
/// \tparam T  the scalar type of matrix to be decomposed
///
/// For given arrays of $a_{i},x_{i} \, (i=1,2,\dots,n)$ a self-adjoint
/// quasi-Cauchy matrix \f$ C \f$ is defined as
///
///   \f[
///     C_{ij} = \frac{a_{i}^{} a_{j}^{\ast}}{x_{i}^{} + x_{j}^{\ast}} \quad
///     (i,j=1,2,\dots,n).
///   \f]
///
/// We assume that the matrix \f$ C \f$ is also positive definite. A
/// quasi-Cauchy matrix is usually rank deficient. Let \f$ m = \text{rank}(C)
/// \f$ then, matrix `C` can have partial Cholesky decomposition of the form
///
///   \f[ C = (PL)D^2(PL)^{\ast}, \f]
///
/// where \f$ L \f$ is \f$ n \times m \f$ unit triangular (trapezoidal) matrix,
/// \f$ D \f$ is \f$ m \times m \f$ diagonal matrix, and \f$ P \f$ is a \f$ m
/// \times n \f$ permutation matrix.
///
/// #### Algorithm
///
/// The factorization is made using the modified Gaussian elimination of
/// complete pivoting (GECP) proposed in Ref. [1], which is also described in
/// Algorithm 2 and 3 in Ref.[2]. Following the algorithms described in Ref.
/// [2], we stop the factorization when the diagonal element \f$ D_{mm} \f$
/// becomes smaller than the threshold value, \f$ D_{mm} \leq \delta^2 \epsilon
/// \f$, at certain \f$ m \f$ where \f$ \delta \f$ is mimimal target size of
/// singular values of the systems to be kept in the later step, and \f$
/// \epsilon \f$ is the machine epsilon. Note that the diagonal elements \f$
/// D_{ii} \f$ are sorted in non-increasing order by complete pivoting.
///
/// #### Complexity
///
/// The complexity of this algorithm is \f$
/// \mathcal{O}(n(\log(\delta\epsilon)^{-1})^2) \f$.
///
///
/// #### References
///
/// 1. J. Demmel, "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED
///    MATRICES", SIAM J. Matrix Anal. Appl. **21** (1999) 562-580.
///    [DOI: https://doi.org/10.1137/S0895479897328716]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
///
template <typename T>
class SelfAdjointQuasiCauchyRRD
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using StorageIndex  = Eigen::Index;
    using Index         = Eigen::Index;
    using MatrixType    = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVectorType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using IndicesType    = Eigen::Matrix<Index, Eigen::Dynamic, 1>;

    ///
    /// Default constructor
    ///
    SelfAdjointQuasiCauchyRRD()
        : m_a(),
          m_x(),
          m_work(),
          m_ipiv(),
          m_matPL(),
          m_vecD(),
          m_threshold(Eigen::NumTraits<RealScalar>::epsilon()),
          m_is_initialized(false)
    {
    }

    ///
    /// Default destructor
    ///
    ~SelfAdjointQuasiCauchyRRD() = default;

    ///
    /// Compute RRD of self-adjoint quasi-Cauchy matrix.
    ///
    /// @param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// @param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecX>
    void compute(const Eigen::EigenBase<VecA>& a,
                 const Eigen::EigenBase<VecX>& x)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);

        m_a = a.derived();
        m_x = x.derived();
        m_work.resize(a.derived().size());
        m_ipiv.resize(a.derived().size());

        const Index rank = pivot_order();
        m_matPL.resize(m_a.derived().size(), rank);
        m_vecD.resize(rank);
        factorize();

        // apply row permutations
        detail::apply_row_permutation(m_ipiv, m_matPL, m_work);

        m_is_initialized = true;
    }

    ///
    /// Set threshold value for GECP termination.
    ///
    /// We stop GECP step as soon as the diagonal element of Cholesky factor
    /// \f$D_{mm}\f$ becomes smaller than this value.
    ///
    SelfAdjointQuasiCauchyRRD& setThreshold(RealScalar threshold)
    {
        m_threshold = threshold;
        return *this;
    }

    ///
    /// \return the rank of matrix revealed.
    ///
    Index rank() const
    {
        return m_vecD.size();
    }
    ///
    /// \return const reference to the rank revealing factor \f$ X=PL \f$
    ///
    const MatrixType& matrixPL() const
    {
        assert(m_is_initialized &&
               "SelfAdjointQuasiCauchyRRD is not initialized");
        return m_matPL;
    }

    ///
    /// \return const reference to the rank revealing factor \f$ D \f$
    ///
    const RealVectorType& vectorD() const
    {
        assert(m_is_initialized &&
               "SelfAdjointQuasiCauchyRRD is not initialized");
        return m_vecD;
    }

    ///
    /// Create dense matrix expression of quasi-Cauchy matrix
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecX>
    static Eigen::CwiseNullaryOp<
        detail::self_adjoint_quasi_cauchy_functor<VecA, VecX>, MatrixType>
    makeDenseExpr(const Eigen::DenseBase<VecA>& a,
                  const Eigen::DenseBase<VecX>& x)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        using functor_t = detail::self_adjoint_quasi_cauchy_functor<VecA, VecX>;

        assert(a.size() == x.size());

        return MatrixType::NullaryExpr(a.size(), a.size(),
                                       functor_t(a.derived(), x.derived()));
    }

protected:
    VectorType m_a;
    VectorType m_x;
    VectorType m_work;
    IndicesType m_ipiv;
    MatrixType m_matPL;
    RealVectorType m_vecD;

    RealScalar m_threshold;
    bool m_is_initialized;

    Index pivot_order();
    void factorize();
};

template <typename T>
Eigen::Index SelfAdjointQuasiCauchyRRD<T>::pivot_order()
{
    using Eigen::numext::abs;
    using Eigen::numext::abs2;
    using Eigen::numext::conj;

    const Index n = m_a.size();

    assert(m_x.size() == n);
    assert(m_work.size() == n);
    assert(m_ipiv.size() == n);

    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    VectorType& g = m_work;

    for (Index i = 0; i < n; ++i)
    {
        g[i] = m_a[i] * conj(m_a[i]) / (m_x[i] + conj(m_x[i]));
    }

    // Initialize rows transposition matrix
    for (Index i = 0; i < n; ++i)
    {
        m_ipiv(i) = i;
    }

    // GECP iteration
    Index m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        Index l;
        auto max_diag = RealScalar();
        for (Index k = m; k < n; ++k)
        {
            const auto abs_gk = abs(g[k]);
            if (abs_gk > max_diag)
            {
                max_diag = abs_gk;
                l        = k;
            }
        }

        if (max_diag < m_threshold)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(m_ipiv[l], m_ipiv[m]);
            std::swap(g[l], g[m]);
            std::swap(m_a[l], m_a[m]);
            std::swap(m_x[l], m_x[m]);
        }

        // Update diagonal of Schur complement
        const auto xm = m_x[m];
        const auto ym = conj(m_x[m]);

        for (Index k = m + 1; k < n; ++k)
        {
            const auto s1 = (m_x[k] - xm) / (m_x[k] + ym);
            g[k] *= s1 * conj(s1);
        }
        ++m;
    }
    //
    // Returns the rank of input matrix
    //
    return m;
}

template <typename T>
void SelfAdjointQuasiCauchyRRD<T>::factorize()
{
    using Eigen::numext::conj;
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    const auto n = m_matPL.rows();
    const auto m = m_matPL.cols();

    m_matPL.setZero();
    const auto b0 = conj(m_a(0));
    const auto y0 = conj(m_x(0));

    for (Index l = 0; l < n; ++l)
    {
        m_matPL(l, 0) = m_a[l] * b0 / (m_x[l] + y0);
    }

    for (Index k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = m_x[k - 1];
        const auto ykm1 = conj(xkm1);
        for (Index l = k; l < n; ++l)
        {
            m_a[l] *= (m_x[l] - xkm1) / (m_x[l] + ykm1);
        }
        // Extract k-th column for Cholesky factors
        const auto bk = conj(m_a[k]);
        const auto yk = conj(m_x[k]);
        for (Index l = k; l < n; ++l)
        {
            m_matPL(l, k) = m_a[l] * bk / (m_x[l] + yk);
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (Index j = 0; j < m; ++j)
    {
        const auto djj   = real(m_matPL(j, j));
        const auto scale = RealScalar(1) / djj;

        m_matPL(j, j) = RealScalar(1);
        m_vecD[j] = sqrt(djj);
        for (Index i = j + 1; i < n; ++i)
        {
            m_matPL(i, j) *= scale;
        }
    }

    return;
}

} // namespace: mxpfit

#endif /* MXPFIT_QUASI_CAUCHY_RRD_HPP */


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

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

#include <dpMM/dpMM.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/niw.hpp>

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

/*
 * DP mixture model
 * following Neal [[http://www.stat.purdue.edu/~rdutta/24.PDF]]
 * Algo 3
 */
template <class Dist>
class DpStickMM : public DpMM<double>
{
public:
  DpStickMM(double alpha, const Dist& H)
    : alpha_(alpha), H_(H)
   {};
  ~DpStickMM()
  {};

  virtual void initialize(const MatrixXd& x);
  virtual void initialize(const shared_ptr<ClGMMData<double> >& cld)
    {cout<<"not supported"<<endl; assert(false);};
  virtual void sampleLabels();
  virtual void sampleParameters(){;};
  virtual const VectorXu & getLabels() {return z_;};
  virtual uint32_t getK() const { return z_.maxCoeff()+1;};

  VectorXd getCounts();

private:
  void removeEmptyClusters();

  double alpha_; //
  vector<Dist> models_;

  MatrixXd x_;
  VectorXu z_; // indicators
  Dist H_; //
};

// ---------------- impl -----------------------------------------------------
template <class Dist>
void DpStickMM<Dist>::initialize(const MatrixXd& x)
{
  uint32_t K0=1;
  x_ = x;
  z_.setZero(x.cols(),1);
  for (uint32_t k=0; k<K0; ++k)
    models_.push_back(Dist(H_));
  cout<<models_.size()<<endl;
  //cout<<x_<<endl;
  cout<<alpha_<<endl;
  if (K0>1)
  {
    VectorXd pi(K0);
    pi.setOnes();
    pi /= static_cast<double>(K0);
    Catd cat(pi,H_.pRndGen_);
    for (uint32_t i=0; i<z_.size(); ++i)
      z_(i) = cat.sample();
  }
};

template <class Dist>
VectorXd DpStickMM<Dist>::getCounts()
{
  VectorXd counts(getK());
  counts.setZero();
//TODO
//  for(uint32_t k=0; k<getK(); ++k)
//    counts(k) = models_[k]->count();
  return counts;
};

template <class Dist>
void DpStickMM<Dist>::sampleLabels()
{
  double N = z_.size();
  for(uint32_t i=0; i<z_.size(); ++i)
  {
    // compute clustercounts
    VectorXd Nk(models_.size()); Nk.setZero(models_.size());
    for(uint32_t ii=0; ii<z_.size(); ++ii)
      Nk(z_(ii))++;
    // compute distribution pi over indicators
    VectorXd pi(models_.size()+1);
    for (uint32_t k=0; k<models_.size(); ++k)
      pi(k) = log(Nk(k))-log(N+alpha_) +models_[k].logPosteriorProb(x_,z_,k,i);
    pi(models_.size()) = log(alpha_)-log(N+alpha_)+H_.logProb(x_.col(i));
    // normalize pi and exponentiate it
    double pi_max = pi.maxCoeff();
    pi = (pi.array()-(pi_max + log((pi.array() - pi_max).exp().sum()))).exp().matrix();
    // sample new indicator
    z_(i) = Catd(pi,H_.pRndGen_).sample();
    // if z_i was a new cluster
    if(z_(i)==models_.size())
      models_.push_back(H_);

    // -------- outputs --------------
    if(i%100 ==0)
    {
      cout<<" @i="<<i
        <<"\tcounts="<<Nk.transpose()<<endl;
    }
  }
  this->removeEmptyClusters();
};

template <class Dist>
void DpStickMM<Dist>::removeEmptyClusters()
{
  for(uint32_t k=models_.size()-1; k>=0; --k)
  {
    bool haveCluster_k = false;
    for(uint32_t i=0; i<z_.size(); ++i)
      if(z_(i)==k)
      {
        haveCluster_k = true;
        break;
      }
    if (!haveCluster_k)
    {
      for (uint32_t i=0; i<z_.size(); ++i)
        if(z_(i) >= k) z_(i) --;
      models_.erase(models_.begin()+k);
    }
  }
}


#pragma once

#include <Eigen/Dense>
#include <tuple>
#include <vector>
#include <type_traits>
#include <utility>
#include <algorithm>

namespace mtao { namespace eigen {
    template <bool Rows, typename... Args, int... N>
        auto _stack(std::integer_sequence<int,N...>, const Args&... args) {
            using namespace Eigen;
            using Scalar = typename std::tuple_element<0,std::tuple<Args...>>::type::Scalar;

            constexpr static int minCompileRows = std::min<int>({Args::RowsAtCompileTime...});
            constexpr static int maxCompileRows = std::max<int>({Args::RowsAtCompileTime...});
            constexpr static int minCompileCols = std::min<int>({Args::ColsAtCompileTime...});
            constexpr static int maxCompileCols = std::max<int>({Args::ColsAtCompileTime...});

            constexpr static int sumCompileRows = (Args::RowsAtCompileTime + ... + 0);
            constexpr static int sumCompileCols = (Args::ColsAtCompileTime + ... + 0);

            //constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?S:1)*maxCompileRows;
            //constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?1:S)*maxCompileCols;
            constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?sumCompileRows:maxCompileRows);
            constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?maxCompileCols:sumCompileCols);
            int rows;
            int cols;
            std::vector<int> offset(1,0);
            auto push_sum = [&](int size) {
                offset.push_back(offset.back() + size);
            };
            if constexpr(Rows) {
                rows = (args.rows() + ... + 0);
                cols = std::max( {args.cols()...}) ;
                (push_sum(args.rows()),...);
            } else {
                rows = std::max( {args.rows()...}) ;
                cols = (args.cols() + ... + 0);
                (push_sum(args.cols()),...);
            }

            using Matf = Matrix<Scalar,myCompRows,myCompCols>;
            Matf A = Matf::Constant(rows,cols,0);

            if constexpr(Rows) {
                (A.block(offset[N],0,args.rows(),args.cols()).operator=(args),...);
            } else {
                (A.block(0,offset[N],args.rows(),args.cols()).operator=(args),...);
            }

            return A;
        }

    template <typename... Args>
        auto vstack(const Args&... args) {
            return _stack<true>(std::make_integer_sequence<int,sizeof...(Args)>(), std::forward<const Args&>(args)...);
        }
    template <typename... Args>
        auto hstack(const Args&... args) {
            return _stack<false>(std::make_integer_sequence<int,sizeof...(Args)>(), std::forward<const Args&>(args)...);
        }

    template <typename BeginIt, typename EndIt>
        auto hstack_iter(BeginIt beginit, EndIt endit) {
            using CDerived = typename std::decay_t<decltype(*beginit)>;

            constexpr static int CRows = CDerived::RowsAtCompileTime;
            using Index = typename CDerived::Scalar;
            using RetCells = Eigen::Matrix<Index,CRows,Eigen::Dynamic>;
            int ccols = 0;
            int crows = 0;

            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                crows = std::max<int>(crows,c.rows());
                ccols += c.cols();
                }
            }
            RetCells mC(crows,ccols);
            ccols = 0;
            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                mC.block(0,ccols,c.rows(),c.cols()) = c;
                ccols += c.cols();
                }
            }
            return mC;
        }
    template <typename BeginIt, typename EndIt>
        auto vstack_iter(BeginIt beginit, EndIt endit) {
            using CDerived = typename std::decay_t<decltype(*beginit)>;

            constexpr static int CCols = CDerived::ColsAtCompileTime;
            using Index = typename CDerived::Scalar;
            using RetCells = Eigen::Matrix<Index,Eigen::Dynamic,CCols>;
            int ccols = 0;
            int crows = 0;

            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                ccols = std::max<int>(ccols,c.cols());
                crows += c.rows();
                }
            }
            RetCells mC(crows,ccols);
            crows = 0;
            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                mC.block(crows,0,c.rows(),c.cols()) = c;
                crows += c.rows();
                }
            }
            return mC;
        }
}}


#include <iostream>
#include <vector>
#include <fstream>
#include <algorithm>
#include <boost/date_time.hpp>
#include "../mylib/heap.hpp"
#include "../mylib/graph.hpp"
#include "../mylib/dijkstra.hpp"

void load_vertices(const std::string &filename, bool get_r,
                my::Graph &vertices, my::Graph &vertices_r,
                u_int64_t &vertice_count, u_int64_t &edge_count){

    std::cout << "reading file " << filename << std::endl;
    std::fstream file(filename, std::ios::in);

    file >> vertice_count >> edge_count;

    vertices.reserve(static_cast<u_int64_t>(vertice_count));
    for(u_int64_t i = 0; i < vertice_count; i++){
        vertices.emplace_back(my::Vertice(i));
        if (get_r) vertices_r.emplace_back(my::Vertice(i));
    }

    u_int64_t fro, to, weight;
    while (file >> fro >> to >> weight) {
        vertices[fro].edges.emplace_back(to, weight);
        if (get_r) vertices_r[to].edges.emplace_back(fro, weight);
    }
    file.close();
}

std::vector<unsigned long> getpath(std::vector<unsigned long> ancestors, my::Vertice start, my::Vertice end){
    std::vector<unsigned long> path;
    for(auto i = start.index; i != ULONG_MAX; i = ancestors[i]){
        path.emplace_back(i);
    }
    return path;
}

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

    std::string filename = "../graphs/vg1";
    if (argc > 1)
        filename = argv[1];

    my::Graph vertices;
    my::Graph vertices_r;
    u_int64_t vertice_count = 0;
    u_int64_t edge_count = 0;

    boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();
    load_vertices(filename, true, vertices, vertices_r, vertice_count, edge_count);
    boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time();
    std::cout << "loaded " << vertices.size() << " vertices in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    auto res = my::dijkstra(vertices, 0);
    auto ancestors = res.first;
    auto distances = res.second;
    end = boost::posix_time::microsec_clock::local_time();
    std::cout << "found dijkstra in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    if (ancestors.size() < 10000000){
        std::string tab = "\t\t\t";
        std::cout << "Node" << "\t" << "ancestor" << "\t" << "distance" << std::endl;
        for(u_int64_t i = 0; i < ancestors.size() and i < 100; i++){
            std::cout << i << tab;
            if (ancestors[i] != ULONG_MAX) {
                std::cout << ancestors[i];
            } else {
                std::cout << " ";
            }
            std::cout << tab;
            if (distances.at(i) != ULONG_MAX) {
                 std::cout << distances[i];
            } else {
                std::cout << "unreachable";
            }
            std::cout << std::endl;
        }
    }

    std::cout << "Path from " << 50 << " to " << 0 << std::endl;
    for(auto i : getpath(ancestors, 50, 0)){
        std::cout << i;
        if (i != 0) std::cout << ", ";
    }
    std::cout << std::endl;

    return EXIT_SUCCESS;

}


//////////////////////////////////////////////////////////////////////////////
// random::location_scale.hpp                                               //
//                                                                          //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_LOCATION_SCALE_HPP_ER_2009
#define BOOST_RANDOM_LOCATION_SCALE_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
//#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared.hpp>
namespace boost{
namespace random{

    // Samples from a location-scale distribution
    //
    // X = sigma Z + mu
    //
    // TODO Z shouldn't be allowed to be a ref because ref_distribution already
    // exists
    template<typename Z>
    class location_scale_distribution{
        public:
            typedef typename remove_cv<
                typename remove_reference<Z>::type
            >::type z_type;
            typedef typename z_type::input_type input_type;
            typedef typename z_type::result_type result_type;

        location_scale_distribution(){}
        location_scale_distribution(
            const result_type& mu,
            const result_type& sigma,
            typename call_traits<Z>::param_type z
        )
        :mu_(mu),sigma_(sigma),z_(z){}

        template<typename U>
        result_type operator()(U& urng){ return (this->impl(urng)); }

        template<typename U>
        result_type operator()(U& urng)const{ return (this->impl(urng)); }

        const result_type& mu()const{ return this->mu_; }
        const result_type& sigma()const{ return this->sigma_; }
        const Z& z(){ return this->z_ ;}

        private:
        result_type mu_;
        result_type sigma_;
        typename call_traits<Z>::value_type z_;
        template<typename U>
        result_type impl(U& urng){
            return (this->mu()) + (this->sigma()) * (this->z_)(urng);
        }

    };

}// random
}// boost

#endif