#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 common_header.hpp
 * \brief The hardware wrapper of the quadruped
 * \author Maximilien Naveau
 * \date 2018
 *
 * This file declares the TestBench8Motors class which defines the test
 * bench with 8 motors.
 */

#pragma once

// read some parameters
#include "yaml_utils/yaml_cpp_fwd.hpp"

// For mathematical operation
#include <Eigen/Eigen>

// manage the exit of the program with ctrl+c
#include <signal.h>  // manage the ctrl+c signal
#include <atomic>    // thread safe flag for application shutdown management

// some real_time_tools in order to have a real time control
#include "real_time_tools/iostream.hpp"
#include "real_time_tools/spinner.hpp"
#include "real_time_tools/thread.hpp"
#include "real_time_tools/timer.hpp"

namespace solo
{
/**
 * @brief Vector2d shortcut for the eigen vector of size 1.
 */
typedef Eigen::Matrix<double, 1, 1> Vector1d;

/**
 * @brief Vector2d shortcut for the eigen vector of size 2.
 */
typedef Eigen::Matrix<double, 2, 1> Vector2d;

/**
 * @brief Vector2d shortcut for the eigen vector of size 6.
 */
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/**
 * @brief Vector8d shortcut for the eigen vector of size 8.
 */
typedef Eigen::Matrix<double, 8, 1> Vector8d;

/**
 * @brief Vector8d shortcut for the eigen vector of size 12.
 */
typedef Eigen::Matrix<double, 12, 1> Vector12d;

}  // namespace solo


/**
 * @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";
  }
}


// Petter Strandmark 2013.
//
// Rough test of sym-ildl.
//
#include <iostream>

#include <catch.hpp>

#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Sparse>

#ifndef USE_SYM_ILDL

TEST_CASE("no-ildl-available")
{
	SUCCEED();
}

#else

extern "C" {
	#include "matrix.h"
	#include "matrix2.h"
	#include "sparse2.h"

	// Evil library...
	#undef min
	#undef max
	#undef catch
	#undef input
}

#include <lilc_matrix.h>

#include <spii/spii.h>
#include <spii/sym-ildl-conversions.h>

template<typename EigenMat>
MAT* Eigen_to_Meschach(const EigenMat& eigen_matrix)
{
	int m = static_cast<int>(eigen_matrix.rows());
	int n = static_cast<int>(eigen_matrix.cols());

	auto A = m_get(m, n);
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			A->me[i][j] = eigen_matrix(i, j);
		}
	}

	return A;
}

TEST_CASE("ildl-sym")
{
	using namespace std;
	using namespace Eigen;
	using namespace spii;

	const int n = 4;

	MatrixXd Aorg(n, n);
	Aorg.row(0) << 1, 2, 3, 1;
	Aorg.row(1) << 2, 6, 1, 8;
	Aorg.row(2) << 3, 1, 7, 6;
	Aorg.row(3) << 1, 8, 6, 6;
	REQUIRE((Aorg - Aorg.transpose()).norm() == 0);

	// Create sym-ildl matrix.
	lilc_matrix<double> Alilc;
	eigen_to_lilc(Aorg, &Alilc);

	// Convert matrix to Meschach format.
	auto Amat = Eigen_to_Meschach(Aorg);
	spii_at_scope_exit(m_free(Amat));

	cerr << "Original A = " << endl;
	cerr << Aorg << endl;
	cerr << "sym-ildl A = " << endl;
	cerr << Alilc << endl;
	cerr << "Meschach A = " << endl;
	m_foutput(stderr, Amat);

	cerr << endl << endl;

	// Factorize the matrix.
	PERM* pivot  = px_get(4);
	spii_at_scope_exit(px_free(pivot));
	PERM* block = px_get(4);
	spii_at_scope_exit(px_free(block));
	BKPfactor(Amat, pivot, block);

	// Print the results.
	cerr << "Meschach factorization = " << endl;
	m_foutput(stderr, Amat);
	px_foutput(stderr, block);
	px_foutput(stderr, pivot);
	cerr << endl << endl;

	cerr << "A is " << Alilc.n_rows() << " by " << Alilc.n_cols() << " with " << Alilc.nnz() << " non-zeros." << endl;
	lilc_matrix<double> Llilc;	     //<The lower triangular factor of A.
	vector<int> perm;	         //<A permutation vector containing all permutations on A.
	perm.reserve(Alilc.n_cols());
	block_diag_matrix<double> Dblockdiag; //<The diagonal factor of A.
	Alilc.sym_equil();
	Alilc.sym_rcm(perm);
	Alilc.sym_perm(perm);
	const double fill_factor = 1.0;
	const double tol         = 0.001;

	Alilc.ildl(Llilc, Dblockdiag, perm, fill_factor, tol, 1.0);

	cerr << "L is " << Llilc.n_rows() << " by " << Llilc.n_cols() << " with " << Llilc.nnz() << " non-zeros." << endl;
	cerr << Llilc << endl;
	cerr << "D is " << Dblockdiag.n_rows() << " by " << Dblockdiag.n_cols() << " with " << Dblockdiag.nnz() << " non-zeros." << endl;
	cerr << Dblockdiag << endl;

	cerr << "P = ";
	for (auto val: perm) {
		cerr << val << " ";
	}
	cerr << endl;

	MyPermutation P(perm);
	MatrixXd I(perm.size(), perm.size());
	I.setIdentity();
	cerr << "P = " << endl << (P * I) << endl;

	auto L = lilc_to_eigen(Llilc);
	cerr << "L = " << endl << L << endl;

	auto D = block_diag_to_eigen(Dblockdiag);
	cerr << "D = " << endl << D << endl;

	auto S = diag_to_eigen(Alilc.S);
	cerr << "S = " << endl << S.toDenseMatrix() << endl;

	auto Btmp = lilc_to_eigen(Alilc, true);
	cerr << "B = " << endl << Btmp << endl;
	cerr << "P^T * S * A * S * P = " << endl << (P.transpose() * (S * Aorg * S) * P) << endl;
	cerr << "L * D * L^T = " << endl << (L * D * L.transpose()) << endl;

	cerr << endl << endl;
	auto SiPLDLtPtSi = S.inverse() * (P * L * D * L.transpose() * P.transpose()) * S.inverse();
	cerr << " S^-1 * P * L * D * L^T * P^T * S^-1 = " << endl
	     << SiPLDLtPtSi << endl;

	REQUIRE((SiPLDLtPtSi - Aorg).norm() <= 1e-6);

	cerr << endl << endl;

	MatrixXd B(n, n);
	MatrixXd Q(n, n);
	VectorXd tau(n);
	VectorXd lambda(n);
	B.setZero();
	Q.setZero();

	SelfAdjointEigenSolver<MatrixXd> eigensolver;

	double delta = 1e-10;

	// Extract the block diagonal matrix.
	bool onebyone;
	for (int i = 0; i < n; i = (onebyone ? i+1 : i+2) ) {
		onebyone = (i == n-1 || D(i+1, i) == 0.0);

		if ( onebyone ) {
		    B(i, i) = D(i, i);
			lambda(i) = B(i, i);
			if (lambda(i) >= delta) {
				tau(i) = 0;
			}
			else {
				tau(i) = delta - lambda(i);
			}
			Q(i, i) = 1;
		}
		else {
		    auto a11 = D(i, i);
		    auto a22 = D(i+1, i+1);
		    auto a12 = D(i+1, i);
			B(i,   i)   = a11;
			B(i+1, i)   = a12;
			B(i,   i+1) = a12;
			B(i+1, i+1) = a22;
			eigensolver.compute(B.block(i, i, 2, 2));

			lambda(i)   = eigensolver.eigenvalues()(0);
			lambda(i+1) = eigensolver.eigenvalues()(1);
			for (int k = i; k <= i + 1; ++k) {
				if (lambda(k) >= delta) {
					tau(k) = 0;
				}
				else {
					tau(k) = delta - lambda(k);
				}
			}

			Q.block(i, i, 2, 2) = eigensolver.eigenvectors();
		}
	}

	cerr << "B = \n" << B << endl << endl;
	cerr << "lambda = " << tau.transpose() << endl << endl;
	cerr << "tau = " << tau.transpose() << endl << endl;
	cerr << "Q = \n" << Q << endl << endl;
	cerr << "Q*lambda*Q^T = \n" << Q * lambda.asDiagonal() * Q.transpose() << endl << endl;

	// Check that the block-wise eigendecomposition was correct.
	CHECK(((B - Q * lambda.asDiagonal() * Q.transpose()).norm()) < 1e-10);

	MatrixXd F = Q * tau.asDiagonal() * Q.transpose();
	cerr << "F = Q*tau*Q^T = \n" << F << endl << endl;
	cerr << "B + F = \n" << B + F << endl << endl;

	// Check that solver works correctly.
	VectorXd b(4);
	b(0) = 2.0;
	b(1) = 1.0;
	b(2) = 3.0;
	b(3) = -1.0;

	VectorXd xorg = Aorg.lu().solve(b);
	VectorXd x;
	solve_system_ildl(Dblockdiag, Llilc, S, P, b, &x);
	CAPTURE(xorg.transpose());
	CAPTURE(x.transpose());
	CHECK( (xorg - x).norm() <= 1e-6 );
}

TEST_CASE("ildl-sym-sparse")
{
	using namespace std;
	using namespace Eigen;
	using namespace spii;

	int n = 8;
	vector<Triplet<double>> triplets;
	auto add_element = [&](int i, int j, double v)
	{
		triplets.emplace_back(i - 1, j- 1, v);
	};
	// Matlab indices which start at 1.
	add_element(1,1, 19);
	add_element(2,1,  7);
	add_element(4,1, 20);
	add_element(6,1,-15);
	add_element(8,1, -6);
	add_element(1,2,  7);
	add_element(2,2, -8);
	add_element(4,2,  4);
	add_element(6,2, -3);
	add_element(8,2, -3);
	add_element(4,3,  4);
	add_element(8,3,  1);
	add_element(1,4, 20);
	add_element(2,4,  4);
	add_element(3,4,  4);
	add_element(4,4, 23);
	add_element(6,4,-12);
	add_element(8,4,  4);
	add_element(5,5,-10);
	add_element(1,6,-15);
	add_element(2,6, -3);
	add_element(4,6,-12);
	add_element(6,6, -1);
	add_element(7,7, -1);
	add_element(1,8, -6);
	add_element(2,8, -3);
	add_element(3,8,  1);
	add_element(4,8,  4);

	SparseMatrix<double> Aorg(n, n);
	Aorg.setFromTriplets(begin(triplets), end(triplets));

	cerr << Aorg << endl;

	// Create sym-ildl matrix.
	lilc_matrix<double> Alilc;
	eigen_to_lilc(Aorg, &Alilc);

	SparseMatrix<double> A;
	lilc_to_eigen(Alilc, &A, true);

	cerr << "Original A = " << endl;
	cerr << Aorg << endl;
	cerr << "sym-ildl A = " << endl;
	cerr << Alilc << endl;
	cerr << "A converted back = " << endl;
	cerr << A << endl;

	SparseMatrix<double> Zero = A - Aorg;
	CHECK(Zero.sum() == 0);

	cerr << "A is " << Alilc.n_rows() << " by " << Alilc.n_cols() << " with " << Alilc.nnz() << " non-zeros." << endl;
	lilc_matrix<double> Llilc;	     //<The lower triangular factor of A.
	vector<int> perm;	         //<A permutation vector containing all permutations on A.
	perm.reserve(Alilc.n_cols());
	block_diag_matrix<double> Dblockdiag; //<The diagonal factor of A.
	Alilc.sym_equil();
	Alilc.sym_rcm(perm);
	Alilc.sym_perm(perm);
	const double fill_factor = 1.0;
	const double tol         = 0.001;

	Alilc.ildl(Llilc, Dblockdiag, perm, fill_factor, tol, 1.0);

	cerr << "L is " << Llilc.n_rows() << " by " << Llilc.n_cols() << " with " << Llilc.nnz() << " non-zeros." << endl;
	cerr << Llilc << endl;
	cerr << "D is " << Dblockdiag.n_rows() << " by " << Dblockdiag.n_cols() << " with " << Dblockdiag.nnz() << " non-zeros." << endl;
	cerr << Dblockdiag << endl;

	cerr << "P = ";
	for (auto val: perm) {
		cerr << val << " ";
	}
	cerr << endl;

	MyPermutation P(perm);
	MatrixXd I(perm.size(), perm.size());
	I.setIdentity();
	cerr << "P = " << endl << (P * I) << endl;

	auto L = lilc_to_eigen(Llilc);
	cerr << "L = " << endl << L << endl;

	SparseMatrix<double> D;
	block_diag_to_eigen(Dblockdiag, &D);
	cerr << "D = " << endl << D << endl;

	auto S = diag_to_eigen(Alilc.S);
	cerr << "S = " << endl << S.toDenseMatrix() << endl;

	SparseMatrix<double> Btmp;
	lilc_to_eigen(Alilc, &Btmp, true);
	cerr << "B = " << endl << Btmp.toDense() << endl;
	cerr << "P^T * S * A * S * P = " << endl << (P.transpose() * (S * A.toDense() * S) * P) << endl;
	cerr << "L * D * L^T = " << endl << (L * D.toDense() * L.transpose()) << endl;

	cerr << endl << endl;
	auto SiPLDLtPtSi = S.inverse() * (P * L * D * L.transpose() * P.transpose()) * S.inverse();
	cerr << " S^-1 * P * L * D * L^T * P^T * S^-1 = " << endl
	     << SiPLDLtPtSi << endl;

	REQUIRE((SiPLDLtPtSi - Aorg.toDense()).norm() <= 1e-6);

	cerr << endl << endl;
}

#endif // #ifndef USE_SYM_ILDL


#include <vector>

//#include <Qt3DIncludes.h>
#include <GaussIncludes.h>
#include <ForceSpring.h>
#include <FEMIncludes.h>
#include <PhysicalSystemParticles.h>
//Any extra things I need such as constraints
#include <ConstraintFixedPoint.h>
#include <TimeStepperEulerImplicitLinear.h>
#include <TimeStepperEulerImplicit.h>
#include <AssemblerParallel.h>

#include <igl/get_seconds.h>
#include <igl/writeDMAT.h>
#include <igl/readDMAT.h>
#include <igl/viewer/Viewer.h>
#include <igl/readMESH.h>
#include <igl/unproject_onto_mesh.h>

#include <stdlib.h>
#include <iostream>
#include <string>
#include <sstream>
#include <string.h>
#include <json.hpp>
#include <boost/filesystem.hpp>

using Eigen::VectorXd;
using Eigen::Vector3d;
using Eigen::VectorXi;
using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::SparseMatrix;
using Eigen::SparseVector;

namespace fs = boost::filesystem;
using json = nlohmann::json;
using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem; //For Force Spring

typedef PhysicalSystemFEM<double, NeohookeanTet> NeohookeanTets;
typedef World<double,
                        std::tuple<PhysicalSystemParticleSingle<double> *, NeohookeanTets *>,
                        std::tuple<ForceSpringFEMParticle<double> *>,
                        std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
typedef TimeStepperEulerImplicit<double, AssemblerEigenSparseMatrix<double>,
 AssemblerEigenVector<double> > MyTimeStepper;

json sim_params;
MatrixXd V;
MatrixXi F, T;

// Take a start and a range so that we can do this in parallel.
void generateEnergyForPoses(NeohookeanTets *tets, const MatrixXd &displacements, int start, int num, MatrixXd &energy_vec_per_pose) {
    MyWorld world;
    world.addSystem(tets);
    fixDisplacementMin(world, tets, sim_params["displacement_axis"], sim_params["displacement_tol"]);
    world.finalize();

    for(int i = start; i - start < num && i < displacements.rows(); i++) {
        // Update the state
        auto q = mapDOFEigen(tets->getQ(), world);
        q = displacements.row(i);

        // Compute the energy
        energy_vec_per_pose.row(i) = tets->getStrainEnergyPerElement(world.getState());
    }
}

void progress_bar(double progress, int barWidth = 70) {
    std::cout << "[";
    int pos = barWidth * progress;
    for (int i = 0; i < barWidth; ++i) {
        if (i < pos) std::cout << "=";
        else if (i == pos) std::cout << ">";
        else std::cout << " ";
    }
    std::cout << "] " << int(progress * 100.0) << " %\r";
    std::cout.flush();
}

int main(int argc, char **argv) {
    fs::path displacements_path(argv[1]); // Takes in a path to a matrix containing an array of flattened displacements
    fs::path mesh_path(argv[2]); // Also takes in a path to the base mesh file.
    fs::path sim_params_path(argv[3]); // Also takes path to the simulation parameters file

    fs::path output_dir_path = displacements_path.parent_path();

    std::cout << "Loading simulation parameters from " << sim_params_path.string() << std::endl;
    std::ifstream fin(sim_params_path.string());
    fin >> sim_params;

    std::cout << "Loading MESH from " << mesh_path.string() << std::endl;
    igl::readMESH(mesh_path.string(), V, T, F);

    std::cout << "Loading displacements from " << displacements_path.string() << std::endl;
    MatrixXd displacements;
    igl::readDMAT(displacements_path.string(), displacements);

    // Time to set up the Tets
    NeohookeanTets tets(V,T);
    for(auto element: tets.getImpl().getElements()) {
        element->setDensity(sim_params["density"]);
        element->setParameters(sim_params["YM"], sim_params["Poisson"]);
    }

    // Reserve space for the matrices
    int n_tets = T.rows();
    int n_poses = displacements.rows();
    MatrixXd energy_vec_per_pose(n_poses, n_tets);
    // MatrixXd force_vec_per_pose(n_poses, n_tets * 4); // Leave this out for now

    // Generate the data
    // TODO parallel?
    std::cout << "Generating energies" << std::endl;
    int n_chunks = 32;
    int num_per_chunk = displacements.rows() / n_chunks;
    int n_finished = 0;
    #pragma omp parallel for
    for(int i = 0; i < n_chunks + 1; i++) {
        generateEnergyForPoses(&tets, displacements, i * num_per_chunk, num_per_chunk, energy_vec_per_pose);
        progress_bar(++n_finished / (double) (n_chunks + 1));
    }
    std::cout << std::endl;

    // Save the data
    fs::path energy_path = output_dir_path / "energies.dmat";
    std::cout << "Saving energy to " << energy_path << std::endl;
    igl::writeDMAT(energy_path.string(), energy_vec_per_pose, false);

    return 0;
}


#include <pnp/learning_plan/exp/SoftMax.h>

#include <boost/bind.hpp>

#include <cmath>
#include <functional>
#include <numeric>
#include <algorithm>

#include <stdlib.h>

using namespace std;
using boost::bind;

namespace learnpnp {

SoftMax::SoftMax(double tau, bool remember) : ExpPolicy(remember), tau(tau) {}

int SoftMax::makeChoice(Learner *learner,const Marking &current, const std::vector<Marking> &states) {

    //this can be impelemented with fewer implicit loops, but I don't want to do it unless some profiling shows
    //it's really necessary to sacrifice readability for efficiency

	vector<double> values;
    transform(states.begin(),states.end(),std::back_inserter(values),
              bind(&Learner::valueOf,learner,_1));

    double min = *min_element(values.begin(),values.end());
    if (min < 0) {
        //make the values positive
        transform(values.begin(),values.end(),values.begin(),bind(plus<double>(),_1,min));
    }

    vector<double> weights;
    transform(values.begin(),values.end(),back_inserter(weights),bind(&SoftMax::computeWeight,this,_1));

    double sum = accumulate(weights.begin(),weights.end(),0.0);

    //normalize the weight with respect to the sum and RAND_MAX
    // for each weight w computes w/sum * RAND_MAX
    transform(weights.begin(),weights.end(),weights.begin(),
              bind(multiplies<double>(),bind(divides<double>(),_1,sum), RAND_MAX));

	//cumulative sums
	/* An example: imagine the normalized weights are 0.2 * RAND_MAX,
	0.3 * RAND_MAX, and 0.5 * RAND_MAX
	we drop RAND_MAX for readability now, the cumulative sums are
	0.2, 0.5, 1
	we extract a value v in [0,1] (in [0,RAND_MAX] in practice)
	and look for the first value among the cumulative sums that is >= v
	*/
	vector<double>cumulative;
	partial_sum(weights.begin(),weights.end(),back_inserter(cumulative));

	int v = rand();

	vector<double>::iterator result = find_if(cumulative.begin(),cumulative.end(),bind(greater_equal<double>(),_1,v));

	return distance(cumulative.begin(),result);

}

double SoftMax::computeWeight(double value) {
    return exp ( value  /tau );
}

}


#include <utt/Configuration.h>

#include <utt/PolyCrypto.h>
#include <utt/NtlLib.h>

#include <vector>
#include <cmath>
#include <iostream>
#include <ctime>
#include <fstream>

#include <libfqfft/polynomial_arithmetic/basic_operations.hpp>
#include <libff/common/double.hpp>

#include <xutils/Log.h>
#include <xutils/Timer.h>
#include <xassert/XAssert.h>

#include <NTL/ZZ_pX.h>

using namespace std;
using namespace libfqfft;
using namespace libutt;

int main() {
    initialize(nullptr, 0);
    std::vector<G1> bases;
    int count = 3, numBench = 0;
    double avgPctage = 0.0;
    size_t maxSize = 1024*128;

    loginfo << "Picking " << maxSize << " random G1 elements to bench multiexponentiation with...";
    bases.resize(maxSize);
    for(size_t i = 0; i < bases.size(); i++) {
        bases[i] = G1::random_element();
    }
    std::cout << endl;

    //for (size_t i = 1; i <= 1024*128; i *= 2) {
    for(size_t i = maxSize; i >= 1; i /= 2) {
        logperf << "poly degree = " << i-1 << ", iters = " << count
                << endl;
        numBench++;
        AveragingTimer conv, exp;

        ZZ_pX poly;
        std::vector<Fr> ffpoly;

        bases.resize(i);

        for (int rep = 0; rep < count; rep++) {
            random(poly, static_cast<long>(i));

            conv.startLap();
            //conv_zp_fr(poly, ffpoly);
            convNtlToLibff(poly, ffpoly);
            conv.endLap();

            exp.startLap();
            multiExp(bases, ffpoly);
            exp.endLap();
        }

        auto avgConv = conv.averageLapTime(), avgExp = exp.averageLapTime();
        double pctage = (double)avgConv/(double)(avgConv+avgExp)*100.0;
        logperf << " + Conv to libff: " << (double) avgConv / 1000000 << " seconds." << endl;
        logperf << " + Multiexp:      " << (double) avgExp / 1000000 << " seconds." << endl;
        logperf << " + " << pctage << "% time spent converting" << endl;
        logperf << endl;

        avgPctage += pctage;
    }

    avgPctage /= numBench;

    logperf << endl;
    logperf << "On average, " << avgPctage << "% time spent converting" << endl;
    return 0;
}


// Software License for MTL
//
// Copyright (C) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>

// #define MTL_HAS_BLAS
// #define MTL_USE_OPTERON_OPTIMIZATION

#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/recursion/bit_masking.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/dmat_dmat_mult.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/operation/mult_assign_mode.hpp>
#include <boost/numeric/mtl/recursion/base_case_test.hpp>

using namespace std;

template <typename MatrixA, typename MatrixB, typename MatrixC>
void test(MatrixA& A, MatrixB& B, MatrixC& C, const char* name)
{
    using mtl::assign::assign_sum; using mtl::assign::plus_sum;
    using mtl::assign::minus_sum; using mtl::assign::mult_assign_mode;
    using mtl::recursion::bound_test_static; using mtl::gen_recursive_dmat_dmat_mult_t;

    hessian_setup(A, 1.0);
    hessian_setup(B, 2.0);

    std::cout << "\n" << name << "  --- calling simple mult:\n"; std::cout.flush();
    typedef mtl::gen_dmat_dmat_mult_t<>  mult_t;
    mult_t                               mult;

    mult(A, B, C);
    // cout << "correct result is:\n" << with_format(C, 5, 3);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    typedef mtl::gen_dmat_dmat_mult_t<plus_sum>  add_mult_t;
    add_mult_t add_mult;

    add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    typedef mtl::gen_dmat_dmat_mult_t<minus_sum>  minus_mult_t;
    minus_mult_t minus_mult;

    minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

#if 0
    std::cout << "\n" << name << "  --- calling mult with cursors and property maps:\n"; std::cout.flush();
    gen_cursor_dmat_dmat_mult_t<>  cursor_mult;

    cursor_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    gen_cursor_dmat_dmat_mult_t<plus_sum>  cursor_add_mult;

    cursor_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    gen_cursor_dmat_dmat_mult_t<minus_sum>  cursor_minus_mult;

    cursor_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);
#endif
    std::cout << "\n" << name << "  --- calling mult with tiling:\n"; std::cout.flush();
    typedef mtl::gen_tiling_dmat_dmat_mult_t<2, 2>  tiling_mult_t;
    tiling_mult_t tiling_mult;

    tiling_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    typedef mtl::gen_tiling_dmat_dmat_mult_t<2, 2, plus_sum>  tiling_add_mult_t;
    tiling_add_mult_t tiling_add_mult;

    tiling_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    typedef mtl::gen_tiling_dmat_dmat_mult_t<2, 2, minus_sum>  tiling_minus_mult_t;
    tiling_minus_mult_t tiling_minus_mult;

    tiling_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    std::cout << "\n" << name << "  --- calling mult with tiling 2x2:\n"; std::cout.flush();
    typedef mtl::gen_tiling_22_dmat_dmat_mult_t<>  tiling_22_mult_t;
    tiling_22_mult_t tiling_22_mult;

    tiling_22_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    // typedef gen_tiling_22_dmat_dmat_mult_t<plus_sum>  tiling_22_add_mult_t;
    typedef typename mult_assign_mode<tiling_22_mult_t, plus_sum>::type   tiling_22_add_mult_t;
    tiling_22_add_mult_t tiling_22_add_mult;

    tiling_22_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    typedef mtl::gen_tiling_22_dmat_dmat_mult_t<minus_sum>  tiling_22_minus_mult_t;
    tiling_22_minus_mult_t tiling_22_minus_mult;

    tiling_22_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    std::cout << "\n" << name << "  --- calling mult with tiling 4x4:\n"; std::cout.flush();
    typedef mtl::gen_tiling_44_dmat_dmat_mult_t<>  tiling_44_mult_t;
    tiling_44_mult_t tiling_44_mult;

    tiling_44_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    MatrixA A9(9, 9); MatrixB B9(9, 9); MatrixC C9(9, 9); // to test all block in 4x4 blocking for better coverage
    hessian_setup(A9, 1.0); hessian_setup(B9, 2.0);

    tiling_44_mult(A9, B9, C9);
    check_hessian_matrix_product(C9, num_cols(A9));

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    typedef mtl::gen_tiling_44_dmat_dmat_mult_t<plus_sum>  tiling_44_add_mult_t;
    tiling_44_add_mult_t tiling_44_add_mult;

    tiling_44_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    typedef mtl::gen_tiling_44_dmat_dmat_mult_t<minus_sum>  tiling_44_minus_mult_t;
    tiling_44_minus_mult_t tiling_44_minus_mult;

    tiling_44_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    typedef mtl::gen_recursive_dmat_dmat_mult_t<add_mult_t, bound_test_static<2>, plus_sum>  recursive_add_mult_t;

    std::cout << "\n" << name << "  --- calling mult recursively:\n"; std::cout.flush();
    // The recursive functor is C= A*B but the base case must be C+= A*B !!!!!!
    // gen_recursive_dmat_dmat_mult_t<add_mult_t, bound_test_static<32> >  recursive_mult;
    typename mult_assign_mode<recursive_add_mult_t, assign_sum>::type	recursive_mult;

    recursive_mult(A, B, C);
    // cout << "recursive result is:\n" << with_format(C, 5, 3);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    recursive_add_mult_t   recursive_add_mult;

    recursive_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    // gen_recursive_dmat_dmat_mult_t<minus_mult_t, bound_test_static<32>, minus_sum>  recursive_minus_mult;
    // check assign mode substitution both on matrix and on block level
    typename mult_assign_mode<recursive_add_mult_t, minus_sum>::type	recursive_minus_mult;
    recursive_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    std::cout << "\n" << name << "  --- calling mult recursively with tiling:\n"; std::cout.flush();
    // The recursive functor is C= A*B but the base case must be C+= A*B !!!!!!
    gen_recursive_dmat_dmat_mult_t<tiling_add_mult_t, bound_test_static<32> >  recursive_tiling_mult;

    recursive_tiling_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    gen_recursive_dmat_dmat_mult_t<tiling_add_mult_t, bound_test_static<32>, plus_sum>  recursive_tiling_add_mult;

    recursive_tiling_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    gen_recursive_dmat_dmat_mult_t<tiling_minus_mult_t, bound_test_static<32>, minus_sum>  recursive_tiling_minus_mult;

    recursive_tiling_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    std::cout << "\n" << name << "  --- calling mult recursively platform specific plus tiling:\n"; std::cout.flush();
    typedef mtl::gen_platform_dmat_dmat_mult_t<plus_sum, tiling_add_mult_t> platform_tiling_add_mult_t;
    gen_recursive_dmat_dmat_mult_t<platform_tiling_add_mult_t, bound_test_static<32> >  recursive_platform_tiling_mult;

    recursive_platform_tiling_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

#ifdef MTL_HAS_BLAS
    std::cout << "\n" << name << "  --- calling blas mult:\n"; std::cout.flush();
    gen_blas_dmat_dmat_mult_t<>  blas_mult;
    blas_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());
#endif

#ifdef MTL_USE_OPTERON_OPTIMIZATION
    std::cout << "\n" << name << "  --- calling platform specific mult (empty):\n"; std::cout.flush();
    gen_platform_dmat_dmat_mult_t<>  platform_mult;
    platform_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check += :\n"; std::cout.flush();
    gen_platform_dmat_dmat_mult_t<plus_sum>  platform_add_mult;

    platform_add_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check -= :\n"; std::cout.flush();
    gen_platform_dmat_dmat_mult_t<minus_sum>  platform_minus_mult;

    platform_minus_mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

#endif

    std::cout << "\n" << name << "  --- using mult(A, B, C) :\n"; std::cout.flush();
    mult(A, B, C);
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    std::cout << "\n" << name << "  --- called as C= A * B:\n"; std::cout.flush();
    C= A * B;
    check_hessian_matrix_product(C, A.num_cols());

    std::cout << "\n" << name << "  --- check C+= A * B:\n"; std::cout.flush();
    C+= A * B;
    check_hessian_matrix_product(C, A.num_cols(), 2.0);

    std::cout << "\n" << name << "  --- check C-= A * B:\n"; std::cout.flush();
    C-= A * B;
    check_hessian_matrix_product(C, A.num_cols(), 1.0);

    if (A.num_cols() <= 10)
	std::cout << A << "\n" << B << "\n" << with_format(C, 4, 4) << "\n";

}

template <typename MatrixA, typename MatrixB, typename MatrixC>
void single_test(MatrixA& A, MatrixB& B, MatrixC& C, const char*)
{
    using mtl::assign::plus_sum; using mtl::assign::minus_sum;
    using mtl::recursion::bound_test_static;

    std::cout << "\n\n before matrix multiplication:\n";
    std::cout << "A:\n" << A;
    std::cout << "B:\n" << B;
    std::cout << "C:\n" << C << '\n';

    typedef mtl::gen_tiling_dmat_dmat_mult_t<2, 2, plus_sum>  tiling_add_mult_t;
    tiling_add_mult_t tiling_add_mult;
    tiling_add_mult(A, B, C);

    std::cout << "\n\n after matrix multiplication:\n";
    std::cout << "A:\n" << A;
    std::cout << "B:\n" << B;
    std::cout << "C:\n" << C << '\n';
}

#ifdef MTL_HAS_BLAS
extern "C" {
void dgemm_(const char* transa, const char* transb,
	    const int* m, const int* n, const int* k,
	    const double* alpha,  const double *da,  const int* lda,
	    const double *db, const int* ldb, const double* dbeta,
	    double *dc, const int* ldc);
}

typedef mtl::dense2D<double, mtl::mat::parameters<col_major> >        dc_t;

struct dgemm_t
{
    void operator()(const dc_t& A, const dc_t& B, dc_t& C)
    {
	int size= A.num_rows();
	double alpha= 1.0, beta= 0.0;
	dgemm_("N", "N", &size, &size, &size, &alpha,
	       const_cast<double*>(&A[0][0]), &size, const_cast<double*>(&B[0][0]),
	       &size, &beta, &C[0][0], &size);

    }
};

void test_blas()
{
    mtl::dense2D<double, mtl::mat::parameters<col_major> > A(7, 7), B(7, 7), C(7, 7);
    hessian_setup(A, 1.0);
    hessian_setup(B, 2.0);
    dgemm_t()(A, B, C);

    std::cout << C;
    check_hessian_matrix_product(C, A.num_cols());

}

#endif // MTL_HAS_BLAS

int main(int argc, char* argv[])
{
    using namespace mtl;

    // Bitmasks:
    const unsigned long morton_mask= generate_mask<true, 0, row_major, 0>::value,
	doppled_32_row_mask_no_shark= generate_mask<true, 5, row_major, 0>::value,
	doppled_32_col_mask_no_shark= generate_mask<true, 5, col_major, 0>::value,
	doppled_32_row_mask= generate_mask<true, 5, row_major, 1>::value,
	doppled_32_col_mask= generate_mask<true, 5, col_major, 1>::value,
	doppled_z_32_row_mask= generate_mask<false, 5, row_major, 1>::value,
	doppled_z_32_col_mask= generate_mask<false, 5, col_major, 1>::value;

    unsigned size= 5;
    if (argc > 1) size= atoi(argv[1]);
    if (size < 2) size= 2;

    dense2D<double>                                  da(size, size-1), db(size-1, size-2), dc(size, size-2);
    dense2D<double, mat::parameters<col_major> >  dca(size, size-1), dcb(size-1, size-2), dcc(size, size-2);
    dense2D<float>                                   fa(size, size-1), fb(size-1, size-2), fc(size, size-2);
    dense2D<float, mat::parameters<col_major> >   fca(size, size-1), fcb(size-1, size-2), fcc(size, size-2);
    morton_dense<double,  morton_mask>               mda(size, size-1), mdb(size-1, size-2), mdc(size, size-2);

    typedef morton_dense<double, doppled_32_row_mask_no_shark>  morton_t;
    morton_dense<double, doppled_32_row_mask_no_shark>      mrans(size, size-1), mrbns(size-1, size-2), mrcns(size, size-2);;
    morton_dense<double, doppled_32_col_mask_no_shark>      mcans(size, size-1), mcbns(size-1, size-2), mccns(size, size-2);
    morton_dense<double, doppled_32_col_mask>      mca(size, size-1), mcb(size-1, size-2), mcc(size, size-2);
    morton_dense<double, doppled_32_row_mask>      mra(size, size-1), mrb(size-1, size-2), mrc(size, size-2);
    morton_dense<double, doppled_z_32_col_mask>    mzca(size, size-1), mzcb(size-1, size-2), mzcc(size, size-2);
    morton_dense<double, doppled_z_32_row_mask>    mzra(size, size-1), mzrb(size-1, size-2), mzrc(size, size-2);
    morton_dense<float, doppled_32_col_mask>       mcaf(size, size-1), mcbf(size-1, size-2), mccf(size, size-2);
    morton_dense<float, doppled_32_row_mask>       mraf(size, size-1), mrbf(size-1, size-2), mrcf(size, size-2);

    transposed_view<dense2D<double> > trans_db(db);
    transposed_view<morton_t >        trans_mrbns(mrbns);

    std::cout << "Testing different products\n";

#if 0
    test(da, trans_db, dc, "dense2D and transposed dense2D");
    test(mrans, trans_mrbns, mrcns, "hybrid with transposed matrix");
#endif

    test(da, db, dc, "dense2D");
    test(dca, dcb, dcc, "dense2D col-major");
    test(da, dcb, dc, "dense2D row x column-major");
    test(fa, fcb, fc, "dense2D float, row x column-major");
    test(da, fcb, fc, "dense2D mixed, dense and float");
    test(mda, mdb, mdc, "pure Morton");
    test(mca, mcb, mcc, "Hybrid col-major");
    test(mra, mrb, mrc, "Hybrid row-major");
    test(mrans, mcbns, mrcns, "Hybrid col-major and row-major, no shark tooth");
    test(mrans, mrbns, mrcns, "Hybrid row-major, no shark tooth");
    test(mraf, mcbf, mrcf, "Hybrid col-major and row-major with float");
    test(mra, mcb, mrc, "Hybrid col-major and row-major");
    test(mzra, mzcb, mzrc, "Hybrid col-major and row-major, Z-order");
    test(mra, mzcb, mzrc, "Hybrid col-major and row-major, Z and E-order");
    test(mra, dcb, mzrc, "Hybrid col-major and row-major, Z and E-order mixed with dense2D");
    test(mra, db, mrcns, "Hybric matrix = Shark * dense2D");
    test(mrans, db, mccns, "Hybric matrix (col-major) = hybrid (row) * dense2D");

    return 0;
}


#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