// [[Rcpp::export]]
Rcpp::RawVector magick_image_write( XPtrImage input, Rcpp::CharacterVector format, Rcpp::IntegerVector quality,
                                    Rcpp::IntegerVector depth, Rcpp::CharacterVector density, Rcpp::CharacterVector comment){
  if(!input->size())
    return Rcpp::RawVector(0);
  XPtrImage image = copy(input);
#if MagickLibVersion >= 0x691
  //suppress write warnings see #74 and #116
  image->front().quiet(true);
#endif
  if(format.size())
    for_each ( image->begin(), image->end(), Magick::magickImage(std::string(format[0])));
  if(quality.size())
    for_each ( image->begin(), image->end(), Magick::qualityImage(quality[0]));
  if(depth.size())
    for_each ( image->begin(), image->end(), Magick::depthImage(depth[0]));
  if(density.size()){
    for_each ( image->begin(), image->end(), Magick::resolutionUnitsImage(Magick::PixelsPerInchResolution));
    for_each ( image->begin(), image->end(), Magick::densityImage(Point(density[0])));
  }
  if(comment.size())
    for_each ( image->begin(), image->end(), Magick::commentImage(std::string(comment.at(0))));
  Magick::Blob output;
  writeImages( image->begin(), image->end(),  &output );
  Rcpp::RawVector res(output.length());
  std::memcpy(res.begin(), output.data(), output.length());
  return res;
}

SEXP assignRawSymmetricMatrixDiagonal(SEXP destination_, SEXP indices_, SEXP source_)
{
BEGIN_RCPP
	Rcpp::S4 destination = destination_;
	Rcpp::RawVector source = source_;
	Rcpp::RawVector destinationData = destination.slot("data");
	Rcpp::IntegerVector indices = indices_;

	if(&(source(0)) == &(destinationData(0)))
	{
		throw std::runtime_error("Source and destination cannot be the same in assignRawSymmetricMatrixDiagonal");
	}

	if((indices.size()*(indices.size()+(R_xlen_t)1))/(R_xlen_t)2 != source.size())
	{
		throw std::runtime_error("Mismatch between index length and source object size");
	}
	for(R_xlen_t column = 0; column < indices.size(); column++)
	{
		for(R_xlen_t row = 0; row <= column; row++)
		{
			R_xlen_t rowIndex = indices[row];
			R_xlen_t columnIndex = indices[column];
			if(rowIndex > columnIndex)
			{
				std::swap(rowIndex, columnIndex);
			}
			destinationData((columnIndex*(columnIndex-(R_xlen_t)1))/(R_xlen_t)2+rowIndex-(R_xlen_t)1) = source((column*(column+(R_xlen_t)1))/(R_xlen_t)2 + row);
		}
	}
END_RCPP
}

// [[Rcpp::export]]
Rcpp::IntegerMatrix quantileNorm(Rcpp::IntegerMatrix mat, Rcpp::IntegerVector ref, int nthreads=1, int seed=13){
    if (mat.nrow() != ref.length()) Rcpp::stop("incompatible arrays...");
    if (!std::is_sorted(ref.begin(), ref.end())) Rcpp::stop("ref must be sorted");
    int ncol = mat.ncol();
    int nrow = mat.nrow();
    //allocate new matrix
    Rcpp::IntegerMatrix res(nrow, ncol);
    Mat<int> oldmat = asMat(mat); 
    Mat<int> newmat = asMat(res);
    Vec<int> ref2 = asVec(ref);
    //allocate a seed for each column
    std::seed_seq sseq{seed};
    std::vector<std::uint32_t> seeds(ncol);
    sseq.generate(seeds.begin(), seeds.end());
    
    #pragma omp parallel num_threads(nthreads)
    {
        std::vector<std::pair<int, int> > storage(nrow);//pairs <value, index>
        #pragma omp for 
        for (int col = 0; col < ncol; ++col){
            std::mt19937 gen(seeds[col]);
            qtlnorm(oldmat.getCol(col), ref2, newmat.getCol(col), storage, gen);
        }
    }
    
    res.attr("dimnames") = mat.attr("dimnames");
    return res;
}

// Calculate mk = sum_i I(M(ti)=k), k=1, ..., M with m0=0;
// where h=(h0, h1, ..., hM) with h0=0 and d=(d0, d1, ..., dM) with d0=0, dM=R_PosInf
void Getmk(Rcpp::IntegerVector& mk, const Rcpp::IntegerVector& Mt){
  int n = Mt.size();
  std::fill(mk.begin(), mk.end(), 0);
  for (int i=0; i<n; ++i){
    int k = Mt[i];
    mk[k] +=1;
  }
}

SEXP hclustThetaMatrix(SEXP mpcrossRF_, SEXP preClusterResults_)
{
BEGIN_RCPP
	Rcpp::List preClusterResults = preClusterResults_;
	bool noDuplicates;
	R_xlen_t preClusterMarkers = countPreClusterMarkers(preClusterResults_, noDuplicates);
	if(!noDuplicates)
	{
		throw std::runtime_error("Duplicate marker indices in call to hclustThetaMatrix");
	}

	Rcpp::S4 mpcrossRF = mpcrossRF_;
	Rcpp::S4 rf = mpcrossRF.slot("rf");

	Rcpp::S4 theta = rf.slot("theta");
	Rcpp::RawVector data = theta.slot("data");
	Rcpp::NumericVector levels = theta.slot("levels");
	Rcpp::CharacterVector markers = theta.slot("markers");
	if(markers.size() != preClusterMarkers)
	{
		throw std::runtime_error("Number of markers in precluster object was inconsistent with number of markers in mpcrossRF object");
	}
	R_xlen_t resultDimension = preClusterResults.size();
	//Allocate enough storage. This symmetric matrix stores the *LOWER* triangular part, in column-major storage. Excluding the diagonal. 
	Rcpp::NumericVector result(((resultDimension-(R_xlen_t)1)*resultDimension)/(R_xlen_t)2);
	for(R_xlen_t column = 0; column < resultDimension; column++)
	{
		Rcpp::IntegerVector columnMarkers = preClusterResults(column);
		for(R_xlen_t row = column + 1; row < resultDimension; row++)
		{
			Rcpp::IntegerVector rowMarkers = preClusterResults(row);
			double total = 0;
			R_xlen_t counter = 0;
			for(R_xlen_t columnMarkerCounter = 0; columnMarkerCounter < columnMarkers.size(); columnMarkerCounter++)
			{
				R_xlen_t marker1 = columnMarkers[columnMarkerCounter]-(R_xlen_t)1;
				for(R_xlen_t rowMarkerCounter = 0; rowMarkerCounter < rowMarkers.size(); rowMarkerCounter++)
				{
					R_xlen_t marker2 = rowMarkers[rowMarkerCounter]-(R_xlen_t)1;
					R_xlen_t column = std::max(marker1, marker2);
					R_xlen_t row = std::min(marker1, marker2);
					Rbyte thetaDataValue = data((column*(column+(R_xlen_t)1))/(R_xlen_t)2 + row);
					if(thetaDataValue != 0xFF)
					{
						total += levels(thetaDataValue);
						counter++;
					}
				}
			}
			if(counter == 0) total = 0.5;
			else total /= counter;
			result(((resultDimension-(R_xlen_t)1)*resultDimension)/(R_xlen_t)2 - ((resultDimension - column)*(resultDimension-column-(R_xlen_t)1))/(R_xlen_t)2 + row-column-(R_xlen_t)1) = total;
		}
	}
	return result;
END_RCPP
}

// [[Rcpp::export]]
Rcpp::NumericMatrix testColPost(Rcpp::NumericMatrix post, Rcpp::List m2u, int nthreads){
    Rcpp::IntegerVector values = Rcpp::as<Rcpp::IntegerVector>(m2u["values"]);
    Rcpp::IntegerVector map = Rcpp::as<Rcpp::IntegerVector>(m2u["map"]);
    if (post.ncol() != map.length()) Rcpp::stop("posteriors doesn't match with m2u");
    
    Rcpp::NumericMatrix smallerPost(post.nrow(), values.length());
    Vec<double> foo; NMPreproc preproc(asVec(values), asVec(map), foo);
    collapsePosteriors_core(asMat(smallerPost), asMat(post), preproc);
    return smallerPost;
}

RcppExport SEXP GetAllPoints(SEXP x,SEXP n,SEXP c) {

    try {
	Rcpp::XPtr< flann::Index<flann::L2<float> >  > index(x);
	Rcpp::NumericVector npoints(n);
	Rcpp::NumericVector cn(c);
	int colNum = cn[0];

	float *data = new float[colNum];
	
	for(int i=0;i<colNum;i++) {
	    data[i] = 0;
	    i++;
	}

	flann::Matrix<float> dataset = flann::Matrix<float>(data,1,colNum);

	delete [] data;

	std::vector< std::vector<int> > indices;
	std::vector< std::vector<float> > dists;

	index->knnSearch(dataset,indices,dists,npoints[0],flann::SearchParams(-1));

	std::sort (indices[0].begin(), indices[0].end()); 

	Rcpp::NumericMatrix results(indices[0].size(), colNum);
	Rcpp::IntegerVector rownames;

	int num = indices[0].size();

	//#pragma omp parallel for ordered schedule(dynamic)
	for(int i=0;i<num;i++) {
	    float* indexPoint = index->getPoint(indices[0][i]);
	    for(int j=0;j<colNum;j++) {
		results(i,j)=(*(indexPoint+j));
	    }

	    //#pragma omp ordered
	    rownames.push_back(indices[0][i]);
	}

	Rcpp::List dimnms = Rcpp::List::create(rownames, Rcpp::Range(1,colNum));
	results.attr("dimnames") = dimnms;

	return results;

    } catch( std::exception &ex ) {		// or use END_RCPP macro
	forward_exception_to_r( ex );
    } catch(...) {
	::Rf_error( "c++ exception (unknown reason)" );
    }
    return R_NilValue; // -Wall
}

//[[Rcpp::export]]
Rcpp::IntegerVector getAllNodesFast (Rcpp::IntegerMatrix edge, bool rooted) {
    Rcpp::IntegerVector tmp = Rcpp::as_vector(edge);
    Rcpp::IntegerVector maxN = Rcpp::range(tmp);
    Rcpp::IntegerVector ans = Rcpp::seq_len(maxN[1] + 1);
    if (rooted) {
	return ans - 1;
    }
    else {
	ans.erase(0);
	return ans - 1;
    }
}

SEXP getAllFunnels(SEXP Rmpcross)
{
	char* stackmem;
	{
		std::string error;
		{
			int nFounders;
			Rcpp::RObject mpcross_ = Rmpcross;
			bool valid = validateMPCross(mpcross_, nFounders, error, true, false, false);
			if(!valid)
			{
				goto signal_error;
			}
			Rcpp::List mpcross = Rmpcross;
			Rcpp::DataFrame pedigree(mpcross["pedigree"]);
			Rcpp::IntegerVector id = mpcross["id"];
			int nFinals = id.length();
			std::vector<int> fid = Rcpp::as<std::vector<int> >(mpcross["fid"]);
			Rcpp::IntegerMatrix output(id.length(), nFounders);
			std::vector<int> nIntercrossingGenerations;
			nIntercrossingGenerations.resize(nFinals, 0);
			//get number of intercrossing generations
			bool ok = intercrossingGenerations(pedigree, nFounders, id, nIntercrossingGenerations);
			if(!ok)
			{
				error = "Problem determining number of intercrossing generations";
				goto signal_error;
			}
			//now get the actual funnels from the pedigree
			int funnel[16];
			for(int i = 0; i < id.length(); i++)
			{
				ok = getFunnel(id[i], pedigree, fid, nIntercrossingGenerations[i], funnel, pedigree.nrows(), nFounders);
				if(!ok)
				{
					std::stringstream ss;
					ss << "Problem with pedigree, for individual number " << (i+1) << ", having id " << id[i];
					error = ss.str();
					goto signal_error;
				}
				for(int j = 0; j < nFounders; j++) output(i, j) = funnel[j];
			}
			return output;
		}
	signal_error:
		stackmem = (char*)alloca(error.size() + 4);
		memset(stackmem, 0, error.size() + 4);
		memcpy(stackmem, error.c_str(), error.size());
	}
	Rf_error(stackmem);
	return R_NilValue;
}

// [[Rcpp::export]]
Rcpp::NumericVector seqC(double from_, double to_, double by_ = 1.0) {
  int adjust = std::pow(10, std::ceil(std::log10(10 / by_)) - 1);
  int from = adjust * from_;
  int to = adjust * to_;
  int by = adjust * by_;
  
  std::size_t n = ((to - from) / by) + 1;
  Rcpp::IntegerVector res = Rcpp::rep(from, n);
  add_multiple ftor(by);
  
  std::transform(res.begin(), res.end(), res.begin(), ftor);
  return Rcpp::NumericVector(res) / adjust;
}

// [[Rcpp::export]]  
Rcpp::IntegerVector tabulate_cpp(const Rcpp::IntegerVector & v, R_xlen_t nlevels) {
  // Using R_xlen_t to avoid checking for entries < 0
  std::vector<R_xlen_t> table(nlevels);   
  R_xlen_t n =  v.size();
  for (R_xlen_t i = 0; i < n; ++i) { 
    table.at( v.at(i) - 1 ) ++;
  }    
  // Wrapp may throw errors with R_xlen_t
  // return wrap(table); 
  IntegerVector  a(table.size());
  std::copy(table.begin(), table.end(), a.begin());
  return a;
}

// [[Rcpp::export]]
Rcpp::List subsetCounts(Rcpp::IntegerVector counts, Rcpp::IntegerVector start, Rcpp::IntegerVector width, Rcpp::LogicalVector strand){
	if (start.length() != width.length() || start.length() != strand.length()) Rcpp::stop("provided vectors have different lengths...");
	int nr = start.length();
	int len = counts.length();
	int tot = 0;
	int* S = start.begin(); int* W = width.begin();
	for (int i = 0; i < nr; ++i){
		int s = S[i] - 1;
		int w = W[i]; 
		if (s < 0) Rcpp::stop("negative start positions are invalid");
		if (s + w > len) Rcpp::stop("range exceeds the lengths of the counts vector");
		tot += w;
	}
	
	Rcpp::IntegerVector res(tot); 
	Rcpp::IntegerVector nstart(nr);
	Rcpp::IntegerVector nend(nr);
	int* R = res.begin(); int* C = counts.begin(); int* ST = strand.begin();
	int* NS = nstart.begin(); int* NE = nend.begin();
	int currpos = 0;
	for (int i = 0; i < nr; ++i){
		NS[i] = currpos + 1;
		int w = W[i];
		if (ST[i]) std::copy(C + S[i]-1, C + S[i]-1 + w, R + currpos);
		else std::reverse_copy(C + S[i]-1, C + S[i]-1 + w, R + currpos);
		currpos += w;
		NE[i] = currpos;
	}
	return List::create(_("counts")=res, _("starts")=nstart, _("ends")=nend);
}

Rcpp::IntegerVector r_classify(const treetree::tree<T>& tr,
                               const std::vector<std::string>& labels) {
  treetree::tree<forest::node<int> > tmp = classify(tr, labels);
  Rcpp::IntegerVector ret;
  std::vector<std::string> names;
  names.reserve(tr.size());
  for (treetree::tree<forest::node<int> >::const_pre_iterator
         it = tmp.begin(); it != tmp.end(); ++it) {
    ret.push_back(it->data_);
    names.push_back(it->label_);
  }
  ret.attr("names") = names;
  return ret;
}

void ribi::ctm::Helper::CalcMaxLikelihood(
  const std::string& newick,
  Rate& birth_rate,
  Rate& death_rate,
  const Part part
) const
{
  assert(!newick.empty());
  auto& r = ribi::Rinside().Get();

  r.parseEval("library(ape)");
  r.parseEval("library(DDD)");
  r["newick"] = newick;
  r.parseEval("phylogeny <- read.tree(text = newick)");
  r.parseEval("branch_lengths <- phylogeny$edge.length");

  switch (part)
  {
    case Part::branch_lengths: r["part"] = 0; break;
    case Part::phylogeny     : r["part"] = 1; break;
  }

  r.parseEval(
    "max_likelihood <- bd_ML("
    "  brts = branch_lengths,"
    "  cond = 1," // # cond == 1 : conditioning on stem or crown age and non-extinction of the phylogeny
    "  btorph = part"
    ")"
  );

  r.parseEval("lambda0 <- max_likelihood$lambda0");
  r.parseEval("mu0 <- max_likelihood$mu0");
  r.parseEval("conv <- max_likelihood$conv");
  const Rcpp::DoubleVector lambda0 = r["lambda0"];
  const Rcpp::DoubleVector mu0 = r["mu0"];
  const Rcpp::IntegerVector conv = r["conv"];
  assert(lambda0.size() == 1);
  assert(mu0.size() == 1);
  assert(conv.size() == 1);
  const bool has_converged = conv[0] == 0;
  if (!has_converged)
  {
    std::stringstream s;
    s << __func__ << ": has not converged" ;
    throw std::runtime_error(s.str());
  }
  birth_rate = lambda0[0] / boost::units::si::second;
  death_rate = mu0[0] / boost::units::si::second;
}

    Permutation::Permutation(Rcpp::IntegerVector &vv)
	: d_perm(vv),
	  n(vv.size()) {
	int *vpt = vv.begin();
	std::vector<bool> chk(n);
	std::fill(chk.begin(), chk.end(), false);
	for (int i = 0; i < n; i++) {
	    int vi = vpt[i];
	    if (vi < 0 || n <= vi)
		throw runtime_error("permutation elements must be in [0,n)");
	    if (chk[vi])
		throw runtime_error("permutation is not a permutation");
	    chk[vi] = true;
	}
    }

static double bic_logistic(Rcpp::NumericMatrix X, 
			   Rcpp::NumericVector y, 
			   Rcpp::NumericMatrix beta_new, 
			   double eps, 
			   Rcpp::IntegerVector nk)
{
  int n_tot = X.nrow();
  int p = X.ncol();
  int K = nk.size();
    
  int idx = 0;
  double ll = 0.0;
  for (int k = 0; k < K; k++) {
    int n = nk[k];
    for (int i = 0; i < n; i++) {
      double lp = 0.0;
      for (int j = 0; j < p; j++) {
	lp += elem(X, idx+i, j) * elem(beta_new, j, k);
      }
      ll += y[idx+i] * lp - log(1.0 + exp(lp));
    }
    idx += n;
  }
  
  double bic = -2.0 * ll + df(beta_new, eps) * log(n_tot);
  return bic;
}

//' Update eigen values, vectors, and inverse matrices for irms
//'
//' @param tpm_prods array of transition probability matrix products
//' @param tpms array of transition probability matrices
//' @param pop_mat population level bookkeeping matrix
//' @param obstimes vector of observation times
//'
//' @return Updated eigenvalues, eigenvectors, and inverse matrices
// [[Rcpp::export]]
void tpmProdSeqs(Rcpp::NumericVector& tpm_prods, Rcpp::NumericVector& tpms, const Rcpp::IntegerVector obs_time_inds) {

        // Get indices
        int n_obs = obs_time_inds.size();
        Rcpp::IntegerVector tpmDims = tpms.attr("dim");

        // Ensure obs_time_inds starts at 0
        Rcpp::IntegerVector obs_inds = obs_time_inds - 1;

        // Set array pointers
        arma::cube prod_arr(tpm_prods.begin(), tpmDims[0], tpmDims[1], tpmDims[2], false);
        arma::cube tpm_arr(tpms.begin(), tpmDims[0], tpmDims[1], tpmDims[2], false);

        // Generate tpm products and store them in the appropriate spots in the tpm product array
        for(int j = 0; j < (n_obs - 1); ++j) {

                prod_arr.slice(obs_inds[j+1] - 1) = tpm_arr.slice(obs_inds[j+1] - 1);

                for(int k = (obs_inds[j+1] - 2); k > (obs_inds[j] - 1); --k) {

                        prod_arr.slice(k) = tpm_arr.slice(k) * prod_arr.slice(k + 1);
                }

        }
}

static void print(Rcpp::IntegerVector a)
{
  for (int i = 0; i < a.size(); i++) {
    printf("%d ", a[i]);
  }
  putchar('\n');
}

// [[Rcpp::export]]
Rcpp::IntegerVector testSortCounts(Rcpp::IntegerVector v){
    Rcpp::IntegerVector res(v.length());
    Vec<int> v2 = asVec(v);
    Vec<int> v3 = asVec(res);
    sortCounts(v2, v3);
    return res;
}

static Rcpp::NumericMatrix x_tilde(Rcpp::NumericMatrix X, 
				   Rcpp::IntegerVector nk,
				   Rcpp::IntegerMatrix groups, 
				   Rcpp::NumericVector d_cur, 
				   Rcpp::NumericMatrix eta_cur)
{
  int K = nk.size();
  int n_tot = X.nrow();
  int p = X.ncol();
  int L = groups.ncol();
  Rcpp::NumericMatrix result(n_tot, p * K);

  int idx = 0;
  for (int k = 0; k < K; k++) {
    int n = nk[k];
    for (int j = 0; j < p; j++) {
      //calculate sum for column j
      double sum = 0.0;
      for (int l = 0; l < L; l++) {
	if (elem(groups, j, l)) {
	  sum += d_cur[l] * elem(eta_cur, l, k);
	} 
      }
      
      //multiply column j in submatrix k of X with sum
      for (int i = 0; i < n; i++) {
	elem(result, idx + i, p * k + j) = elem(X, idx + i, j) * sum;
      }
    }
    idx += n;
  }
  return result;
}