#include "splineGeneration.h"

#include <Eigen/Dense>
#include <iostream>
#include <sstream>
#include <numeric>
#include <stdexcept>

using namespace std;
using namespace Eigen;

template <typename DerivedM>
void setConstraintMatrixPart(double time, int derivative_order, MatrixBase<DerivedM> & constraint_matrix, double scaling = 1.0)
{
  double time_power = 1.0;
  typename MatrixBase<DerivedM>::Index num_coefficients = constraint_matrix.cols();

  for (int col = derivative_order; col < num_coefficients; col++)
  {
     double column_power = 1.0;
     for (int i = 0; i< derivative_order; i++)
     {
        column_power *= (col - i);
     }
     constraint_matrix(0, col) = scaling * time_power * column_power;
     time_power *= time;
  }
}

PiecewisePolynomial<double> generateSpline(const SplineInformation& spline_information) {
  int num_segments = spline_information.getNumberOfSegments();
  int num_constraints = spline_information.getNumberOfConstraints();
  int num_coefficients = spline_information.getTotalNumberOfCoefficients();

  if (num_constraints != num_coefficients) {
    stringstream msg;
    msg << "Only the case where the number of coefficients equals the number of variables is currently handled." << endl;
    msg << "Number of coefficients: " << num_coefficients << ", Number of constraints: " << num_constraints << endl;
    throw runtime_error(msg.str().c_str());
  }

  MatrixXd constraint_matrix = MatrixXd::Zero(num_constraints, num_coefficients);
  VectorXd right_hand_side(num_constraints); // should get overwritten completely

  int constraint_row_start = 0;
  std::vector<int> segment_col_starts;
  int segment_col_start = 0;
  for (int i = 0; i < num_segments; i++) {
    segment_col_starts.push_back(segment_col_start);
    segment_col_start += spline_information.getNumberOfCoefficients(i);
  }

  // handle value constraints
  for (int i = 0; i < num_segments; i++) {
    std::vector<ValueConstraint> const & value_constraints = spline_information.getValueConstraints(i);
    for (auto it = value_constraints.begin(); it != value_constraints.end(); ++it) {
      const ValueConstraint& constraint = *it;
      int number_of_coefficients = spline_information.getNumberOfCoefficients(i);
      int segment_col_start = segment_col_starts[i];
      auto constraint_matrix_segment_part = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_start, 1, number_of_coefficients);
      double t_local = constraint.getTime() - spline_information.getStartTime(i);
      setConstraintMatrixPart(t_local, constraint.getDerivativeOrder(), constraint_matrix_segment_part);
      right_hand_side(constraint_row_start) = constraint.getValue();
      constraint_row_start += 1;
    }
  }

  // handle continuity constraints
  std::vector<ContinuityConstraint> continuity_constraints = spline_information.getContinuityConstraints();
  for (auto it = continuity_constraints.begin(); it != continuity_constraints.end(); ++it) {
    const ContinuityConstraint& constraint = *it;
    int first_spline_index = constraint.getFirstSplineIndex();
    int number_of_coefficients_1 = spline_information.getNumberOfCoefficients(first_spline_index);
    auto constraint_matrix_segment_part_1 = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_starts[first_spline_index], 1, number_of_coefficients_1);
    double t_local_1 = spline_information.getEndTime(first_spline_index) - spline_information.getStartTime(first_spline_index);
    setConstraintMatrixPart(t_local_1, constraint.getDerivativeOrder(), constraint_matrix_segment_part_1, 1.0);

    int second_spline_index = constraint.getSecondSplineIndex();
    int number_of_coefficients_2 = spline_information.getNumberOfCoefficients(second_spline_index);
    auto constraint_matrix_segment_part_2 = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_starts[second_spline_index], 1, number_of_coefficients_2);
    setConstraintMatrixPart(0.0, constraint.getDerivativeOrder(), constraint_matrix_segment_part_2, -1.0);

    right_hand_side(constraint_row_start) = 0.0;

    constraint_row_start += 1;
  }

  // solve
  auto decomposition = constraint_matrix.colPivHouseholderQr();
  if (!decomposition.isInvertible())
    throw ConstraintMatrixSingularError();
  VectorXd solution = decomposition.solve(right_hand_side);

  // create Polynomials
  std::vector<Polynomial<double>> polynomials;
  for (int i = 0; i < num_segments; i++) {
    auto coefficients = solution.segment(segment_col_starts[i], spline_information.getNumberOfCoefficients(i));
    polynomials.push_back(Polynomial<double>(coefficients));
  }

  // return a PiecewisePolynomial
  return PiecewisePolynomial<double>(polynomials, spline_information.getSegmentTimes());
}

PiecewisePolynomial<double> twoWaypointCubicSpline(const vector<double>& segment_times, double x0, double xd0, double xf, double xdf, double x1, double x2) {
  const int num_segments = 3;
  assert(segment_times.size() == num_segments + 1);

  int polynomial_order = 3;
  vector<int> segment_polynomial_orders;
  for (int i = 0; i < num_segments; ++i) {
    segment_polynomial_orders.push_back(polynomial_order);
  }

  SplineInformation spline_information(segment_polynomial_orders, segment_times);
  spline_information.addValueConstraint(0, ValueConstraint(0, spline_information.getStartTime(0), x0));
  spline_information.addValueConstraint(0, ValueConstraint(1, spline_information.getStartTime(0), xd0));
  spline_information.addValueConstraint(num_segments - 1, ValueConstraint(0, spline_information.getEndTime(num_segments - 1), xf));
  spline_information.addValueConstraint(num_segments - 1, ValueConstraint(1, spline_information.getEndTime(num_segments - 1), xdf));
  spline_information.addValueConstraint(1, ValueConstraint(0, spline_information.getStartTime(1), x1));
  spline_information.addValueConstraint(2, ValueConstraint(0, spline_information.getStartTime(2), x2));

  int num_knots = num_segments - 1;
  for (int i = 0; i < num_knots; i++) {
    for (int derivative_order = 0; derivative_order < 3; derivative_order++) {
      spline_information.addContinuityConstraint(ContinuityConstraint(derivative_order, i, i + 1));
    }
  }

  return generateSpline(spline_information);
}


#ifndef HYPSYS1D_RUNGE_KUTTA_HPP
#define HYPSYS1D_RUNGE_KUTTA_HPP

#include <Eigen/Dense>
#include <map>
#include <memory>

#include <ancse/includes.hpp>
#include <ancse/config.hpp>
#include <ancse/dg_limiting.hpp>
#include <ancse/boundary_condition.hpp>
#include <ancse/rate_of_change.hpp>

/// Interface advancing the solution of a PDE by one step.
class TimeIntegrator {
  public:
    virtual ~TimeIntegrator() = default;

    /// Update 'u1' starting from 'u0' using a time-step `dt`.
    virtual void operator()(Eigen::MatrixXd &u1,
                            const Eigen::MatrixXd &u0,
                            double dt) const = 0;
};

/// Runge-Kutta time integration methods.
/** Note: not all methods are Runge-Kutta methods. Therefore, 'RungeKutta' can
 *        not be the interface. It should be the base for all (explicit)
 *        Runge-Kutta methods.
 */
class RungeKutta : public TimeIntegrator {
  public:
    RungeKutta(std::shared_ptr<RateOfChange> rate_of_change_,
               std::shared_ptr<BoundaryCondition> boundary_condition_,
               std::shared_ptr<Limiting> limiting_)
  : rate_of_change(std::move(rate_of_change_)),
    boundary_condition(std::move(boundary_condition_)),
    limiting(std::move(limiting_))
  {}

  protected:
    void post_euler_step(Eigen::MatrixXd &u) const {
      (*boundary_condition)(u);
      if(limiting != nullptr) {
        (*limiting)(u);
      }
      (*boundary_condition)(u);
    }

  protected:
    std::shared_ptr<RateOfChange> rate_of_change;
    std::shared_ptr<BoundaryCondition> boundary_condition;
    std::shared_ptr<Limiting> limiting;
};

class ForwardEuler : public RungeKutta {
  private:
    using super = RungeKutta;

  public:
    ForwardEuler(std::shared_ptr<RateOfChange> rate_of_change_,
                 std::shared_ptr<BoundaryCondition> boundary_condition_,
                 std::shared_ptr<Limiting> limiting_,
                 int n_rows,
                 int n_cols)
        : super(std::move(rate_of_change_),
                std::move(boundary_condition_),
                std::move(limiting_)),
          dudt(n_rows, n_cols) {}

    virtual void operator()(Eigen::MatrixXd &u1,
                            const Eigen::MatrixXd &u0,
                            double dt) const override {

        (*rate_of_change)(dudt, u0);
        u1 = u0 + dt * dudt;
        post_euler_step(u1);
    }

  private:
    mutable Eigen::MatrixXd dudt;
};

// Mishra lecture notes 5.8.1. p. 76, cf. Toro p. 539
class SSP2 : public RungeKutta {
    private:
        using super= RungeKutta;

    public:
        SSP2(std::shared_ptr<RateOfChange> rate_of_change_,
             std::shared_ptr<BoundaryCondition> boundary_condition_,
             std::shared_ptr<Limiting> limiting_,
             int n_rows,
             int n_cols)
             : super(std::move(rate_of_change_),
                     std::move(boundary_condition_),
                     std::move(limiting_)),
               dudt(n_rows, n_cols) {}

        virtual void operator() (Eigen::MatrixXd &u1,
                                 const Eigen::MatrixXd &u0,
                                 double dt) const override
        {
            // u* step: using u1 as temp variable:
            (*rate_of_change)(dudt, u0);
            u1= u0 + dt * dudt;
            post_euler_step(u1);

            // u** step: again using u1 as temp variable:
            (*rate_of_change)(dudt, u1);
            u1= u1 + dt * dudt;
            post_euler_step(u1);

            // final RK2 assembly:
            u1= 0.5 * (u0 + u1);
            (*boundary_condition)(u1);
        }

    private:
        mutable Eigen::MatrixXd dudt;
};

/// make Runge Kutta for FVM
std::shared_ptr<RungeKutta>
make_runge_kutta(const nlohmann::json &config,
                 const std::shared_ptr<RateOfChange> &rate_of_change,
                 const std::shared_ptr<BoundaryCondition> &boundary_condition,
                 int n_rows,
                 int n_cols);

/// make Runge Kutta for DG
std::shared_ptr<RungeKutta>
make_runge_kutta(const nlohmann::json &config,
                 const std::shared_ptr<RateOfChange> &rate_of_change,
                 const std::shared_ptr<BoundaryCondition> &boundary_condition,
                 const std::shared_ptr<Limiting> &dg_limiting,
                 int n_rows,
                 int n_cols);

#endif // HYPSYS1D_RUNGE_KUTTA_HPP


#include "iris.h"
#include <stdexcept>
#include <iostream>
#include <numeric>
#include <chrono>
#include <Eigen/LU>
#include <Eigen/StdVector>
#include "iris/iris_mosek.h"
#include "iris/cvxgen_ldp.h"

namespace iris {

template <typename T>
std::vector<size_t> arg_sort(const std::vector<T> &vec) {
  std::vector<size_t> idx(vec.size());
  std::iota(idx.begin(), idx.end(), 0);
  std::sort(idx.begin(), idx.end(), [&vec](size_t i0, size_t i1) {return vec[i0] < vec[i1];});
  return idx;
}

typedef std::pair<Eigen::VectorXd, double> hyperplane;

hyperplane tangent_plane_through_point(const Ellipsoid &ellipsoid, const Eigen::MatrixXd &Cinv2, const Eigen::VectorXd &x) {
  Eigen::VectorXd nhat = (2 * Cinv2 * (x - ellipsoid.getD())).normalized();
  std::pair<Eigen::VectorXd, double> plane(nhat,
                                    nhat.transpose() * x);
  // std::cout << "tangent plane through point: " << x.transpose() << std::endl;
  // std::cout << plane.first.transpose() << " | " << plane.second << std::endl;
  return plane;
}

void choose_closest_point_solver(const Eigen::MatrixXd &Points, Eigen::VectorXd &result, MSKenv_t &env) {
  // std::cout << "points: " << std::endl << Points << std::endl;
  if (Points.rows() <= IRIS_CVXGEN_LDP_MAX_ROWS && Points.cols() <= IRIS_CVXGEN_LDP_MAX_COLS) {
    iris_cvxgen::closest_point_in_convex_hull(Points, result);
  } else {
    if (!env) {
      // std::cout << "making env" << std::endl;
      iris_mosek::check_res(MSK_makeenv(&env, NULL));
    }
    iris_mosek::closest_point_in_convex_hull(Points, result, &env);
  }
  // std::cout << "closest point: " << result.transpose() << std::endl;
}

void separating_hyperplanes(const std::vector<Eigen::MatrixXd> obstacle_pts, const Ellipsoid &ellipsoid, Polyhedron &polyhedron, bool &infeasible_start) {

  int dim = ellipsoid.getDimension();
  infeasible_start = false;
  int n_obs = obstacle_pts.size();

  if (n_obs == 0) {
    polyhedron.setA(Eigen::MatrixXd::Zero(0, dim));
    polyhedron.setB(Eigen::VectorXd::Zero(0));
    return;
  }

  Eigen::MatrixXd Cinv = ellipsoid.getC().inverse();
  Eigen::MatrixXd Cinv2 = Cinv * Cinv.transpose();

  Eigen::Matrix<bool, Eigen::Dynamic, 1> uncovered_obstacles = Eigen::Matrix<bool, Eigen::Dynamic, 1>::Constant(n_obs, true);

  std::vector<Eigen::MatrixXd> image_pts(n_obs);
  for (int i=0; i < n_obs; i++) {
    image_pts[i] = Cinv * (obstacle_pts[i].colwise() - ellipsoid.getD());
  }

  std::vector<Eigen::VectorXd> image_squared_dists(n_obs);
  for (int i=0; i < n_obs; i++) {
    image_squared_dists[i] = image_pts[i].colwise().squaredNorm();
  }

  std::vector<double> obs_min_squared_image_dists(n_obs);
  for (int i=0; i < n_obs; i++) {
    obs_min_squared_image_dists[i] = image_squared_dists[i].minCoeff();
  }
  std::vector<size_t> obs_sort_idx = arg_sort(obs_min_squared_image_dists);

  std::vector<std::pair<Eigen::VectorXd, double>> planes;

  MSKenv_t env = NULL;
  for (auto it = obs_sort_idx.begin(); it != obs_sort_idx.end(); ++it) {
    size_t i = *it;
    if (!uncovered_obstacles(i)) {
      continue;
    }
    Eigen::DenseIndex idx;
    image_squared_dists[i].minCoeff(&idx);
    hyperplane plane = tangent_plane_through_point(ellipsoid, Cinv2, obstacle_pts[i].col(idx));
    if ((((plane.first.transpose() * obstacle_pts[i]).array() - plane.second) >= 0).all()) {
      // nhat already separates the ellipsoid from obstacle i, so we can skip the optimization
      planes.push_back(plane);
    } else {
      Eigen::VectorXd ystar(dim);
      choose_closest_point_solver(image_pts[i], ystar, env);

      if (ystar.squaredNorm() < 1e-6) {
        // d is inside the obstacle. So we'll just reverse nhat to try to push the
        // ellipsoid out of the obstacle.
        infeasible_start = true;
        planes.emplace_back(-plane.first, -plane.first.transpose() * obstacle_pts[i].col(idx));
      } else {
        Eigen::VectorXd xstar = ellipsoid.getC() * ystar + ellipsoid.getD();
        planes.push_back(tangent_plane_through_point(ellipsoid, Cinv2, xstar));
      }
    }

    for (size_t j=0; j < n_obs; j++) {
      if (((planes.back().first.transpose() * obstacle_pts[j]).array() >= planes.back().second).all()) {
        uncovered_obstacles(j) = false;
      }
    }
    uncovered_obstacles(i) = false; // even if it doesn't pass the strict check, we're done with this obstacle

    if (!uncovered_obstacles.any()) {
      break;
    }
  }

  // Eigen::MatrixXd A = polyhedron.getA();
  // Eigen::VectorXd b = polyhedron.getB();
  // A.resize(planes.size(), dim);
  // b.resize(planes.size(), 1);
  Eigen::MatrixXd A(planes.size(), dim);
  Eigen::VectorXd b(planes.size());

  for (auto it = planes.begin(); it != planes.end(); ++it) {
    A.row(it - planes.begin()) = it->first.transpose();
    b(it - planes.begin()) = it->second;
  }
  polyhedron.setA(A);
  polyhedron.setB(b);

  return;
}

IRISRegion inflate_region(const IRISProblem &problem, const IRISOptions &options, IRISDebugData *debug) {
  // std::cout << "running IRIS with the following inputs: " << std::endl;
  // std::cout << "bounds: " << std::endl << problem.getBounds().getA() << std::endl << problem.getBounds().getB() << std::endl;
  // std::cout << "obstacles: " << std::endl;
  // auto debug_obstacles = problem.getObstacles();
  // for (auto it = debug_obstacles.begin(); it != debug_obstacles.end(); ++it) {
  //   std::cout << *it << std::endl;
  // }

  IRISRegion region(problem.getDimension());
  region.ellipsoid.setC(problem.getSeed().getC());
  region.ellipsoid.setD(problem.getSeed().getD());

  double best_vol = pow(ELLIPSOID_C_EPSILON, problem.getDimension());
  double volume;
  long int iter = 0;
  bool infeasible_start;
  Polyhedron new_poly(problem.getDimension());

  if (debug) {
    // std::cout << "starting debug" << std::endl;
    debug->bounds = problem.getBounds();
    debug->ellipsoid_history.push_back(region.ellipsoid);
    // std::cout << "pushing back obstacles" << std::endl;
    auto obstacles = problem.getObstacles();
    for (auto obs = obstacles.begin(); obs != obstacles.end(); ++obs) {
      // std::cout << "pushing back obstacle: " << *obs << std::endl;
      debug->obstacles.push_back(*obs);
    }
    // debug->obstacles = std::vector<Eigen::MatrixXd>(problem.getObstacles().begin(), problem.getObstacles().end());
  }

  float p_time = 0;
  float e_time = 0;

  while (1) {
    auto begin = std::chrono::high_resolution_clock::now();
    // std::cout << "calling hyperplanes with: " << std::endl;
    // std::cout << "C: " << region.ellipsoid->getC() << std::endl;
    // std::cout << "d: " << region.ellipsoid->getD() << std::endl;
    separating_hyperplanes(problem.getObstacles(), region.ellipsoid, new_poly, infeasible_start);
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
    p_time += elapsed.count();

    if (options.error_on_infeasible_start && infeasible_start) {
      throw(InitialPointInfeasibleError());
    }

    new_poly.appendConstraints(problem.getBounds());

    // std::cout << "A: " << std::endl << new_poly.getA() << std::endl;
    // std::cout << "b: " << new_poly.getB().transpose() << std::endl;

    if (options.require_containment) {
      bool all_points_contained;
      if (options.required_containment_points.size()) {
        all_points_contained = true;
        for (auto pt = options.required_containment_points.begin(); pt != options.required_containment_points.end(); ++pt) {
          if (!new_poly.contains(*pt, 0.0)) {
            all_points_contained = false;
            break;
          }
        }
      } else {
        all_points_contained = new_poly.contains(problem.getSeed().getD(), 0.0);
      }

      if (all_points_contained || infeasible_start) {
        region.polyhedron = new_poly;
        if (debug) {
          debug->polyhedron_history.push_back(new_poly);
        }
      } else {
        std::cout << "breaking early because the start point is no longer contained in the polyhedron" << std::endl;
        return region;
      }
    } else {
      region.polyhedron = new_poly;
      if (debug) {
        debug->polyhedron_history.push_back(new_poly);
      }
    }

    // std::cout << "calling inner_ellipsoid with: " << std::endl;
    // std::cout << "A: " << region.polyhedron->getA() << std::endl;
    // std::cout << "b: " << region.polyhedron->getB() << std::endl;
    begin = std::chrono::high_resolution_clock::now();
    volume = iris_mosek::inner_ellipsoid(region.polyhedron, &region.ellipsoid);
    end = std::chrono::high_resolution_clock::now();
    elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
    e_time += elapsed.count();

    if (debug) {
      debug->ellipsoid_history.push_back(region.ellipsoid);
    }
    // std::cout << "C: " << region.ellipsoid->getC() << std::endl;
    // std::cout << "volume: " << volume << std::endl;
    // std::cout << "det: " << region.ellipsoid->getC().determinant() << std::endl;

    const bool at_iter_limit = (options.iter_limit > 0) && (iter + 1 >= options.iter_limit);
    const bool insufficient_progress = (std::abs(volume - best_vol) / best_vol) < options.termination_threshold;
    if (at_iter_limit || insufficient_progress) {
      // std::cout << "(abs(volume - best_vol) / best_vol): " << (std::abs(volume - best_vol) / best_vol) << std::endl;
      // std::cout << "term thresh: " << options.termination_threshold << std::endl;
      break;
    }

    best_vol = volume; // always true because ellipsoid volume is guaranteed to be non-decreasing (see Deits14).
    iter++;
    if (debug) {
      debug->iters = iter;
    }
  }

  // std::cout << "c++ p time: " << p_time << std::endl;
  // std::cout << "c++ e time: " << e_time << std::endl;
  // std::cout << "c++ iters: " << iter << std::endl;
  return region;
}

}


#include "eval.hpp"
#include <Superpixels.hpp>
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <numeric>

namespace dasp {
namespace eval {

float Area(const Superpixels& u)
{
	// compute labels
	slimage::Image1i labels = u.ComputeLabels();
	std::vector<unsigned int> sp_area(u.clusterCount());
	for(std::size_t i=0; i<labels.size(); i++) {
		int label = labels[i];
		if(label == -1) {
			continue;
		}
		// compute pixel area
		sp_area[label] ++;
	}
	// mean
	return static_cast<float>(std::accumulate(sp_area.begin(), sp_area.end(), 0.0f)) / static_cast<float>(sp_area.size());
}

float Area3D(const Superpixels& u)
{
	// compute labels
	slimage::Image1i labels = u.ComputeLabels();
	std::vector<float> sp_area(u.clusterCount());
	for(std::size_t i=0; i<labels.size(); i++) {
		int label = labels[i];
		if(label == -1) {
			continue;
		}
		// 3D
		const Point& p = u.points[i];
		float size_of_a_px = p.depth() / u.opt.camera.focal;
		float area_of_a_px = size_of_a_px*size_of_a_px / p.computeCircularity();
		// compute pixel area
		sp_area[label] += area_of_a_px;
	}
	// mean
	return std::accumulate(sp_area.begin(), sp_area.end(), 0.0f) / static_cast<float>(sp_area.size());
}

slimage::Image1i MarkBoundary(const slimage::Image1i& labels)
{
	slimage::Image1i boundary(labels.dimensions(), slimage::Pixel1i{1});
	for(unsigned int y=1; y+1<boundary.height(); y++) {
		for(unsigned int x=1; x+1<boundary.width(); x++) {
			int q = labels(x, y);
			int qp0 = labels(x+1, y  );
			int qm0 = labels(x-1, y  );
			int q0p = labels(x  , y+1);
			int q0m = labels(x  , y-1);
			if(q == qp0 && q == qm0 && q == q0p && q == q0m) {
				boundary(x,y) = 0;
			}
		}
	}
	return boundary;
}

template<typename K>
std::pair<float,std::vector<float>> IsoperimetricQuotientImpl(const std::vector<std::pair<K,K>>& sp_area_length)
{
	// per cluster: v_i = 4*pi*A_i/L_i^2
	// total: sum_i v_i*A_i
	std::vector<float> ipq(sp_area_length.size());
	float ipq_total = 0.0f;
	float num_total = 0.0f;
	for(std::size_t i=0; i<ipq.size(); i++) {
		const auto& q = sp_area_length[i];
		float area = static_cast<float>(q.first);
		float len = static_cast<float>(q.second);
		float x = 4.0f*boost::math::constants::pi<float>() * area / (len*len);
		ipq[i] = x;
		ipq_total += x*area;
		num_total += area;
//		std::cout << "area=" << area << ", len=" << len << ", ipq=" << x << std::endl;
	}
	return {ipq_total / num_total, ipq};
}

std::pair<float,std::vector<float>> IsoperimetricQuotient(const Superpixels& u)
{
	// compute labels
	slimage::Image1i labels = u.ComputeLabels();
	// compute boundary image
	slimage::Image1i boundary = MarkBoundary(labels);
	// compute total pixel count and number of boundary pixels for each cluster
	std::vector<std::pair<int,int>> ipq_els(u.clusterCount(), {0,0});
	for(std::size_t i=0; i<labels.size(); i++) {
		int label = labels[i];
		if(label == -1) {
			continue;
		}
		if(boundary[i] == 1) {
			ipq_els[label].second ++;
		}
		else {
			ipq_els[label].first ++;
		}
	}
	// finalize
	return IsoperimetricQuotientImpl(ipq_els);
}

std::pair<float,std::vector<float>> IsoperimetricQuotient3D(const Superpixels& u)
{
	// compute labels
	slimage::Image1i labels = u.ComputeLabels();
	// compute boundary image
	slimage::Image1i boundary = MarkBoundary(labels);
	// compute total pixel count and number of boundary pixels for each cluster
	std::vector<std::pair<float,float>> ipq_els(u.clusterCount(), {0.0f,0.0f});
//	std::vector<std::vector<std::size_t>> seg_bnd(u.clusterCount());
	for(std::size_t i=0; i<labels.size(); i++) {
		int label = labels[i];
		if(label == -1) {
			continue;
		}
		// 3D
		const Point& p = u.points[i];
		float size_of_a_px = p.depth() / u.opt.camera.focal;
		float gamma = p.computeCircularity();
		float px_area = size_of_a_px*size_of_a_px / gamma;
		float px_len = (1.571 - 0.571 * gamma) * size_of_a_px;
		// compute pixel area
		ipq_els[label].first += px_area;
		// compute pixel diameter
		if(boundary[i] == 1) {
//			seg_bnd[label].push_back(i);
			ipq_els[label].second += px_len;
		}
	}
// 	// compute segment boundary length
// 	unsigned int w = u.width();
// 	for(unsigned int k=0; k<seg_bnd.size(); k++) {
// 		float L = 0.0f;
// 		const std::vector<std::size_t>& B = seg_bnd[k];
// 		for(unsigned int i=0; i<B.size(); i++) {
// 			unsigned int ii = B[i];
// 			int iy = ii/w;
// 			int ix = ii - iy*w;
// 			for(unsigned int j=i+1; j<B.size(); j++) {
// 				unsigned int jj = B[j];
// 				int jy = jj/w;
// 				int jx = jj - jy*w;
// 				int dx = std::abs(ix-jx);
// 				int dy = std::abs(iy-jy);
// 				if((dx == 1 && dy == 0) || (dx == 0 && dy == 1)) {
// 					L += (u.points[ii].position - u.points[jj].position).norm();
// 				}
// 			}
// 		}
// //		std::cout << k << ": " << ipq_els[k].second << " -> " << L << std::endl;
// 		ipq_els[k].second = L;
// 	}
	// finalize
	return IsoperimetricQuotientImpl(ipq_els);
}

}}


#include <boost/random.hpp>

#include "math_functions.hpp"
#include <cmath>

namespace pwalk {

// Define random number generator type
typedef boost::mt19937 rng_t;

template <typename Dtype>
void sample_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& r) {
  static rng_t gen(1234567);
  static boost::normal_distribution<Dtype> random_distribution(a, sigma);
  static boost::variate_generator<rng_t&, boost::normal_distribution<Dtype> >
      variate_generator(gen, random_distribution);

  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template <typename Dtype>
Dtype rng_uniform(const Dtype a, const Dtype b) {
  static rng_t gen(1234567);
  static boost::uniform_real<Dtype> random_distribution(a, b);
  static boost::variate_generator<rng_t&, boost::uniform_real<Dtype> >
      variate_generator(gen, random_distribution);

  return variate_generator();
}

template <typename Dtype>
Dtype gaussian_density(const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& x, const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& mu, const Eigen::Matrix<Dtype, Eigen::Dynamic, Eigen::Dynamic>& sqrt_cov) {
  Eigen::Matrix<Dtype, Eigen::Dynamic, 1> c = sqrt_cov * (x - mu);
  return std::exp(-0.5*c.dot(c)) * sqrt_cov.determinant();
}

template
void sample_gaussian<float>(const int n, const float mu,
                               const float sigma, Eigen::Matrix<float, Eigen::Dynamic, 1>& r);

template
void sample_gaussian<double>(const int n, const double mu,
                                const double sigma, Eigen::Matrix<double, Eigen::Dynamic, 1>& r);

template
float rng_uniform<float>(const float a, const float b);

template
double rng_uniform<double>(const double a, const double b);

template
float gaussian_density(const Eigen::Matrix<float, Eigen::Dynamic, 1>& x, const Eigen::Matrix<float, Eigen::Dynamic, 1>& mu, const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>& sqrt_inv_cov);

template
double gaussian_density(const Eigen::Matrix<double, Eigen::Dynamic, 1>& x, const Eigen::Matrix<double, Eigen::Dynamic, 1>& mu, const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& sqrt_inv_cov);

} // namespace pwalk


/* ScaFES
 * Copyright (c) 2018, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file MRIDataPBHEqnFDM.hpp
 *
 *  @brief Implementation of a n-dimensional Pennes bioheat equation problem
 *         with data from MRI.
 */

#include <iostream>
#include "ScaFES.hpp"

#include <boost/property_tree/ini_parser.hpp>

/******************************************************************************
 *****************************************************************************/
/**
 * \class MRIDataPBHEqnFDM
 *  @brief Class for discretized Pennes bioheat equation problem.
 *
*/
template<typename CT, std::size_t DIM>
class MRIDataPBHEqnFDM : public ScaFES::Problem<MRIDataPBHEqnFDM<CT,DIM>, CT, DIM> {
  private:
    /** Parser for ini files. */
    using PTree = boost::property_tree::ptree;
    const PTree ptree;

  public:
    /** constant h. Ambient convetion. */
    const CT H; /* W/(m^2 K) */

    /** constant T_amb. Ambient temperature. */
    const CT T_AMB; /* K */

    /** constant q_bc. Heat flux at surface inside the brain. */
    const CT Q_BC; /* W/(m^2) */

    /** constant q_skull. Heat flux at upper surface under skull. */
    const CT Q_SKULL; /* W/(m^2) */

    /** constant epsilon. Emissivity. */
    const CT EPSILON; /* - */

    /** constant sigma. Stefan–Boltzmann constant. */
    const CT SIGMA = 5.670373e-8; /* W/(m^2 K^4) */

    /** All fields which are related to the underlying problem
     * are added in terms of an entry of the parameters of
     * type \c std::vector.
     * @param params Set of ScaFES parameters.
     * @param gg Global grid.
     * @param useLeapfrog Should the leap frog scheme be used?
     * @param nameDatafield Name of the fields.
     * @param stencilWidth Stencil width of the fields.
     * @param isKnownDf Is the data field are known or unknown one?
     * @param ptree_ Config file parser.
     * @param nLayers Number of layers at the global boundary.
     * @param defaultValue Default value of fields.
     * @param writeToFile How often should the data field be written to file.
     * @param computeError Should the Linf error between the numerical
     *                     and exact solution be computed?
     * @param geomparamsInit Initial guess of geometrical parameters.
     * @param checkConvergence Should convergence be checked?
     */
    MRIDataPBHEqnFDM(ScaFES::Parameters const& params,
                     ScaFES::GridGlobal<DIM> const& gg,
                     bool useLeapfrog,
                     std::vector<std::string> const& nameDatafield,
                     std::vector<int> const& stencilWidth,
                     std::vector<bool> const& isKnownDf,
                     PTree const& ptree_,
                     std::vector<int> const& nLayers = std::vector<int>(),
                     std::vector<CT> const& defaultValue = std::vector<CT>(),
                     std::vector<ScaFES::WriteHowOften> const& writeToFile
                         = std::vector<ScaFES::WriteHowOften>(),
                     std::vector<bool> const& computeError
                         = std::vector<bool>(),
                     std::vector<CT> const& geomparamsInit = std::vector<CT>(),
                     std::vector<bool> const& checkConvergence
                         = std::vector<bool>() )
        : ScaFES::Problem<MRIDataPBHEqnFDM<CT, DIM>, CT, DIM>(params, gg,
                                                              useLeapfrog,
                                                              nameDatafield,
                                                              stencilWidth,
                                                              isKnownDf,
                                                              nLayers,
                                                              defaultValue,
                                                              writeToFile,
                                                              computeError,
                                                              geomparamsInit,
                                                              checkConvergence),
        ptree(ptree_),
        H(ptree.get<CT>("Parameters.H")),
        T_AMB(ptree.get<CT>("Parameters.T_INF")),
        Q_BC(ptree.get<CT>("Parameters.Q_BC")),
        Q_SKULL(ptree.get<CT>("Parameters.Q_SKULL")),
        EPSILON(ptree.get<CT>("Parameters.EPSILON"))
        { }

    /** Evaluates all fields at one given global inner grid node.
     */
    void evalInner(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                   ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                   int const& /*timestep*/) {
    }

    /** Evaluates all fields at one given global border grid node.
     */
    void evalBorder(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                    ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                    int const& /*timestep*/) {
    }

    /** Initializes all unknown fields at one given global inner grid node.
     */
    template<typename TT>
    void initInner(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
                   std::vector<TT> const& /*vOld*/,
                   ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                   int const& /*timestep*/) {
    }

    /** Initializes all unknown fields at one given global border grid node.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
                   std::vector<TT> const& /*vOld*/,
                   ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                   int const& /*timestep*/) {
    }

    /** Updates all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateInner(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                     std::vector<ScaFES::DataField<TT,DIM>> const& vOld,
                     ScaFES::Ntuple<int,DIM> const& idxNode,
                     int const& /*timestep*/) {
        CT rho = this->knownDf(0, idxNode);
        CT c = this->knownDf(1, idxNode);
        CT k = this->knownDf(2, idxNode);
        CT rho_blood = this->knownDf(3, idxNode);
        CT c_blood = this->knownDf(4, idxNode);
        CT omega = this->knownDf(5, idxNode);
        CT T_blood = this->knownDf(6, idxNode);
        CT q = this->knownDf(7, idxNode);

        /* Discrete Pennes Bioheat Equation for updating inner nodes. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                * (vOld[0](this->connect(idxNode, 2*pp))
                                   + vOld[0](this->connect(idxNode, 2*pp+1))
                                   - 2.0 * vOld[0](idxNode))
                                / (this->gridsize(pp) * this->gridsize(pp));
        }
        vNew[0](idxNode) += this->tau() * ((rho_blood*c_blood)/(rho*c)) * omega
                            * (T_blood - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(rho*c)) * q;
    }

    /** Updates all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateBorder(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                      std::vector<ScaFES::DataField<TT,DIM>>const& vOld,
                      ScaFES::Ntuple<int,DIM> const& idxNode,
                      int const& /*timestep*/) {
        CT rho = this->knownDf(0, idxNode);
        CT c = this->knownDf(1, idxNode);
        CT k = this->knownDf(2, idxNode);
        CT rho_blood = this->knownDf(3, idxNode);
        CT c_blood = this->knownDf(4, idxNode);
        CT omega = this->knownDf(5, idxNode);
        CT T_blood = this->knownDf(6, idxNode);
        CT q = this->knownDf(7, idxNode);
        int trepanationArea = this->knownDf(8, idxNode);

        /* Discrete Pennes Bioheat Equation with boundary conditions. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            if (idxNode.elem(pp) == (this->nNodes(pp)-1)) {
            /* vOld[0](this->connect(idxNode, 2*pp+1) needs to be replaced. */
                if (pp == (DIM-1)) {
                /* Last node/edge/surface in highest dimension will be brain surface. */
                    if (trepanationArea == 1) {
                    /* Open skull: Cauchy boundary condition and
                     * thermal radiation boundary condition. */
                        CT tempOld = vNew[0](idxNode) + 273.15;
                        CT tempOldPow4 = tempOld * tempOld * tempOld * tempOld;
                        CT tempAmb = T_AMB + 273.15;
                        CT tempAmbPow4 = tempAmb * tempAmb * tempAmb * tempAmb;

                        vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                            * (vOld[0](this->connect(idxNode, 2*pp))
                                            /* vOld[0](this->connect(idxNode, 2*pp+1)
                                             * is replaced by                         */
                                               + vOld[0](this->connect(idxNode, 2*pp))
                                               - ((2.0*this->gridsize(pp)/k)
                                                  * H * (vOld[0](idxNode) - T_AMB))
                                               - ((2.0*this->gridsize(pp)/k)
                                                  * EPSILON * SIGMA
                                                  * (tempOldPow4 - tempAmbPow4))
                                            /********************************************/
                                               - 2.0 * vOld[0](idxNode))
                                            / (this->gridsize(pp) * this->gridsize(pp));
                    } else {
                    /* Closed skull: Neumann boundary condition. */
                        vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                            * (vOld[0](this->connect(idxNode, 2*pp))
                                            /* vOld[0](this->connect(idxNode, 2*pp+1))
                                               is replaced by                          */
                                               + vOld[0](this->connect(idxNode, 2*pp))
                                               - ((2.0*this->gridsize(pp)/k)
                                                  * (-1.0 * Q_SKULL))
                                            /*********************************************/
                                               - 2.0 * vOld[0](idxNode))
                                            / (this->gridsize(pp) * this->gridsize(pp));
                    }
                } else {
                /* Neumann boundary condition. */
                    vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                        * (vOld[0](this->connect(idxNode, 2*pp))
                                        /* vOld[0](this->connect(idxNode, 2*pp+1))
                                           is replaced by                            */
                                           + vOld[0](this->connect(idxNode, 2*pp))
                                           - ((2.0*this->gridsize(pp)/k)
                                              * (-1.0 * Q_BC))
                                        /*********************************************/
                                           - 2.0 * vOld[0](idxNode))
                                        / (this->gridsize(pp) * this->gridsize(pp));
                }
            } else if (idxNode.elem(pp) == 0){
            /* vOld[0](this->connect(idxNode, 2*pp) needs to be replaced.
             * Neumann boundary condition. */
                vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                    /* vOld[0](this->connect(idxNode, 2*pp))
                                       is replaced by                          */
                                    * (vOld[0](this->connect(idxNode, 2*pp+1))
                                       + ((2.0*this->gridsize(pp)/k) * Q_BC)
                                    /*******************************************/
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            } else {
            /* No value needs to be replaced.
             * Use central differencing scheme. */
                vNew[0](idxNode) += this->tau() * (k/(rho*c))
                                    * (vOld[0](this->connect(idxNode, 2*pp))
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            }
        }

        /* These terms are independet of the boundary condition. */
        vNew[0](idxNode) += this->tau() * ((rho_blood*c_blood)/(rho*c)) * omega
                            * (T_blood - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(rho*c)) * q;
    }

    /** Updates (2nd cycle) all unknown fields at one given global inner grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateInner2(std::vector<ScaFES::DataField<TT,DIM>>&,
                      std::vector<ScaFES::DataField<TT,DIM>> const&,
                      ScaFES::Ntuple<int,DIM> const&,
                      int const&) { }

    /** Updates (2nd cycle) all unknown fields at one given global border
     *  grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateBorder2(std::vector<ScaFES::DataField<TT,DIM>>&,
                       std::vector<ScaFES::DataField<TT,DIM>>const&,
                       ScaFES::Ntuple<int,DIM> const&,
                       int const&) { }
};


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_LAPACK_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_LAPACK_HPP

// linear systems

#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/lapack/posv.hpp>
#include <boost/numeric/bindings/lapack/ppsv.hpp>
#include <boost/numeric/bindings/lapack/sysv.hpp>
#include <boost/numeric/bindings/lapack/spsv.hpp>
#include <boost/numeric/bindings/lapack/hesv.hpp>
#include <boost/numeric/bindings/lapack/hpsv.hpp>

// eigenproblems

#include <boost/numeric/bindings/lapack/gees.hpp>
#include <boost/numeric/bindings/lapack/trevc.hpp>
#include <boost/numeric/bindings/lapack/trexc.hpp>
#include <boost/numeric/bindings/lapack/hbev.hpp>
#include <boost/numeric/bindings/lapack/syev.hpp>

// SVD

#include <boost/numeric/bindings/lapack/gesvd.hpp>
#include <boost/numeric/bindings/lapack/gesdd.hpp>
#include <boost/numeric/bindings/lapack/sygv.hpp>

// Miscellaneous
// QR
#include <boost/numeric/bindings/lapack/geqrf.hpp>
#include <boost/numeric/bindings/lapack/ormqr.hpp>
#include <boost/numeric/bindings/lapack/orgqr.hpp>

#endif // BOOST_NUMERIC_BINDINGS_LAPACK_LAPACK_HPP


<%
cfg['compiler_args'] = ['-std=c++14','-fopenmp']
cfg['linker_args'] = ['-fopenmp']
cfg['include_dirs'] = ['/home/eigen-3.3.7']
setup_pybind11(cfg)
%>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/SparseCore>
#include <Eigen/Dense>
#include <omp.h>
#define BATCHSIZE 8
namespace py = pybind11;
using namespace Eigen;

typedef SparseMatrix<std::complex<double>>  SpMat;

VectorXcd dot(int i, int l, SpMat un, VectorXcd wave, int size)
{
 py::gil_scoped_acquire acquire; /* Acquire GIL before calling Python code */
 initParallel(); // not required after eigen 3.3 and c++11 compiler
 setNbThreads(8);
 int k;
 int chunk = pow(2, l-2*i); // dimensions of the incoming unitary sparse matrix
 int dim = pow(2, l)/size;
 int split = pow(2, 2*i)/size; // # of splitted wavefunction
 int batch = BATCHSIZE; // specify the batch size for each threads
 VectorXcd temp(dim); // temp vector to keep tract of mat-vec multiplication
 temp = VectorXcd::Zero(dim);
 // paralleling using openmp
 #pragma omp parallel shared(un, wave) private(k)
 {
 // since each mat-vec dot product is totally independent, we can safely parallel the loop
  #pragma omp for schedule(dynamic,batch)
  for (k = 0; k < split; k++){
    temp.segment(k*chunk, chunk) = un * wave.segment(k*chunk, chunk);
  }
}

 return temp;

}
// simple implementation without openmp; only for benchmark purpose
VectorXcd dot_simple(SpMat un, VectorXcd wave)
{
 return un*wave;
}

PYBIND11_MODULE(eigen_dot, m) {
    // Release GIL before calling into C++ code
    // and also set up the default argument to be one
    m.def("dot", &dot, py::call_guard<py::gil_scoped_release>());
    m.def("dot_simple", &dot_simple, py::call_guard<py::gil_scoped_release>());
 }


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_RNG_HPP

#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/mat/err/check_pos_definite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/trace_inv_quad_form_ldlt.hpp>
#include <stan/math/prim/mat/fun/log_determinant_ldlt.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudo-random vector with a multi-variate normal
     * distribution given the specified location parameter and
     * covariance matrix and pseudo-random number generator.
     *
     * If calculating more than one multivariate-normal random draw
     * then it is more efficient to calculate the Cholesky factor of
     * the covariance matrix and use the function
     * stan::math::multi_normal_cholesky_rng.
     *
     * @tparam RNG Type of pseudo-random number generator.
     * @param mu Location parameter.
     * @param S Covariance parameter.
     * @param rng Pseudo-random number generator.
     */
    template <class RNG>
    inline Eigen::VectorXd
    multi_normal_rng(const Eigen::VectorXd& mu, const Eigen::MatrixXd& S,
                     RNG& rng) {
      using boost::variate_generator;
      using boost::normal_distribution;

      static const char* function("multi_normal_rng");

      check_positive(function, "Covariance matrix rows", S.rows());
      check_symmetric(function, "Covariance matrix", S);
      check_finite(function, "Location parameter", mu);

      Eigen::LLT<Eigen::MatrixXd> llt_of_S = S.llt();
      check_pos_definite("multi_normal_rng", "covariance matrix argument",
                         llt_of_S);

      variate_generator<RNG&, normal_distribution<> >
        std_normal_rng(rng, normal_distribution<>(0, 1));

      Eigen::VectorXd z(S.cols());
      for (int i = 0; i < S.cols(); i++)
        z(i) = std_normal_rng();

      return mu + llt_of_S.matrixL() * z;
    }

  }
}
#endif


#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/algorithm/string.hpp>

long no_paren_solution(std::string line) {
  std::string result;
  std::vector<std::string> items;
  split(items, line, boost::is_any_of(" "), boost::token_compress_on);
  if (items.size() == 1) {
    return std::stol(items[0]);
  } else if (items[1] == "*") {
    result = std::to_string(std::stol(items[0]) * std::stol(items[2]));
  } else if (items[1] == "+") {
    result = std::to_string(std::stol(items[0]) + std::stol(items[2]));
  }
  items.erase(items.begin(), items.begin() + 3);
  items.insert(items.begin(), result);
  return no_paren_solution(boost::join(items, " "));
}

long no_paren_solution_ordered(std::string line) {
  std::string result;
  std::vector<std::string> items;
  split(items, line, boost::is_any_of(" "), boost::token_compress_on);
  auto it = std::find(items.begin(), items.end(), "+");
  int first_sum = std::distance(items.begin(), it);
  if (it == items.end()){
    return no_paren_solution(line);
  }
  result = std::to_string(std::stol(items[first_sum - 1]) +
                          std::stol(items[first_sum + 1]));
  items.erase(it-1, it + 2);
  items.insert(it-1, result);
  return no_paren_solution_ordered(boost::join(items, " "));
}

long full_solution(std::string problem,
                   long (*no_paren_func)(std::string line)) {
  int open_par = problem.find_first_of("(");
  if (open_par == std::string::npos){
    return no_paren_func(problem);
  } else {
    int close_par = open_par;
    int n_open = 1;
    while (n_open > 0){
      close_par++;
      if (problem[close_par] == ')'){
        n_open--;
      } else if (problem[close_par] == '('){
        n_open++;
      }
    }
    std::string inside_paren;
    inside_paren = problem.substr(open_par+1, close_par-open_par-1);
    std::string new_problem = problem;
    std::string solved_inside = std::to_string(full_solution(inside_paren, no_paren_func));
    new_problem.replace(open_par, close_par-open_par+1, solved_inside);
    return full_solution(new_problem, no_paren_func);
  }
}

int main() {

  long answer_pt1 = 0;
  long answer_pt2 = 0;

  for (std::string line; std::getline(std::cin, line);){
    long curr_sol_pt1 = full_solution(line, no_paren_solution);
    answer_pt1 += curr_sol_pt1;
    long curr_sol_pt2 = full_solution(line, no_paren_solution_ordered);
    answer_pt2 += curr_sol_pt2;
    // break;
  }
  std::cout << "Part 1: " << answer_pt1 << std::endl;
  std::cout << "Part 2: " << answer_pt2 << std::endl;
}


/**
 *  @file    Problem.hpp
 *  @brief   Define a finite difference problem.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef PROBLEM_H
#define PROBLEM_H

#include <map>
#include <string>
#include <vector>
#include <Eigen/SparseCore>
#include "spdlog/spdlog.h"
#include "Parameters.hpp"
#include "Mesh.hpp"

namespace numerical {

namespace fdm {

/*
 * This class defines the finite difference problem of the diffusion type:
 *
 * /f[
 *    \frac{\partial u}{\partial t} = \nabla\left(\alpha\nabla u) + f
 * /f]
 *
 * over the hypercube.
 */
template <typename T>
class Problem {
typedef Eigen::SparseMatrix<T> SpMat;
typedef Eigen::Triplet<T> Trip;
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
private:
    int m_dim;

protected:
    Parameters<T>* m_params;
    Vec m_u;  // The solution vector
public:
    Problem(Parameters<T>* params):
      m_params(params) {
        // define other variables variables
        m_dim = m_params->lengths.size();
  	    // define mesh
  	    Mesh<T> mesh = Mesh<T>(m_params->lengths, m_params->t0, m_params->tend,
          m_params->n, m_params->nt);
    }
    virtual ~Problem(){
    }
    /**
    * Diffusion coefficient value. The default is a constant obtained from
    * m_params.
    *
    * @param x The x-coordinate of the mesh node.
    * @param y The y-coordinate of the mesh node.
    * @param z The z-coordinate of the mesh node.
    * @param t The discrete time.
    * @return The diffusion coefficient value at a specified mesh location.
    */
    virtual T alpha(T x, T y, T z, T t){
  	    return m_params->alpha;
    }

    /**
    * @return The spatial dimension of the problem.
    */
    int dim(){
    	return m_dim;
    }

    /**
    * Left boundary value, i.e. for x = lengths[0][0].
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T left(int type, T x1, T x2, T t){
  	    return 0.0;
    }

    /**
    * Right boundary value, i.e. for x = lengths[0][1].
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T right(int type, T x1, T x2, T t){
        return 0.0;
    }

    /**
    * Bottom boundary value, i.e. at y = lengths[1][0]. Only for 2D and
    * 3D models.
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T bottom(int type, T x1, T x2, T t){
        return 0.0;
    }

    /**
    * Top boundary value, i.e. at y = lengths[1][1]. Only for 2D and 3D models.
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T top(int type, T x1, T x2, T t){
        return 0.0;
    }

    /**
    * Front boundary value, i.e. at cz = length[2][0]. Only for 3D models.
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T front(int type, T x1, T x2, T t){
        return 0.0;
    }

    /**
    * Back boundary value, i.e. at z = length[2][0]. Only for 3D models.
    *
    * @param type Dirichlet = 0, Neumann = 1.
    * @param x1 The first coordinate of the mesh node.
    * @param x2 The second coordinate of the mesh node.
    * @param t The discrete time.
    * @return The boundary value at a specified mesh location.
    */
    virtual T back(int type, T x1, T x2, T t){
        return 0.0;
    }

    /**
    * Initial  value.
    *
    * @param x The x-coordinate of the mesh node.
    * @param y The y-coordinate of the mesh node.
    * @param z The z-coordinate of the mesh node.
    * @return The initial value at a specified mesh location.
    */
    virtual T initial_value(T x, T y, T z){
  	    return 0.0;
    }

    /**
    *  Get the reference solution at a specified mesh location.
    *
    * @param x The x-coordinate of the mesh node.
    * @param y The y-coordinate of the mesh node.
    * @param z The z-coordinate of the mesh node.
    * @param t The discrete time.
    * @return The reference solution at a specified mesh location.
    */
    virtual T reference(T x, T y, T z, T t){
    	return 0.0;
    }

    /**
    *  Get the computed solution at a specified mesh location.
    *
    * @param x The x-coordinate of the mesh node.
    * @param y The y-coordinate of the mesh node.
    * @param z The z-coordinate of the mesh node.
    * @param t The discrete time.
    * @return The computed solution at a specified mesh location.
    */
    virtual T solution(T x, T y, T z, T t){
    	return 0.0;
    }

    /**
    * Source term.
    *
    * @param x The x-coordinate of the mesh node.
    * @param y The y-coordinate of the mesh node.
    * @param z The z-coordinate of the mesh node.
    * @param t The discrete time.
    * @return The source term at a specified mesh location.
    */
    virtual T source(T x, T y, T z, T t){
  	    return 0.0;
    }

};

}  // namespace fdm

}  // namespace numerical

#endif  // PROBLEM_H


#include <armadillo>
#include <boost/program_options.hpp>
#include <HSMM.hpp>
#include <Multivariate_Gaussian_emission.hpp>
#include <random>

using namespace arma;
using namespace hsmm;
using namespace robotics::random;
using namespace std;
namespace po = boost::program_options;

// MLE transition in the HMM case. This means self-transitions are allowed.
mat getHmmTransitionFromLabels(const field<ivec>& labels_seq, int nstates) {
    mat hmm_transition(nstates, nstates, fill::zeros);
    for(const ivec &s : labels_seq)
        for(int i = 0; i < s.n_elem - 1; i++)
            hmm_transition(s(i), s(i + 1))++;

    for(int i = 0; i < nstates; i++)
        hmm_transition.row(i) = hmm_transition.row(i) / accu(
                hmm_transition.row(i));
    return hmm_transition;
}

// MLE categorical distribution over labels for the IID case.
mat getMixtureModelTransitionFromLabels(const field<ivec>& labels_seq,
        int nstates) {
    mat transition(nstates, nstates, fill::zeros);
    for(const ivec &s : labels_seq)
        for(int i = 0; i < s.n_elem; i++)
            transition.col(s(i)) += 1;

    for(int i = 0; i < nstates; i++)
        transition.row(i) = transition.row(i) / accu(transition.row(i));
    return transition;
}

// Equivalent to np.fft.rfftfreq(512, 1.0 / 128.0) in Python.
vec discreteFourierTransformSampleFrequencies() {
    return linspace(0, 64, 257);
}

vec extract_frequency_features(vec input) {
    vec nu = discreteFourierTransformSampleFrequencies();
    uvec delta_band = find(nu > 0.4 && nu < 4.1);
    uvec theta_band = find(nu > 5.9 && nu < 10.1);
    uvec alpha_band = find(nu > 10 && nu < 15.1);
    uvec all_bands = find(nu > 3.9 && nu < 40.1);

    cx_mat t = fft(input);

    // Ensuring the same size as np.fft.rfft(input).
    t = t.head_rows(input.n_elem / 2 + 1);
    mat norm = abs(t);
    norm = norm % norm;
    vec features = {accu(norm.elem(delta_band)), accu(norm.elem(theta_band)),
            accu(norm.elem(alpha_band)), accu(norm.elem(all_bands))};
    return features;
}

vec extract_eeg_features(vec eeg) {
    return extract_frequency_features(eeg).head_rows(3);
}

vec extract_emg_features(vec emg) {
    return extract_frequency_features(emg).tail_rows(1);
}

field<vec> getFeatureVectors(const mat& eeg1, const mat& eeg2, const mat& emg) {
    int nobs = eeg1.n_cols;
    assert(nobs == eeg2.n_cols);
    assert(nobs == emg.n_cols);
    field<vec> features(nobs);
    for(int i = 0; i < nobs; i++) {
        vec f = join_vert(extract_eeg_features(eeg1.col(i)),
                extract_eeg_features(eeg2.col(i)));
        features(i) = join_vert(f, extract_emg_features(emg.col(i)));
    }
    return features;
}

ivec predict_labels_iid(shared_ptr<MultivariateGaussianEmission> e,
        const mat& test_input, const vec& class_prior) {
    assert(class_prior.n_elem == e->getNumberStates());
    ivec ret(test_input.n_cols);
    double seq_loglikelihood = 0;
    for(int i = 0; i < test_input.n_cols; i++) {
        vec loglikelihoods(e->getNumberStates());
        for(int j = 0; j < e->getNumberStates(); j++)
            loglikelihoods(j) = e->loglikelihood(j,
                    test_input.col(i)) + log(class_prior(j));
        ret(i) = (int) loglikelihoods.index_max();
        seq_loglikelihood += logsumexp(loglikelihoods);
    }
    cout << "IIDtestloglikelihood " << seq_loglikelihood << endl;
    return ret;
}

ivec predict_labels_from_filtering(const mat& filtering_state_marginals) {
    ivec ret(filtering_state_marginals.n_cols);
    for(int i = 0; i < ret.n_elem; i++)
        ret(i) = (int) filtering_state_marginals.col(i).index_max();
    return ret;
}

vector<NormalDist> get_normal_distributions(int nstates, int ndurations,
        int min_duration, int ndimension) {
    mt19937 gen(0);
    vector<NormalDist> states;
    vector<vec> samples;
    vector<int> labels;
    for(int i = 0; i < nstates; i++) {
        vec mean = ones<vec>(ndimension) * i * 10;
        mat cov = eye(ndimension, ndimension);
        NormalDist a(mean, cov);
        states.push_back(a);

        // Generating toy data.
        int nsamples = 100;
        vector<vec> s = sample_multivariate_normal(gen, a, nsamples);
        vector<int> l = conv_to<vector<int>>::from(ones<ivec>(nsamples) * i);
        samples.insert(samples.end(), s.begin(), s.end());
        labels.insert(labels.end(), l.begin(), l.end());
    }

    // Toy data handling.
    ivec labels_vec = conv_to<ivec>::from(labels);
    field<vec> obs_field(samples.size());
    for(int i = 0; i < samples.size(); i++)
        obs_field(i) = samples.at(i);

    // Debug
    shared_ptr<MultivariateGaussianEmission> emission(
            new MultivariateGaussianEmission(states));
    OnlineHSMMRunlengthBased model(emission, nstates, ndurations, min_duration);
    for(int i = 0; i < samples.size(); i++)
        model.addNewObservation(samples.at(i));
    return states;
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    vector<string> input_features, input_labels;
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<vector<string>>(&input_features)->multitoken(),
                "Path to the (multiple) input features")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("nstates", po::value<int>(), "Number of hidden states for the HSMM")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("labels,l", po::value<vector<string>>(&input_labels)->multitoken(),
                "Path to input labels")
        ("nodur", "Flag to deactivate the learning of durations")
        ("iid", "Flag to deactivate the learning of transitions")
        ("alphadurprior", po::value<double>(),
                "Alpha for Dirichlet prior for the duration")
        ("filteringprediction", po::value<string>(), "Path to predicted labels"
                " based on the filtering distribution over states")
        ("mr", po::value<string>(), "Runlength marginals output filename")
        ("ms", po::value<string>(), "States marginals output filename")
        ("ms2", po::value<string>(), "States marginals output filename. This "
                "one is based on the residual time posterior instead of the "
                "runlength posterior")
        ("md", po::value<string>(), "Duration marginals output filename")
        ("ml", po::value<string>(), "Remaining runlength marginals output"
                " filename")
        ("leaveoneout", po::value<int>(), "Index of the sequence that will be"
                " left out for validation")
        ("savefiletype", po::value<string>()->default_value("arma_binary"),
                "File type to save the matrices after inference");
    assert(input_features.size() == input_labels.size());
    vector<string> required_fields = {"input", "labels", "leaveoneout",
            "nstates", "mindur", "ndur"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    int nstates = vm["nstates"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    int leaveoneout = vm["leaveoneout"].as<int>();
    int min_duration = vm["mindur"].as<int>();
    int nseq = input_features.size();

    field<ivec> labels_seq(nseq - 1);
    field<mat> obs_seq(nseq - 1);
    ivec test_labels;
    mat test_obs;
    int train_idx = 0;
    for(int i = 0; i < nseq; i++) {
        if (i == leaveoneout) {
            test_obs.load(input_features[i], raw_ascii);
            test_labels.load(input_labels[i], raw_ascii);
        }
        else {
            labels_seq(train_idx).load(input_labels[i], raw_ascii);
            obs_seq(train_idx).load(input_features[i], raw_ascii);
            train_idx++;
        }
    }
    assert(train_idx == nseq - 1);
    int ndimension = test_obs.n_rows;

    // Creating normal distributions for the emission process.
    vector<NormalDist> states = get_normal_distributions(nstates, ndurations,
            min_duration, ndimension);

    // Creating the emission process.
    shared_ptr<MultivariateGaussianEmission> emission(
            new MultivariateGaussianEmission(states));

    // Training the emission based on the labels.
    emission->fitFromLabels(obs_seq, labels_seq);

    // Creating the online HSMM whose emission process doesnt take into account
    // the total segment duration. The pmfs are uniformly initialized.
    OnlineHSMMRunlengthBased model(emission, nstates, ndurations, min_duration);

    // Setting a Dirichlet prior over the durations.
    if (vm.count("alphadurprior")) {
        mat alphas = ones<mat>(nstates, ndurations) *
            vm["alphadurprior"].as<double>();
        model.setDurationDirichletPrior(alphas);
    }

    // Learning the HSMM parameters from the labels.
    if (!vm.count("iid")) {
        if (min_duration == 1 && ndurations == 1)
            model.setTransition(getHmmTransitionFromLabels(labels_seq,nstates));
        else
            model.setTransitionFromLabels(labels_seq);
    }
    else
        model.setTransition(getMixtureModelTransitionFromLabels(labels_seq,
                    nstates));
    if (!vm.count("nodur"))
        model.setDurationFromLabels(labels_seq);

    if (vm.count("output")) {
        ofstream output_params(vm["output"].as<string>());
        nlohmann::json current_params = model.to_stream();
        output_params << std::setw(4) << current_params << endl;
        output_params.close();
    }
    mat runlength_marginals;
    mat state_marginals, state_marginals_2;
    mat remaining_runlength_marginals;
    mat duration_marginals;

    if (vm.count("mr"))
        runlength_marginals = zeros<mat>(min_duration + ndurations - 1,
                test_obs.n_cols);
    if (vm.count("ms") || vm.count("filteringprediction"))
        state_marginals = zeros<mat>(nstates, test_obs.n_cols);
    if (vm.count("ms2"))
        state_marginals_2 = zeros<mat>(nstates, test_obs.n_cols);
    if (vm.count("md"))
        duration_marginals = zeros<mat>(ndurations, test_obs.n_cols);
    if (vm.count("ml"))
        remaining_runlength_marginals = zeros<mat>(
                min_duration + ndurations - 1, test_obs.n_cols);
    vec loglikelihoods(test_obs.n_cols);
    for(int i = 0; i < test_obs.n_cols; i++) {
        loglikelihoods(i) = model.oneStepAheadLoglikelihood(test_obs.col(i));
        model.addNewObservation(test_obs.col(i));
        if (vm.count("mr"))
            runlength_marginals.col(i) = model.getRunlengthMarginal();
        if (vm.count("ms") || vm.count("filteringprediction"))
            state_marginals.col(i) = model.getStateMarginal();
        if (vm.count("ms2"))
            state_marginals_2.col(i) = model.getStateMarginal2();
        if (vm.count("ml"))
            remaining_runlength_marginals.col(i) =
                    model.getResidualTimeMarginal();
    }

    // The test log-likelihood.
    cout << accu(loglikelihoods) << endl;

    // Saving the filtering inferences.
    auto file_type = vm["savefiletype"].as<string>().compare(
            "arma_binary") == 0 ? arma_binary : raw_ascii;
    if (vm.count("mr"))
        runlength_marginals.save(vm["mr"].as<string>(), file_type);
    if (vm.count("ms"))
        state_marginals.save(vm["ms"].as<string>(), file_type);
    if (vm.count("ms2"))
        state_marginals_2.save(vm["ms2"].as<string>(), file_type);
    if (vm.count("ml"))
        remaining_runlength_marginals.save(vm["ml"].as<string>(), file_type);
    if (vm.count("filteringprediction")) {
        ivec filtering_prediction = predict_labels_from_filtering(
                state_marginals);
        filtering_prediction.save(vm["filteringprediction"].as<string>(),
                raw_ascii);
    }
    return 0;
}


#include "util/coordinate_calculation.hpp"

#include "util/string_util.hpp"
#include "util/trigonometry_table.hpp"

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

#include <cmath>

#include <limits>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

double haversineDistance(const int lat1, const int lon1, const int lat2, const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;
    const double dlat1 = lt1 * (RAD);

    const double dlong1 = ln1 * (RAD);
    const double dlat2 = lt2 * (RAD);
    const double dlong2 = ln2 * (RAD);

    const double dlong = dlong1 - dlong2;
    const double dlat = dlat1 - dlat2;

    const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
    return EARTH_RADIUS * charv;
}

double haversineDistance(const FixedPointCoordinate coordinate_1,
                         const FixedPointCoordinate coordinate_2)
{
    return haversineDistance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                             coordinate_2.lon);
}

double greatCircleDistance(const FixedPointCoordinate coordinate_1,
                           const FixedPointCoordinate coordinate_2)
{
    return greatCircleDistance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                               coordinate_2.lon);
}

double greatCircleDistance(const int lat1, const int lon1, const int lat2, const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const double float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const double float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const double float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const double float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
    const double y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * EARTH_RADIUS;
}

double perpendicularDistance(const FixedPointCoordinate source_coordinate,
                             const FixedPointCoordinate target_coordinate,
                             const FixedPointCoordinate query_location)
{
    double ratio;
    FixedPointCoordinate nearest_location;

    return perpendicularDistance(source_coordinate, target_coordinate, query_location,
                                 nearest_location, ratio);
}

double perpendicularDistance(const FixedPointCoordinate segment_source,
                             const FixedPointCoordinate segment_target,
                             const FixedPointCoordinate query_location,
                             FixedPointCoordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    return perpendicularDistanceFromProjectedCoordinate(
        segment_source, segment_target, query_location,
        {mercator::latToY(query_location.lat / COORDINATE_PRECISION),
         query_location.lon / COORDINATE_PRECISION},
        nearest_location, ratio);
}

double
perpendicularDistanceFromProjectedCoordinate(const FixedPointCoordinate source_coordinate,
                                             const FixedPointCoordinate target_coordinate,
                                             const FixedPointCoordinate query_location,
                                             const std::pair<double, double> projected_coordinate)
{
    double ratio;
    FixedPointCoordinate nearest_location;

    return perpendicularDistanceFromProjectedCoordinate(source_coordinate, target_coordinate,
                                                        query_location, projected_coordinate,
                                                        nearest_location, ratio);
}

double
perpendicularDistanceFromProjectedCoordinate(const FixedPointCoordinate segment_source,
                                             const FixedPointCoordinate segment_target,
                                             const FixedPointCoordinate query_location,
                                             const std::pair<double, double> projected_coordinate,
                                             FixedPointCoordinate &nearest_location,
                                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    // initialize values
    const double x = projected_coordinate.first;
    const double y = projected_coordinate.second;
    const double a = mercator::latToY(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = mercator::latToY(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, new_y;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.0 + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    new_y = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(new_y) < (1.0 / COORDINATE_PRECISION))
    {
        new_y = 0.0;
    }

    // compute ratio
    ratio = static_cast<double>((p - new_y * a) /
                                c); // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.0 : 0.0);
    }
    else if (std::abs(ratio) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 0.0;
    }
    else if (std::abs(ratio - 1.0) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 1.0;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.0)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.0)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(mercator::yToLat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.IsValid());

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}

double bearing(const FixedPointCoordinate first_coordinate,
               const FixedPointCoordinate second_coordinate)
{
    const double lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const double lon_delta = degToRad(lon_diff);
    const double lat1 = degToRad(first_coordinate.lat / COORDINATE_PRECISION);
    const double lat2 = degToRad(second_coordinate.lat / COORDINATE_PRECISION);
    const double y = std::sin(lon_delta) * std::cos(lat2);
    const double x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    double result = radToDeg(std::atan2(y, x));
    while (result < 0.0)
    {
        result += 360.0;
    }

    while (result >= 360.0)
    {
        result -= 360.0;
    }
    return result;
}

double computeAngle(const FixedPointCoordinate first,
                    const FixedPointCoordinate second,
                    const FixedPointCoordinate third)
{
    using namespace boost::math::constants;
    using namespace coordinate_calculation;

    const double v1x = (first.lon - second.lon) / COORDINATE_PRECISION;
    const double v1y = mercator::latToY(first.lat / COORDINATE_PRECISION) -
                       mercator::latToY(second.lat / COORDINATE_PRECISION);
    const double v2x = (third.lon - second.lon) / COORDINATE_PRECISION;
    const double v2y = mercator::latToY(third.lat / COORDINATE_PRECISION) -
                       mercator::latToY(second.lat / COORDINATE_PRECISION);

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. / pi<double>();

    while (angle < 0.)
    {
        angle += 360.;
    }

    return angle;
}

namespace mercator
{
double yToLat(const double value)
{
    using namespace boost::math::constants;

    return 180. * (1. / pi<long double>()) *
           (2. * std::atan(std::exp(value * pi<double>() / 180.)) - half_pi<double>());
}

double latToY(const double latitude)
{
    using namespace boost::math::constants;

    return 180. * (1. / pi<double>()) *
           std::log(std::tan((pi<double>() / 4.) + latitude * (pi<double>() / 180.) / 2.));
}
} // ns mercato // ns mercatorr
} // ns coordinate_calculation
} // ns util
} // ns osrm


/*--
    Solver.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    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 3 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, see <http://www.gnu.org/licenses/>.
*/

#include "Solver.h"
#include <Eigen/Cholesky>
#include <iostream> //TODO: TMP
#include <cmath>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

LSSolver::LSSolver(LSProblem *problem, const vector<LSBoxConstraint> &constraints)
: _problem(problem), _constraints(constraints), _damping(1.), _maxIter(100),
  _increaseDampingAfter(0), _dampingIncreaseFactor(1.)
{
};

VectorXd LSSolver::solve(const VectorXd &guess, bool *is_valid)
{
	if (is_valid != nullptr)
		*is_valid = true;

    VectorXd best;
    double bestError = 1e100;
    VectorXd x = guess;
    LSEvalData *evalData = _problem->createEvalData();

    set<LSBoxConstraint> activeSet = _clamp(x);

    VectorXd delta;
    int iter;
    for(iter = 0; iter < _maxIter; ++iter)
    {
        if(iter > _increaseDampingAfter)
            _damping *= _dampingIncreaseFactor;
        _problem->eval(x, evalData);

        double error = evalData->error();
        //printf("Iter = %d, error = %lf\n", iter, error);
        if(error < bestError)
        {
            bestError = error;
            best = x;

            if(error < 1e-10)
                break;
        }

        set<LSBoxConstraint> prevActiveSet = activeSet;
        evalData->solveForDelta(_damping, delta, activeSet);

		double delta_norm = delta.squaredNorm();
		if (isnan(delta_norm)) {
			if (is_valid != nullptr)
				*is_valid = false;
			break;
		}

		if(delta_norm < 1e-14)
            break;

        int newConstraint = _project(x, delta, prevActiveSet);

        if(newConstraint != -1)
            activeSet.insert(_constraints[newConstraint]);

        x += delta;

        int halvings = 0;
        while(_problem->error(x, evalData) > error && delta.squaredNorm() > 1e-8)
        {
            //printf("Halving\n");
            delta *= 0.5;
            x -= delta;
            ++halvings;
        }
        if(halvings > 0) //halve again -- won't hurt and may actually help
        {
            delta *= 0.5;
            x -= delta;
        }
    }

    double error = _problem->error(x, evalData);
    if(iter > 5)
        Debugging::get()->printf("After %d iterations, error = %lf", iter, sqrt(error));
    if(error < bestError)
    {
        best = x;
    }

    delete evalData;
    return best;
}

set<LSBoxConstraint> LSSolver::_clamp(VectorXd &x)
{
    set<LSBoxConstraint> out;
    for(int i = 0; i < (int)_constraints.size(); ++i)
    {
        const LSBoxConstraint &c = _constraints[i];
        if(c.sign == 0 || (x[c.index] - c.value) * c.sign < 0.)
        {
            x[c.index] = c.value;
            out.insert(c);
            //cout << "Clamping constraint " << i << endl;
        }
    }
    return out;
}

int LSSolver::_project(const VectorXd &from, VectorXd &delta, const set<LSBoxConstraint> &activeSet)
{
    int closestConstraint = -1;
    double minScale = 1.;

    for(int i = 0; i < (int)_constraints.size(); ++i)
    {
        const LSBoxConstraint &c = _constraints[i];

        if(c.sign == 0)
            delta[c.index] = 0; //just in case

        if(activeSet.count(c))
            continue; //already constrained

        double scale = (c.value - from[c.index]) / delta[c.index];

        if((from[c.index] + delta[c.index] - c.value) * c.sign >= 0.)
            continue;

        if(scale < minScale)
        {
            minScale = scale;
            closestConstraint = i;
        }
    }

    if(closestConstraint >= 0)
    {
        delta *= minScale;
        //cout << "Projecting up to constraint " << closestConstraint << " by " << minScale << endl;
    }

    return closestConstraint;
}

bool LSSolver::verifyDerivatives(const Eigen::VectorXd &pt, double eps) const
{
    LSEvalData *evalData = _problem->createEvalData();
    _problem->eval(pt, evalData);

    MatrixXd exactDer = evalData->errVecDer();
    MatrixXd numDer = exactDer;

    for(int i = 0; i < numDer.cols(); ++i)
    {
        VectorXd mod = pt;
        mod[i] += eps;

        _problem->eval(mod, evalData);
        VectorXd plus = evalData->errVec();

        mod[i] = pt[i] - eps;
        _problem->eval(mod, evalData);
        VectorXd minus = evalData->errVec();

        numDer.col(i) = (plus - minus) / (2 * eps);
    }

    double err = (numDer - exactDer).norm();

    //TODO: just print the error for now
    Debugging::get()->printf("Derivative Error = %lf", err);
#if 0
    for(int i = 0; i < numDer.cols(); ++i)
        Debugging::get()->printf("Col %d err = %lf", i, (numDer.col(i) - exactDer.col(i)).norm());
    for(int i = 0; i < numDer.rows(); ++i)
        Debugging::get()->printf("Row %d err = %lf", i, (numDer.row(i) - exactDer.row(i)).norm());
#endif
    delete evalData;

    return true;
}

void LSDenseEvalData::solveForDelta(double damping, VectorXd &out, set<LSBoxConstraint> &constraints)
{
    int vars = (int)_errDer.cols();
    if(constraints.empty())
    {
        LDLT<MatrixXd> ldlt(MatrixXd::Identity(vars, vars) * damping + _errDer.transpose() * _errDer);
        out = ldlt.solve(-_errDer.transpose() * _err);
    }
    else
    {
        VectorXd rhs = -_err;

        vector<bool> constraintIndices(vars, false);
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it)
            constraintIndices[it->index] = true;

        if(vars > (int)constraints.size())
        {
            MatrixXd lhs(_errDer.rows(), vars - (int)constraints.size());
            int offs = 0;
            for(int i = 0; i < vars; ++i)
            {
                if(constraintIndices[i])
                {
                    ++offs;
                    continue;
                }
                lhs.col(i - offs) = _errDer.col(i);
            }

            LDLT<MatrixXd> ldlt(MatrixXd::Identity(lhs.cols(), lhs.cols()) * damping + lhs.transpose() * lhs);
            VectorXd x = ldlt.solve(lhs.transpose() * rhs);

            out.resize(_errDer.cols());
            offs = 0;

            for(int i = 0; i < vars; ++i)
            {
                if(constraintIndices[i])
                {
                    out[i] = 0.;
                    ++offs;
                    continue;
                }
                out[i] = x[i - offs];
            }
        }
        else //as many variables as constraints
        {
            out = VectorXd::Zero(_errDer.cols());
        }

        //check which constraints we don't need
        VectorXd gradient = _errDer.transpose() * (_errDer * out - rhs);
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); )
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(gradient[it->index] * it->sign < 0) //if sign is zero, constraint will not get erased
            {
                //cout << "Unsetting constraint on variable at index " << it->index << endl;
                constraints.erase(it);
            }
            it = next;
        }
    }
}

END_NAMESPACE_Cornu


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include <ql/experimental/volatility/stickybpvolswaptioncube.hpp>
#include <ql/experimental/volatility/multiplicativesmilesection.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/flatextrapolation2d.hpp>
#include <ql/quotes/simplequote.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

StickyBpVolSwaptionCube::StickyBpVolSwaptionCube(
    const boost::shared_ptr<SwaptionVolatilityCube> &sourceCube,
    Handle<Quote> atmVolatilitySpread)
    : SwaptionVolatilityCube(
          sourceCube->atmVol(), sourceCube->optionTenors(),
          sourceCube->swapTenors(), sourceCube->strikeSpreads(),
          sourceCube->volSpreads(), sourceCube->swapIndexBase(),
          sourceCube->shortSwapIndexBase(), sourceCube->vegaWeightedSmileFit()),
      sourceCube_(sourceCube),
      atmVolatilitySpread_(atmVolatilitySpread) {

    registerWith(sourceCube_);
    registerWith(atmVolatilitySpread_);

    boost::shared_ptr<SwaptionVolatilityDiscrete> atm =
        boost::dynamic_pointer_cast<SwaptionVolatilityDiscrete>(
            *sourceCube->atmVol());

    // this should never happen (?)
    QL_REQUIRE(atm != NULL, "atm is not of type SwaptionVolatilityDiscrete");

    atmLevel_ = Matrix(optionDates().size(), swapTenors().size());

    for (Size j = 0; j < atm->swapTenors().size(); ++j)
        for (Size i = 0; i < atm->optionDates().size(); ++i)
            atmLevel_[i][j] = sourceCube->atmStrike(atm->optionDates()[i],
                                                    atm->swapTenors()[j]);

    originalAtm_ = FlatExtrapolator2D(boost::make_shared<BilinearInterpolation>(
        atm->swapLengths().begin(), atm->swapLengths().end(),
        atm->optionTimes().begin(), atm->optionTimes().end(), atmLevel_));

    originalAtm_.enableExtrapolation();
}

boost::shared_ptr<SmileSection>
StickyBpVolSwaptionCube::smileSectionImpl(Time optionTime,
                                          Time swapLength) const {

    boost::shared_ptr<SmileSection> source =
        sourceCube_->smileSection(optionTime, swapLength, true);

    Real newAtm = source->atmLevel();

    QL_REQUIRE(newAtm != Null<Real>(),
               "source smile section does not provide atm level");

    Real mul = originalAtm_(optionTime, swapLength) / newAtm;
    Real add = 0.0;
    if(!atmVolatilitySpread_.empty()) {
        if(atmVolatilitySpread_->isValid()) {
            add = atmVolatilitySpread_->value() * mul;
        }
    }

    boost::shared_ptr<SmileSection> tmp =
        boost::make_shared<MultiplicativeSmileSection>(
            source, Handle<Quote>(boost::make_shared<SimpleQuote>(mul)),
            Handle<Quote>(boost::make_shared<SimpleQuote>(add)));
    return tmp;
}

Volatility StickyBpVolSwaptionCube::volatilityImpl(Time optionTime,
                                                   Time swapLength,
                                                   Rate strike) const {
    return smileSectionImpl(optionTime, swapLength)->volatility(strike);
}
}