void extractInternalFaces(const Dune::CpGrid& grid,
                          Eigen::Array<int, Eigen::Dynamic, 1>& internal_faces,
                          Eigen::Array<int, Eigen::Dynamic, 2, Eigen::RowMajor>& nbi)
{
    // Extracts the internal faces of the grid.
    // These are stored in internal_faces.
    int nf=numFaces(grid);
    int num_internal=0;
    for(int f=0; f<nf; ++f)
    {
        if(grid.faceCell(f, 0)<0 || grid.faceCell(f, 1)<0)
            continue;
        ++num_internal;
    }
    // std::cout << num_internal << " internal faces." << std::endl;
    nbi.resize(num_internal, 2);
    internal_faces.resize(num_internal);
    int fi = 0;

    for (int f = 0; f < nf; ++f) {
        if(grid.faceCell(f, 0)>=0 && grid.faceCell(f, 1)>=0) {
            internal_faces[fi] = f;
            nbi(fi,0) = grid.faceCell(f, 0);
            nbi(fi,1) = grid.faceCell(f, 1);
            ++fi;
        }
    }
}

    void
    factor_U(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& U,
             Eigen::Array<T, Eigen::Dynamic, 1>& CPCs) {
      size_t K = U.rows();
      size_t position = 0;
      size_t pull = K - 1;

      if (K == 2) {
        CPCs(0) = atanh(U(0, 1));
        return;
      }

      Eigen::Array<T, 1, Eigen::Dynamic> temp = U.row(0).tail(pull);

      CPCs.head(pull) = temp;

      Eigen::Array<T, Eigen::Dynamic, 1> acc(K);
      acc(0) = -0.0;
      acc.tail(pull) = 1.0 - temp.square();
      for (size_t i = 1; i < (K - 1); i++) {
        position += pull;
        pull--;
        temp = U.row(i).tail(pull);
        temp /= sqrt(acc.tail(pull) / acc(i));
        CPCs.segment(position, pull) = temp;
        acc.tail(pull) *= 1.0 - temp.square();
      }
      CPCs = 0.5 * ( (1.0 + CPCs) / (1.0 - CPCs) ).log();  // now unbounded
    }

    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
    read_corr_L(const Eigen::Array<T, Eigen::Dynamic, 1>& CPCs,  // on (-1, 1)
                size_t K) {
      Eigen::Array<T, Eigen::Dynamic, 1> temp;
      Eigen::Array<T, Eigen::Dynamic, 1> acc(K-1);
      acc.setOnes();
      // Cholesky factor of correlation matrix
      Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> L(K, K);
      L.setZero();

      size_t position = 0;
      size_t pull = K - 1;

      L(0, 0) = 1.0;
      L.col(0).tail(pull) = temp = CPCs.head(pull);
      acc.tail(pull) = T(1.0) - temp.square();
      for (size_t i = 1; i < (K - 1); i++) {
        position += pull;
        pull--;
        temp = CPCs.segment(position, pull);
        L(i, i) = sqrt(acc(i-1));
        L.col(i).tail(pull) = temp * acc.tail(pull).sqrt();
        acc.tail(pull) *= T(1.0) - temp.square();
      }
      L(K-1, K-1) = sqrt(acc(K-2));
      return L.matrix();
    }

void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename)
{
  Eigen::Array<unsigned char,Eigen::Dynamic,Eigen::Dynamic> bits = (x*255).cast<unsigned char>();
  QImage img(bits.data(), n,n,QImage::Format_Indexed8);
  img.setColorCount(256);
  for(int i=0;i<256;i++) img.setColor(i,qRgb(i,i,i));
  img.save(filename);
}

 typename boost::math::tools::promote_args<T_y,T_loc,T_scale,T_shape>::type
 lkj_cov_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
             const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu,
             const Eigen::Matrix<T_scale,Eigen::Dynamic,1>& sigma,
             const T_shape& eta,
             const Policy&) {
   static const char* function = "stan::prob::lkj_cov_log(%1%)";
   
   using stan::math::check_size_match;
   using stan::math::check_finite;
   using stan::math::check_positive;
   using boost::math::tools::promote_args;
   
   typename promote_args<T_y,T_loc,T_scale,T_shape>::type lp(0.0);
   if (!check_size_match(function, 
       mu.rows(), "Rows of location parameter",
       sigma.rows(), "columns of scale parameter",
       &lp, Policy()))
     return lp;
   if (!check_size_match(function, 
       y.rows(), "Rows of random variable",
       y.cols(), "columns of random variable",
       &lp, Policy()))
   return lp;
   if (!check_size_match(function, 
       y.rows(), "Rows of random variable",
       mu.rows(), "rows of location parameter",
       &lp, Policy()))
     return lp;
   if (!check_positive(function, eta, "Shape parameter", &lp, Policy()))
     return lp;
   if (!check_finite(function, mu, "Location parameter", &lp, Policy()))
     return lp;
   if (!check_finite(function, sigma, "Scale parameter", &lp, Policy()))
     return lp;
   // FIXME: build vectorized versions
   for (int m = 0; m < y.rows(); ++m)
     for (int n = 0; n < y.cols(); ++n)
       if (!check_finite(function, y(m,n), "Covariance matrix", &lp, Policy()))
         return lp;
   
   const unsigned int K = y.rows();
   const Eigen::Array<T_y,Eigen::Dynamic,1> sds
     = y.diagonal().array().sqrt();
   for (unsigned int k = 0; k < K; k++) {
     lp += lognormal_log<propto>(sds(k), mu(k), sigma(k), Policy());
   }
   if (stan::is_constant<typename stan::scalar_type<T_shape> >::value
       && eta == 1.0) {
     // no need to rescale y into a correlation matrix
     lp += lkj_corr_log<propto,T_y,T_shape,Policy>(y, eta, Policy()); 
     return lp;
   }
   Eigen::DiagonalMatrix<T_y,Eigen::Dynamic> D(K);
   D.diagonal() = sds.inverse();
   lp += lkj_corr_log<propto,T_y,T_shape,Policy>(D * y * D, eta, Policy());
   return lp;
 }

 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
 read_cov_L(const Eigen::Array<T, Eigen::Dynamic, 1>& CPCs,
            const Eigen::Array<T, Eigen::Dynamic, 1>& sds,
            T& log_prob) {
   size_t K = sds.rows();
   // adjust due to transformation from correlations to covariances
   log_prob += (sds.log().sum() + LOG_2) * K;
   return sds.matrix().asDiagonal() * read_corr_L(CPCs, K, log_prob);
 }

void SLHA_io::convert_symmetric_fermion_mixings_to_slha(Eigen::Array<double, N, 1>& m,
                                                        Eigen::Matrix<std::complex<double>, N, N>& z)
{
   for (int i = 0; i < N; i++) {
      // check if i'th row contains non-zero imaginary parts
      if (!is_zero(z.row(i).imag().cwiseAbs().maxCoeff())) {
         z.row(i) *= std::complex<double>(0.0,1.0);
         m(i) *= -1;
      }
   }
}

 Eigen::MatrixXd eddy2scheme (const Eigen::MatrixXd& config, const Eigen::Array<int, Eigen::Dynamic, 1>& indices)
 {
   if (config.cols() != 4)
     throw Exception ("Expected 4 columns in EDDY-format phase-encoding config file");
   Eigen::MatrixXd result (indices.size(), 4);
   for (ssize_t row = 0; row != indices.size(); ++row) {
     if (indices[row] > config.rows())
       throw Exception ("Malformed EDDY-style phase-encoding information: Index exceeds number of config entries");
     result.row(row) = config.row(indices[row]-1);
   }
   return result;
 }

IGL_INLINE void igl::ears(
  const Eigen::MatrixBase<DerivedF> & F,
  Eigen::PlainObjectBase<Derivedear> & ear,
  Eigen::PlainObjectBase<Derivedear_opp> & ear_opp)
{
  assert(F.cols() == 3 && "F should contain triangles");
  Eigen::Array<bool,Eigen::Dynamic,3> B;
  {
    Eigen::Array<bool,Eigen::Dynamic,1> I;
    on_boundary(F,I,B);
  }
  find(B.rowwise().count() == 2,ear);
  Eigen::Array<bool,Eigen::Dynamic,3> Bear;
  slice(B,ear,1,Bear);
  Eigen::Array<bool,Eigen::Dynamic,1> M;
  mat_min(Bear,2,M,ear_opp);
}

void SLHA_io::convert_symmetric_fermion_mixings_to_hk(Eigen::Array<double, N, 1>& m,
                                                      Eigen::Matrix<std::complex<double>, N, N>& z)
{
   for (int i = 0; i < N; i++) {
      if (m(i) < 0.) {
         z.row(i) *= std::complex<double>(0.0,1.0);
         m(i) *= -1;
      }
   }
}

void Streamer::calculateAverageUpdateTime()
{
	static boost::chrono::steady_clock::time_point currentTime, lastRecordedTime;
	static Eigen::Array<float, 5, 1> recordedTimes = Eigen::Array<float, 5, 1>::Zero();
	currentTime = boost::chrono::steady_clock::now();
	if (lastRecordedTime.time_since_epoch().count())
	{
		if (!(recordedTimes > 0).all())
		{
			boost::chrono::duration<float> elapsedTime = currentTime - lastRecordedTime;
			recordedTimes[(recordedTimes > 0).count()] = elapsedTime.count();
		}
		else
		{
			averageUpdateTime = recordedTimes.mean() * 50.0f;
			recordedTimes.setZero();
		}
	}
	lastRecordedTime = currentTime;
}

int FeatureTransformationEstimator::consensus3D(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d T, double thresh, Eigen::Array<bool, 1, Eigen::Dynamic> &consensusSet)
{
    int consensus = 0;

    P = T * P.colwise().homogeneous();
    Eigen::MatrixXd norms = (P - Q).colwise().norm();
    consensusSet = norms.array() < thresh;
    consensus = consensusSet.count();

    return consensus;
}

    void LQCDA::readVector(AsciiReader& reader, Eigen::Array<double, Eigen::Dynamic, 1>& output)
    {
	try {
	    int i;
	    reader.read(i);
	    output.resize(i);
	    for(int _i=0; _i<i; ++_i)
		reader.read(output(_i));
	}
	catch (const IOException& e) {

	}
    }

// project points outside of domain back to boundary
void distmesh::utils::projectPointsToBoundary(
    Functional const& distanceFunction, double const initialPointDistance,
    Eigen::Ref<Eigen::ArrayXXd> points) {
    Eigen::ArrayXd distance = distanceFunction(points);

    // check for points outside of boundary
    Eigen::Array<bool, Eigen::Dynamic, 1> outside = distance > 0.0;
    if (outside.any()) {
        // calculate gradient
        Eigen::ArrayXXd gradient(points.rows(), points.cols());
        Eigen::ArrayXXd deltaX = Eigen::ArrayXXd::Zero(points.rows(), points.cols());

        for (int dim = 0; dim < points.cols(); ++dim) {
            deltaX.col(dim).fill(constants::deltaX * initialPointDistance);
            gradient.col(dim) = (distanceFunction(points + deltaX) - distance) /
                (constants::deltaX * initialPointDistance);
            deltaX.col(dim).fill(0.0);
        }

        // project points back to boundary
        points -= outside.replicate(1, points.cols()).select(
            gradient.colwise() * distance / gradient.square().rowwise().sum(), 0.0);
    }
}

    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
    read_corr_L(const Eigen::Array<T, Eigen::Dynamic, 1>& CPCs,
                size_t K,
                T& log_prob) {
      Eigen::Matrix<T, Eigen::Dynamic, 1> values(CPCs.rows() - 1);
      size_t pos = 0;
      // no need to abs() because this Jacobian determinant
      // is strictly positive (and triangular)
      // see inverse of Jacobian in equation 11 of LKJ paper
      for (size_t k = 1; k <= (K - 2); k++)
        for (size_t i = k + 1; i <= K; i++) {
          values(pos) = (K - k - 1) * log1m(square(CPCs(pos)));
          pos++;
        }

      log_prob += 0.5 * sum(values);
      return read_corr_L(CPCs, K);
    }

//-----------------------------------------------------------
// Utility functions
//-----------------------------------------------------------
    void LQCDA::readArray(AsciiReader& reader, Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic>& output)
    {
	try {
	    std::string line;
	    reader.readLine(line);
	    std::istringstream isstr (line);
	    int i, j;
	    isstr >> i >> j;
	    output.resize(i, j);
	    for(int _i=0; _i<i; ++_i)
		for(int _j=0; _j<j; ++_j)
		{
		    reader.read(output(_i,_j));
		}
	}
	catch (const IOException& e) {

	}
    }

int FeatureTransformationEstimator::consensusReprojection(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d T, double thresh, Eigen::Array<bool, 1, Eigen::Dynamic> &consensusSet)
{
    Eigen::MatrixXd Pproj(2, P.cols());
    Eigen::MatrixXd Qproj(2, P.cols());
    for (int i = 0; i < P.cols(); i++) {
        Eigen::Vector3d PT = T * P.col(i).homogeneous();
        Pproj(0,i) = pnp.uc + pnp.fu * PT(0) / PT(2);
        Pproj(1,i) = pnp.vc + pnp.fv * PT(1) / PT(2);

        Qproj(0,i) = pnp.uc + pnp.fu * Q(0,i) / Q(2,i);
        Qproj(1,i) = pnp.vc + pnp.fv * Q(1,i) / Q(2,i);
    }

    int consensus = 0;
    Eigen::MatrixXd norms = (Pproj - Qproj).colwise().norm();
    consensusSet = norms.array() < thresh;
    consensus = consensusSet.count();

    return consensus;
}

	    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> operator() (
		const Eigen::Array<Scalar, Rows, Cols>& a,
		const Eigen::Array<Scalar, Rows, Cols>& b) {
		return a.matrix() * b.matrix().transpose();
	    }

bool SavePng(std::string filename, const DepthImage& img)
{
    // Open up a file for writing
    FILE* output = fopen(filename.c_str(), "wb");
    if (output == NULL)
    {
        printf("Failed to open PNG file for writing (errno = %i)\n", errno);
        return false;
    }

    // Create a png pointer with the callbacks above
    png_structp png_ptr = png_create_write_struct(
        PNG_LIBPNG_VER_STRING, NULL, on_png_error, on_png_warn);
    if (png_ptr == NULL)
    {
        fprintf(stderr, "Failed to allocate png write_struct\n");
        fclose(output);
        return false;
    }

    // Create an info pointer
    png_infop info_ptr = png_create_info_struct(png_ptr);
    if (info_ptr == NULL)
    {
        fprintf(stderr, "Failed to create png info_struct");
        png_destroy_write_struct(&png_ptr, &info_ptr);
        fclose(output);
        return false;
    }

    // Set physical vars
    png_set_IHDR(png_ptr, info_ptr, img.cols(), img.rows(), 16,
                 PNG_COLOR_TYPE_GRAY, PNG_INTERLACE_NONE,
                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);

    png_init_io(png_ptr, output);

    const float zmax = img.maxCoeff();
    const float zmin = (img == -std::numeric_limits<float>::infinity())
            .select(DepthImage::Constant(img.rows(), img.cols(), zmax),
                    img)
            .minCoeff();

    auto scaled = (zmax == zmin)
        ? DepthImage((img - zmin) + 65535)
        : DepthImage((img - zmin) * 65534 / (zmax - zmin) + 1);
    Eigen::Array<uint16_t, Eigen::Dynamic, Eigen::Dynamic>
        pixels = scaled.cast<uint16_t>().transpose();

    std::vector<uint16_t*> rows;
    for (int i=pixels.cols() - 1; i >= 0; --i)
    {
        rows.push_back(pixels.data() + i * pixels.rows());
    }

    png_set_rows(png_ptr, info_ptr, reinterpret_cast<png_bytepp>(&rows[0]));
    png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_SWAP_ENDIAN, NULL);
    fclose(output);

    png_destroy_write_struct(&png_ptr, &info_ptr);
    return true;
}

    mglData mgl(const Eigen::Array<Scalar, Eigen::Dynamic, 1>& d)
    {
	return mglData(d.size(), d.data());
    }