#include <nori/object.h>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <boost/math/special_functions/fpclassify.hpp>

#if defined(PLATFORM_LINUX)
#include <malloc.h>
#endif

#if defined(PLATFORM_WINDOWS)
#include <windows.h>
#endif

#if defined(PLATFORM_MACOS)
#include <sys/sysctl.h>
#endif

#if !defined(L1_CACHE_LINE_SIZE)
#define L1_CACHE_LINE_SIZE 64
#endif

NORI_NAMESPACE_BEGIN

Color3f Color3f::toSRGB() const {
	Color3f result;

	for (int i=0; i<3; ++i) {
		float value = coeff(i);

		if (value <= 0.0031308f)
			result[i] = 12.92f * value;
		else
			result[i] = (1.0f + 0.055f)
				* std::pow(value, 1.0f/2.4f) -  0.055f;
	}

	return result;
}

Color3f Color3f::toLinearRGB() const {
	Color3f result;

	for (int i=0; i<3; ++i) {
		float value = coeff(i);

		if (value <= 0.04045f)
			result[i] = value * (1.0f / 12.92f);
		else
			result[i] = std::pow((value + 0.055f)
				* (1.0f / 1.055f), 2.4f);
	}

	return result;
}

bool Color3f::isValid() const {
	for (int i=0; i<3; ++i) {
		float value = coeff(i);
		int cl = boost::math::fpclassify(value);
		if (value < 0 || cl == FP_INFINITE || cl == FP_NAN)
			return false;
	}
	return true;
}

float Color3f::getLuminance() const {
	return coeff(0) * 0.212671f + coeff(1) * 0.715160f + coeff(2) * 0.072169f;
}

Transform::Transform(const Eigen::Matrix4f &trafo)
	: m_transform(trafo), m_inverse(trafo.inverse()) { }

QString Transform::toString() const {
	std::ostringstream oss;
	oss << m_transform.format(Eigen::IOFormat(4, 0, ", ", ";\n", "", "", "[", "]"));
	return QString(oss.str().c_str());
}
QString Transform::toLineString() const {
	std::ostringstream oss;
        for(int row = 0; row < 4; ++row){
            if(row > 0) oss << "; ";
            for(int col = 0; col < 4; ++col){
                if(col > 0) oss << ", ";
                oss << m_transform(row, col);
            }
        }
	return QString(oss.str().c_str());
}

Vector3f squareToUniformSphere(const Point2f &sample) {
	float z = 1.0f - 2.0f * sample.y();
	float r = std::sqrt(std::max((float) 0.0f, 1.0f - z*z));
	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.x(), &sinPhi, &cosPhi);
	return Vector3f(r * cosPhi, r * sinPhi, z);
}

Vector3f squareToUniformHemisphere(const Point2f &sample) {
	float cosTheta = sample.x();
	float sinTheta = std::sqrt(std::max((float) 0, 1-cosTheta*cosTheta));

	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.y(), &sinPhi, &cosPhi);

	return Vector3f(cosPhi * sinTheta, sinPhi * sinTheta, cosTheta);
}

Point2f squareToUniformDisk(const Point2f &sample) {
	float r = std::sqrt(sample.x());
	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.y(), &sinPhi, &cosPhi);

	return Point2f(
		cosPhi * r,
		sinPhi * r
	);
}

Point2f squareToUniformDiskConcentric(const Point2f &sample) {
	float r1 = 2.0f*sample.x() - 1.0f;
	float r2 = 2.0f*sample.y() - 1.0f;

	Point2f coords;
	if (r1 == 0 && r2 == 0) {
		coords = Point2f(0, 0);
	} else if (r1 > -r2) { /* Regions 1/2 */
		if (r1 > r2)
			coords = Point2f(r1, (M_PI/4.0f) * r2/r1);
		else
			coords = Point2f(r2, (M_PI/4.0f) * (2.0f - r1/r2));
	} else { /* Regions 3/4 */
		if (r1<r2)
			coords = Point2f(-r1, (M_PI/4.0f) * (4.0f + r2/r1));
		else
			coords = Point2f(-r2, (M_PI/4.0f) * (6.0f - r1/r2));
	}

	Point2f result;
	sincosf(coords.y(), &result[1], &result[0]);
	return result*coords.x();
}

Point2f squareToUniformTriangle(const Point2f &sample) {
	float a = std::sqrt(1.0f - sample.x());
	return Point2f(1 - a, a * sample.y());
}

float intervalToTent(float sample) {
	float sign;

	if (sample < 0.5f) {
		sign = 1;
		sample *= 2;
	} else {
		sign = -1;
		sample = 2 * (sample - 0.5f);
	}

	return sign * (1 - std::sqrt(sample));
}

Point2f squareToTent(const Point2f &sample) {
	return Point2f(
		intervalToTent(sample.x()),
		intervalToTent(sample.y())
	);
}

Point2f squareToTriangle(const Point2f &sample) {
	float a = std::sqrt(1.0f - sample.x());
	return Point2f(1 - a, a * sample.y());
}

Vector3f sphericalDirection(float theta, float phi) {
	float sinTheta, cosTheta, sinPhi, cosPhi;

	sincosf(theta, &sinTheta, &cosTheta);
	sincosf(phi, &sinPhi, &cosPhi);

	return Vector3f(
		sinTheta * cosPhi,
		sinTheta * sinPhi,
		cosTheta
	);
}

Point2f sphericalCoordinates(const Vector3f &v) {
	Point2f result(
		std::acos(v.z()),
		std::atan2(v.y(), v.x())
	);
	if (result.y() < 0)
		result.y() += 2*M_PI;
	return result;
}

void coordinateSystem(const Vector3f &a, Vector3f &b, Vector3f &c) {
	if (std::abs(a.x()) > std::abs(a.y())) {
		float invLen = 1.0f / std::sqrt(a.x() * a.x() + a.z() * a.z());
		c = Vector3f(a.z() * invLen, 0.0f, -a.x() * invLen);
	} else {
		float invLen = 1.0f / std::sqrt(a.y() * a.y() + a.z() * a.z());
		c = Vector3f(0.0f, a.z() * invLen, -a.y() * invLen);
	}
	b = c.cross(a);
}

void *allocAligned(size_t size) {
#if defined(PLATFORM_WINDOWS)
	return _aligned_malloc(size, L1_CACHE_LINE_SIZE);
#elif defined(PLATFORM_MACOS)
	/* OSX malloc already returns 16-byte aligned data suitable
	   for AltiVec and SSE computations */
	return malloc(size);
#else
	return memalign(L1_CACHE_LINE_SIZE, size);
#endif
}

void freeAligned(void *ptr) {
#if defined(PLATFORM_WINDOWS)
	_aligned_free(ptr);
#else
	free(ptr);
#endif
}

int getCoreCount() {
#if defined(PLATFORM_WINDOWS)
	SYSTEM_INFO sys_info;
	GetSystemInfo(&sys_info);
	return sys_info.dwNumberOfProcessors;
#elif defined(PLATFORM_MACOS)
	int nprocs;
	size_t nprocsSize = sizeof(int);
	if (sysctlbyname("hw.activecpu", &nprocs, &nprocsSize, NULL, 0))
		throw NoriException("Could not detect the number of processors!");
	return (int) nprocs;
#else
	return sysconf(_SC_NPROCESSORS_CONF);
#endif
}

QString indent(const QString &string, int amount) {
	QString result = string;
	result.replace("\n", QString("\n") + QString(" ").repeated(amount));
	return result;
}

float fresnel(float cosThetaI, float extIOR, float intIOR) {
	float etaI = extIOR, etaT = intIOR;

	if (extIOR == intIOR)
		return 0.0f;

	/* Swap the indices of refraction if the interaction starts
	   at the inside of the object */
	if (cosThetaI < 0.0f) {
		std::swap(etaI, etaT);
		cosThetaI = -cosThetaI;
	}

	/* Using Snell's law, calculate the squared sine of the
	   angle between the normal and the transmitted ray */
	float eta = etaI / etaT,
		  sinThetaTSqr = eta*eta * (1-cosThetaI*cosThetaI);

	if (sinThetaTSqr > 1.0f)
		return 1.0f;  /* Total internal reflection! */

	float cosThetaT = std::sqrt(1.0f - sinThetaTSqr);

	float Rs = (etaI * cosThetaI - etaT * cosThetaT)
	         / (etaI * cosThetaI + etaT * cosThetaT);
	float Rp = (etaT * cosThetaI - etaI * cosThetaT)
	         / (etaT * cosThetaI + etaI * cosThetaT);

	return (Rs * Rs + Rp * Rp) / 2.0f;
}

NORI_NAMESPACE_END


//
//  mex_top_eig.cpp
//
//
//  Created by Bo_Royce on 8/17/16.
//
//
#include <mex.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <SymEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>
#include <time.h>
using namespace Eigen;
using namespace Spectra;
void make_top_eigenvectors(double *val,double *ind, int KK, int NN, int NK, double *eigenvectors,double *eigenvalues){
    clock_t begin = clock();
    Eigen::SparseMatrix<double> mat((const int) NN,(const int) NN);         // default is column major
    mat.reserve(Eigen::VectorXi::Constant((const int) NN, (const int) NK));
    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletList;
    tripletList.reserve((const int) NN*NK);
    for(int i=0; i<NN; i++){
        for (int j = 0; j< NK; j++){
            tripletList.push_back(T((int) ind[i+NN*j]-1,i,val[i+j*NN]));
        }
    }
    mat.setFromTriplets(tripletList.begin(), tripletList.end());
    mat += Eigen::SparseMatrix<double>(mat.transpose());
    clock_t end = clock();
    //printf("Elapsed time in initialization is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);

    SparseSymMatProd<double> op(mat);
    begin = clock();
    // Construct eigen solver object, requesting the largest KK eigenvalues
    SymEigsSolver< double, LARGEST_ALGE, SparseSymMatProd<double> > eigs(&op, KK, 2*KK);

    // Initialize and compute
    eigs.init();
    int nconv = eigs.compute();

    // Retrieve results

    Eigen::VectorXd evalues;
    Eigen::MatrixXd evectors;
    if(eigs.info() == SUCCESSFUL){
        evalues = eigs.eigenvalues();
        evectors = eigs.eigenvectors();
    }
    //std::cout << "Eigenvalues found:\n" << evalues << std::endl;
    end = clock();
    printf("Elapsed time in eigen-decomposition is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
    begin = clock();
    for (int j = 0; j< KK; j++){
        eigenvalues[j] = evalues[j];
        for (int i = 0; i < NN; i++){
            eigenvectors[i+NN*j] = evectors.col(j)[i];
        }
    }
   end = clock();
    printf("Elapsed time in copying eigenvectors is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
}

/// usage: eigenvectors = mex_top_eig(val, ind, KK);
/// input: val of size NxK, the value of transition matrix
///        ind of size NxK, the index of the values (Note ind starts with 0);
///        KK , the number of eigenvectors
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    double *val, *ind, *eigenvectors,*eigenvalues;
    int KK, NN, NK;
    val = mxGetPr(prhs[0]);
    ind = mxGetPr(prhs[1]); //
    KK = (int) mxGetScalar(prhs[2]); //number of eigenvalues
    NN = mxGetM(prhs[0]);
    NK = mxGetN(prhs[1]);
    plhs[0] = mxCreateDoubleMatrix(NN,KK,mxREAL);
    eigenvectors = mxGetPr(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix(KK,1,mxREAL);
    eigenvalues = mxGetPr(plhs[1]);
    make_top_eigenvectors(val,ind, KK,  NN, NK, eigenvectors,eigenvalues);
}


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef FIXEDGRID3_HPP_6KPVVRZF
#define FIXEDGRID3_HPP_6KPVVRZF

#include <math.h>
#include <stdint.h>

#include <vector>

#include <boost/shared_ptr.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <ros/console.h>

#include <pcl_util/point_types.hpp>
#include <geom_cast/geom_cast.hpp>

#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"

namespace ca
{

template<class Scalar>
class FixedGrid3 {
public:
  typedef Scalar ScalarType; // TODO what is my convention for this?
  typedef Eigen::Matrix<Scalar, 3, 1> Vec3;
  typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Mat3;

  typedef boost::shared_ptr<FixedGrid3> Ptr;
  typedef boost::shared_ptr<const FixedGrid3> ConstPtr;

public:
  FixedGrid3() :
      box_(),
      origin_(0, 0, 0),
      min_world_corner_ijk_(0, 0, 0),
      dimension_(0, 0, 0),
      num_cells_(0),
      strides_(0, 0, 0),
      resolution_(0)
  { }

  /**
   * @param center: center of the grid in global frame
   * @param dimension: number of grid cells along each coordinate
   * @param resolution: size of each grid cell side. they are cubic.
   * Default assumes "ZYX" layout, i.e. z changes fastest.
   */
  FixedGrid3(const Vec3& center,
             const Vec3Ix& dimension,
             Scalar resolution,
             bool x_fastest=false) :
      box_(center-(dimension.cast<Scalar>()*resolution)/2,
           center+(dimension.cast<Scalar>()*resolution)/2),
      origin_(center-box_.radius()),
      dimension_(dimension),
      num_cells_(dimension.prod()),
      resolution_(resolution)
  {
    if (x_fastest) {
      strides_ = Vec3Ix(1, dimension[0], dimension.head<2>().prod());
    } else {
      strides_ = Vec3Ix(dimension.tail<2>().prod(), dimension[2], 1);
    }
    this->calc_min_corner_ijk();

  }

  virtual ~FixedGrid3() { }

  FixedGrid3(const FixedGrid3& other) :
      box_(other.box_),
      origin_(other.origin_),
      min_world_corner_ijk_(other.min_world_corner_ijk_),
      dimension_(other.dimension_),
      num_cells_(other.num_cells_),
      strides_(other.strides_),
      resolution_(other.resolution_)

  {
  }

  FixedGrid3& operator=(const FixedGrid3& other) {
    if (this==&other) { return *this; }
    box_ = other.box_;
    origin_ = other.origin_;
    min_world_corner_ijk_ = other.min_world_corner_ijk_;
    dimension_ = other.dimension_;
    num_cells_ = other.num_cells_;
    strides_ = other.strides_;
    resolution_ = other.resolution_;
    return *this;
  }

public:

  /**
   * see ctor for params
   */
  void reset(const Vec3& center,
             const Vec3Ix& dimension,
             Scalar resolution,
             bool x_fastest=false) {
    box_.set_center(center);
    box_.set_radius((dimension.template cast<Scalar>()*resolution)/2);
    origin_ = center - box_.radius();
    dimension_ = dimension;
    num_cells_ = dimension.prod();
    if (x_fastest) {
      strides_ = Vec3Ix(1, dimension[0], dimension.head<2>().prod());
    } else {
      strides_ = Vec3Ix(dimension.tail<2>().prod(), dimension[2], 1);
    }
    resolution_ = resolution;
  }

  /**
   * Is pt inside 3D box containing grid?
   * @param pt point in same frame as center (probably world_view)
   */
  bool is_inside_box(const Vec3& pt) const {
    return box_.contains(pt);
  }

  /**
   */
  Eigen::VectorXi multiple_is_inside_box(const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& pts) {
    Eigen::VectorXi out(pts.cols());
    for (int i=0; i < pts.cols(); ++i) {
      const Eigen::Matrix<Scalar, 3, 1>& pt(pts.col(i));
      int inside = box_.contains(pt);
      out[i] = inside;
    }
    return out;
  }

  bool is_inside_box(Scalar x, Scalar y, Scalar z) const {
    return box_.contains(Vec3(x, y, z));
  }

  template<class PointT>
  bool is_inside_box(const PointT& pt) const {
    return box_.contains(ca::point_cast<Eigen::Vector3d>(pt));
  }

  /**
   * is i, j, k inside the grid limits?
   */
  bool is_inside_grid(const Vec3Ix& grid_ix) const {
    return ((grid_ix.array() >= 0).all() &&
            (grid_ix.array() < dimension_.array()).all());
  }

  bool is_inside_grid(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->is_inside_grid(Vec3Ix(i, j, k));
  }

  /**
   * Given position in world coordinates, return ijk grid coordinates.
   * Note: does not check if point is inside grid.
   */
  Vec3Ix world_to_grid(const Vec3& xyz) const {
    Vec3 tmp = ((xyz - origin_).array() - 0.5*resolution_)/resolution_;
    //return tmp.cast<grid_ix_t>();
    return Vec3Ix(round(tmp.x()), round(tmp.y()), round(tmp.z()));
  }

  Vec3Ix world_to_grid(Scalar x, Scalar y, Scalar z) const {
    return this->world_to_grid(Vec3(x, y, z));
  }

  /**
   * Given ijk grid coordinates resurn xyz world coordinates.
   * xyz is center of corresponding voxel.
   */
  Vec3 grid_to_world(const Vec3Ix& grid_ix) const {
    Vec3 w((grid_ix.cast<Scalar>()*resolution_ + origin_).array() + 0.5*resolution_);
    return w;
  }

  Vec3 grid_to_world(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->grid_to_world(Vec3Ix(i, j, k));
  }

  Mat3 multiple_grid_to_world(const Mat3Ix& grid_indices) {

    Mat3 out( 3, grid_indices.cols() );
    for (int ix=0; ix < grid_indices.cols(); ++ix) {
      Vec3Ix gix(grid_indices.col(ix));
      out.col(ix) = this->grid_to_world( gix );
    }
    return out;

  }

  /**
   * given grid ijk coordinate, return linear memory index.
   */
  mem_ix_t grid_to_mem(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->grid_to_mem(Vec3Ix(i, j, k));
  }

  mem_ix_t grid_to_mem(const Vec3Ix& grid_ix) const {
    return strides_.dot(grid_ix);
  }

  MemIxVector multiple_grid_to_mem(const Mat3Ix& grid_indices) {
    MemIxVector out( grid_indices.cols() );
    for (int ix=0; ix < grid_indices.cols(); ++ix) {
      Vec3Ix gix(grid_indices.col(ix));
      out[ix] = this->grid_to_mem( gix );
    }
    return out;
  }

  /**
   * given linear memory index, return ijk coordinate.
   */
  Vec3Ix mem_to_grid(mem_ix_t mem_ix) const {
    grid_ix_t i = mem_ix/strides_[0];
    mem_ix -= i*strides_[0];
    grid_ix_t j = mem_ix/strides_[1];
    mem_ix -= j*strides_[1];
    grid_ix_t k = mem_ix;

    return Vec3Ix(i, j, k);
  }

  /**
   * pack into grid_ix_t as [int16, int16, int16, 0]
   * note grid_ix_it is a signed 64-bit type
   * this gives range of [-32768, 32768] for each coordinate
   * so if voxel is 5cm, [-1638.4 m, 1638.4 m] relative to initial center.
   * this is useful as a unique hash.
   * this is better than mem_ix because mem_ix is ambiguous for absolute ijk.
   * if we use linear mem_ix as a hash,
   * then because of scrolling there may be collisions, and mem_ix become
   * invalidated once the corresponding voxel scrolls out.
   */
  hash_ix_t grid_to_hash(const Vec3Ix& grid_ix) const {
    // grid2 should be all positive
    Vec3Ix grid2(grid_ix - min_world_corner_ijk_);

    hash_ix_t hi = static_cast<hash_ix_t>(grid2[0]);
    hash_ix_t hj = static_cast<hash_ix_t>(grid2[1]);
    hash_ix_t hk = static_cast<hash_ix_t>(grid2[2]);
    hash_ix_t h = (hi << 48) | (hj << 32) | (hk << 16);
    return h;
  }

  Vec3Ix hash_to_grid(hash_ix_t hix) const {
    hash_ix_t hi = (hix & 0xffff000000000000) >> 48;
    hash_ix_t hj = (hix & 0x0000ffff00000000) >> 32;
    hash_ix_t hk = (hix & 0x00000000ffff0000) >> 16;
    Vec3Ix grid_ix(hi, hj, hk);
    grid_ix += min_world_corner_ijk_;
    return grid_ix;
  }

  Mat3Ix multiple_hash_to_grid(const HashIxVector& hindices) const {
    Mat3Ix out( 3, hindices.rows() );
    for (size_t i=0; i < hindices.rows(); ++i) {
      Vec3Ix gix = this->hash_to_grid(hindices[i]);
      out.col(i) = gix;
    }
    return out;
  }

  /**
   * Like the above but does not offset by origin.
   * In the fixed case we assume the min ijk for origin is 0,0,0.
   */
  hash_ix_t local_grid_to_hash(const Vec3Ix& grid_ix) const {
    // TODO assumes grid_ix is inside box.
    hash_ix_t hi = static_cast<hash_ix_t>(grid_ix[0]);
    hash_ix_t hj = static_cast<hash_ix_t>(grid_ix[1]);
    hash_ix_t hk = static_cast<hash_ix_t>(grid_ix[2]);
    hash_ix_t h = (hi << 48) | (hj << 32) | (hk << 16);
    return h;
  }

  Vec3Ix hash_to_local_grid(hash_ix_t hix) const {
    hash_ix_t hi = (hix & 0xffff000000000000) >> 48;
    hash_ix_t hj = (hix & 0x0000ffff00000000) >> 32;
    hash_ix_t hk = (hix & 0x00000000ffff0000) >> 16;
    Vec3Ix grid_ix(hi, hj, hk);
    return grid_ix;
  }

  Mat3Ix multiple_local_hash_to_grid(const HashIxVector& hindices) const {
    Mat3Ix out( 3, hindices.rows() );
    for (size_t i=0; i < hindices.rows(); ++i) {
      Vec3Ix gix = this->local_hash_to_grid(hindices[i]);
      out.col(i) = gix;
    }
    return out;
  }

 public:
  const ca::scrollgrid::Box<Scalar, 3>& box() const { return box_; }
  grid_ix_t dim_i() const { return dimension_[0]; }
  grid_ix_t dim_j() const { return dimension_[1]; }
  grid_ix_t dim_k() const { return dimension_[2]; }
  // this is the case because of the way origin is set at construction.
  // TODO should we allow arbitrary origins?
  grid_ix_t first_i() const { return 0; }
  grid_ix_t first_j() const { return 0; }
  grid_ix_t first_k() const { return 0; }
  grid_ix_t last_i() const { return dimension_[0]; }
  grid_ix_t last_j() const { return dimension_[1]; }
  grid_ix_t last_k() const { return dimension_[2]; }
  const Vec3Ix& dimension() const { return dimension_; }
  const Vec3& radius() const { return box_.radius(); }
  const Vec3& origin() const { return origin_; }
  Vec3 min_pt() const { return box_.min_pt(); }
  Vec3 max_pt() const { return box_.max_pt(); }
  const Vec3& center() const { return box_.center(); }
  Scalar resolution() const { return resolution_; }

  // basically equivalent to scroll_offset = (0, 0, 0)
  grid_ix_t num_cells() const { return num_cells_; }

 private:
  void calc_min_corner_ijk() {
    // set the "minimum possible" ijk, assuming the grid
    // won't stray "too far" from the initial position.
    // too far == more than 2^15 grid cells.
    // so if you voxel resolution is 1 cm, 327.68 m.
    Scalar m = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    Vec3 m3(m, m, m);
    //std::cerr << "m3 = " << m3.transpose() << std::endl;
    m3 += box_.center();
    //std::cerr << "m3pcenter = " << m3.transpose() << std::endl;
    min_world_corner_ijk_ = this->world_to_grid(m3);
    //std::cerr << "min_world_corner_ijk_ = " << min_world_corner_ijk_.transpose() << std::endl;
  }

 private:
  // 3d box enclosing grid. In whatever coordinates were given (probably
  // world_view)
  ca::scrollgrid::Box<Scalar, 3> box_;

  // static origin of the grid coordinate system.
  // it's center - box.radius
  Vec3 origin_;

  // minimum world corner in ijk. used for hash
  Vec3Ix min_world_corner_ijk_;

  // number of grid cells along each axis
  Vec3Ix dimension_;

  // number of cells
  grid_ix_t num_cells_;

  // grid strides to translate from linear to 3D layout.
  // C-ordering, ie x slowest, z fastest.
  Vec3Ix strides_;

  // size of grid cells
  Scalar resolution_;

};

typedef FixedGrid3<double> FixedGrid3d;
typedef FixedGrid3<float> FixedGrid3f;

} /* ca */

#endif /* end of include guard: FIXEDGRID3_HPP_6KPVVRZF */


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: Calculation of eigenvalues using Lanczos' method (lanczos.cpp and lanczos.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

// include necessary system headers
#include <iostream>

#ifndef NDEBUG
  #define NDEBUG
#endif

#define VIENNACL_WITH_UBLAS

//include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/lanczos.hpp"
#include "viennacl/io/matrix_market.hpp"
// Some helper functions for this tutorial:
#include <iostream>
#include <fstream>
#include <limits>
#include <string>
#include <iomanip>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

template <typename MatrixType>
std::vector<double> initEig(MatrixType const & A)
{
  viennacl::linalg::lanczos_tag ltag(0.75, 10, viennacl::linalg::lanczos_tag::partial_reorthogonalization, 1700);
  std::vector<double> lanczos_eigenvalues = viennacl::linalg::eig(A, ltag);
  for(std::size_t i = 0; i< lanczos_eigenvalues.size(); i++){
          std::cout << "Eigenvalue " << i+1 << ": " << std::setprecision(10) << lanczos_eigenvalues[i] << std::endl;
  }

  return lanczos_eigenvalues;
}

int main()
{
  typedef double     ScalarType;

  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_A;

  if (!viennacl::io::read_matrix_market_file(ublas_A, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }

  std::cout << "Running Lanczos algorithm (this might take a while)..." << std::endl;
  std::vector<double> eigenvalues = initEig(ublas_A);
}


// Includes
// ========
#include <iostream>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, long> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> flow_graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

// Custom edge adder class
class edge_adder {
 weighted_graph &G;

 public:
  explicit edge_adder(weighted_graph &G) : G(G) {}
  void add_edge(int from, int to, long weight) {
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    w_map[e] = weight;   // new assign cost
  }
};

// Custom edge adder class, highly recommended
class edge_adder_flow {
  flow_graph &G;

 public:
  explicit edge_adder_flow(flow_graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void testcase() {
    int n, m, a, s, c, d;
    std::cin >> n >> m >> a >> s >> c >> d;
    weighted_graph G(n);
    edge_adder adder(G);

    for(int i = 0; i < m; i++) {
        char w; int x, y, z;
        std::cin >> w >> x >> y >> z;
        adder.add_edge(x, y, z);
        if(w == 'L') {
            adder.add_edge(y, x, z);
        }
    }

    std::vector<int> agent(a);
    for(int i = 0; i < a; i++) {
        std::cin >> agent[i];
    }

    std::vector<int> shelter(s);
    for(int i = 0; i < s; i++) {
        std::cin >> shelter[i];
    }

    std::vector<std::vector<long>> distance_to(a, std::vector<long>(s));
    long max_dist = 0;
    for(int i = 0; i < a; i++) {
        std::vector<int> dist_map(n);
        boost::dijkstra_shortest_paths(G, agent[i],
            boost::distance_map(boost::make_iterator_property_map(
            dist_map.begin(), boost::get(boost::vertex_index, G))));
        for(int j = 0; j < s; j++) {
            distance_to[i][j] = dist_map[shelter[j]];
            if(distance_to[i][j] < std::numeric_limits<int>::max()) {
                max_dist = std::max(max_dist, distance_to[i][j]);
            }
        }
    }

    int l = 0, r = max_dist + c * d;

    while(l < r) {
        // compute matching for all edges dist + c * d <= mid
        flow_graph G_f(a + c * s);
        edge_adder_flow adder_f(G_f);
        int mid = (l + r) / 2;
        const int v_source = boost::add_vertex(G_f);
        const int v_sink = boost::add_vertex(G_f);
        for(int i = 0; i < a; i++) {
            adder_f.add_edge(v_source, i, 1);
            for(int j = 0; j < s; j++) {
                if(distance_to[i][j] + d <= mid) adder_f.add_edge(i, a + j, 1);
                if(c == 2) {
                    if(distance_to[i][j] + 2 * d <= mid) adder_f.add_edge(i, a + s + j, 1);
                }
            }
        }
        for(int j = 0; j < s; j++) {
            adder_f.add_edge(a + j, v_sink, 1);
            if(c == 2) adder_f.add_edge(a + s + j, v_sink, 1);
        }
        long flow = boost::push_relabel_max_flow(G_f, v_source, v_sink);
        if(flow == a) { // matching maximal -> all agents arrive at shelter
            r = mid;
        } else {
            l = mid + 1;
        }
    }
    std::cout << l << std::endl;
    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


#include <iostream>
#include <Eigen/Dense>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

using namespace Eigen;
namespace py = pybind11;

using RowMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

Eigen::MatrixXf multMatrix(MatrixXf matriz)
{
     MatrixXf c;
     c = 2*matriz;
     std::cout << "2*matriz =\n" << c << std::endl;

     return c;
}

void multMatrix_Ref(Eigen::Ref<MatrixXf> matriz)
{
     matriz = 2*matriz;
     std::cout << "2*matriz =\n" << matriz << std::endl;
}

void multRowMatrix_Ref(Eigen::Ref<RowMatrixXf> matriz)
{
     matriz = 2*matriz;
     std::cout << "2*matriz =\n" << matriz << std::endl;
}

Eigen::MatrixXf multMatrixByMatrix_Ref(Eigen::Ref<MatrixXf> matriz1, Eigen::Ref<MatrixXf> matriz2)
{

     MatrixXf mult = matriz1 * matriz2.transpose();
     //std::cout << "matriz1*matriz2 =\n" << mult << std::endl;
     return mult;
}

PYBIND11_MODULE(demo, m) {
     m.doc() = "pybind11 example plugin"; // optional module docstring
     m.def("multMatrix", &multMatrix, "Funcion con un argumento",
          py::arg("m"));
     m.def("multMatrixRef", &multMatrix_Ref, "Funcion con un argumento pasado por referencia",
          py::arg("m"));
     m.def("multRowMatrixRef", &multRowMatrix_Ref, "Funcion con un argumento pasado por referencia con rowMajor",
          py::arg("m"));
     m.def("multMatrixByMatrix_Ref", &multMatrixByMatrix_Ref, "Funcion con 2 argumento pasado por referencia",
          py::arg("matriz1"), py::arg("matriz2"));
}


#include "gcd.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <algorithm>
#include <cmath>
namespace HT
{
    void gcd(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()<2)
          throw std::runtime_error("Gcd should have at least 1 parameter");
        auto  secondCh = (++astnode->ch.begin() );
        myParserHelper.parse(*secondCh);
        if ((*secondCh)->token.tokenType != Complex) throw std::runtime_error("the argument of gcd must be complex"+ (*secondCh)->token.raw);
        auto cast = boost::get<ComplexType>((*secondCh)->token.info);
        if (!cast.isInt())
          throw std::runtime_error("the argument of gcd must be int");
        auto now = cast.toInt();

        std::for_each(++secondCh, astnode->ch.end(), [&](PASTNode an)
                    {
                    myParserHelper.parse(an);
                    if (an->token.tokenType!=Complex) throw std::runtime_error("the argument of gcd must be complex" + an->token.raw);
                    cast = boost::get<ComplexType>(an->token.info);
                    if (!cast.isInt())
                        throw std::runtime_error("the argument of gcd must be int");
                    if (!cast.toInt().isZero())
                    now = gcd( now, cast.toInt());
                    });

        astnode->token.info = ComplexType(now.setSign(true));
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        astnode->remove();
    }
}


#include "teca_laplacian.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;
using std::tan;

//#define TECA_DEBUG

namespace {

template <typename num_t>
constexpr num_t deg_to_rad() { return num_t(M_PI)/num_t(180); }

template <typename num_t>
constexpr num_t earth_radius() { return num_t(6371.0e3); }

// compute the laplacian. This  assumes fixed mesh spacing. Here we add periodic
// bc in lon and apply unit stride vector optimization strategy to loops
template <typename num_t, typename pt_t>
void laplacian(num_t *w, const pt_t *lon, const pt_t *lat,
    const num_t *f, unsigned long n_lon,
    unsigned long n_lat, bool periodic_lon=true)
{
    size_t n_bytes = n_lat*sizeof(num_t);
    num_t *delta_lon_sq = static_cast<num_t*>(malloc(n_bytes));

    // delta lon squared as a function of latitude
    num_t d_lon = (lon[1] - lon[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    // tan(lat)
    num_t *tan_lat = static_cast<num_t*>(malloc(n_bytes));
    for (unsigned long j = 0; j < n_lat; ++j)
    {
        delta_lon_sq[j] = pow(d_lon * cos(lat[j] * deg_to_rad<num_t>()),2);
    	tan_lat[j] = tan(lat[j] * deg_to_rad<num_t>());
    }

    // delta lat squared
    num_t delta_v = (lat[1] - lat[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    num_t dlat = num_t(2)*delta_v;
    num_t dlat_sq = delta_v*delta_v;
    dlat *= earth_radius<num_t>(); // scale dlat by R for the tan term

    unsigned long max_i = n_lon - 1;
    unsigned long max_j = n_lat - 1;

    // laplacian
    for (unsigned long j = 1; j < max_j; ++j)
    {
	// set the current row in the u/v/w arrays
        unsigned long jj = j*n_lon;
	/*
	 * The following f_* variables describe the field
	 * f in a grid oriented fashion:
	 *
	 *	f_ipjm	f_ipj	f_ipjp
	 *
	 *	f_ijm	f_ji	f_ijp
	 *
	 *	f_imjm	f_imj	f_imjp
	 *
	 * The 'j' direction represents longitude, the
	 * 'i' direciton represents latitude.
	 *
	 * Note: The laplacian represented here uses the chain
	 * rule to separate the (1/cos(lat)*d(cos(lat)*df/dlat)/dlat
	 * term into two terms.
	 *
	 */
	// Set array pointer locations so that index 'i' refers to the
	// shifted location in all variables
        const num_t *f_ij = f + jj;          // i,j
        const num_t *f_ipj = f + jj + n_lon; // i+1, j
        const num_t *f_imj = f + jj - n_lon; // i-1, j
        const num_t *f_ijp = f + jj + 1;     // i,   j + 1
        const num_t *f_ijm = f + jj - 1;     // i,   j - 1

	// set the pointer index for the output field w
	// ... this is index i,j
        num_t *ww = w + jj;
	// create a dummy variable for u**2
        num_t dlon_sq = delta_lon_sq[j];

        for (unsigned long i = 1; i < max_i; ++i)
        {
	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[i] = (f_imj[i] - num_t(2)*f_ij[i] + f_ipj[i])/dlat_sq -
		    tan_lat[j]*(f_ipj[i]-f_imj[i])/dlat +
                    (f_ijm[i] - num_t(2)*f_ij[i] + f_ijp[i])/dlon_sq;
        }
    }

    if (periodic_lon)
    {
        // periodic in longitude; leftmost boundary
        for (unsigned long j = 1; j < max_j; ++j)
        {
	    // set the current row in the u/v/w arrays
            unsigned long jj = j*n_lon;
	    // Set array pointer locations so that index 'i' refers to the
	    // shifted location in all variables
            const num_t *f_ij = f + jj;          // i,j
            const num_t *f_ipj = f + jj + n_lon; // i+1, j
            const num_t *f_imj = f + jj - n_lon; // i-1, j
            const num_t *f_ijp = f + jj + 1;     // i,   j + 1
            const num_t *f_ijm = f + jj - max_i; // i,   j - 1

	    // set the pointer index for the output field w
	    // ... this is index i,j
            num_t *ww = w + jj;
	    // create a dummy variable for u**2
            num_t dlon_sq = delta_lon_sq[j];

	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[0] = (f_imj[0] - num_t(2)*f_ij[0] + f_ipj[0])/dlat_sq -
		    tan_lat[j]*(f_ipj[0]-f_imj[0])/dlat +
                    (f_ijm[0] - num_t(2)*f_ij[0] + f_ijp[0])/dlon_sq;
        }

        // periodic in longitude; rightmost boundary
        for (unsigned long j = 1; j < max_j; ++j)
        {
	    // set the current row in the u/v/w arrays
            unsigned long jj = j*n_lon;

	    // Set array pointer locations so that index 'i' refers to the
	    // shifted location in all variables
            const num_t *f_ij = f + jj + max_i;          // i,j
            const num_t *f_ipj = f + jj + max_i + n_lon; // i+1, j
            const num_t *f_imj = f + jj + max_i - n_lon; // i-1, j
            const num_t *f_ijp = f + jj;                 // i,   j + 1
            const num_t *f_ijm = f + jj - max_i;         // i,   j - 1

	    // set the pointer index for the output field w
	    // ... this is index i,j
            num_t *ww = w + jj + max_i;
	    // create a dummy variable for u**2
            num_t dlon_sq = delta_lon_sq[j];

	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[0] = (f_imj[0] - num_t(2)*f_ij[0] + f_ipj[0])/dlat_sq -
		    tan_lat[j]*(f_ipj[0]-f_imj[0])/dlat +
                    (f_ijm[0] - num_t(2)*f_ij[0] + f_ijp[0])/dlon_sq;
        }
    }
    else
    {
        // zero it out
        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon] = num_t();

        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon + max_i] = num_t();
    }

    // extend values into lat boundaries
    num_t *dest = w;
    num_t *src = w + n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i+n_lon];

    dest = w + max_j*n_lon;
    src = dest - n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i];

    free(delta_lon_sq);
    free(tan_lat);

    return;
}
};

// --------------------------------------------------------------------------
teca_laplacian::teca_laplacian() :
    component_0_variable(),
    laplacian_variable("laplacian")
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_laplacian::~teca_laplacian()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_laplacian::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_laplacian":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_0_variable,
            "array containing the input variable")
        TECA_POPTS_GET(std::string, prefix, laplacian_variable,
            "array to store the computed laplacian in")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_laplacian::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::string, prefix, component_0_variable)
    TECA_POPTS_SET(opts, std::string, prefix, laplacian_variable)
}
#endif

// --------------------------------------------------------------------------
std::string teca_laplacian::get_component_0_variable(
    const teca_metadata &request)
{
    std::string comp_0_var = this->component_0_variable;

    if (comp_0_var.empty() &&
        request.has("teca_laplacian::component_0_variable"))
            request.get("teca_laplacian::component_0_variable", comp_0_var);

    return comp_0_var;
}

// --------------------------------------------------------------------------
std::string teca_laplacian::get_laplacian_variable(
    const teca_metadata &request)
{
    std::string lapl_var = this->laplacian_variable;

    if (lapl_var.empty())
    {
        if (request.has("teca_laplacian::laplacian_variable"))
            request.get("teca_laplacian::laplacian_variable", lapl_var);
        else
            lapl_var = "laplacian";
    }

    return lapl_var;
}

// --------------------------------------------------------------------------
teca_metadata teca_laplacian::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_laplacian::get_output_metadata" << endl;
#endif
    (void)port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);
    out_md.append("variables", this->laplacian_variable);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_laplacian::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs;

    // get the name of the arrays we need to request
    std::string comp_0_var = this->get_component_0_variable(request);
    if (comp_0_var.empty())
    {
        TECA_ERROR("component 0 array was not specified")
        return up_reqs;
    }

    // copy the incoming request to preserve the downstream
    // requirements and add the arrays we need
    teca_metadata req(request);

    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    arrays.insert(this->component_0_variable);

    // capture the array we produce
    arrays.erase(this->get_laplacian_variable(request));

    // update the request
    req.set("arrays", arrays);

    // send it up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_laplacian::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_laplacian::execute" << endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_ERROR("teca_cartesian_mesh is required")
        return nullptr;
    }

    // get component 0 array
    std::string comp_0_var = this->get_component_0_variable(request);

    if (comp_0_var.empty())
    {
        TECA_ERROR("component_0_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array comp_0
        = in_mesh->get_point_arrays()->get(comp_0_var);

    if (!comp_0)
    {
        TECA_ERROR("requested array \"" << comp_0_var << "\" not present.")
        return nullptr;
    }

    // get the input coordinate arrays
    const_p_teca_variant_array lon = in_mesh->get_x_coordinates();
    const_p_teca_variant_array lat = in_mesh->get_y_coordinates();

    if (!lon || !lat)
    {
        TECA_ERROR("lat lon mesh cooridinates not present.")
        return nullptr;
    }

    // allocate the output array
    p_teca_variant_array lapl = comp_0->new_instance();
    lapl->resize(comp_0->size());

    // compute laplacian
    NESTED_TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        lon.get(), 1,

        const NT1 *p_lon = dynamic_cast<const TT1*>(lon.get())->get();
        const NT1 *p_lat = dynamic_cast<const TT1*>(lat.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(
            teca_variant_array_impl,
            lapl.get(), 2,

            const NT2 *p_comp_0 = dynamic_cast<const TT2*>(comp_0.get())->get();
            NT2 *p_lapl = dynamic_cast<TT2*>(lapl.get())->get();

            ::laplacian(p_lapl, p_lon, p_lat,
                p_comp_0, lon->size(), lat->size());
            )
        )

    // create the output mesh, pass everything through, and
    // add the laplacian array
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    out_mesh->get_point_arrays()->append(
        this->get_laplacian_variable(request), lapl);

    return out_mesh;
}


/**
 * @file sparse_coding_test.cpp
 *
 * Test for Sparse Coding
 */

// Note: We don't use BOOST_REQUIRE_CLOSE in the code below because we need
// to use FPC_WEAK, and it's not at all intuitive how to do that.

#include <mlpack/core.hpp>
#include <mlpack/methods/sparse_coding/sparse_coding.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace arma;
using namespace mlpack;
using namespace mlpack::regression;
using namespace mlpack::sparse_coding;

BOOST_AUTO_TEST_SUITE(SparseCodingTest);

void SCVerifyCorrectness(vec beta, vec errCorr, double lambda)
{
  const double tol = 1e-12;
  size_t nDims = beta.n_elem;
  for(size_t j = 0; j < nDims; j++)
  {
    if (beta(j) == 0)
    {
      // Make sure that errCorr(j) <= lambda.
      BOOST_REQUIRE_SMALL(std::max(fabs(errCorr(j)) - lambda, 0.0), tol);
    }
    else if (beta(j) < 0)
    {
      // Make sure that errCorr(j) == lambda.
      BOOST_REQUIRE_SMALL(errCorr(j) - lambda, tol);
    }
    else // beta(j) > 0.
    {
      // Make sure that errCorr(j) == -lambda.
      BOOST_REQUIRE_SMALL(errCorr(j) + lambda, tol);
    }
  }
}

BOOST_AUTO_TEST_CASE(SparseCodingTestCodingStepLasso)
{
  double lambda1 = 0.1;
  uword nAtoms = 25;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // Normalize each point since these are images.
  for (uword i = 0; i < nPoints; ++i) {
    X.col(i) /= norm(X.col(i), 2);
  }

  SparseCoding<> sc(X, nAtoms, lambda1);
  sc.OptimizeCode();

  mat D = sc.Dictionary();
  mat Z = sc.Codes();

  for (uword i = 0; i < nPoints; ++i)
  {
    vec errCorr = trans(D) * (D * Z.unsafe_col(i) - X.unsafe_col(i));
    SCVerifyCorrectness(Z.unsafe_col(i), errCorr, lambda1);
  }
}

BOOST_AUTO_TEST_CASE(SparseCodingTestCodingStepElasticNet)
{
  double lambda1 = 0.1;
  double lambda2 = 0.2;
  uword nAtoms = 25;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // Normalize each point since these are images.
  for (uword i = 0; i < nPoints; ++i)
    X.col(i) /= norm(X.col(i), 2);

  SparseCoding<> sc(X, nAtoms, lambda1, lambda2);
  sc.OptimizeCode();

  mat D = sc.Dictionary();
  mat Z = sc.Codes();

  for(uword i = 0; i < nPoints; ++i)
  {
    vec errCorr =
      (trans(D) * D + lambda2 * eye(nAtoms, nAtoms)) * Z.unsafe_col(i)
      - trans(D) * X.unsafe_col(i);

    SCVerifyCorrectness(Z.unsafe_col(i), errCorr, lambda1);
  }
}

BOOST_AUTO_TEST_CASE(SparseCodingTestDictionaryStep)
{
  const double tol = 1e-6;

  double lambda1 = 0.1;
  uword nAtoms = 25;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // Normalize each point since these are images.
  for (uword i = 0; i < nPoints; ++i)
    X.col(i) /= norm(X.col(i), 2);

  SparseCoding<> sc(X, nAtoms, lambda1);
  sc.OptimizeCode();

  mat D = sc.Dictionary();
  mat Z = sc.Codes();

  uvec adjacencies = find(Z);
  double normGradient = sc.OptimizeDictionary(adjacencies, 1e-15);

  BOOST_REQUIRE_SMALL(normGradient, tol);
}

BOOST_AUTO_TEST_SUITE_END();


/** $Id: polygon.cxx 137064 2015-08-31 18:24:47Z jvansanten $
 * @file
 * @author Jakob van Santen <jakob.van.santen@desy.de>
 *
 * $Revision: 137064 $
 * $Date: 2015-08-31 12:24:47 -0600 (Mon, 31 Aug 2015) $
 */

#include <phys-services/surfaces/detail/polygon.h>
#include <boost/utility.hpp>

namespace I3Surfaces { namespace polygon {

namespace {

/// A counterclockwise curve is the basic building block of a convex hull
class ccw_curve : public std::vector<vec2> {
public:
	// Add a point to the curve
	void operator()(const vec2 &p)
	{
		// Remove points until the curve will be counterclockwise
		while (size() >= 2 && !ccw((*this)[size()-2], (*this)[size()-1], p))
			pop_back();
		push_back(p);
	}
private:
	static bool
	ccw(const vec2 &o, const vec2 &a, const vec2 &b)
	{
		// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
		// positive, if OAB makes a counter-clockwise turn,
		// negative for clockwise turn, and zero if the points are collinear.
		return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) > 0;
	}
};

}

/// Lifted from http://code.icecube.wisc.edu/svn/sandbox/ckopper/eventinjector/python/util/__init__.py
std::vector<vec2>
convex_hull(const std::vector<I3Position> &positions)
{
	std::vector<vec2> hull;

	// Build a set of unique points, sorted lexicographically
	std::set<vec2> points;
	std::transform(positions.begin(), positions.end(),
	    std::inserter(points, points.end()), vec2::from_I3Position);

	// Boring case: 1 point (perhaps repeated)
	if (points.size() <= 1) {
		std::copy(points.begin(), points.end(), std::back_inserter(hull));
		return hull;
	}

	// Build lower and upper hulls
	std::vector<vec2> lower = std::for_each(points.begin(), points.end(), ccw_curve());
	std::vector<vec2> upper = std::for_each(points.rbegin(), points.rend(), ccw_curve());

	// Concatenation of the lower and upper hulls gives the convex hull.
	// Last point of each list is omitted because it is repeated at the
	// beginning of the other list.
	std::copy(lower.begin(), lower.end()-1, std::back_inserter(hull));
	std::copy(upper.begin(), upper.end()-1, std::back_inserter(hull));

	return hull;
}

std::vector<vec2>
expand_polygon(const std::vector<vec2> &hull, double padding)
{
	std::vector<vec2> points;
	for (std::vector<vec2>::const_iterator p = hull.begin(); p != hull.end(); p++) {
		std::vector<vec2>::const_iterator next = boost::next(p);
		if (next == hull.end())
			next = hull.begin();
		std::vector<vec2>::const_iterator prev = boost::prior(
		    p == hull.begin() ? hull.end() : p);
		// normalized vector connecting this vertex to the next one
		vec2 d = vec2::normalized(next->x-p->x, next->y-p->y);
		// and the previous vertex to this one
		vec2 prev_d = vec2::normalized(p->x-prev->x, p->y-prev->y);
		// sine of the inner angle between the segments that meet here
		double det = prev_d.x*d.y - prev_d.y*d.x;
		if (det == 0.)
			log_fatal("Edges can't be [anti]parallel");
		vec2 outwards(prev_d.x-d.x, prev_d.y-d.y);
		points.push_back(vec2(p->x + outwards.x*padding/det, p->y + outwards.y*padding/det));
	}

	return points;
}

vec2::vec2(double xi, double yi) : x(xi), y(yi)
{}

template <typename Archive>
void vec2::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("X", x);
	ar & make_nvp("Y", y);
}

vec2
vec2::from_I3Position(const I3Position &p)
{
	return vec2(p.GetX(), p.GetY());
}

vec2
vec2::normalized(double xi, double yi)
{
	double l = hypot(xi, yi);
	return vec2(xi/l, yi/l);
}

bool
operator<(const vec2 &a, const vec2 &b)
{
	if (a.x < b.x)
		return true;
	else if (a.x > b.x)
		return false;
	else if (a.y < b.y)
		return true;
	else
		return false;
}

side::side(const vec2 &p, const vec2 &np) : origin(p),
    vector(np.x-p.x, np.y-p.y), length(hypot(vector.x, vector.y)),
	normal(vector.y/length, -vector.x/length, 0.)
{}

}}

I3_SERIALIZABLE(I3Surfaces::polygon::vec2);


#include <iostream>
#include <vector>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

void ConstructBernoulliMatrix(int M, int N, unsigned seed, boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> &P);

void CompressBernoulli(std::vector<double> x, double * y, int M, unsigned seed);


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

template<typename Scalar>
Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> createProblems(size_t nr_problems)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> F(nr_problems);

	// Initialize data
	for (size_t i = 0; i < nr_problems; i++)
	{
		Eigen::Matrix<Scalar, 3, 3> rnd;
		rnd << d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng);
		F.template at<Scalar>(i) = rnd.transpose() * rnd;
	}

	return std::move(F);
}

void perfEigenEigen
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	Vcl::Util::PreciseTimer timer;
	timer.start();
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems; i++)
	{
		// Map data
		Vcl::Matrix3f A = F.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		resU.at<float>(i) = solver.eigenvectors();
		resS.at<float>(i) = solver.eigenvalues();
	}
	timer.stop();
	std::cout << "Eigen Jacobi SVD: " << timer.interval() / nr_problems * 1e9 << "[ns]" << std::endl;
}

void perfEigenEigenDirect
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	Vcl::Util::PreciseTimer timer;
	timer.start();
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems; i++)
	{
		// Map data
		Vcl::Matrix3f A = F.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.computeDirect(A, Eigen::ComputeEigenvectors);

		resU.at<float>(i) = solver.eigenvectors();
		resS.at<float>(i) = solver.eigenvalues();
	}
	timer.stop();
	std::cout << "Eigen Jacobi SVD: " << timer.interval() / nr_problems * 1e9 << "[ns]" << std::endl;
}

template<typename WideScalar>
void perfJacobiEigen
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Util::PreciseTimer timer;
	timer.start();
	int avg_nr_iter = 0;
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using 2-sided Jacobi iterations (Brent)
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
	timer.stop();
	std::cout << "Self-adjoint Jacobi Eigen Decomposition: " << timer.interval() / nr_problems * 1e9 << "[ns], Avg. iterations: " << (double) (avg_nr_iter * width) / (double) nr_problems << std::endl;
}

template<typename WideScalar>
void perfJacobiEigenQuat
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Util::PreciseTimer timer;
	timer.start();
	int avg_nr_iter = 0;
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using Jacobi iterations and QR decomposition
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
	timer.stop();
	std::cout << "Self-adjoint Jacobi Quaternion Eigen Decomposition: " << timer.interval() / nr_problems * 1e9 << "[ns], Avg. iterations: " << (double) (avg_nr_iter * width) / (double) nr_problems << std::endl;
}
int main(int, char**)
{
	size_t nr_problems = 1024*1024;

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(nr_problems);

	// Initialize data
	auto F = createProblems<float>(nr_problems);

	// Test Performance: Eigen Jacobi Decomposition
	perfEigenEigen(nr_problems, F, resU, resS);
	perfEigenEigenDirect(nr_problems, F, resU, resS);

	// Test Performance: Jacobi Eigenvalue Decomposition
	perfJacobiEigen<float>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float4>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float8>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float16>(nr_problems, F, resU, resS);

	// Test Performance: Jacobi Eigenvalue Decomposition using quaternions
	perfJacobiEigenQuat<float>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float4>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float8>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float16>(nr_problems, F, resU, resS);
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#include <fcppt/text.hpp>
#include <fcppt/io/cerr.hpp>
#include <fcppt/math/box/comparison.hpp>
#include <fcppt/math/box/extend_bounding_box.hpp>
#include <fcppt/math/box/object_impl.hpp>
#include <fcppt/math/box/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_box_extend_bounding_box_vector
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::box::object<
		int,
		2
	>
	box_type;

	box_type b(
		box_type::vector(
			1,
			1
		),
		box_type::dim(
			0,
			0
		)
	);

	fcppt::io::cerr()
		<< FCPPT_TEXT("Original box: ")
		<< b
		<< FCPPT_TEXT('\n');

	b =
		fcppt::math::box::extend_bounding_box(
			b,
			box_type::vector(
				3,
				4
			)
		);

	fcppt::io::cerr()
		<< FCPPT_TEXT("Added (3,4), now: ")
		<< b
		<< FCPPT_TEXT('\n');

	BOOST_CHECK_EQUAL(
		b,
		box_type(
			box_type::vector(
				1,
				1
			),
			box_type::dim(
				2,
				3
			)
		)
	);

	b =
		fcppt::math::box::extend_bounding_box(
			b,
			box_type::vector(
				0,0
			)
		);

	fcppt::io::cerr()
		<< FCPPT_TEXT("Added (0,0), now: ")
		<< b
		<< FCPPT_TEXT('\n');

	// The tests are incremental, so require is...required here
	BOOST_REQUIRE_EQUAL(
		b,
		box_type(
			box_type::vector(
				0,
				0
			),
			box_type::dim(
				3,
				4
			)
		)
	);

	// This point is inside the bounding box -> nothing should change
	b =
		fcppt::math::box::extend_bounding_box(
			b,
			box_type::vector(
				1,
				1
			)
		);

	fcppt::io::cerr()
		<< FCPPT_TEXT("Added (1,1), now: ")
		<< b
		<< FCPPT_TEXT('\n');

	BOOST_REQUIRE_EQUAL(
		b,
		box_type(
			box_type::vector(
				0,
				0
			),
			box_type::dim(
				3,
				4
			)
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_box_extend_bounding_box_box
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::box::object<
		int,
		2
	>
	box_type;

	box_type const box1(
		box_type::vector(
			1,
			2
		),
		box_type::dim(
			2,
			3
		)
	);

	box_type const box2(
		box_type::vector(
			0,
			1
		),
		box_type::dim(
			2,
			1
		)
	);

	box_type const expected(
		box_type::vector(
			0,
			1
		),
		box_type::dim(
			3,
			4
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::box::extend_bounding_box(
			box1,
			box2
		),
		expected
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::box::extend_bounding_box(
			box2,
			box1
		),
		expected
	);
}


/*
    tests/eigen.cpp -- automatic conversion of Eigen types

    Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>

    All rights reserved. Use of this source code is governed by a
    BSD-style license that can be found in the LICENSE file.
*/

#include "pybind11_tests.h"
#include <pybind11/eigen.h>
#include <Eigen/Cholesky>

Eigen::VectorXf double_col(const Eigen::VectorXf& x)
{ return 2.0f * x; }

Eigen::RowVectorXf double_row(const Eigen::RowVectorXf& x)
{ return 2.0f * x; }

Eigen::MatrixXf double_mat_cm(const Eigen::MatrixXf& x)
{ return 2.0f * x; }

// Different ways of passing via Eigen::Ref; the first and second are the Eigen-recommended
Eigen::MatrixXd cholesky1(Eigen::Ref<Eigen::MatrixXd> &x) { return x.llt().matrixL(); }
Eigen::MatrixXd cholesky2(const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.llt().matrixL(); }
Eigen::MatrixXd cholesky3(const Eigen::Ref<Eigen::MatrixXd> &x) { return x.llt().matrixL(); }
Eigen::MatrixXd cholesky4(Eigen::Ref<const Eigen::MatrixXd> &x) { return x.llt().matrixL(); }
Eigen::MatrixXd cholesky5(Eigen::Ref<Eigen::MatrixXd> x) { return x.llt().matrixL(); }
Eigen::MatrixXd cholesky6(Eigen::Ref<const Eigen::MatrixXd> x) { return x.llt().matrixL(); }

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXfRowMajor;
MatrixXfRowMajor double_mat_rm(const MatrixXfRowMajor& x)
{ return 2.0f * x; }

test_initializer eigen([](py::module &m) {
    typedef Eigen::Matrix<float, 5, 6, Eigen::RowMajor> FixedMatrixR;
    typedef Eigen::Matrix<float, 5, 6> FixedMatrixC;
    typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> DenseMatrixR;
    typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> DenseMatrixC;
    typedef Eigen::SparseMatrix<float, Eigen::RowMajor> SparseMatrixR;
    typedef Eigen::SparseMatrix<float> SparseMatrixC;

    m.attr("have_eigen") = py::cast(true);

    // Non-symmetric matrix with zero elements
    Eigen::MatrixXf mat(5, 6);
    mat << 0, 3, 0, 0, 0, 11, 22, 0, 0, 0, 17, 11, 7, 5, 0, 1, 0, 11, 0,
        0, 0, 0, 0, 11, 0, 0, 14, 0, 8, 11;

    m.def("double_col", &double_col);
    m.def("double_row", &double_row);
    m.def("double_mat_cm", &double_mat_cm);
    m.def("double_mat_rm", &double_mat_rm);
    m.def("cholesky1", &cholesky1);
    m.def("cholesky2", &cholesky2);
    m.def("cholesky3", &cholesky3);
    m.def("cholesky4", &cholesky4);
    m.def("cholesky5", &cholesky5);
    m.def("cholesky6", &cholesky6);

    // Returns diagonals: a vector-like object with an inner stride != 1
    m.def("diagonal", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal(); });
    m.def("diagonal_1", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal<1>(); });
    m.def("diagonal_n", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int index) { return x.diagonal(index); });

    // Return a block of a matrix (gives non-standard strides)
    m.def("block", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int start_row, int start_col, int block_rows, int block_cols) {
        return x.block(start_row, start_col, block_rows, block_cols);
    });

    // Returns a DiagonalMatrix with diagonal (1,2,3,...)
    m.def("incr_diag", [](int k) {
        Eigen::DiagonalMatrix<int, Eigen::Dynamic> m(k);
        for (int i = 0; i < k; i++) m.diagonal()[i] = i+1;
        return m;
    });

    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_lower", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Lower>();
    });
    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_upper", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Upper>();
    });

    m.def("fixed_r", [mat]() -> FixedMatrixR {
        return FixedMatrixR(mat);
    });

    m.def("fixed_c", [mat]() -> FixedMatrixC {
        return FixedMatrixC(mat);
    });

    m.def("fixed_passthrough_r", [](const FixedMatrixR &m) -> FixedMatrixR {
        return m;
    });

    m.def("fixed_passthrough_c", [](const FixedMatrixC &m) -> FixedMatrixC {
        return m;
    });

    m.def("dense_r", [mat]() -> DenseMatrixR {
        return DenseMatrixR(mat);
    });

    m.def("dense_c", [mat]() -> DenseMatrixC {
        return DenseMatrixC(mat);
    });

    m.def("dense_passthrough_r", [](const DenseMatrixR &m) -> DenseMatrixR {
        return m;
    });

    m.def("dense_passthrough_c", [](const DenseMatrixC &m) -> DenseMatrixC {
        return m;
    });

    m.def("sparse_r", [mat]() -> SparseMatrixR {
        return Eigen::SparseView<Eigen::MatrixXf>(mat);
    });

    m.def("sparse_c", [mat]() -> SparseMatrixC {
        return Eigen::SparseView<Eigen::MatrixXf>(mat);
    });

    m.def("sparse_passthrough_r", [](const SparseMatrixR &m) -> SparseMatrixR {
        return m;
    });

    m.def("sparse_passthrough_c", [](const SparseMatrixC &m) -> SparseMatrixC {
        return m;
    });
});


#include "solvers.hpp"

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Sparse>

#include <Eigen/QR>
#include <Eigen/LU>
#include <Eigen/SparseLU>

// -----------------------------------------------------------------------------

typedef Eigen::Triplet<double, int> Triplet;
/// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<double> Sparse_mat;

// -----------------------------------------------------------------------------

/// Are angles obtuse between e0 and e1 (meaning a < PI/2)
static bool check_obtuse(const Vec3& e0,
                         const Vec3& e1)
{
    return e0.normalized().dot( e1.normalized() ) >= 0.f;
}

// -----------------------------------------------------------------------------

/**
 * @brief Check if a triangle is obtuse (every angles < PI/2)
 * Here how edges must be defined :
   @code
      p0
      |\
      | \
      |  \
      |   \
      |____\
     p1     p2

    Vec3 e0 = p1 - p0;
    Vec3 e1 = p2 - p0;
    Vec3 e2 = p2 - p1;
   @endcode
 */
static bool check_obtuse(const Vec3& e0,
                         const Vec3& e1,
                         const Vec3& e2)
{
    return check_obtuse(e0, e1) && check_obtuse(-e0, e2) && check_obtuse(-e1, -e2);
}

// -----------------------------------------------------------------------------

inline static
double mixed_voronoi_area(const Vec3& pi,
                          const Vec3& pj0,
                          const Vec3& pj1)
{
    double area = 0.;
    Vec3 e0 = pj0 - pi ;
    Vec3 e1 = pj1 - pi;
    Vec3 e2 = pj1 - pj0;

    if( check_obtuse(e0, e1, e2) )
    {
        area = (1/8.) * (double)(e0.norm_squared() * (-e0).cotan( e2) +
                                 e1.norm_squared() * (-e1).cotan(-e2));
    }
    else
    {
        const double ta = (double)e0.cross( e1 ).norm() / 2.; // Tri area
        area = ta / (check_obtuse(e0, e1) ? 2. : 4.);
    }
    return area;
}

// -----------------------------------------------------------------------------

static
double get_cell_area(int vidx,
                     const std::vector< Vec3 >& vertices,
                     const std::vector< std::vector<int> >& edges )
{
    double area = 0.0;
    const Vec3 c_pos = vertices[vidx];
    //get triangles areas
    for(int e = 0; e < (int)edges[vidx].size(); ++e)
    {
        int ne = (e + 1) % edges[vidx].size();

        if(true){
            // Supposidely more precise (but a bit more complex to compute)
            Vec3 p0 = vertices[edges[vidx][e ] ];
            Vec3 p1 = vertices[edges[vidx][ne]];
            area += mixed_voronoi_area(c_pos, p0, p1 );
        }else{
            // This should give descent results as well:
            Vec3 edge0 = vertices[edges[vidx][e ]] - c_pos;
            Vec3 edge1 = vertices[edges[vidx][ne]] - c_pos;
            area += (edge0.cross(edge1)).norm() / 2.f;
        }
    }
    return area;
}

// -----------------------------------------------------------------------------

static
float angle_between(const Vec3& v1, const Vec3& v2)
{
    float cosa = v1.dot(v2);
    if(cosa >= 1.f)
        return 0.f;
    else if(cosa <= -1.f)
        return M_PI;
    else
        return std::acos(cosa);
}

// -----------------------------------------------------------------------------

/// @return A sparse representation of the Laplacian matrix 'L'
/// list[ith_row][list of columns] = Triplet(ith_row, jth_column, matrix value)
static
std::vector<std::vector<Triplet>>
get_laplacian(const std::vector< Vec3 >& vertices,
              const std::vector< std::vector<int> >& edges )
{
    std::cout << "BUILD LAPLACIAN MATRIX" << std::endl;
    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];

        //get laplacian
        double sum = 0.;
        int nb_edges = edges[i].size();
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            /*                                 next_edge
                                    e ◀---v4---(cotan2)
                                   ◥ ◤         /
                                  /   \       /
                                 v2    v5    v3
                                /       \   /
                               /         \ ◣
                        (cotan1)----v1---▶c_pos
                       prev_edge
            */
            Vec3 v1 = c_pos - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos - vertices[edges[i][next_edge]];
            double w = 0.0;
            if(true)
            {
                /* Cotangent weights
                 * (may be negative and undesirable in certain situations)
                */
                Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
                Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

                double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
                double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

                // TODO: check for edge cases such as
                // the mesh corners and boundaries and adjust cotan weights
                // appropriatly ...
                w = (cotan1 + cotan2) * 0.5f;
            } else {
                // Mean value coordinations weights:
                // doesn't really work something must be wrong
                Vec3 v5 = c_pos - vertices[edges[i][e]];
                v1.normalize();
                v3.normalize();
                float v5_norm = v5.normalize();
                double tan1 = std::tan(angle_between(-v1, v5)*0.5f);
                double tan2 = std::tan(angle_between(-v3, v5)*0.5f);
                w = (tan1 + tan2) / (1e-6 + v5_norm);
            }

            // Disable / Enable multiplying against the inverse of
            // the Mass matrix 'M':
            if(false)
            {
                // If we want to return M^{-1}.L instead of just L
                // Then we can do it here since its more efficient
                // than building M^{-1} and then do the product M^{-1}.L
                // Since we solve for harmonic weights
                // M^{-1}.L = 0 can be simplified to L = 0
                // and this step safely ignored
                double area = get_cell_area(i, vertices, edges);
                area = 1. / ((1e-10 + area));
                w *= area;
            }

            sum += w;

            mat_elemts[i].push_back( Triplet(i, edges[i][e], w) );
        }

        mat_elemts[i].push_back( Triplet(i, i, -sum) );
    }
    return mat_elemts;
}

//------------------------------------------------------------------------------

/*
    Alternate implementation of the Laplacian matrix using only the
    list of triangles instead of the first ring neighboors.
*/
static
std::vector<std::vector<Triplet>>
get_laplacian(const std::vector< Vec3 >& vertices,
              const std::vector< Tri_face >& triangles )
{

    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for( const Tri_face& f : triangles)
    {
        struct Edge { int i, j, org; };
        std::vector<Edge> edges =
        {
            {f.a, f.b, f.c},
            {f.b, f.c, f.a},
            {f.c, f.a, f.b},
        };

        for(Edge edge : edges)
        {
            /*
                                    j
                                   ◥
                                  /  \
                                 v2   \
                                /      \
                               /        \
                        (cotan)----v1---▶ i
                           org
            */
            Vec3 v1 = vertices[edge.org] - vertices[edge.i];
            Vec3 v2 = vertices[edge.org] - vertices[edge.j];
            double cotan = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            float w = cotan * 0.5f;

            int i = edge.i;
            int j = edge.j;
            // Note Eigen::setFromTriplets will sum up duplicate elements for us
            mat_elemts[i].push_back( Triplet(i, j,  w) );
            mat_elemts[j].push_back( Triplet(j, i,  w) );
            mat_elemts[i].push_back( Triplet(i, i, -w) );
            mat_elemts[j].push_back( Triplet(j, j, -w) );
        }
    }
    return mat_elemts;
}

//------------------------------------------------------------------------------

// Compute harmonic weights
void solve_laplace_equation(const std::vector< Vec3 >& vertices,
        const std::vector< std::vector<int> >& edges,
        const std::vector<Tri_face>& triangles,
        const std::vector<std::pair<Vert_idx, float> >& boundaries,
        std::vector<double>& harmonic_weight_map)
{
    std::cout << "COMPUTE LAPLACE EQUATION" << std::endl;

    int nv = vertices.size();

    // compute laplacian matrix of the mesh
    /*
        We can build the laplacian 'L' either from the half edge data structure
        (edges) or simply the list of triangles.
        For reference both versions are implemented here.
    */
    assert(edges.size() > 0 || triangles.size() > 0 );
    std::vector<std::vector<Triplet>> mat_elemts;
    if( edges.size() > 0)
        mat_elemts = get_laplacian(vertices, edges);
    else if( triangles.size() > 0 )
        mat_elemts = get_laplacian(vertices, triangles);

    // Set boundary conditions
    Eigen::VectorXd rhs = Eigen::VectorXd::Constant(nv, 0.);
    // Initialize handle
    for(const std::pair<int, float>& elt : boundaries){
        rhs( elt.first ) = double(elt.second);
        // Set row to 0.0f
        mat_elemts[elt.first].clear();
        // Set
        mat_elemts[elt.first].push_back( Triplet(elt.first, elt.first, 1.0) );
    }

#if 0
    // Solving with a dense matrix is Extremely slow
    Eigen::MatrixXd L = Eigen::MatrixXd::Constant(nv, nv, 0.);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            L(elt.row(), elt.col()) = elt.value();

    //Eigen::ColPivHouseholderQR<Eigen::MatrixXd> llt;
    Eigen::FullPivLU<Eigen::MatrixXd> solver;
    std::cout << "BEGIN MATRIX FACTORIZATION" << std::endl;
    solver.compute( L );
    std::cout << "END MATRIX FACTORIZATION" << std::endl;

#else
    Sparse_mat L(nv, nv);
    // Convert to triplets
    std::vector<Triplet> triplets;
    triplets.reserve(nv * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

    L.setFromTriplets(triplets.begin(), triplets.end());

    Eigen::SparseLU<Sparse_mat> solver;
    std::cout << "BEGIN SPARSE MATRIX FACTORIZATION" << std::endl;
    solver.compute( L );
    std::cout << "END SPARSE MATRIX FACTORIZATION" << std::endl;
#endif

    harmonic_weight_map.resize(nv);
    Eigen::VectorXd res = solver.solve( rhs );
    for(int i = 0; i < nv; ++i)
        harmonic_weight_map[i] = res(i);

    return;
}

// -----------------------------------------------------------------------------


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaP, gamma_p) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::gamma_p;

  fvar<double> x(0.5);
  x.d_ = 1.0;
  fvar<double> y(1.0);
  y.d_ = 1.0;

  fvar<double> a = gamma_p(x, y);
  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(-0.18228334, a.d_);

  double z = 1.0;
  double w = 0.5;

  a = gamma_p(x, z);
  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(-0.389837, a.d_);

  a = gamma_p(w, y);
  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_);

  EXPECT_THROW(gamma_p(-x, y), std::domain_error);
  EXPECT_THROW(gamma_p(x, -y), std::domain_error);
}

TEST(AgradFwdGammaP, FvarFvarDouble) {
  using boost::math::gamma_p;
  using stan::math::fvar;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_);
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_);
}

struct gamma_p_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return gamma_p(arg1, arg2);
  }
};

TEST(AgradFwdGammaP, nan) {
  gamma_p_fun gamma_p_;
  test_nan_fwd(gamma_p_, 3.0, 5.0, false);
}


// Author: Tucker Haydon

#include "gnuplot-iostream.h"

#include <cstdlib>
#include <Eigen/Dense>

#include "polynomial_solver.h"
#include "polynomial_sampler.h"

using namespace p4;

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

  // Time in seconds
  const std::vector<double> times = {0, 1, 2.5};

  // NodeEqualityBound(dimension_idx, node_idx, derivative_idx, value)
  const std::vector<NodeEqualityBound> node_equality_bounds = {
    // Constraining position and velocity of first node to zero
    NodeEqualityBound(0,0,0,0),
    NodeEqualityBound(1,0,0,0),
    NodeEqualityBound(2,0,0,0),
    NodeEqualityBound(0,0,1,0),
    NodeEqualityBound(1,0,1,0),
    NodeEqualityBound(2,0,1,0),

    // Other nodes may constrain whatever they want
    // The second node is constraining position to (1,0,0)
    NodeEqualityBound(0,1,0,1),
    NodeEqualityBound(1,1,0,0),
    NodeEqualityBound(2,1,0,0),

    // The third node is contraining position to (1,1,free)
    NodeEqualityBound(0,2,0,1),
    NodeEqualityBound(1,2,0,1),
  };

  // NodeInequalityBound(dimension_idx, node_idx, derivative_idx, lower, upper)
  const std::vector<NodeInequalityBound> node_inequality_bounds = {
    // Constraining the z value of the third node above 0.5
    NodeInequalityBound(2,2,0,0.5,NodeInequalityBound::INFTY),
  };

  // SegmentInequalityBound(segment_idx, derivative_idx, mapping, value)
  // Segment inequality bounds constrain a derivative of a segment to
  //   dot(a,x) < b
  const std::vector<SegmentInequalityBound> segment_inequality_bounds = {
    // Constraining the x-acceleration of the first segment below 4 m/s^2
    SegmentInequalityBound(0,2,Eigen::Vector3d(1,0,0),4),
  };

  // Configure solver options
  PolynomialSolver::Options solver_options;
  solver_options.num_dimensions   = 3;   // 3D
  solver_options.polynomial_order = 8;   // Fit an 8th-order polynomial
  solver_options.continuity_order = 4;   // Require continuity through the 4th derivative
  solver_options.derivative_order = 2;   // Minimize the 2nd derivative (acceleration)

  // Configure the OSQP settings
  // Reference: https://osqp.org/docs/interfaces/cc++#settings
  solver_options.osqp_settings.polish = true;       // Polish the solution, getting the best answer possible
  solver_options.osqp_settings.verbose = false;     // Suppress the printout

  // Solve
  PolynomialSolver solver(solver_options);
  const PolynomialSolver::Solution solution
    = solver.Run(
        times,
        node_equality_bounds,
        node_inequality_bounds,
        segment_inequality_bounds);

  // Print some output info
  // Reference: https://osqp.org/docs/interfaces/cc++#info
  std::cout << "Status:                    " << solution.workspace->info->status << std::endl;
  std::cout << "Status Val (1 == success): " << solution.workspace->info->status_val << std::endl;
  std::cout << "Optimal Cost:              " << solution.workspace->info->obj_val << std::endl;

  // Sampling and Plotting
  { // Plot acceleration profiles
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 100;
    sampler_options.derivative_order = 2;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "plot '-' using 1:2 with lines title 'X-Acceleration'";
    gp << ", '-' using 1:2 with lines title 'Y-Acceleration'";
    gp << ", '-' using 1:2 with lines title 'Z-Acceleration'";
    gp << std::endl;
    gp.send1d(boost::make_tuple(t_hist, x_hist));
    gp.send1d(boost::make_tuple(t_hist, y_hist));
    gp.send1d(boost::make_tuple(t_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;
  }

  { // Plot velocity profiles
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 100;
    sampler_options.derivative_order = 1;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "plot '-' using 1:2 with lines title 'X-Velocity'";
    gp << ", '-' using 1:2 with lines title 'Y-Velocity'";
    gp << ", '-' using 1:2 with lines title 'Z-Velocity'";
    gp << std::endl;
    gp.send1d(boost::make_tuple(t_hist, x_hist));
    gp.send1d(boost::make_tuple(t_hist, y_hist));
    gp.send1d(boost::make_tuple(t_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;
  }

  { // Plot 3D position
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 50;
    sampler_options.derivative_order = 0;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "splot '-' using 1:2:3 with lines title 'Trajectory'" << std::endl;
    gp.send1d(boost::make_tuple(x_hist, y_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;

    // Must keep position gp in scope to rotate 3D graph
    std::cout << "Press enter to exit." << std::endl;
    std::cin.get();
  }

  return EXIT_SUCCESS;
}


/**
 * @file expfittedupwind_main.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>

#include "expfittedupwind.h"

int main() {
  // Define Mesh-independent Data:
#if SOLUTION
  auto f = [](Eigen::Vector2d x) { return 0.0; };

  Eigen::Vector2d q = Eigen::Vector2d::Ones(2);
  auto Psi = [&q](Eigen::Vector2d x) { return q.dot(x); };

  auto g = [&Psi](Eigen::Vector2d x) { return std::exp(Psi(x)); };

  auto ref_sol = [&Psi](Eigen::Vector2d x) { return std::exp(Psi(x)); };
#else
  //====================
  // Your code goes here
  //====================
#endif

  // Output file
  std::ofstream L2output;
  L2output.open("L2error.txt");
  L2output << "No. of dofs, L2 error" << std::endl;

  // generate a mesh hierarchy:
  unsigned int reflevels = 6;
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  auto top_mesh = builder.Build();

  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(top_mesh,
                                                              reflevels);
  lf::refinement::MeshHierarchy& multi_mesh{*multi_mesh_p};
  multi_mesh.PrintInfo(std::cout);

  // get number of levels:
  auto L = multi_mesh.NumLevels();

  // perform computations on all levels:
  for (int l = 0; l < L; ++l) {
    // Compute finite element solution and compute L2 error on current level:
    double L2_err = 1.0;

    // get current mesh and fe space
    auto mesh_p = multi_mesh.getMesh(l);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};
    const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

#if SOLUTION
    // wrap Psi and the reference solution into a mesh function on the current
    // level
    auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(Psi);
    Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);
    auto mf_ref_sol = lf::mesh::utils::MeshFunctionGlobal(ref_sol);

    // compute the finite element solution and wrap it into a mesh function
    Eigen::VectorXd sol_vec =
        ExpFittedUpwind::SolveDriftDiffusionDirBVP(fe_space, mu, f, g);
    auto mf_sol = lf::fe::MeshFunctionFE(fe_space, sol_vec);

    // evaluate L2 error:
    L2_err = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::uscalfe::squaredNorm(mf_sol - mf_ref_sol), 3));
#else
    //====================
    // Your code goes here
    //====================
#endif

    L2output << N_dofs << ", " << L2_err << std::endl;
    std::cout << N_dofs << "," << L2_err << std::endl;
  }

  L2output.close();

  // Plot the computed L2 error
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/L2error.txt " CURRENT_BINARY_DIR "/results.eps");

  return 0;
}


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include <dlib/string.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.matrix_eig");

    dlib::rand rnd;

// ----------------------------------------------------------------------------------------

    template <typename type>
    const matrix<type> randm(long r, long c)
    {
        matrix<type> m(r,c);
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

    template <typename type, long NR, long NC>
    const matrix<type,NR,NC> randm()
    {
        matrix<type,NR,NC> m;
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

// ----------------------------------------------------------------------------------------

    template <typename matrix_type, typename U>
    void test_eigenvalue_impl ( const matrix_type& m,  const eigenvalue_decomposition<U>& test )
    {
        typedef typename matrix_type::type type;
        const type eps = 10*max(abs(m))*sqrt(std::numeric_limits<type>::epsilon());
        dlog << LDEBUG << "test_eigenvalue():  " << m.nr() << " x " << m.nc() << "  eps: " << eps;
        print_spinner();

        DLIB_TEST(test.dim() == m.nr());

        // make sure all the various ways of asking for the eigenvalues are actually returning a
        // consistent set of eigenvalues.
        DLIB_TEST(equal(real(test.get_eigenvalues()), test.get_real_eigenvalues(), eps));
        DLIB_TEST(equal(imag(test.get_eigenvalues()), test.get_imag_eigenvalues(), eps));
        DLIB_TEST(equal(real(diag(test.get_d())), test.get_real_eigenvalues(), eps));
        DLIB_TEST(equal(imag(diag(test.get_d())), test.get_imag_eigenvalues(), eps));

        matrix<type> eig1 ( real_eigenvalues(m));
        matrix<type> eig2 ( test.get_real_eigenvalues());
        sort(&eig1(0), &eig1(0) + eig1.size());
        sort(&eig2(0), &eig2(0) + eig2.size());
        DLIB_TEST(max(abs(eig1 - eig2)) < eps);

        const matrix<type> V = test.get_pseudo_v();
        const matrix<type> D = test.get_pseudo_d();
        const matrix<complex<type> > CV = test.get_v();
        const matrix<complex<type> > CD = test.get_d();
        const matrix<complex<type> > CM = complex_matrix(m, uniform_matrix<type>(m.nr(),m.nc(),0));

        DLIB_TEST(V.nr() == test.dim());
        DLIB_TEST(V.nc() == test.dim());
        DLIB_TEST(D.nr() == test.dim());
        DLIB_TEST(D.nc() == test.dim());

        // CD is a diagonal matrix
        DLIB_TEST(diagm(diag(CD)) == CD);

        // verify that these things are actually eigenvalues and eigenvectors of m
        DLIB_TEST_MSG(max(abs(m*V - V*D)) < eps, max(abs(m*V - V*D)) << "   " << eps);
        DLIB_TEST(max(norm(CM*CV - CV*CD)) < eps);

        // if m is a symmetric matrix
        if (max(abs(m-trans(m))) < 1e-5)
        {
            dlog << LTRACE << "m is symmetric";
            // there aren't any imaginary eigenvalues
            DLIB_TEST(max(abs(test.get_imag_eigenvalues())) < eps);
            DLIB_TEST(diagm(diag(D)) == D);

            // only check the determinant against the eigenvalues for small matrices
            // because for huge ones the determinant might be so big it overflows a floating point number.
            if (m.nr() < 50)
            {
                const type mdet = det(m);
                DLIB_TEST_MSG(std::abs(prod(test.get_real_eigenvalues()) - mdet) < std::abs(mdet)*sqrt(std::numeric_limits<type>::epsilon()),
                              std::abs(prod(test.get_real_eigenvalues()) - mdet) <<"    eps: " << std::abs(mdet)*sqrt(std::numeric_limits<type>::epsilon())
                              << "  mdet: "<< mdet << "   prod(eig): " << prod(test.get_real_eigenvalues())
                );
            }

            // V is orthogonal
            DLIB_TEST(equal(V*trans(V), identity_matrix<type>(test.dim()), eps));
            DLIB_TEST(equal(m , V*D*trans(V), eps));
        }
        else
        {
            dlog << LTRACE << "m is NOT symmetric";
            DLIB_TEST_MSG(equal(m , V*D*inv(V), eps), max(abs(m - V*D*inv(V))));
        }
    }

// ----------------------------------------------------------------------------------------

    template <typename matrix_type>
    void test_eigenvalue ( const matrix_type& m )
    {
        typedef typename matrix_type::type type;
        typedef typename matrix_type::mem_manager_type MM;
        matrix<type,matrix_type::NR, matrix_type::NC, MM, row_major_layout> mr(m);
        matrix<type,matrix_type::NR, matrix_type::NC, MM, column_major_layout> mc(m);

        {
        eigenvalue_decomposition<matrix_type> test(mr);
        test_eigenvalue_impl(mr, test);

        eigenvalue_decomposition<matrix_type> test_symm(make_symmetric(mr));
        test_eigenvalue_impl(make_symmetric(mr), test_symm);
        }

        {
        eigenvalue_decomposition<matrix_type> test(mc);
        test_eigenvalue_impl(mc, test);

        eigenvalue_decomposition<matrix_type> test_symm(make_symmetric(mc));
        test_eigenvalue_impl(make_symmetric(mc), test_symm);
        }
    }

// ----------------------------------------------------------------------------------------

    void matrix_test_double()
    {

        test_eigenvalue(10*randm<double>(1,1));
        test_eigenvalue(10*randm<double>(2,2));
        test_eigenvalue(10*randm<double>(3,3));
        test_eigenvalue(10*randm<double>(4,4));
        test_eigenvalue(10*randm<double>(15,15));
        test_eigenvalue(10*randm<double>(150,150));

        test_eigenvalue(10*randm<double,1,1>());
        test_eigenvalue(10*randm<double,2,2>());
        test_eigenvalue(10*randm<double,3,3>());
    }

// ----------------------------------------------------------------------------------------

    void matrix_test_float()
    {

        test_eigenvalue(10*randm<float>(1,1));
        test_eigenvalue(10*randm<float>(2,2));
        test_eigenvalue(10*randm<float>(3,3));
        test_eigenvalue(10*randm<float>(4,4));
        test_eigenvalue(10*randm<float>(15,15));
        test_eigenvalue(10*randm<float>(50,50));

        test_eigenvalue(10*randm<float,1,1>());
        test_eigenvalue(10*randm<float,2,2>());
        test_eigenvalue(10*randm<float,3,3>());
    }

// ----------------------------------------------------------------------------------------

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix_eig",
                    "Runs tests on the matrix eigen decomp component.")
        {
            //rnd.set_seed(cast_to_string(time(0)));
        }

        void perform_test (
        )
        {
            dlog << LINFO << "seed string: " << rnd.get_seed();

            dlog << LINFO << "begin testing with double";
            matrix_test_double();
            dlog << LINFO << "begin testing with float";
            matrix_test_float();
        }
    } a;

}


#pragma once
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/segment.hpp>
#include <boost/optional.hpp>
#include <vector>

namespace sssp {

struct rgb {
    rgb() : r(0.0), g(0.0), b(0.0) {}
    rgb(double r, double g, double b) : r(r), g(g), b(b) {}
    double r;
    double g;
    double b;
};

struct vec2 {
    vec2() : x(0.0), y(0.0) {}
    vec2(double x, double y) : x(x), y(y) {}
    double x;
    double y;
};

inline bool operator==(const vec2& a, const vec2& b) {
    return a.x == b.x && a.y == b.y;
}

inline bool operator!=(const vec2& a, const vec2& b) {
    return !(a == b);
}

inline vec2 operator+(const vec2& a, const vec2& b) {
    return vec2(a.x + b.x, a.y + b.y);
}

inline vec2 operator-(const vec2& a, const vec2& b) {
    return vec2(a.x - b.x, a.y - b.y);
}

inline vec2 normalize(const vec2& v) {
    double f = std::sqrt(v.x * v.x + v.y * v.y);
    return vec2(v.x / f, v.y / f);
}

struct line {
    line(const vec2& start, const vec2& end) : start(start), end(end) {}
    vec2 start;
    vec2 end;
};

double distance(const vec2& a, const vec2& b);
bool intersects(const line& a, const line& b);

} // namespace sssp

BOOST_GEOMETRY_REGISTER_POINT_2D(sssp::vec2, double, boost::geometry::cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_SEGMENT(sssp::line, sssp::vec2, start, end)


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================

#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <iostream>

// Example from Introduction to Algorithms by Cormen, et all p.537.

// Sample output:
//  r: inifinity
//  s: 0
//  t: 2
//  u: 6
//  v: 5
//  x: 3

int main()
{
  using namespace boost;
  typedef adjacency_list<vecS, vecS, directedS,
    property<vertex_distance_t, int>, property<edge_weight_t, int> > graph_t;
  graph_t g(6);
  enum verts { r, s, t, u, v, x };
  char name[] = "rstuvx";
  add_edge(r, s, 5, g);
  add_edge(r, t, 3, g);
  add_edge(s, t, 2, g);
  add_edge(s, u, 6, g);
  add_edge(t, u, 7, g);
  add_edge(t, v, 4, g);
  add_edge(t, x, 2, g);
  add_edge(u, v, -1, g);
  add_edge(u, x, 1, g);
  add_edge(v, x, -2, g);

  property_map<graph_t, vertex_distance_t>::type
    d_map = get(vertex_distance, g);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ has trouble with the named-parameter mechanism, so
  // we make a direct call to the underlying implementation function.
  std::vector<default_color_type> color(num_vertices(g));
  std::vector<std::size_t> pred(num_vertices(g));
  default_dijkstra_visitor vis;
  std::less<int> compare;
  closed_plus<int> combine;
  property_map<graph_t, edge_weight_t>::type w_map = get(edge_weight, g);
  dag_shortest_paths(g, s, d_map, w_map, &color[0], &pred[0],
     vis, compare, combine, (std::numeric_limits<int>::max)(), 0);
#else
  dag_shortest_paths(g, s, distance_map(d_map));
#endif

  graph_traits<graph_t>::vertex_iterator vi , vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    if (d_map[*vi] == (std::numeric_limits<int>::max)())
      std::cout << name[*vi] << ": inifinity\n";
    else
      std::cout << name[*vi] << ": " << d_map[*vi] << '\n';
  return 0;
}