text:
#include <string>
#include <sstream>
#include <cassert>
#include <algorithm> // std::max(a,b)

//Boost
#include <fstream>
#include <iostream>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>

//Root
#include "TApplication.h"
#include "TComplex.h"
#include <TGraph.h>
#include "TAxis.h"
#include "TCanvas.h"
#include "TH1I.h"
#include "THStack.h"
#include "TLegend.h"
#include "TH2D.h"

using namespace std;

char muted_in(string ATCG_data);

double compute_mut_freq(string ATCG_data, char muted_in);

int compute_coverage(string ATCG_data);

double compute_mut_rate(double P_hat, double death_prob, int extant);

bool endpoint_muted(double mut_freq, int total_coverage, int extant);

/*
Count base as muted if endpoint frequency

	f >= Cutoff(Coverage, f_min) e Coverage = TotCoverage/3

	cutoff (Coverage, f_min) = f_min + alpha/Sqrt[Coverage]

based on extant f_min and corresponding alphas are

	f_min = {1/8, 1/12, 1/16, 1/20, 1/24, 1/28, 1/32, 1/48, 1/75, 1/100}

	alpha = {0.52, 0.45, 0.36, 0.32, 0.3, 0.28, 0.25, 0.2, 0.18, 0.15}

	extant: for defining minimum mut_freq to consider endpoint as muted

	extant must be in {8, 12, 16, 20, 24, 28, 32, 48, 75, 100}
			    |                                   |
			    |-----------10 elements-------------|
*/

int main(){

	//*******************************************
	//          THRESHOLDS
	//*******************************************
	int ancestor_coverage_min = 30;
	long int discarded_by_threshold = 0;
	double endpoint_max_mut_freq = 0.2;

	//*******************************************
	//          FILENAMES
	//*******************************************
	string ancestor_1 = "CRC1307-02-0.all_common.gz";
	string ancestor_2 = "CRC1307-09-0.all_common.gz";

	string endpoint_1 = "CRC1307-02-1-E.all_common.gz";
	string endpoint_2 = "CRC1307-09-1-B.all_common.gz";

	//*******************************************
	//          COMPRESSED INPUT
	//*******************************************
	//(IN) ANCESTOR FILE 1 --------------------------------------------------
	ifstream file_ancestor_1(ancestor_1, ios_base::in | ios_base::binary);

    	if (file_ancestor_1.is_open() == false) {
		cerr << ancestor_1 << " not opened: exit!" <<endl;
		return -1;
	}
	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> ancestor_1_inbuf;
    	ancestor_1_inbuf.push(boost::iostreams::gzip_decompressor());
    	ancestor_1_inbuf.push(file_ancestor_1);
    	//Convert streambuf to istream
    	istream ancestor_1_instream(&ancestor_1_inbuf);

	cerr << endl << ancestor_1 << " opened." << endl;

	//(IN) ANCESTOR FILE 2 --------------------------------------------------
	ifstream file_ancestor_2(ancestor_2, ios_base::in | ios_base::binary);

    	    	if (file_ancestor_2.is_open() == false) {
		cerr << ancestor_2 << " not opened: exit!" <<endl;
		return -1;
	}
	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> ancestor_2_inbuf;
    	ancestor_2_inbuf.push(boost::iostreams::gzip_decompressor());
    	ancestor_2_inbuf.push(file_ancestor_2);
    	//Convert streambuf to istream
    	istream ancestor_2_instream(&ancestor_2_inbuf);

	cerr << endl << ancestor_2 << " opened." << endl;

	//(IN) ENDPOINT FILE 1 --------------------------------------------------
	ifstream file_endpoint_1(endpoint_1, ios_base::in | ios_base::binary);

    	if (file_endpoint_1.is_open() == false) {
		cerr << endpoint_1 << " not opened: exit!" <<endl;
		return -1;
	}
	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> endpoint_1_inbuf;
    	endpoint_1_inbuf.push(boost::iostreams::gzip_decompressor());
    	endpoint_1_inbuf.push(file_endpoint_1);
    	//Convert streambuf to istream
    	istream endpoint_1_instream(&endpoint_1_inbuf);

	cerr << endl << endpoint_1 << " opened." << endl;

	//(IN) ENDPOINT FILE 2 --------------------------------------------------
	ifstream file_endpoint_2(endpoint_2, ios_base::in | ios_base::binary);

    	if (file_endpoint_2.is_open() == false) {
		cerr << endpoint_2 << " not opened: exit!" <<endl;
		return -1;
	}
	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> endpoint_2_inbuf;
    	endpoint_2_inbuf.push(boost::iostreams::gzip_decompressor());
    	endpoint_2_inbuf.push(file_endpoint_2);
    	//Convert streambuf to istream
    	istream endpoint_2_instream(&endpoint_2_inbuf);

	cerr << endl << endpoint_2 << " opened." << endl;

	//*******************************************
	//          PARSING VARIABLES
	//*******************************************
	//Ancestor 1 parsing variables-------------
	string ancestor_1_line; //line of the file to parse
	string ancestor_1_chromosome; //pos
	int ancestor_1_chromosome_number = 0;
	long int ancestor_1_base_number = 0; //1-based
	string ancestor_1_ATCG_data;

	//Ancestor 2 parsing variables-------------
	string ancestor_2_line; //line of the file to parse
	string ancestor_2_chromosome; //pos
	int ancestor_2_chromosome_number = 0;
	long int ancestor_2_base_number = 0; //1-based
	string ancestor_2_ATCG_data;

	//Endpoint 1 parsing variables-------------
	string endpoint_1_line; //line of the file to parse
	string endpoint_1_chromosome; //pos
	int endpoint_1_chromosome_number = 0;
	long int endpoint_1_base_number = 0; //1-based
	string endpoint_1_ATCG_data;

	//Endpoint 2 parsing variables-------------
	string endpoint_2_line; //line of the file to parse
	string endpoint_2_chromosome; //pos
	int endpoint_2_chromosome_number = 0;
	long int endpoint_2_base_number = 0; //1-based
	string endpoint_2_ATCG_data;

	//*******************************************
	//          USEFUL VARIABLES
	//*******************************************
	long int total_lines = 0;
	long int both_not_muted = 0;
	long int both_muted = 0;
	long int both_same_mutation = 0;
	long int one_muted_and_one_not = 0;

	char endpoint_1_muted_in;
	char endpoint_2_muted_in;
	double endpoint_1_mut_freq;
	double endpoint_2_mut_freq;

	int extants [10] = {8, 12, 16, 20, 24, 28, 32, 48, 75, 100};

	long int endpoint_1_muted_bases [10] = {}; //10 long int values, each initialized with a value of zero
	long int endpoint_1_not_muted_bases [10] = {};
	double endpoint_1_P_hat [10] = {};

	long int endpoint_2_muted_bases [10] = {};
	long int endpoint_2_not_muted_bases [10] = {};
	double endpoint_2_P_hat [10] = {};

	//ROOT
	TApplication myApp("myApp",0,0);

	//Histograms (frequecy_1, frequency_2)
	TH2D *h_all = new TH2D("All mutations", "All mutations", 201, 0., 1.005, 201, 0., 1.005);

	TH2D *h_same = new TH2D("Same mutation for both endpoints", "Same mutation for both endpoints", 201, 0., 1.005, 201, 0., 1.005);

	TH2D *h_different = new TH2D("Different mutation for the two endpoints", "Different mutation for the two endpoints", 201, 0., 1.005, 201, 0., 1.005);

	//*******************************************
	//          START
	//*******************************************
	cout<< endl << "Start reading files..." << endl;
	//Loop for all line
	while (getline(ancestor_1_instream, ancestor_1_line)) {
		//Read a line from each file
		getline(ancestor_2_instream, ancestor_2_line);
		getline(endpoint_1_instream, endpoint_1_line);
		getline(endpoint_2_instream, endpoint_2_line);

		// Parse ancestor_1 line
		stringstream ancestor_1_linestream(ancestor_1_line);
		//get file_2 line pos finding the separator ('\t')
		getline(ancestor_1_linestream, ancestor_1_chromosome, '\t');
		//get file_2 loc
		ancestor_1_linestream >> ancestor_1_base_number;
		//we need chromosome numeber and loc
		//extract chromosome number from chromosome string
		// "chrN", N integer
		//     ^
		ancestor_1_chromosome_number = atoi(&ancestor_1_chromosome[3]);
		//get file_2 ATCG data finding the separator ('\n')
		getline(ancestor_1_linestream, ancestor_1_ATCG_data, '\n');

		// Parse ancestor_2 line
		stringstream ancestor_2_linestream(ancestor_2_line);
		getline(ancestor_2_linestream, ancestor_2_chromosome, '\t');
		ancestor_2_linestream >> ancestor_2_base_number;
		ancestor_2_chromosome_number = atoi(&ancestor_2_chromosome[3]);
		getline(ancestor_2_linestream, ancestor_2_ATCG_data, '\n');

		// Parse endpoint_1 line
		stringstream endpoint_1_linestream(endpoint_1_line);
		getline(endpoint_1_linestream, endpoint_1_chromosome, '\t');
		endpoint_1_linestream >> endpoint_1_base_number;
		endpoint_1_chromosome_number = atoi(&endpoint_1_chromosome[3]);
		getline(endpoint_1_linestream, endpoint_1_ATCG_data, '\n');

		// Parse endpoint_2 line
		stringstream endpoint_2_linestream(endpoint_2_line);
		getline(endpoint_2_linestream, endpoint_2_chromosome, '\t');
		endpoint_2_linestream >> endpoint_2_base_number;
		endpoint_2_chromosome_number = atoi(&endpoint_2_chromosome[3]);
		getline(endpoint_2_linestream, endpoint_2_ATCG_data, '\n');

		//check that we loaded corrisponding lines (=> input files are ok)
		assert((ancestor_1_chromosome_number == ancestor_2_chromosome_number)
			&& (ancestor_2_chromosome_number == endpoint_1_chromosome_number)
			&& (ancestor_2_chromosome_number == endpoint_2_chromosome_number));

		assert((ancestor_1_base_number == ancestor_2_base_number)
			&& (ancestor_2_base_number == endpoint_1_base_number)
			&& (ancestor_2_base_number == endpoint_2_base_number));

		//Count loaded lines
		total_lines++;
		if (total_lines % 100000000 == 0){
			cout << total_lines/100000000 << "00 millions of lines read." << endl;
		}

		//Check ancestor coverage and discard according to threshold
		if (compute_coverage(ancestor_1_ATCG_data) < ancestor_coverage_min ||
			compute_coverage(ancestor_2_ATCG_data) < ancestor_coverage_min) {
			//one or both of the ancestors have a coverage under threshold
			//we discard the corresponding base
			discarded_by_threshold++;
			//skip this base and don't put it in the histograms
			//load next lines from files files
			continue;

		}

		//Check if endpoint 1 muted
		endpoint_1_muted_in = muted_in(endpoint_1_ATCG_data);
		if (endpoint_1_muted_in != ' '){
			endpoint_1_mut_freq = compute_mut_freq(endpoint_1_ATCG_data, endpoint_1_muted_in);
		} else endpoint_1_mut_freq = 0.;

		//Check if endpoint 2 muted
		endpoint_2_muted_in = muted_in(endpoint_2_ATCG_data);
		if (endpoint_2_muted_in != ' '){
			endpoint_2_mut_freq = compute_mut_freq(endpoint_2_ATCG_data, endpoint_2_muted_in);
		} else endpoint_2_mut_freq = 0.;

		//Check endpointand discard according to threshold
		if (endpoint_1_mut_freq > endpoint_max_mut_freq
		    || endpoint_2_mut_freq > endpoint_max_mut_freq) {
		    	discarded_by_threshold++;
		    	continue;
		    }

		//compute enpoint 1 and 2 coverages
		int endpoint_1_coverage = compute_coverage(endpoint_1_ATCG_data);
		int endpoint_2_coverage = compute_coverage(endpoint_2_ATCG_data);

		if (endpoint_1_muted_in == ' ' && endpoint_2_muted_in == ' '){
			//Both endpoints totally confim reference
			both_not_muted++;
			//count these lines in both ancestors' denominator
			//for all extants
			for (int i = 0; i < 10; i++){
				endpoint_1_not_muted_bases [i] ++;
				endpoint_2_not_muted_bases [i] ++;
			}
		} else {
			//at least one muted
			h_all -> Fill(endpoint_1_mut_freq, endpoint_2_mut_freq);

			if(endpoint_1_muted_in == ' ' || endpoint_2_muted_in == ' '){
				//one muted and one not
				one_muted_and_one_not++;
				h_different -> Fill(endpoint_1_mut_freq, endpoint_2_mut_freq);
				//Need to count that line based on the cutoff(extant)
				//for one of the two endpoints

				if (endpoint_2_muted_in == ' '){
					//endpoint 1 muted and 2 not

					//label endpoint_1 as muted or not
					//using cutoff varying extant
					for (int i = 0; i < 10; i++){
						//base is not muted for endpoint 2
						endpoint_2_not_muted_bases [i] ++;
						//check cutoff for endpoint 1
						if (endpoint_muted(endpoint_1_mut_freq, endpoint_1_coverage, extants[i]) ){
							endpoint_1_muted_bases [i] ++;
						} else{
							endpoint_1_not_muted_bases [i] ++;
						}
					}
				} else {
					//endpoint 2 muted and 1 not
					//label endpoint_2 as muted or not
					//using cutoff varying extant
					for (int i = 0; i < 10; i++){
						//base not muted for endpoint 1
						endpoint_1_not_muted_bases [i] ++;
						//check cutoff for endpoint 2
						if (endpoint_muted(endpoint_2_mut_freq, endpoint_2_coverage, extants[i]) ){
							endpoint_2_muted_bases [i] ++;
						} else{
							endpoint_2_not_muted_bases [i] ++;
						}
					}
				}
			} else {
				both_muted++;
				if(endpoint_1_muted_in == endpoint_2_muted_in) {
					//Same mutation for both endpoints
					both_same_mutation++;
					h_same -> Fill(endpoint_1_mut_freq, endpoint_2_mut_freq);
					//Skip these lines for the mut rate computation
				} else {
					//both muted but bifferent mutation
					h_different -> Fill(endpoint_1_mut_freq, endpoint_2_mut_freq);
					//Need to count these lines based on the cutoff(extant)
					//for both the endpoints

					for (int i = 0; i < 10; i++){
						//check cutoff for endpoint 1
						if (endpoint_muted(endpoint_1_mut_freq, endpoint_1_coverage, extants[i]) ){
							endpoint_1_muted_bases [i] ++;
						} else{
							endpoint_1_not_muted_bases [i] ++;
						}
						//check cutoff for endpoint 2
						if (endpoint_muted(endpoint_2_mut_freq, endpoint_2_coverage, extants[i]) ){
							endpoint_2_muted_bases [i] ++;
						} else{
							endpoint_2_not_muted_bases [i] ++;
						}
					}

				}
			}
		}

	} //EOF

	assert(total_lines - discarded_by_threshold == both_not_muted + both_muted + one_muted_and_one_not);
	assert(both_same_mutation <= both_muted);

	cout <<"Total bases in files: " << total_lines << endl<<endl;
	cout <<"Total lines discarded by threshold (ancestor coverage min): " << discarded_by_threshold << endl;
	cout <<"Both endpoints not muted: " << both_not_muted <<endl;
	cout <<"Both endpoints muted: " << both_muted << endl;
	cout <<"Both endpoints with the same mutation: " << both_same_mutation << endl;
	cout <<"One endpoint muted and one not: "<< one_muted_and_one_not <<endl;

	//*******************************************
	//          FILES CLEANUP
	//*******************************************
	file_ancestor_1.close();
	file_ancestor_2.close();
	file_endpoint_1.close();
	file_endpoint_2.close();

	//*******************************************
	//         COMPUTE MUT RATE
	//*******************************************
	cout << "Total lines considered for the estimation of mut rate (denominator): "<< total_lines - discarded_by_threshold - both_same_mutation <<endl;
	double min_mu, max_mu;

	ofstream endpoint_1_results;
	ofstream endpoint_2_results;
	endpoint_1_results.open("CRC1307-02-1-E.all_common_results.csv");
	endpoint_2_results.open("CRC1307-09-1-B.all_common_results.csv");

	cout << endl << endl << "Computing mutation rate for endpoint CRC1307-02-1-E"<< endl<< endl;
	cout << "extant\tmuted_bases\tp_hat\tmin_mu\tmax_mu" << endl;

	endpoint_1_results << "extant,p_hat,min_mu,max_mu" << endl;

	for (int i = 0; i < 10; i++){
		assert(total_lines - discarded_by_threshold - both_same_mutation == endpoint_1_muted_bases[i] + endpoint_1_not_muted_bases[i]);
		endpoint_1_results << extants [i] << ",";
		cout << extants [i] << "\t";
		cout << endpoint_1_muted_bases[i] << "\t";

		endpoint_1_P_hat [i] = double(endpoint_1_muted_bases[i]) / double(endpoint_1_muted_bases[i] + endpoint_1_not_muted_bases[i]);

		endpoint_1_results << endpoint_1_P_hat [i] << ",";
		cout << endpoint_1_P_hat [i] << "\t";

		min_mu = compute_mut_rate(endpoint_1_P_hat [i], 0.45, extants [i]);
		max_mu = compute_mut_rate(endpoint_1_P_hat [i], 0.1, extants [i]);

		endpoint_1_results << min_mu << "," << max_mu << endl;
		cout << min_mu << "\t" << max_mu << endl;
	}

	cout << endl << endl << "Computing mutation rate for endpoint CRC1307-09-1-B"<< endl <<endl;
	cout << "extant\tmuted_bases\tp_hat\tmin_mu\tmax_mu" << endl;

	endpoint_2_results << "extant,p_hat,min_mu,max_mu" << endl;

	for (int i = 0; i < 10; i++){
		assert(total_lines - discarded_by_threshold - both_same_mutation == endpoint_2_muted_bases[i] + endpoint_2_not_muted_bases[i]);
		endpoint_2_results << extants [i] << ",";
		cout << extants [i] << "\t";
		cout << endpoint_2_muted_bases[i] << "\t";

		endpoint_2_P_hat [i] = double(endpoint_2_muted_bases[i]) / double(endpoint_2_muted_bases[i] + endpoint_2_not_muted_bases[i]);

		endpoint_2_results << endpoint_2_P_hat [i] << ",";
		cout << endpoint_2_P_hat [i] << "\t";

		min_mu = compute_mut_rate(endpoint_2_P_hat [i], 0.45, extants [i]);
		max_mu = compute_mut_rate(endpoint_2_P_hat [i], 0.1, extants [i]);

		endpoint_2_results << min_mu << "," << max_mu << endl;
		cout << min_mu << "\t" << max_mu << endl;
	}

	endpoint_1_results.close();
	endpoint_2_results.close();

	cout << "Results written on .csv files." << endl << "Starting ROOT graphs." << endl;

	//Start Root graphics
	TCanvas c1("Same mutation", "Same mutation",800,800);
   	h_same -> GetXaxis() -> SetTitle("mut freq endpoint 1");
   	h_same -> GetYaxis() -> SetTitle("mut freq endpoint 2");
   	h_same -> GetZaxis() -> SetTitle("Counts");
   	c1.SetLogx();
   	c1.SetLogy();
   	c1.SetLogz();
   	h_same -> Draw("colz");

   	TCanvas c2("Different mutation", "Different mutation",800,800);
   	h_different -> GetXaxis() -> SetTitle("mut freq endpoint 1");
   	h_different -> GetYaxis() -> SetTitle("mut freq endpoint 2");
   	h_different -> GetZaxis() -> SetTitle("Counts");
   	c2.SetLogx();
   	c2.SetLogy();
   	c2.SetLogz();
   	h_different -> Draw("colz");

   	TCanvas c3("All mutations", "All mutations",800,800);
   	h_all -> GetXaxis() -> SetTitle("mut freq endpoint 1");
   	h_all -> GetYaxis() -> SetTitle("mut freq endpoint 2");
   	h_all -> GetZaxis() -> SetTitle("Counts");
   	c3.SetLogx();
   	c3.SetLogy();
   	c3.SetLogz();
   	h_all -> Draw("colz");

   	myApp.Run(1);

	return 0;
}

char muted_in(string ATCG_data){

	char reference;
	int A_counter = 0;
	int a_counter = 0;
	int T_counter = 0;
	int t_counter = 0;
	int C_counter = 0;
	int c_counter = 0;
	int G_counter = 0;
	int g_counter = 0;
	int coverage = 0;
	char muted_in;

	stringstream ATCG_linestream(ATCG_data);
	ATCG_linestream >> reference;
	//counters
	ATCG_linestream >> A_counter; coverage += A_counter;
	ATCG_linestream >> T_counter; coverage += T_counter;
	ATCG_linestream >> C_counter; coverage += C_counter;
	ATCG_linestream >> G_counter; coverage += G_counter;
	ATCG_linestream >> a_counter; coverage += a_counter;
	ATCG_linestream >> t_counter; coverage += t_counter;
	ATCG_linestream >> c_counter; coverage += c_counter;
	ATCG_linestream >> g_counter; coverage += g_counter;

	//total counts (forward+reverse)
	int A = A_counter + a_counter;
	int T = T_counter + t_counter;
	int C = C_counter + c_counter;
	int G = G_counter + g_counter;

	if (reference == 'A'){
		int muted_reads = max(T,C);
		muted_reads = max(muted_reads,G);
		if (muted_reads != 0) {
			if (muted_reads == T) muted_in = 'T';
			if (muted_reads == C) muted_in = 'C';
			if (muted_reads == G) muted_in = 'G';
		} else muted_in = ' ';
	}
	if (reference == 'T'){
		int muted_reads = max(A,C);
		muted_reads = max(muted_reads,G);
		if (muted_reads != 0) {
			if (muted_reads == A) muted_in = 'A';
			if (muted_reads == C) muted_in = 'C';
			if (muted_reads == G) muted_in = 'G';
		} else muted_in = ' ';
	}
	if (reference == 'C'){
		int muted_reads = max(A,T);
		muted_reads = max(muted_reads,G);
		if (muted_reads != 0) {
			if (muted_reads == A) muted_in = 'A';
			if (muted_reads == T) muted_in = 'T';
			if (muted_reads == G) muted_in = 'G';
		} else muted_in = ' ';
	}
	if (reference == 'G'){
		int muted_reads = max(A,T);
		muted_reads = max(muted_reads,C);
		if (muted_reads != 0) {
			if (muted_reads == A) muted_in = 'A';
			if (muted_reads == T) muted_in = 'T';
			if (muted_reads == C) muted_in = 'C';
		} else muted_in = ' ';
	}

	return muted_in;
}
double compute_mut_freq(string ATCG_data, char muted_in){
	assert(muted_in == 'A' || muted_in == 'T' || muted_in == 'C' || muted_in == 'G');
	char reference;
	int A_counter = 0;
	int a_counter = 0;
	int T_counter = 0;
	int t_counter = 0;
	int C_counter = 0;
	int c_counter = 0;
	int G_counter = 0;
	int g_counter = 0;
	int coverage = 0;

	double mut_freq;
	int muted_reads=0;

	stringstream ATCG_linestream(ATCG_data);
	ATCG_linestream >> reference;

	assert(muted_in != reference);

	//counters
	ATCG_linestream >> A_counter; coverage += A_counter;
	ATCG_linestream >> T_counter; coverage += T_counter;
	ATCG_linestream >> C_counter; coverage += C_counter;
	ATCG_linestream >> G_counter; coverage += G_counter;
	ATCG_linestream >> a_counter; coverage += a_counter;
	ATCG_linestream >> t_counter; coverage += t_counter;
	ATCG_linestream >> c_counter; coverage += c_counter;
	ATCG_linestream >> g_counter; coverage += g_counter;

	if(muted_in == 'A'){
		muted_reads = A_counter + a_counter;
		mut_freq = double(muted_reads)/double(coverage);
		return mut_freq;
	}
	if(muted_in == 'T'){
		muted_reads = T_counter + t_counter;
		mut_freq = double(muted_reads)/double(coverage);
		return mut_freq;
	}
	if(muted_in == 'C'){
		muted_reads = C_counter + c_counter;
		mut_freq = double(muted_reads)/double(coverage);
		return mut_freq;
	}
	if(muted_in == 'G'){
		muted_reads = G_counter + g_counter;
		mut_freq = double(muted_reads)/double(coverage);
		return mut_freq;
	}

}

int compute_coverage(string ATCG_data){
	char reference;
	int A_counter = 0;
	int a_counter = 0;
	int T_counter = 0;
	int t_counter = 0;
	int C_counter = 0;
	int c_counter = 0;
	int G_counter = 0;
	int g_counter = 0;
	int coverage = 0;

	stringstream ATCG_linestream(ATCG_data);
	ATCG_linestream >> reference;

	//counters
	ATCG_linestream >> A_counter; coverage += A_counter;
	ATCG_linestream >> T_counter; coverage += T_counter;
	ATCG_linestream >> C_counter; coverage += C_counter;
	ATCG_linestream >> G_counter; coverage += G_counter;
	ATCG_linestream >> a_counter; coverage += a_counter;
	ATCG_linestream >> t_counter; coverage += t_counter;
	ATCG_linestream >> c_counter; coverage += c_counter;
	ATCG_linestream >> g_counter; coverage += g_counter;

	return coverage;
}

double compute_mut_rate(double P_hat, double death_prob, int extant){

	// gen = log_{2*(1-death_prob)} extant
	float generations = log(extant)/log(2.*(1.-death_prob));

	//integral estimate with continuous time
	float attempts = (1. - death_prob*death_prob) * (
				( pow((2*(1-death_prob)), generations - 1.) - 1.) /
			  	log(2.*(1.- death_prob))
			  	) + extant;

	double mut_rate = -1. *( log(1. - P_hat)/attempts);

	return mut_rate;

}

bool endpoint_muted(double mut_freq, int total_coverage, int extant){
	/*Count base as muted if endpoint frequency

	f >= Cutoff(Coverage, f_min) e Coverage = TotCoverage/3

	cutoff (Coverage, f_min) = f_min + alpha/Sqrt[Coverage]

based on extant f_min and corresponding alphas are

	f_min = {1/8, 1/12, 1/16, 1/20, 1/24, 1/28, 1/32, 1/48, 1/75, 1/100}

	alpha = {0.52, 0.45, 0.36, 0.32, 0.3, 0.28, 0.25, 0.2, 0.18, 0.15}
*/
	assert( extant == 8 || extant == 12 || extant == 16 || extant == 20
		|| extant == 24 || extant == 28 || extant == 32 || extant == 48
		|| extant == 75 || extant == 100);

	double f_min = 0.;
	float coverage = float(total_coverage)/3.;
	double alpha = 0.;

	f_min = 1./double(extant);

	if (extant == 8){
		alpha = 0.52;
	}
	if (extant == 12){
		alpha = 0.45;
	}

	if (extant == 16){
		alpha = 0.36;
	}
	if (extant == 20){
		alpha = 0.32;
	}
	if (extant == 24){
		alpha = 0.3;
	}
	if (extant == 28){
		alpha = 0.28;
	}
	if (extant == 32){
		alpha = 0.25;
	}
	if (extant == 48){
		alpha = 0.2;
	}

	if (extant == 75){
		alpha = 0.18;
	}
	if (extant == 100){
		alpha = 0.15;
	}

	if ( mut_freq >= f_min + alpha/sqrt(coverage) ) {
		//muted
		return true;
	} else {
		//not muted
		return false;
	}
}

max_stars_repo_name: PietroRivetti/LD-mut-rate

max_issues_repo_name: PietroRivetti/LD-mut-rate

max_forks_repo_name: PietroRivetti/LD-mut-rate


text: #include <boost/range/algorithm_ext.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 * Copyright (C) 2012-2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <string>
#include <fstream>
#include <gtest/gtest.h>
#include <boost/filesystem.hpp>

#include "test_config.h"

boost::filesystem::path g_mkRonn;
boost::filesystem::path g_gzRoff;
boost::filesystem::path g_toolBinPath;
boost::filesystem::path g_toolSrcPath;
boost::filesystem::path g_serverBinPath;
boost::filesystem::path g_serverSrcPath;
boost::filesystem::path g_guiBinPath;
boost::filesystem::path g_guiSrcPath;
std::string g_gazeboPATH;

/////////////////////////////////////////////////
std::string customExec(std::string _cmd)
{
  _cmd += " 2>/dev/null";
  FILE* pipe = popen(_cmd.c_str(), "r");

  if (!pipe)
    return "ERROR";

  char buffer[128];
  std::string result = "";

  while (!feof(pipe))
  {
    if (fgets(buffer, 128, pipe) != NULL)
      result += buffer;
  }

  pclose(pipe);
  return result;
}

/////////////////////////////////////////////////
std::string readFile(const std::string &_filename)
{
  std::string result;
  std::string line;

  std::ifstream ronnFile(_filename.c_str(), std::ios::in);
  while (ronnFile)
  {
    std::getline(ronnFile, line);
    if (ronnFile)
      result += line + "\n";
  }

  return result;
}

/////////////////////////////////////////////////
void toolTest(const std::string &_command)
{
  std::string ronnOut = customExec(g_mkRonn.string()
      + " " + (g_toolBinPath / _command).string());

  boost::filesystem::path origRonnFilename =
    g_toolSrcPath / (_command + ".1.ronn");

  std::string origRonn = readFile(origRonnFilename.string());

  EXPECT_EQ(origRonn, ronnOut);
}

/////////////////////////////////////////////////
void serverTest(const std::string &_command)
{
  std::string ronnOut = customExec(g_gazeboPATH + " " + g_mkRonn.string()
      + " " + (g_serverBinPath / _command).string());

  boost::filesystem::path origRonnFilename =
    g_serverSrcPath / (_command + ".1.ronn");

  std::string origRonn = readFile(origRonnFilename.string());

  EXPECT_EQ(origRonn, ronnOut);
}

/////////////////////////////////////////////////
void guiTest(const std::string &_command)
{
  std::string ronnOut = customExec(g_mkRonn.string()
      + " " + (g_guiBinPath / _command).string());

  boost::filesystem::path origRonnFilename =
    g_guiSrcPath / (_command + ".1.ronn");

  std::string origRonn = readFile(origRonnFilename.string());

  EXPECT_EQ(origRonn, ronnOut);
}

/////////////////////////////////////////////////
TEST(ManTest, gzclient)
{
  guiTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gzserver)
{
  serverTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gazebo)
{
  serverTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gzfactory)
{
  toolTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gzlog)
{
  toolTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gzsdf)
{
  toolTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gzstats)
{
  toolTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gztopic)
{
  toolTest(::testing::UnitTest::GetInstance()->current_test_info()->name());
}

/////////////////////////////////////////////////
TEST(ManTest, gz)
{
  std::string command =
    ::testing::UnitTest::GetInstance()->current_test_info()->name();

  std::string roffOut = customExec(g_gzRoff.string()
      + " " + (g_toolBinPath / command).string());

  boost::filesystem::path origRoffFilename =
    g_toolSrcPath / (command + ".1.roff");

  std::string origRoff = readFile(origRoffFilename.string());

  EXPECT_EQ(origRoff, roffOut);
}

/////////////////////////////////////////////////
int main(int argc, char **argv)
{
  g_mkRonn = PROJECT_SOURCE_PATH;
  g_mkRonn = g_mkRonn / "tools" / "make_ronn.py";

  g_gzRoff = PROJECT_SOURCE_PATH;
  g_gzRoff = g_gzRoff / "tools" / "gz_roff.py";

  g_toolBinPath = PROJECT_BINARY_PATH;
  g_toolBinPath = g_toolBinPath / "tools";

  g_toolSrcPath = PROJECT_SOURCE_PATH;
  g_toolSrcPath = g_toolSrcPath / "tools";

  g_serverBinPath = PROJECT_BINARY_PATH;
  g_serverBinPath = g_serverBinPath / "gazebo";

  g_serverSrcPath = PROJECT_SOURCE_PATH;
  g_serverSrcPath = g_serverSrcPath / "gazebo";

  g_guiBinPath = g_serverBinPath / "gui";
  g_guiSrcPath = g_serverSrcPath / "gui";

  g_gazeboPATH = std::string("PATH=") + g_serverBinPath.string() + ":"
      + g_guiBinPath.string() + ":$PATH";

  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

ext: cc

max_stars_repo_name: hyunoklee/Gazebo

max_issues_repo_name: hyunoklee/Gazebo

max_forks_repo_name: hyunoklee/Gazebo


text:
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// Name        :
// Author      : Avi
// Revision    : $Revision: #10 $
//
// Copyright 2009- ECMWF.
// This software is licensed under the terms of the Apache Licence version 2.0
// which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
// In applying this licence, ECMWF does not waive the privileges and immunities
// granted to it by virtue of its status as an intergovernmental organisation
// nor does it submit to any jurisdiction.
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/path.hpp"

#include "Defs.hpp"
#include "Suite.hpp"
#include "Family.hpp"
#include "Task.hpp"
#include "Alias.hpp"
#include "Str.hpp"
#include "File.hpp"

namespace fs = boost::filesystem;
using namespace std;
using namespace ecf;

BOOST_AUTO_TEST_SUITE( NodeTestSuite )

BOOST_AUTO_TEST_CASE( test_alias_create )
{
   cout << "ANode:: ...test_alias_create\n";
   std::string ecf_home = File::test_data("ANode/test/data/alias","ANode");

   task_ptr t ;
   Defs theDefs;
   {
      suite_ptr s = theDefs.add_suite("test_alias_create");
      family_ptr f = s->add_family("f");
      t = f->add_task("t");
      // ECFLOW-1278  set values for event,meter,labels, then ensure alias creation clears them
      Meter meter1("met",0,100,100); meter1.set_value(10);
      Meter meter2("met2",0,100,100); meter2.set_value(10);
      Event event1(1,"event1"); event1.set_value(true);
      Event event2(1,"event2"); event2.set_value(true);
      Label label1("name","value"); label1.set_new_value("new_value");
      Label label2("name2","value"); label2.set_new_value("new_value");
      t->addMeter(meter1);
      t->addMeter(meter2);
      t->addEvent(event1);
      t->addEvent(event2);
      t->addLabel(label1);
      t->addLabel(label2);
      s->add_variable(Str::ECF_HOME(),ecf_home);
   }

   // Create .usr file content
   std::vector<std::string> usr_file_vec;
   usr_file_vec.emplace_back("#This is a .usr file.");

   // Create list of variables
   NameValueVec used_variables;
   used_variables.push_back( std::make_pair(std::string("a"),std::string("a_value")));
   used_variables.push_back( std::make_pair(std::string("b"),std::string("b_value")));

   // Finally create the alias.
   alias_ptr alias = t->add_alias(usr_file_vec,used_variables);

   // Test that default node state is QUEUED
   BOOST_CHECK_MESSAGE(alias->state() == NState::QUEUED,"Expected initial state of QUEUED");

   // Test that CHILD specific attributes event, meter, labels are copied over from the task  AND that they are reset.
   BOOST_CHECK_MESSAGE(alias->meters().size() == 2,"Expected 2 meter to be copied from task but found " << alias->meters().size());
   BOOST_CHECK_MESSAGE(alias->events().size() == 2,"Expected 2 events to be copied from task but found " << alias->events().size());
   BOOST_CHECK_MESSAGE(alias->labels().size() == 2,"Expected 2 labels to be copied from task but found " << alias->labels().size());
   for(const Meter& meter :alias->meters()){BOOST_CHECK_MESSAGE(meter.value() == meter.min(),"Expected meter value " << meter.min() << " but found " << meter.value() << " for " << meter.dump());}
   for(const Event& event: alias->events()){BOOST_CHECK_MESSAGE(!event.value(),"Expected " << event.dump() << " to be clear");}
   for(const Label& label: alias->labels()){BOOST_CHECK_MESSAGE(label.new_value().empty(),"Expected " << label.dump() << " to be clear");}

   // Ensure Task state was not changed
   BOOST_CHECK_MESSAGE(t->meters().size() == 2,"Did not expect task state to change, expected 2 meter but found: " << t->meters().size());
   BOOST_CHECK_MESSAGE(t->events().size() == 2,"Did not expect task state to change, expected 2 events but found:" << t->events().size());
   BOOST_CHECK_MESSAGE(t->labels().size() == 2,"Did not expect task state to change, expected 2 labels but found:" << t->labels().size());
   for(const Meter& meter: t->meters()){BOOST_CHECK_MESSAGE(meter.value() == 10,"Expected meter value 10 but found " << meter.value() << " for " << meter.dump());}
   for(const Event& event: t->events()){BOOST_CHECK_MESSAGE(event.value(),"Expected " << event.dump() << " to be set");}
   for(const Label& label: t->labels()){BOOST_CHECK_MESSAGE(label.new_value() == "new_value","Expected label with 'new_value' but found " << label.new_value());}

   // test that user variables passed in got added as Variables
   BOOST_CHECK_MESSAGE(alias->variables().size() == 2,"Expected 2 variables to be create from input args but found " << alias->variables().size());

   // Test directory creation
   fs::path dir(ecf_home + "/" + t->absNodePath());
   BOOST_CHECK_MESSAGE(fs::exists(dir),"Expected directory to be created " + dir.string());

   // Test alias0.usr file creation
   fs::path usr_file(ecf_home + "/" + t->absNodePath() + "/" + "alias0.usr");
   BOOST_CHECK_MESSAGE(fs::exists(usr_file),"Expected alias0.usr file to be created " + usr_file.string());

   // Create another alias. This should get created as alias1.usr
   alias_ptr alias1 = t->add_alias(usr_file_vec,used_variables);
   fs::path usr_file1(ecf_home + "/" + t->absNodePath() + "/" + "alias1.usr");
   BOOST_CHECK_MESSAGE(fs::exists(usr_file1),"Expected alias1.usr file to be created " + usr_file1.string());

   // Test Defs::get_all_aliases()
   std::vector<alias_ptr> alias_vec;
   theDefs.get_all_aliases(alias_vec);
   BOOST_CHECK_MESSAGE(alias_vec.size() == 2,"Expected 2 aliases but found " << alias_vec.size());

   // Check alias remove
   for(alias_ptr al:alias_vec) { al->remove();}
   alias_vec.clear();
   theDefs.get_all_aliases(alias_vec);
   BOOST_CHECK_MESSAGE(alias_vec.empty(),"Expected no aliases but found " << alias_vec.size());

   // Cleanup by removing the created directory
   fs::remove_all(ecf_home);
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: mpartio/ecflow

max_issues_repo_name: mpartio/ecflow

max_forks_repo_name: mpartio/ecflow


text:
#include "queen_attack.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(queens_in_default_positions)
{
    const queen_attack::chess_board board;

    BOOST_REQUIRE_EQUAL(std::make_pair(0, 3).first, board.white().first);
    BOOST_REQUIRE_EQUAL(std::make_pair(0, 3).second, board.white().second);
    BOOST_REQUIRE_EQUAL(std::make_pair(7, 3).first, board.black().first);
    BOOST_REQUIRE_EQUAL(std::make_pair(7, 3).second, board.black().second);
}

#if defined(EXERCISM_RUN_ALL_TESTS)
BOOST_AUTO_TEST_CASE(initialized_with_specific_positions)
{
    const auto white = std::make_pair(3, 7);
    const auto black = std::make_pair(6, 1);
    const queen_attack::chess_board board{white, black};

    BOOST_REQUIRE_EQUAL(white.first, board.white().first);
    BOOST_REQUIRE_EQUAL(white.second, board.white().second);
    BOOST_REQUIRE_EQUAL(black.first, board.black().first);
    BOOST_REQUIRE_EQUAL(black.second, board.black().second);
}

BOOST_AUTO_TEST_CASE(queen_positions_must_be_distinct)
{
    const auto pos = std::make_pair(3, 7);
    BOOST_REQUIRE_THROW((queen_attack::chess_board{pos, pos}), std::domain_error);
}

BOOST_AUTO_TEST_CASE(string_representation)
{
    const queen_attack::chess_board board{std::make_pair(2, 4), std::make_pair(6, 6)};

    const std::string expected{
        "_ _ _ _ _ _ _ _\n"
        "_ _ _ _ _ _ _ _\n"
        "_ _ _ _ W _ _ _\n"
        "_ _ _ _ _ _ _ _\n"
        "_ _ _ _ _ _ _ _\n"
        "_ _ _ _ _ _ _ _\n"
        "_ _ _ _ _ _ B _\n"
        "_ _ _ _ _ _ _ _\n"};
    BOOST_REQUIRE_EQUAL(expected, static_cast<std::string>(board));
}

BOOST_AUTO_TEST_CASE(queens_cannot_attack)
{
    const queen_attack::chess_board board{std::make_pair(2, 3), std::make_pair(4, 7)};

    BOOST_REQUIRE(!board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_when_they_are_on_the_same_row)
{
    const queen_attack::chess_board board{std::make_pair(2, 4), std::make_pair(2, 7)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_when_they_are_on_the_same_column)
{
    const queen_attack::chess_board board{std::make_pair(5, 4), std::make_pair(2, 4)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_diagonally)
{
    const queen_attack::chess_board board{std::make_pair(1, 1), std::make_pair(6, 6)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_another_diagonally)
{
    const queen_attack::chess_board board{std::make_pair(0, 6), std::make_pair(1, 7)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_yet_another_diagonally)
{
    const queen_attack::chess_board board{std::make_pair(4, 1), std::make_pair(6, 3)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_can_attack_on_the_nw_so_diagonal)
{
    const queen_attack::chess_board board{std::make_pair(1, 6), std::make_pair(6, 1)};

    BOOST_REQUIRE(board.can_attack());
}

BOOST_AUTO_TEST_CASE(queens_cannot_attack_if_not_on_same_row_column_or_diagonal)
{
    const queen_attack::chess_board board{std::make_pair(1, 1), std::make_pair(3, 7)};

    BOOST_REQUIRE(!board.can_attack());
}
#endif

max_stars_repo_name: menaczar/cpp

max_issues_repo_name: menaczar/cpp

max_forks_repo_name: menaczar/cpp


text:
/**
 * @file cf_model.hpp
 * @author Wenhao Huang
 * @author Khizir Siddiqui
 *
 * A serializable CF model, used by the main program.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_CF_CF_MODEL_HPP
#define MLPACK_METHODS_CF_CF_MODEL_HPP

#include <mlpack/core.hpp>
#include <boost/variant.hpp>
#include "cf.hpp"

#include <mlpack/methods/cf/decomposition_policies/batch_svd_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/randomized_svd_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/regularized_svd_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/svd_complete_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/svd_incomplete_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/bias_svd_method.hpp>
#include <mlpack/methods/cf/decomposition_policies/svdplusplus_method.hpp>

#include <mlpack/methods/cf/normalization/no_normalization.hpp>
#include <mlpack/methods/cf/normalization/overall_mean_normalization.hpp>
#include <mlpack/methods/cf/normalization/user_mean_normalization.hpp>
#include <mlpack/methods/cf/normalization/item_mean_normalization.hpp>
#include <mlpack/methods/cf/normalization/z_score_normalization.hpp>

namespace mlpack {
namespace cf {

/**
 * DeleteVisitor deletes the CFType<> object which is pointed to by the
 * variable cf in class CFModel.
 */
class DeleteVisitor : public boost::static_visitor<void>
{
 public:
  //! Delete CFType object.
  template <typename DecompositionPolicy,
            typename NormalizationType = NoNormalization>
  void operator()(CFType<DecompositionPolicy, NormalizationType>* c) const;
};

/**
 * GetValueVisitor returns the pointer which points to the CFType object.
 */
class GetValueVisitor : public boost::static_visitor<void*>
{
 public:
  //! Return stored pointer as void* type.
  template <typename DecompositionPolicy,
            typename NormalizationType = NoNormalization>
  void* operator()(CFType<DecompositionPolicy, NormalizationType>* c) const;
};

/**
 * PredictVisitor uses the CFType object to make predictions on the given
 * combinations of users and items.
 */
template <typename NeighborSearchPolicy,
          typename InterpolationPolicy>
class PredictVisitor : public boost::static_visitor<void>
{
 private:
  //! User/item combinations to predict.
  const arma::Mat<size_t>& combinations;
  //! Predicted ratings for each user/item combination.
  arma::vec& predictions;

 public:
  //! Predict ratings for each user-item combination.
  template <typename DecompositionPolicy,
            typename NormalizationType = NoNormalization>
  void operator()(CFType<DecompositionPolicy, NormalizationType>* c) const;

  //! Visitor constructor.
  PredictVisitor(const arma::Mat<size_t>& combinations,
                 arma::vec& predictions);
};

/**
 * RecommendationVisitor uses the CFType object to get recommendations for the
 * given users.
 */
template <typename NeighborSearchPolicy,
          typename InterpolationPolicy>
class RecommendationVisitor : public boost::static_visitor<void>
{
 private:
  //! Number of Recommendations.
  const size_t numRecs;
  //! Recommendations matrix to save recommendations.
  arma::Mat<size_t>& recommendations;
  //! Users for which recommendations are to be generated.
  const arma::Col<size_t>& users;
  //! Whether users are given.
  const bool usersGiven;

 public:
  //! Visitor constructor.
  RecommendationVisitor(const size_t numRecs,
                        arma::Mat<size_t>& recommendations,
                        const arma::Col<size_t>& users,
                        const bool usersGiven);

  //! Generates the given number of recommendations.
  template <typename DecompositionPolicy,
            typename NormalizationType = NoNormalization>
  void operator()(CFType<DecompositionPolicy, NormalizationType>* c) const;
};

/**
 * The model to save to disk.
 */
class CFModel
{
 private:
  /**
   * cf holds an instance of the CFType class for the current
   * decompositionPolicy and normalizationType. It is initialized every time
   * Train() is executed. We access to the contained value through the visitor
   * classes defined above.
   */
  boost::variant<CFType<NMFPolicy, NoNormalization>*,
                 CFType<BatchSVDPolicy, NoNormalization>*,
                 CFType<RandomizedSVDPolicy, NoNormalization>*,
                 CFType<RegSVDPolicy, NoNormalization>*,
                 CFType<SVDCompletePolicy, NoNormalization>*,
                 CFType<SVDIncompletePolicy, NoNormalization>*,
                 CFType<BiasSVDPolicy, NoNormalization>*,
                 CFType<SVDPlusPlusPolicy, NoNormalization>*,

                 CFType<NMFPolicy, ItemMeanNormalization>*,
                 CFType<BatchSVDPolicy, ItemMeanNormalization>*,
                 CFType<RandomizedSVDPolicy, ItemMeanNormalization>*,
                 CFType<RegSVDPolicy, ItemMeanNormalization>*,
                 CFType<SVDCompletePolicy, ItemMeanNormalization>*,
                 CFType<SVDIncompletePolicy, ItemMeanNormalization>*,
                 CFType<BiasSVDPolicy, ItemMeanNormalization>*,
                 CFType<SVDPlusPlusPolicy, ItemMeanNormalization>*,

                 CFType<NMFPolicy, UserMeanNormalization>*,
                 CFType<BatchSVDPolicy, UserMeanNormalization>*,
                 CFType<RandomizedSVDPolicy, UserMeanNormalization>*,
                 CFType<RegSVDPolicy, UserMeanNormalization>*,
                 CFType<SVDCompletePolicy, UserMeanNormalization>*,
                 CFType<SVDIncompletePolicy, UserMeanNormalization>*,
                 CFType<BiasSVDPolicy, UserMeanNormalization>*,
                 CFType<SVDPlusPlusPolicy, UserMeanNormalization>*,

                 CFType<NMFPolicy, OverallMeanNormalization>*,
                 CFType<BatchSVDPolicy, OverallMeanNormalization>*,
                 CFType<RandomizedSVDPolicy, OverallMeanNormalization>*,
                 CFType<RegSVDPolicy, OverallMeanNormalization>*,
                 CFType<SVDCompletePolicy, OverallMeanNormalization>*,
                 CFType<SVDIncompletePolicy, OverallMeanNormalization>*,
                 CFType<BiasSVDPolicy, OverallMeanNormalization>*,
                 CFType<SVDPlusPlusPolicy, OverallMeanNormalization>*,

                 CFType<NMFPolicy, ZScoreNormalization>*,
                 CFType<BatchSVDPolicy, ZScoreNormalization>*,
                 CFType<RandomizedSVDPolicy, ZScoreNormalization>*,
                 CFType<RegSVDPolicy, ZScoreNormalization>*,
                 CFType<SVDCompletePolicy, ZScoreNormalization>*,
                 CFType<SVDIncompletePolicy, ZScoreNormalization>*,
                 CFType<BiasSVDPolicy, ZScoreNormalization>*,
                 CFType<SVDPlusPlusPolicy, ZScoreNormalization>*> cf;

 public:
  //! Create an empty CF model.
  CFModel() { }

  //! Clean up memory.
  ~CFModel();

  //! Get the pointer to CFType<> object.
  template <typename DecompositionPolicy,
            typename NormalizationType = NoNormalization>
  const CFType<DecompositionPolicy, NormalizationType>* CFPtr() const;

  //! Train the model.
  template<typename DecompositionPolicy,
           typename MatType>
  void Train(const MatType& data,
             const size_t numUsersForSimilarity,
             const size_t rank,
             const size_t maxIterations,
             const double minResidue,
             const bool mit,
             const std::string& normalizationType = "none");

  //! Make predictions.
  template <typename NeighborSearchPolicy,
            typename InterpolationPolicy>
  void Predict(const arma::Mat<size_t>& combinations,
               arma::vec& predictions);

  //! Compute recommendations for query users.
  template<typename NeighborSearchPolicy,
           typename InterpolationPolicy>
  void GetRecommendations(const size_t numRecs,
                          arma::Mat<size_t>& recommendations,
                          const arma::Col<size_t>& users);

  //! Compute recommendations for all users.
  template<typename NeighborSearchPolicy,
           typename InterpolationPolicy>
  void GetRecommendations(const size_t numRecs,
                          arma::Mat<size_t>& recommendations);

  //! Serialize the model.
  template<typename Archive>
  void serialize(Archive& ar, const unsigned int /* version */);
};

} // namespace cf
} // namespace mlpack

// Include implementation.
#include "cf_model_impl.hpp"

#endif

max_stars_repo_name: mhmohona/mlpack

max_issues_repo_name: mhmohona/mlpack

max_forks_repo_name: mhmohona/mlpack


text:
/*
The MIT License

Copyright (c) 2019 Mert Turanli

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.
*/

#include "ros/ros.h"
#include "hnn/AgentPosition.h"
#include "hnn/EstimationVector.h"

#include "hnn.h"

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_msgs/Float64.h>

#include <tf/LinearMath/Quaternion.h>
#include <tf/LinearMath/Matrix3x3.h>

#include <string>
#include <cstdlib>
#include <cstring>

#include <unistd.h>

#include <boost/thread/mutex.hpp>
#include <boost/timer/timer.hpp>

#include <time.h>

#include <stdlib.h>

boost::mutex mainMutex;

double pose[3] = {0};
double velocity[3] = {0};
double xt[2] = {0};

int poseReceived = 0;
int xtReceived = 0;

using namespace boost::timer;
using namespace std;

void xtCallback(const hnn::AgentPosition& msg) {

	ROS_INFO("xt data received\n");

	mainMutex.lock();

	xt[0] = msg.x;
	xt[1] = msg.y;

	xtReceived = 1;

	mainMutex.unlock();

	return;
}

void groundTruthCallback(const nav_msgs::Odometry& msg) {
	//ROS_INFO("position data received\n");

	mainMutex.lock();

	tf::Quaternion q( 	msg.pose.pose.orientation.x,
						msg.pose.pose.orientation.y,
						msg.pose.pose.orientation.z,
						msg.pose.pose.orientation.w );

	double yaw, pitch, roll;
	tf::Matrix3x3 mat(q);
	mat.getEulerYPR(yaw, pitch, roll);

	//

	pose[0] = msg.pose.pose.position.x;
	pose[1] = msg.pose.pose.position.y;
	pose[2] = yaw;

	velocity[0] = msg.twist.twist.linear.x;
	velocity[1] = msg.twist.twist.linear.y;
	velocity[2] = msg.twist.twist.angular.z;

	ROS_INFO(" ground truth callback");
	ROS_INFO("  x = %lf\n", pose[0]);
	ROS_INFO("  y = %lf\n", pose[1]);
	ROS_INFO("  theta = %lf\n", pose[2]);

	poseReceived = 1;

	mainMutex.unlock();

	return;
}

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

	ros::init(argc, argv, "hnn");
	ros::NodeHandle n("~");

	int agentId = atoi(argv[1]);
	double hfAlpha = 0, hfBeta = 0;

	n.getParam("hfAlpha", hfAlpha);
	n.getParam("hfBeta", hfBeta);

	HNN* hnn = new HNN(hfAlpha, hfBeta);

	std::cout << "Agent id: " << agentId << std::endl;
	std::cout << "hfAlpha: " << hfAlpha << std::endl;
	std::cout << "hfBeta: " << hfBeta << std::endl;

	vector<double> v(2);
	n.getParam("realVector", v);
	double vector[2] = {v[0], v[1]};
	hnn->setRealVector(vector);

	std::ostringstream conv;
	string strRobot("/robot");
	string str1;

	// ground truth
	conv.clear();
	conv.str("");
	conv << agentId + 1;
	str1 = strRobot + conv.str() + "/pose";
	ros::Subscriber subscriberGroundTruth = n.subscribe(str1, 1000, groundTruthCallback);

	// xt
	conv.clear();
	conv.str("");
	conv << agentId + 1;
	str1 = strRobot + conv.str() + "/xt";
	ros::Subscriber subscriberTrajectory = n.subscribe(str1, 1000, xtCallback);

	// est. err.
	str1 = strRobot + conv.str() + "/estErr";
	ros::Publisher estErrPub = n.advertise<std_msgs::Float64>(str1, 10);

	// est. vec.
	str1 = strRobot + conv.str() + "/estVector";
	ros::Publisher estVecPub = n.advertise<hnn::EstimationVector>(str1, 10);

	//

	double poseBuffer[3];
	double velocityBuffer[3];
	double xtBuffer[2];

	ros::Time currentTime;
	ros::Time lastTime = ros::Time::now();
	double dt = 0;

	// time
	ros::Rate r(100.0);

	// loop
	while (ros::ok()) {

		// get dt
		currentTime = ros::Time::now();
		dt = (currentTime - lastTime).toSec();

		mainMutex.lock();

		if (poseReceived == 1 && xtReceived == 1) {
			// ground truth
			poseBuffer[0] = pose[0];
			poseBuffer[1] = pose[1];
			poseBuffer[2] = pose[2];

			velocityBuffer[0] = velocity[0];
			velocityBuffer[1] = velocity[1];
			velocityBuffer[2] = velocity[2];

			xtBuffer[0] = xt[0];
			xtBuffer[1] = xt[1];
		}

		if (dt >= 0.1) {

			cout << "dt = " << dt << endl;

			// main algorithm
			if (poseReceived == 1 && xtReceived == 1) {

				ROS_INFO("Position (%d) = (%f, %f, %f)\n", agentId, poseBuffer[0], poseBuffer[1], poseBuffer[2]);
				ROS_INFO("Velocity (%d) = (%f, %f, %f)\n", agentId, velocityBuffer[0], velocityBuffer[1], velocityBuffer[2]);

				hnn->set(poseBuffer, velocityBuffer, xtBuffer);

				hnn->estimate();

				// publish est. err.
				std_msgs::Float64 estErr;
				estErr.data = hnn->getEstimationError();

				estErrPub.publish(estErr);

				// publish est. vector
				hnn::EstimationVector estVec;
				double Ki_est[2];

				hnn->getEstimationVector(Ki_est);

				estVec.data[0] = Ki_est[0];
				estVec.data[1] = Ki_est[1];

				estVecPub.publish(estVec);

			}

			//
			lastTime = currentTime;

		}

		mainMutex.unlock();

		// spin
		ros::spinOnce();

		// sleep
		r.sleep();
	}

	delete hnn;

	return 0;
}

max_stars_repo_name: mertturanli/hnn

max_issues_repo_name: mertturanli/hnn

max_forks_repo_name: mertturanli/hnn


text:
//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_LOOKUP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_LOOKUP_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD( lookup_
                         , (typename A0, typename A1)
                         , bs::avx2_
                         , bs::pack_<bd::ints32_<A0>, bs::avx_>
                         , bs::pack_<bd::ints32_<A1>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return _mm256_permutevar8x32_epi32(a0,a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD( lookup_
                         , (typename A0, typename A1)
                         , bs::avx2_
                         , bs::pack_<bd::single_<A0>, bs::avx_>
                         , bs::pack_<bd::ints32_<A1>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return _mm256_permutevar8x32_ps(a0,a1);
    }
  };

} } }

#endif

max_stars_repo_name: SylvainCorlay/pythran

max_issues_repo_name: SylvainCorlay/pythran

max_forks_repo_name: SylvainCorlay/pythran


text:
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/filters/crop_box.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/segmentation/sac_segmentation.h>
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/kdtree/kdtree.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
 #include <boost/thread/thread.hpp>
 #include <vector>
 #include <unordered_set>
 #include <string>
 #include <chrono>

int main()
 {
/*
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::io::loadPCDFile ("/home/sunzengpeng/Downloads/test/test.pcd", *cloud);
   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
   viewer.showCloud (cloud);
   while (!viewer.wasStopped ())
   {
   }
   return 0;
*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("/apollo/test/test.pcd", *cloud1);
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	int v1(0);
	int v2(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->addPointCloud(cloud1, "name", v1);
	viewer->addPointCloud(cloud1, "name2", v2);
	viewer->spin();
 }


text:
// Copyright 2015 National ICT Australia Limited (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <SgtCore/Random.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/weibull_distribution.hpp>

#include <ctime>

namespace Sgt
{
    namespace
    {
        boost::random::mt19937 sRng; // Random number generator.
    }

    void randSeedWithTime()
    {
        sRng.seed(static_cast<unsigned int>(std::time(nullptr)));
    }

    void randSeed(const unsigned int n)
    {
        sRng.seed(n);
    }

    double randDiscrete(const double probs[], int nProbs)
    {
        boost::random::discrete_distribution<> dist(&probs[0], &probs[nProbs]);
        return dist(sRng);
    }

    double randNormal(double mu, double sig)
    {
        typedef boost::random::normal_distribution<> DistType;
        static DistType dist(0.0, 1.0);
        return dist(sRng, DistType::param_type(mu, sig));
    }

    double randUniform(double lower, double upper)
    {
        typedef boost::random::uniform_real_distribution<> DistType;
        static DistType dist(0.0, 1.0);
        return dist(sRng,  DistType::param_type(lower, upper));
    }

    int randUniformInt(int lower, int upper)
    {
        typedef boost::random::uniform_int_distribution<> DistType;
        static DistType dist(0, 1);
        return dist(sRng,  DistType::param_type(lower, upper));
    }

    double randWeibull(double a, double b)
    {
        typedef boost::random::weibull_distribution<> DistType;
        static DistType dist(0.0, 1.0);
        return dist(sRng,  DistType::param_type(a, b));
    }
}

max_stars_repo_name: dexterurbane/SmartGridToolbox

max_issues_repo_name: dexterurbane/SmartGridToolbox

max_forks_repo_name: dexterurbane/SmartGridToolbox


text:
#ifndef DART_NEURAL_SNAPSHOT_HPP_
#define DART_NEURAL_SNAPSHOT_HPP_

#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "dart/neural/DifferentiableContactConstraint.hpp"
#include "dart/neural/NeuralConstants.hpp"
#include "dart/neural/NeuralUtils.hpp"
#include "dart/neural/WithRespectTo.hpp"
#include "dart/performance/PerformanceLog.hpp"
#include "dart/simulation/World.hpp"

namespace dart {

using namespace performance;

namespace neural {

class BackpropSnapshot
{
  friend class MappedBackpropSnapshot;

public:
  /// This saves a snapshot from a forward pass, with all the info we need in
  /// order to efficiently compute a backwards pass. Crucially, the positions
  /// must all be snapshots from before the timestep, yet this constructor must
  /// be called after the timestep.
  BackpropSnapshot(
      simulation::WorldPtr world,
      Eigen::VectorXs preStepPosition,
      Eigen::VectorXs preStepVelocity,
      Eigen::VectorXs preStepTorques,
      Eigen::VectorXs preConstraintVelocities,
      Eigen::VectorXs preStepLCPCache);

  /// This computes the implicit backprop without forming intermediate
  /// Jacobians. It takes a LossGradient with the position and velocity vectors
  /// filled it, though the loss with respect to torque is ignored and can be
  /// null. It returns a LossGradient with all three values filled in, position,
  /// velocity, and torque.
  void backprop(
      simulation::WorldPtr world,
      LossGradient& thisTimestepLoss,
      const LossGradient& nextTimestepLoss,
      PerformanceLog* perfLog = nullptr,
      bool exploreAlternateStrategies = false);

  /// This computes backprop in the high-level RL API's space, use `state` and
  /// `action` as the primitives we're taking gradients wrt to.
  LossGradientHighLevelAPI backpropState(
      simulation::WorldPtr world,
      const Eigen::VectorXs& nextTimestepStateLossGrad,
      PerformanceLog* perfLog = nullptr,
      bool exploreAlternateStrategies = false);

  /// This zeros out any components of the gradient that would want to push us
  /// out of the box-bounds encoded in the world for pos, vel, or force.
  void clipLossGradientsToBounds(
      simulation::WorldPtr world,
      Eigen::VectorXs& lossWrtPos,
      Eigen::VectorXs& lossWrtVel,
      Eigen::VectorXs& lossWrtForce);

  /// This computes and returns the whole vel-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getVelVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole pos-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getPosVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole force-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getControlForceVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole mass-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getMassVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole pos-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getPosPosJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole vel-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getVelPosJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the component of the pos-pos and pos-vel
  /// jacobians due to bounce approximation. For backprop, you don't actually
  /// need this matrix, you can compute backprop directly. This is here if you
  /// want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getBounceApproximationJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This returns the Jacobian for state_t -> state_{t+1}.
  Eigen::MatrixXs getStateJacobian(simulation::WorldPtr world);

  /// This returns the Jacobian for action_t -> state_{t+1}.
  Eigen::MatrixXs getActionJacobian(simulation::WorldPtr world);

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  Eigen::VectorXs getPreStepPosition();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  Eigen::VectorXs getPreStepVelocity();

  /// Returns a concatenated vector of all the joint torques that were applied
  /// during the forward pass, BEFORE the timestep.
  Eigen::VectorXs getPreStepTorques();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER integrating forward dynamics but BEFORE
  /// running the LCP.
  Eigen::VectorXs getPreConstraintVelocity();

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  Eigen::VectorXs getPostStepPosition();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  Eigen::VectorXs getPostStepVelocity();

  /// Returns a concatenated vector of all the joint torques that were applied
  /// during the forward pass, AFTER the timestep.
  Eigen::VectorXs getPostStepTorques();

  /// Returns the LCP's cached solution from before the step
  const Eigen::VectorXs& getPreStepLCPCache();

  /////////////////////////////////////////////////////////////////////////////
  /// Just public for testing
  /////////////////////////////////////////////////////////////////////////////

  /// This returns the A_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getClampingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the V_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getMassedClampingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the A_ub matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getUpperBoundConstraintMatrix(simulation::WorldPtr world);

  /// This returns the V_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getMassedUpperBoundConstraintMatrix(
      simulation::WorldPtr world);

  /// This returns the E matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getUpperBoundMappingMatrix();

  /// This returns the B matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getBouncingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the mass matrix for the whole world, a block diagonal
  /// concatenation of the skeleton mass matrices.
  Eigen::MatrixXs getMassMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This returns the inverse mass matrix for the whole world, a block diagonal
  /// concatenation of the skeleton inverse mass matrices.
  Eigen::MatrixXs getInvMassMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This is the subset of the A matrix from the original LCP that corresponds
  /// to clamping indices.
  Eigen::MatrixXs getClampingAMatrix();

  /// This returns the pos-C(pos,vel) Jacobian for the whole world, a block
  /// diagonal concatenation of the skeleton pos-C(pos,vel) Jacobians.
  Eigen::MatrixXs getPosCJacobian(simulation::WorldPtr world);

  /// This returns the vel-C(pos,vel) Jacobian for the whole world, a block
  /// diagonal concatenation of the skeleton vel-C(pos,vel) Jacobians.
  Eigen::MatrixXs getVelCJacobian(simulation::WorldPtr world);

  /// This computes and returns the whole vel-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole vel-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole pos-C(pos,vel) jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole pos-C(pos,vel) jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole force-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceForceVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole force-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersForceVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole mass-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceMassVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole mass-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersMassVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole pos-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosPosJacobian(
      simulation::WorldPtr world,
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes and returns the whole pos-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosPosJacobian(
      simulation::WorldPtr world, std::size_t subdivisions = 20);

  /// This computes and returns the whole vel-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelPosJacobian(
      simulation::WorldPtr world,
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes and returns the whole vel-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelPosJacobian(
      simulation::WorldPtr world, std::size_t subdivisions = 20);

  /// This computes and returns the whole wrt-vel jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the whole wrt-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the whole wrt-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the whole wrt-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the P_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getProjectionIntoClampsMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This replaces x with the result of M*x in place, without explicitly
  /// forming M
  Eigen::VectorXs implicitMultiplyByMassMatrix(
      simulation::WorldPtr world, const Eigen::VectorXs& x);

  /// This replaces x with the result of Minv*x in place, without explicitly
  /// forming Minv
  Eigen::VectorXs implicitMultiplyByInvMassMatrix(
      simulation::WorldPtr world, const Eigen::VectorXs& x);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchAnalytical(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchFiniteDifference(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchFiniteDifferenceRidders(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This predicts what the next velocity will be using our linear algebra
  /// formula. This is only here for testing, to compare it against the actual
  /// result of a timestep.
  ///
  /// The `morePreciseButSlower` flag tells this function to do brute force
  /// steps to get constraint matrices A_c and A_ub, rather than use 1st order
  /// approximations. This is important because when we're doing
  /// finite-differencing over tiny EPS (1e-9) then tiny errors in the 1st order
  /// approximations blow up to become huge errors in gradients.
  Eigen::VectorXs getAnalyticalNextV(
      simulation::WorldPtr world, bool morePreciseButSlower = false);

  /// This computes and returns the whole wrt-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  Eigen::MatrixXs getVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the whole wrt-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  Eigen::MatrixXs getPosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of constraint force, holding everyhing constant
  /// except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the analytical expression for the Jacobian of Q*b, holding b
  /// constant, if there are some upper-bound indices
  Eigen::MatrixXs dQ_WithUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::MatrixXs& E,
      Eigen::MatrixXs& A_c_ub_E,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);
  /// This returns the analytical expression for the Jacobian of Q^T*b, holding
  /// b constant, if there are some upper-bound indices
  Eigen::MatrixXs dQT_WithUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::MatrixXs& E,
      Eigen::MatrixXs& A_ub,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);
  /// This returns the analytical expression for the Jacobian of Q*b, holding b
  /// constant, if there are no upper-bound indices
  Eigen::MatrixXs dQ_WithoutUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);

  /// This returns the jacobian of Qb, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfQb(
      simulation::WorldPtr world,
      Eigen::VectorXs b,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This returns the jacobian of Qb, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfQb(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the vector of constants that get added to the diagonal of Q
  /// to guarantee that Q is full-rank
  Eigen::VectorXs getConstraintForceMixingDiagonal();

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt
  Eigen::MatrixXs getJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world,
      Eigen::VectorXs b,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt, by Ridders extrapolated finite differencing
  Eigen::MatrixXs
  finiteDifferenceRiddersJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt
  Eigen::MatrixXs getJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// Ridders extrapolated finite differencing
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPEstimatedOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// Ridders extrapolated finite differencing
  Eigen::MatrixXs
  finiteDifferenceRiddersJacobianOfLCPEstimatedOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the subset of the A matrix used by the original LCP for just
  /// the clamping constraints. It relates constraint force to constraint
  /// acceleration. It's a mass matrix, just in a weird frame.
  void computeLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Q,
      const Eigen::MatrixXs& A_c,
      const Eigen::MatrixXs& A_ub,
      const Eigen::MatrixXs& E);

  /// This returns the subset of the b vector used by the original LCP for just
  /// the clamping constraints. It's just the relative velocity at the clamping
  /// contact points.
  void computeLCPOffsetClampingSubset(
      simulation::WorldPtr world,
      Eigen::VectorXs& b,
      const Eigen::MatrixXs& A_c);

  /// This computes and returns an estimate of the constraint impulses for the
  /// clamping constraints. This is based on a linear approximation of the
  /// constraint impulses.
  Eigen::VectorXs estimateClampingConstraintImpulses(
      simulation::WorldPtr world,
      const Eigen::MatrixXs& A_c,
      const Eigen::MatrixXs& A_ub,
      const Eigen::MatrixXs& E);

  /// This returns the jacobian of P_c * v, holding everyhing constant except
  /// the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This computes and returns the jacobian of P_c * v by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world,
      Eigen::VectorXs v,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of P_c * v by Ridders extrapolated
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This returns the jacobian of M^{-1}(pos, inertia) * tau, holding
  /// everything constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfMinv(
      simulation::WorldPtr world, Eigen::VectorXs tau, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * tau by
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfMinv(
      simulation::WorldPtr world,
      Eigen::VectorXs tau,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * tau by
  /// Ridders extrapolated finite differences. This is SUPER SLOW, and is
  /// only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfMinv(
      simulation::WorldPtr world, Eigen::VectorXs tau, WithRespectTo* wrt);

  /// This returns the jacobian of M(pos, inertia) * v, holding
  /// everything constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfM(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M(pos, inertia) * v by
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfM(
      simulation::WorldPtr world,
      Eigen::VectorXs v,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of M(pos, inertia) * v by
  /// Ridders extrapolated finite differences. This is SUPER SLOW, and is
  /// only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfM(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This returns the jacobian of C(pos, inertia, vel), holding everything
  /// constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of C(pos, inertia, vel), holding everything
  /// constant except the value of WithRespectTo
  Eigen::MatrixXs computeJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the jacobian of C(pos, inertia, vel) by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the jacobian of C(pos, inertia, vel) by Ridders
  /// extrapolated finite differences. This is SUPER SLOW, and is only here
  /// for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of M^{-1}(pos, inertia) * (C(pos, inertia, vel)
  /// + mPreStepTorques), holding everything constant except the value of
  /// WithRespectTo
  Eigen::MatrixXs getJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * C(pos,
  /// inertia, vel) by finite differences. This is SUPER SLOW, and is only here
  /// for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = false);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * C(pos,
  /// inertia, vel) by Ridders extrapolated finite differences. This is SUPER
  /// SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns a fast approximation to A_c in the neighborhood of the
  /// original
  Eigen::MatrixXs estimateClampingConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// This returns a fast approximation to A_ub in the neighborhood of the
  /// original
  Eigen::MatrixXs estimateUpperBoundConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of A_c at the
  /// desired position.
  Eigen::MatrixXs getClampingConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of A_ub at the
  /// desired position.
  Eigen::MatrixXs getUpperBoundConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of E at the
  /// desired position.
  Eigen::MatrixXs getUpperBoundMappingMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of the bounce
  /// diagonals at the desired position.
  Eigen::VectorXs getBounceDiagonalsAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// This computes the Jacobian of A_c*f0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the finite difference Jacobian of A_c*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_c*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Using Ridders, so even slower.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the Jacobian of A_c^T*v0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This computes the finite difference Jacobian of A_c^T*v0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_c^T*v0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Using Ridders, so even slower.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This computes the Jacobian of A_ub*(E*f0) with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs E_f0);

  /// This computes the finite difference Jacobian of A_ub*E*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_ub*E*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the Jacobian of A_ub^T*v0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfUpperBoundConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This returns the jacobian of constraint force, holding everything constant
  /// except the value of WithRespectTo
  Eigen::MatrixXs finiteDifferenceJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of constraint force, holding everything constant
  /// except the value of WithRespectTo. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of estimated constraint force, without actually
  /// running forward passes, holding everyhing constant except the value of
  /// WithRespectTo
  Eigen::MatrixXs finiteDifferenceJacobianOfEstimatedConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of estimated constraint force, without actually
  /// running forward passes, holding everyhing constant except the value of
  /// WithRespectTo. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfEstimatedConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// These was the mX() vector used to construct this. Pretty much only here
  /// for testing.
  Eigen::VectorXs getContactConstraintImpulses();

  /// These was the fIndex() vector used to construct this. Pretty much only
  /// here for testing.
  Eigen::VectorXi getContactConstraintMappings();

  /// Returns the vector of the coefficients on the diagonal of the bounce
  /// matrix. These are 1+restitutionCoeff[i].
  Eigen::VectorXs getBounceDiagonals();

  /// Returns the vector of the restitution coeffs, sized for the number of
  /// bouncing collisions.
  Eigen::VectorXs getRestitutionDiagonals();

  /// Returns the penetration correction hack "bounce" (or 0 if the contact is
  /// not inter-penetrating or is actively bouncing) at each contact point.
  Eigen::VectorXs getPenetrationCorrectionVelocities();

  /// Returns the constraint impulses along the clamping constraints
  Eigen::VectorXs getClampingConstraintImpulses();

  /// Returns the relative velocities along the clamping constraints
  Eigen::VectorXs getClampingConstraintRelativeVels();

  /// Returns the velocity change caused by illegal impulses in the LCP this
  /// timestep
  Eigen::VectorXs getVelocityDueToIllegalImpulses();

  /// Returns the velocity pre-LCP
  Eigen::VectorXs getPreLCPVelocity();

  /// Returns true if there were any bounces in this snapshot.
  bool hasBounces();

  /// Returns the number of contacts (regardless of state) in this snapshot.
  std::size_t getNumContacts();

  /// Returns the number of clamping contacts in this snapshot.
  std::size_t getNumClamping();

  /// Returns the number of upper bound contacts in this snapshot.
  std::size_t getNumUpperBound();

  /// These are the gradient constraint matrices from the LCP solver
  std::vector<std::shared_ptr<ConstrainedGroupGradientMatrices>>
      mGradientMatrices;

  /// This is the clamping constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getDifferentiableConstraints();

  /// This is the clamping constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getClampingConstraints();

  /// This is the upper bound constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getUpperBoundConstraints();

  /// This verifies that the two matrices are equal to some tolerance, and if
  /// they're not it prints the information needed to replicated this scenario
  /// and it exits the program.
  void equalsOrCrash(
      std::shared_ptr<simulation::World> world,
      Eigen::MatrixXs analytical,
      Eigen::MatrixXs bruteForce,
      std::string name);

  /// This compares our analytical sub-Jacobians (like dMinv), to attempt to
  /// diagnose where there are differences creeping in between our finite
  /// differencing and our analytical results.
  void diagnoseSubJacobianErrors(
      std::shared_ptr<simulation::World> world, WithRespectTo* wrt);

  /// This prints code to the console to replicate a scenario for testing.
  void printReplicationInstructions(std::shared_ptr<simulation::World> world);

  /// Returns true if we were able to standardize our LCP results, false if we
  /// weren't
  bool areResultsStandardized() const;

  /// If this is true, we use finite-differencing to compute all of the
  /// requested Jacobians. This override can be useful to verify if there's a
  /// bug in the analytical Jacobians that's causing learning to not converge.
  void setUseFDOverride(bool override);

  /// If this is true, we check all Jacobians against their finite-differencing
  /// counterparts at runtime. If they aren't sufficiently close, we immediately
  /// crash the program and print what went wrong and some simple replication
  /// instructions.
  void setSlowDebugResultsAgainstFD(bool slowDebug);

  /// This does a battery of tests comparing the speeds to compute all the
  /// different Jacobians, both with finite differencing and analytically, and
  /// prints the results to std out.
  void benchmarkJacobians(
      std::shared_ptr<simulation::World> world, int numSamples);

protected:
  /// If this is true, we use finite-differencing to compute all of the
  /// requested Jacobians. This override can be useful to verify if there's a
  /// bug in the analytical Jacobians that's causing learning to not converge.
  bool mUseFDOverride;

  /// If this is true, we check all Jacobians against their finite-differencing
  /// counterparts at runtime. If they aren't sufficiently close, we immediately
  /// crash the program and print what went wrong and some simple replication
  /// instructions.
  bool mSlowDebugResultsAgainstFD;

  /// This is the global timestep length. This is included here because it shows
  /// up as a constant in some of the matrices.
  s_t mTimeStep;

  /// This is the total DOFs for this World
  std::size_t mNumDOFs;

  /// This is the number of total dimensions on all the constraints active in
  /// the world
  std::size_t mNumConstraintDim;

  /// This is the number of total constraint dimensions that are clamping
  std::size_t mNumClamping;

  /// This is the number of total constraint dimensions that are upper bounded
  std::size_t mNumUpperBound;

  /// This is the number of total constraint dimensions that are upper bounded
  std::size_t mNumBouncing;

  /// These are the offsets into the total degrees of freedom for each skeleton
  std::unordered_map<std::string, std::size_t> mSkeletonOffset;

  /// These are the number of degrees of freedom for each skeleton
  std::unordered_map<std::string, std::size_t> mSkeletonDofs;

  /// The position of all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepPosition;

  /// The velocities of all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepVelocity;

  /// The torques on all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepTorques;

  /// The LCP's initial cached value BEFORE the timestep
  Eigen::VectorXs mPreStepLCPCache;

  /// The velocities of all the DOFs of the world AFTER an unconstrained forward
  /// step, but BEFORE the LCP runs
  Eigen::VectorXs mPreConstraintVelocities;

  /// The position of all the DOFs of the world AFTER the timestep
  Eigen::VectorXs mPostStepPosition;

  /// The velocities of all the DOFs of the world AFTER the timestep
  /// created
  Eigen::VectorXs mPostStepVelocity;

  /// The torques on all the DOFs of the world AFTER the timestep
  Eigen::VectorXs mPostStepTorques;

private:
  /// These are mCached versions of the various Jacobians
  bool mCachedPosPosDirty;
  Eigen::MatrixXs mCachedPosPos;
  bool mCachedPosVelDirty;
  Eigen::MatrixXs mCachedPosVel;
  bool mCachedBounceApproximationDirty;
  Eigen::MatrixXs mCachedBounceApproximation;
  bool mCachedVelPosDirty;
  Eigen::MatrixXs mCachedVelPos;
  bool mCachedVelVelDirty;
  Eigen::MatrixXs mCachedVelVel;
  bool mCachedForcePosDirty;
  Eigen::MatrixXs mCachedForcePos;
  bool mCachedForceVelDirty;
  Eigen::MatrixXs mCachedForceVel;
  bool mCachedMassVelDirty;
  Eigen::MatrixXs mCachedMassVel;
  bool mCachedPosCDirty;
  Eigen::MatrixXs mCachedPosC;
  bool mCachedVelCDirty;
  Eigen::MatrixXs mCachedVelC;

  Eigen::VectorXs scratch(simulation::WorldPtr world);

  enum MatrixToAssemble
  {
    CLAMPING,
    MASSED_CLAMPING,
    UPPER_BOUND,
    MASSED_UPPER_BOUND,
    BOUNCING
  };

  Eigen::MatrixXs assembleMatrix(
      simulation::WorldPtr world, MatrixToAssemble whichMatrix);

  enum BlockDiagonalMatrixToAssemble
  {
    MASS,
    INV_MASS,
    POS_C,
    VEL_C
  };

  Eigen::MatrixXs assembleBlockDiagonalMatrix(
      simulation::WorldPtr world,
      BlockDiagonalMatrixToAssemble whichMatrix,
      bool forFiniteDifferencing = false);

  enum VectorToAssemble
  {
    CONTACT_CONSTRAINT_IMPULSES,
    CONTACT_CONSTRAINT_MAPPINGS,
    BOUNCE_DIAGONALS,
    RESTITUTION_DIAGONALS,
    PENETRATION_VELOCITY_HACK,
    CLAMPING_CONSTRAINT_IMPULSES,
    CLAMPING_CONSTRAINT_RELATIVE_VELS,
    VEL_DUE_TO_ILLEGAL,
    PRE_STEP_VEL,
    PRE_STEP_TAU,
    PRE_LCP_VEL,
    CFM_CONSTANTS
  };
  template <typename Vec>
  Vec assembleVector(VectorToAssemble whichVector);

  template <typename Vec>
  const Vec& getVectorToAssemble(
      std::shared_ptr<ConstrainedGroupGradientMatrices> matrices,
      VectorToAssemble whichVector);
};

using BackpropSnapshotPtr = std::shared_ptr<BackpropSnapshot>;

} // namespace neural
} // namespace dart

#endif