#include "RcppArmadillo.h"
#include <Rcpp.h>
// [[Rcpp::depends(RcppArmadillo)]]
using namespace Rcpp;
using namespace std; 

// [[Rcpp::export]]
Rcpp::List rq_init_cpp(arma::mat X,
					   arma::vec R,
					   arma::vec beta, 
					   unsigned int n,
					   arma::vec weights){

	//arma::vec idx(n);
	arma::mat L, U, Pi;
	unsigned int Lr0 = count(R.begin(), R.end(), 0);
	//unsigned int idx;

	if(Lr0 > beta.n_elem){

		Rcout << "Worked" << "\n";
		arma::uvec Irs = sort_index(abs(R));
		Irs = Irs.subvec(0,Lr0-1);
		arma::mat Xh = X.rows(Irs);

		lu(L, U, Pi, Xh);
		arma::vec In = arma::linspace(0,Lr0,Lr0+1);
		arma::vec rI = arma::linspace(0, (Lr0)-beta.n_elem-1, (Lr0)-beta.n_elem);


		for(unsigned int i = (beta.n_elem); i < Lr0; i++){
			//idx = In(find(Pi.row(i) == 1));
			R(Irs(find(Pi.row(i) == 1))).fill(1.e-15);

		}
		//R(Irs(rI)) = 1.e-15;
	}

	// Find all index where r == 0 - Vertices
	arma::uvec Ih = find(R == 0);
	arma::uvec Ihc = find(abs(R) > 0);
	arma::vec P = sign(R.elem(Ihc));
	R.elem(find(abs(R) < 1.e-15)).fill(0);

	arma::vec W = weights % R;


	arma::vec xB = join_cols(beta, abs(W(Ihc)));

	return List::create(Named("xB") = xB,
						Named("Ih") = Ih,
						Named("Ihc") = Ihc,
						Named("P") = P,
						Named("R") = R,
						Named("X") = X);
}