#include <cstring>
#include <iostream>
#include <map>
#include <fstream>
#include <vector>
#include <string>
#include <map>
#include <set>
#include <cmath>
#include <numeric>
#include <random>
#include <algorithm>
#include <thread>
#include <cassert>
#include <climits>

#include <boost/program_options.hpp>

#include "acc-rec.h"
#include "global.h"

using std::cout;
using std::ifstream;
using std::ofstream;
using std::vector;
using std::string;
using std::map;
using std::set;
using std::pair;
using std::make_pair;
using std::endl;
using std::thread;
using std::to_string;
using std::max;

namespace po = boost::program_options;

/********************************************************************************************************
*
* Simple function for string splitting
*
********************************************************************************************************/
vector<string> Split(const string& line, char separator)
{
	vector<string> parts;
	int prev_position = 0;

	for (int i = 0; i <= (int)line.length(); ++i) {
		if (i == (int)line.length() || line.at(i) == separator) {
			parts.push_back(line.substr(prev_position, i - prev_position));
			prev_position = i + 1;
		}
	}

	return parts;
}

/********************************************************************************************************
*
* Force vector to unit L2-norm.
*
********************************************************************************************************/
void norm(double **v, int item) {

	double n = 0.0;

	for (int i = 0; i < g_dim; ++i)
		n += SQUARE(v[item][i]);

	n = sqrt(n);

	for (int i = 0; i < g_dim; ++i)
		v[item][i] /= n;
}

/****************************************************************
*
* Simple function for string splitting.
*
****************************************************************/
void ReadUnaryCount(const string& fileName, const map<string, int>& item_nums, vector<double> *item_weights)
{
	ifstream ifs(fileName.c_str());

	cout << "STREAM1" << endl;

	while(ifs.good()) {

		string s;
		std::getline(ifs, s);

		if(s.length() == 0)
			continue;

		vector<string> parts = Split(s, '\t');

		cout << parts[0] << endl;

		if (item_nums.find(parts[0]) == item_nums.end())
			continue;

		int item_num = item_nums.at(parts[0]);
		int count = std::stod(parts[1]);

		item_weights->at(item_num) = count;

		cout << item_num << " " << count << endl;
	}
	ifs.close();
}

/****************************************************************
*
* Enumerate items features from a string.
*
****************************************************************/
void EnumFeatures(const vector<string>& tokens, map<string, int> *token_nums, vector<int>* features)
{
	for (auto token : tokens) {
		if (token.size()) {
			int token_idx = 0;

			if (token_nums->find(token) == token_nums->end()) {
				token_idx = token_nums->size();
				token_nums->insert(make_pair(token, token_idx));
				token_ids.push_back(token);
			}
			else {
				token_idx = token_nums->find(token)->second;
			}

			features->push_back(token_idx);
		}
	}
}

/****************************************************************
*
* Reads features of items from file.
*
****************************************************************/
void ReadItemFeatures(const string& fileName, const map<string, int>& item_nums, map<string, int>* token_nums, vector<vector<int> >* item_features)
{
	ifstream ifs(fileName.c_str());
	item_features->resize(g_item_count);

	while(ifs.good()) {

		string s;
		std::getline(ifs, s);

		if(s.length() == 0)
			continue;

		vector<string> parts = Split(s, '\t');

		if (item_nums.find(parts[0]) == item_nums.end())  // ITEM NOT FOUND
			continue;

		int item_num = item_nums.at(parts[0]);
		item_features->at(item_num) = vector<int>();

		vector<string> tokens = Split(parts[1], ',');
		EnumFeatures(tokens, token_nums, &(item_features->at(item_num)));
	}
	ifs.close();
}

/****************************************************************
*
* Remove bias from views PMI.
*
****************************************************************/
void RemoveBias(vector<ViewObject> *views)
{
	double sum = 0.0;

	for (auto view : *views)
		sum += view.PMI;

	double avg = sum / views->size();

	for (auto& view : *views)
		view.PMI -= avg;
}

/****************************************************************
*
* Remove bias from file.
*
****************************************************************/
void ReadViews(const string& fileName, vector<string> *item_ids, map<string, int> *item_nums, map<string, int> *token_nums, vector<ViewObject> *views)
{
	ifstream ifs(fileName.c_str());

	while(ifs.good()) {

		string s, item_id;
		std::getline(ifs, s);

		if(s.length() == 0)
			continue;

		vector<string> parts = Split(s, '\t');
		ViewObject view;

		if (not g_views_features) {
			item_id = parts[0];

			if (item_nums->find(item_id) == item_nums->end()) {
				int idx = item_nums->size();
				item_nums->insert(make_pair(item_id, idx));
			}

			item_id = parts[1];

			if (item_nums->find(item_id) == item_nums->end()) {
				int idx = item_nums->size();
				item_nums->insert(make_pair(item_id, idx));
			}

			view.Item1 = item_nums->at(parts[0]);
			view.Item2 = item_nums->at(parts[1]);
		}
		else {
			vector<string> tokens = Split(parts[0], ',');
			EnumFeatures(tokens, token_nums, &view.features1);

			tokens = Split(parts[1], ',');
			EnumFeatures(tokens, token_nums, &view.features2);
		}

		view.PMI = std::stod(parts[2]);

		views->push_back(view);
	}
	ifs.close();

	item_ids->resize(item_nums->size());

	for (auto item_iter : *item_nums)
		item_ids->at(item_iter.second) = item_iter.first;
}

/****************************************************************
*
* Read baskets from file
*
****************************************************************/
void ReadBaskets(const string& fileName, vector<Basket> *baskets, vector<string> *item_ids, map<string, int> *item_nums, set<int> *new_items)
{
	map<string, vector<int> > baskets_map;
	ifstream ifs(fileName.c_str());

	while(ifs.good()) {

		string s;
		std::getline(ifs, s);

		if(s.length() == 0)
			continue;

		vector<string> parts = Split(s, '\t');
		string sid = parts[0];
		string item_id = parts[1];

		if (item_nums->find(item_id) == item_nums->end()) {
			int idx = item_nums->size();
			item_nums->insert(make_pair(item_id, idx));
			if (new_items)
				 new_items->insert(idx);
		}

		if (baskets_map.find(sid) == baskets_map.end()) {
			baskets_map[sid] = vector<int>();
		}

		int item_num = item_nums->at(item_id);
		baskets_map[sid].push_back(item_num);
	}
	ifs.close();

	item_ids->resize(item_nums->size());

	for (auto item_iter : *item_nums)
		item_ids->at(item_iter.second) = item_iter.first;

	for (auto basket : baskets_map) {
		baskets->push_back(Basket());

		for (auto item_num : basket.second) {
			baskets->back().push_back(item_num);
			//items_with_counts.push_back(item_num);
		}
	}
}

/****************************************************************
*
* Safe function for sigmoid calculation.
*
****************************************************************/
double sigmoid(double x)
{
	if (x > -15.0) {
		return 1.0 / (1.0 + exp(-x));
	}
	else {
		return 0.0;
	}
}

/****************************************************************
*
* Calculates inner product of 2 vectors.
*
****************************************************************/
double inner_prod(double *a, double *b, int n)
{
	double prod = 0.0;

	for (int i = 0; i < n; ++i) {
		prod += a[i] * b[i];
	}

	return prod * SQUARE(g_c);
}

/****************************************************************
*
* Samples random negative items.
*
****************************************************************/
vector<int> GetNegativeItems(int count) {

	// FIXME! maybe not safe in multithreading

	vector<int> neg_items;

	for (int i = 0; i < count; ++i) {
		if (g_negative_sampling == 1) {
			int idx = std::rand() % g_items_with_counts.size();
			neg_items.push_back(g_items_with_counts[idx]);
		}
		else if (g_negative_sampling == 0) {
			//int item_num = std::rand() % g_item_count;
			//neg_items.push_back(item_num);
			int idx = std::rand() % g_train_items.size();  // sampling of negative items from train only!
			neg_items.push_back(g_train_items[idx]);
		}
	}

	return neg_items;
}

/****************************************************************
*
* Initializes matrix of (dim1 * dim2) sizes.
*
****************************************************************/
double** init_matrix(int dim1, int dim2)
{
	double **v = (double**)calloc(dim1, sizeof(double*));

	for (int i = 0; i < dim1; ++i)
		v[i] = (double*)calloc(dim2, sizeof(double));

	return v;
}

/****************************************************************
*
* Releases memory allocated for matrix.
*
****************************************************************/
void free_matrix(double **v, int dim1)
{
	for (int i = 0; i < dim1; ++i)
		free(v[i]);

	free(v);
}

/****************************************************************
*
* Calculates negative log-likelihood (NLL) for one object.
*
****************************************************************/
double CalcNLLObject(int item_k, int item_m, const vector<int>& neg_items, double **v_in, double **v_out)
{
	/***************/
	/* Update v_in */
	/***************/
	double nll = 0.0;

	// positive
	double p1 = sigmoid(-inner_prod(v_out[item_m], v_in[item_k], g_dim));
	p1 = LIMIT(p1);
	nll += -log(1 - p1);

	// negative
	for (int j = 0; j < (int)neg_items.size(); ++j) {
		int item_r = neg_items[j];
		double p2 = sigmoid(inner_prod(v_out[item_r], v_in[item_k], g_dim));
		p2 = LIMIT(p2);
		nll += -log(1 - p2);
	}

	return nll;
}

/****************************************************************
*
* Calculates part of NLL objective, required for multithreading.
*
****************************************************************/
void CalcNLLPart(const vector<pair<int, int> >& val_pairs, int from_idx, int to_idx, const vector<vector<int> >& neg_items, double **v_in, double **v_out, double *nll)
{
	*nll = 0.0;

	for (int i = from_idx; i < to_idx; ++i) {
		*nll += CalcNLLObject(val_pairs[i].first, val_pairs[i].second, neg_items[i], v_in, v_out);
	}
}

/****************************************************************
*
* Calculates NLL objective.
*
****************************************************************/
double CalcNLL(const vector<pair<int, int> >& val_pairs, const vector<vector<int> >& neg_items, double **v_in, double **v_out)
{
	vector<thread> threads;
	double nll = 0.0;
	double nlls[g_threads];

	for (int i = 0; i < g_threads; ++i) {
		int from_idx = (val_pairs.size() * i)/ g_threads;
		int to_idx =  (val_pairs.size() * (i + 1)) / g_threads;
		nlls[i] = 0;

		threads.push_back(thread(CalcNLLPart, val_pairs, from_idx, to_idx, neg_items, v_in, v_out, &nlls[i]));
	}

	for (int i = 0; i < g_threads; ++i) {
		threads[i].join();
		nll += nlls[i];
	}

	return nll / val_pairs.size();
}

/****************************************************************
*
* Calculates inner product of 2 items vectors, including features of items.
*
****************************************************************/
inline double calc_inner_prod(int item_m, int item_k, double **v_out, double **v_in)
{
	double prod = inner_prod(v_out[item_m], v_in[item_k], g_dim);

	if (g_use_features) {
		for (auto t : item_features[item_m])
			prod += inner_prod(w_out[t], v_in[item_k], g_dim);

		for (auto t : item_features[item_k])
			prod += inner_prod(v_out[item_m], w_in[t], g_dim);

		for (auto tm : item_features[item_m])
			for (auto tk : item_features[item_k])
				prod += inner_prod(w_out[tm], w_in[tk], g_dim);
	}

	return prod;
}

/**************************************************************************
*
* Calculates part of all inner product pairs, required for multithreading.
*
***************************************************************************/
void CalcInnerProdsPart(double **v_in, double **v_out, int from_idx, int to_idx, double **inner_prods)
{
	for (int i = from_idx; i < to_idx; ++i) {
		for (int j = 0; j < (int)g_item_count; ++j) {

			inner_prods[i][j] = calc_inner_prod(j, i, v_out, v_in);
		}
	}
}

/**************************************************************************
*
* Calculates all pair-wide inner products.
*
***************************************************************************/
void CalcInnerProds(double **v_in, double **v_out, double **inner_prods)
{
	vector<thread> threads;
	int recall_threads = 8; //g_threads

	for (int i = 0; i < recall_threads; ++i) {
		int from_idx = (g_item_count * i) / recall_threads;
		int to_idx =  (g_item_count * (i + 1)) / recall_threads;

		threads.push_back(thread(CalcInnerProdsPart, v_in, v_out, from_idx, to_idx, inner_prods));
	}

	for (int i = 0; i < recall_threads; ++i)
		threads[i].join();
}

/**************************************************************************
*
* Calculates recall@length at validation set.
*
***************************************************************************/
void CalcRecall(const vector<pair<int, int> >& val_pairs, int length, double **v_in, double **v_out, double *score, double *wscore)
{
	*score = 0;
	*wscore = 0.0;

	if (val_pairs.size() == 0)
		return;

	double **inner_prods = init_matrix(g_item_count, g_item_count);
	CalcInnerProds(v_in, v_out, inner_prods);

	double max_score = 0.0, max_wscore = 0.0;
	map<int, map<int, int> > pred_cache;

	for (auto r : val_pairs) {

		int query_item = r.first;

		if (pred_cache.find(query_item) == pred_cache.end()) {

			vector<pair<int, double> > candidates;

			for (int item = 0; item < g_item_count; ++item) {
				if (item != query_item)
					if (!g_new_items.count(item) || g_recommend_new)
						candidates.push_back(make_pair(item, inner_prods[query_item][item]));
			}

			sort(candidates.begin(), candidates.end(), [](pair<int, double> x, pair<int, double> y) { return x.second > y.second; });

			pred_cache[query_item] = map<int, int>();

			for (int i = 0; i < length; ++i) {
				pred_cache[query_item].insert(make_pair(candidates[i].first, i));
			}
		}

		if (pred_cache[r.first].count(r.second)) {
			*score += 1.0 / g_item_weights[r.second];

			int pos = pred_cache[r.first][r.second];
			*wscore += 1.0 / log(2 + pos) / g_item_weights[r.second];
		}

		max_score += 1.0 / g_item_weights[r.second];
		max_wscore += 1.0 / log(2) / g_item_weights[r.second];
	}

	free_matrix(inner_prods, g_item_count);

	*score /= max_score;
	*wscore /= max_wscore;
}

/**************************************************************************
*
* Calculates part of predictions, required for multi-threading.
*
***************************************************************************/
void GetPredictionsPart(double **v_in, double **v_out, int from_idx, int to_idx, cache_type *res)
{
	for (int query_item = from_idx; query_item < to_idx; ++query_item) {
		vector<pair<int, double> > candidates;

		for (int item = 0; item < g_item_count; ++item) {
			if (item != query_item) {
				double inner_prod = calc_inner_prod(item, query_item, v_out, v_in);
				candidates.push_back(make_pair(item, inner_prod));
			}
		}

		sort(candidates.begin(), candidates.end(), [](pair<int, double> x, pair<int, double> y) { return x.second > y.second; });
		res->insert(make_pair(query_item, vector<pair<int, double> >(candidates.begin(), candidates.begin() + 1000)));
	}
}

/**************************************************************************
*
* Calculates predictions for all items.
*
***************************************************************************/
void GetPredictions(const string filename, vector<string>& item_ids, double **v_in, double **v_out)
{
	cout << "calculating predictions..." << endl;

	int threads_cnt = 8; //g_threads
	vector<thread> threads;

	vector<cache_type> caches(threads_cnt);

	for (int i = 0; i < threads_cnt; ++i) {
		int from_idx = (g_item_count * i) / threads_cnt;
		int to_idx =  (g_item_count * (i + 1)) / threads_cnt;

		threads.push_back(thread(GetPredictionsPart, v_in, v_out, from_idx, to_idx, &caches[i]));
	}

	for (int i = 0; i < threads_cnt; ++i)
		threads[i].join();

	ofstream ofs(filename.c_str());

	for (int k = 0; k < threads_cnt; ++k) {
		int from_idx = (g_item_count * k) / threads_cnt;
		int to_idx =  (g_item_count * (k + 1)) / threads_cnt;

		for (int i = from_idx; i < to_idx; ++i) {
			ofs << item_ids[i] << "\t";

			for (int j = 0; j < (int)caches[k][i].size(); ++j) {
				if (j != 0) ofs << ",";
				ofs << item_ids[caches[k][i][j].first];
			}

			ofs << endl;
		}
	}

	ofs.close();
}

/**************************************************************************
*
* Update component of a vector via AdaGrad rule.
*
***************************************************************************/
void inline update_adagrad(double grad, int item, int k, double **v, double **grad_sq_sum)
{
	grad *= g_c;

	grad_sq_sum[item][k] += SQUARE(grad);
	v[item][k] -= (g_eta / g_c) * grad / sqrt(grad_sq_sum[item][k] + 1e-12);
}

/**************************************************************************
*
* Update embeddings from views objective.
*
***************************************************************************/
void UpdateViews(const ViewObject& view, int no_update_idx1, int no_update_idx2,
		double **v1, double **v2, double **sq_sum1, double **sq_sum2,
		double **w1, double **w2, double **w_sq_sum1, double **w_sq_sum2,
		double *mse)
{
	int item_k = 0, item_m = 0;
	double prod = 0.0;
	double emb1[g_dim], emb2[g_dim];

	double v_item_m_copy[g_dim];

	if (not g_views_features) {
		item_k = view.Item1;
		item_m = view.Item2;

		prod = inner_prod(v2[item_k], v1[item_m], g_dim);
		memcpy(v_item_m_copy, v1[item_m], g_dim * sizeof(double));
	}
	else {
		memset(emb1, 0, g_dim * sizeof(double));
		memset(emb2, 0, g_dim * sizeof(double));

		for (auto t : view.features1)
			for (int k = 0; k < g_dim; ++k)
				emb1[k] += w1[t][k];

		for (auto t : view.features2)
			for (int k = 0; k < g_dim; ++k)
				emb2[k] += w2[t][k];

		for (int k = 0; k < g_dim; ++k)
			prod += emb1[k] * emb2[k];
	}

	*mse = SQUARE(view.PMI - prod);

	/*
	*
	*
	*/

	double grad = 0;

	for (int k = 0; k < g_dim; ++k) {

		if (not g_views_features) {

			if (k != no_update_idx1) {
				grad = g_mult_mf * (prod - view.PMI) * v2[item_k][k] + g_lambda_2 * v1[item_m][k];
				update_adagrad(grad, item_m, k, v1, sq_sum1);
			}

			if (k != no_update_idx2) {
				grad = g_mult_mf * (prod - view.PMI) * v_item_m_copy[k] + g_lambda_2 * v2[item_m][k];
				update_adagrad(grad, item_k, k, v2, sq_sum2);
			}
		}
		else {
			for (auto t : view.features1) {
				grad = g_mult_mf * (prod - view.PMI) * emb2[k] + g_lambda_2 * w1[t][k];
				update_adagrad(grad, t, k, w1, w_sq_sum1);
			}

			for (auto t : view.features2) {
				grad = g_mult_mf * (prod - view.PMI) * emb1[k] + g_lambda_2 * w2[t][k];
				update_adagrad(grad, t, k, w2, w_sq_sum2);
			}
		}
	}

	if (g_norm_v_out) {
		norm(v1, item_m);
		norm(v2, item_k);
	}
}

/**************************************************************************
*
* Update embeddings for regularization part.
*
***************************************************************************/
void UpdateReg(double **v, double **grad_sq_sum, int item, vector<int>* v_ts)
{
	int age = g_time - v_ts->at(item);

	if (age > 1) {
		for (int k = 0; k < g_dim; ++k) {
			double grad = g_lambda_2 * v[item][k];
			double a;

			if (grad_sq_sum[item][k])
				a = g_eta * g_lambda_2 / sqrt(grad_sq_sum[item][k]);
			else
				a = g_eta * g_lambda_2;

			double factor = pow(1 - a, age - 1);

			v[item][k] *= factor;
			grad_sq_sum[item][k] += age * SQUARE(grad);
		}
	}

	v_ts->at(item) = g_time;
}

/**************************************************************************
*
* Update for multi-modal embeddings, ranking objective.
*
***************************************************************************/
void UpdateRank(int item_k, int item_m, const vector<Basket>& baskets, double **v_in, double **v_out, double **grad_in_sq_sum, double **grad_out_sq_sum, double *nll)
{
	/************************/
	/* Calc update for v_in */
	/************************/
	*nll = 0.0;
	vector<double> v_in_grad(g_dim, 0);
	map<int, double> inner_prods_cache;

	double prod_m = calc_inner_prod(item_m, item_k, v_out, v_in);
	inner_prods_cache[item_m] = prod_m;

	vector<int> neg_items = GetNegativeItems(g_neg_count);

	if (g_lambda_2) {
		UpdateReg(v_in, grad_in_sq_sum, item_k, &v_in_ts);
		UpdateReg(v_out, grad_out_sq_sum, item_m, &v_out_ts);

		for (auto item_r : neg_items)
			UpdateReg(v_out, grad_out_sq_sum, item_r, &v_out_ts);
	}

	for (auto item_r : neg_items) {
		double prod_r = calc_inner_prod(item_r, item_k, v_out, v_in);
		inner_prods_cache[item_r] = prod_r;

		double p = sigmoid(prod_r - prod_m);
		p = LIMIT(p);
		*nll += -log(1 - p);

		for (int k = 0; k < g_dim; ++k)
			v_in_grad[k] += p * (v_out[item_r][k] - v_out[item_m][k]);

		if (g_use_features) {
			for (auto t : item_features[item_r])
				for (int k = 0; k < g_dim; ++k)
					v_in_grad[k] += p * w_out[t][k];

			for (auto t : item_features[item_m])
				for (int k = 0; k < g_dim; ++k)
					v_in_grad[k] += -p * w_out[t][k];
		}
	}

	/****************/
	/* Update v_in  */
	/****************/
	double v_in_copy[g_dim];
	memcpy(v_in_copy, v_in[item_k], g_dim * sizeof(double));

	if (g_use_features) {
		for (auto t : item_features[item_k])
			for (int k = 0; k < g_dim; ++k)
				v_in_copy[k] += w_in[t][k];
	}

	for (int k = 0; k < g_dim; ++k) {
		if (k == V_IN_CONST_IDX)
			continue;

		double grad = g_mult_st * v_in_grad[k] + g_lambda_2 * v_in[item_k][k];
		update_adagrad(grad, item_k, k, v_in, grad_in_sq_sum);
	}

	// Update features
	if (g_use_features) {
		for (auto t : item_features[item_k]) {
			for (int k = 0; k < g_dim; ++k) {
				double grad = g_mult_st * v_in_grad[k] + g_lambda_2 * w_in[t][k];
				update_adagrad(grad, t, k, w_in, grad_w_in_sq_sum);
			}
		}
	}

	/*****************/
	/* Update v_out  */
	/*****************/
	for (auto item_r : neg_items) {
		double p = sigmoid(inner_prods_cache[item_r] - inner_prods_cache[item_m]);

		for (int k = 0; k < g_dim; ++k) {
			if (k == V_OUT_CONST_IDX)
				continue;

			// update v_out for item_m
			double grad = -g_mult_st * p * v_in_copy[k] + g_lambda_2 * v_out[item_m][k];
			update_adagrad(grad, item_m, k, v_out, grad_out_sq_sum);

			if (g_use_features) {
				for (auto t : item_features[item_m])
					update_adagrad(grad, t, k, w_out, grad_w_out_sq_sum);
			}

			// update v_out for negative samples
			grad = g_mult_st * p * v_in_copy[k] + g_lambda_2 * v_out[item_r][k];
			update_adagrad(grad, item_r, k, v_out, grad_out_sq_sum);

			if (g_use_features)  {
				for (auto t : item_features[item_r])
					update_adagrad(grad, t, k, w_out, grad_w_out_sq_sum);
			}
		}

		// FIXME !!!
		//if (g_norm_v_out) {
			norm(v_out, item_r);
			norm(v_out, item_m);
		//}
	}
}

/**************************************************************************
*
* Update for multi-modal embeddings,  OBSOLETE
*
***************************************************************************/
void UpdateClass(int item_k, int item_m, const vector<Basket>& baskets, double **v_in, double **v_out, double **grad_in_sq_sum, double **grad_out_sq_sum, double *nll)
{
	/***************/
	/* Update v_in */
	/***************/
	*nll = 0.0;
	vector<double> v_in_grad(g_dim, 0);
	map<int, double> inner_prods_cache;

	// positive examples
	double prod = inner_prod(v_out[item_m], v_in[item_k], g_dim);
	inner_prods_cache[item_m] = prod;

	double p1 = sigmoid(-prod);
	p1 = LIMIT(p1);
	*nll += -log(1 - p1);

	for (int k = 0; k < g_dim; ++k) {
		v_in_grad[k] = -p1 * v_out[item_m][k];
	}

	// negative examples
	vector<int> neg_items = GetNegativeItems(g_neg_count);

	if (g_lambda_2) {
		UpdateReg(v_in, grad_in_sq_sum, item_k, &v_in_ts);
		UpdateReg(v_out, grad_out_sq_sum, item_m, &v_out_ts);

		for (auto item_r : neg_items)
			UpdateReg(v_out, grad_out_sq_sum, item_r, &v_out_ts);
	}

	for (int j = 0; j < (int)neg_items.size(); ++j) {
		int item_r = neg_items[j];

		prod = inner_prod(v_out[item_r], v_in[item_k], g_dim);
		inner_prods_cache[item_r] = prod;

		double p2 = sigmoid(prod);
		p2 = LIMIT(p2);
		*nll += -log(1 - p2);

		for (int k = 0; k < g_dim; ++k) {
			v_in_grad[k] += g_dw * p2 * v_out[item_r][k];

		}
	}

	/****************/
	/* Update v_out */
	/****************/
	map<int, int> pos_updates;
	map<int, int> neg_updates;
	map<int, int> all_updates;

	// positive examples
	INCREMENT(pos_updates, item_m);
	all_updates[item_m] = 1;

	// negative examples
	for (int j = 0; j < (int)neg_items.size(); ++j) {

		int item_r = neg_items[j];
		INCREMENT(neg_updates, item_r);
		all_updates[item_r] = 1;
	}

	// update v_in
	double v_in_copy[g_dim];
	memcpy(v_in_copy, v_in[item_k], g_dim * sizeof(double));

	for (int k = 0; k < g_dim; ++k) {
		if (k == V_IN_CONST_IDX)
			continue;

		double grad = v_in_grad[k] + g_lambda_2 * v_in[item_k][k];
		grad *= g_mult_st;

		//grad_in_sq_sum[item_k][k] += SQUARE(grad);
		//v_in[item_k][k] -= (g_eta / g_c) * grad / sqrt(grad_in_sq_sum[item_k][k] + 1e-12);

		update_adagrad(grad, item_k, k, v_in, grad_in_sq_sum);
	}

	// update v_out
	for (auto r : all_updates) {

		int item_p = r.first; // item to predict
		int pos_count = GET(pos_updates, item_p, 0);
		int neg_count = GET(neg_updates, item_p, 0);
		double p1 = 0.0, p2 = 0.0;

		if (pos_count) {
			p1 = sigmoid(-inner_prods_cache[item_p]);
			p1 = LIMIT(p1);
		}

		if (neg_count) {
			p2 = sigmoid(inner_prods_cache[item_p]);
			p2 = LIMIT(p2);
		}

		for (int k = 0; k < g_dim; ++k) {

			if (k == V_OUT_CONST_IDX)
				continue;

			double grad = (-p1 * pos_count + g_dw * p2 * neg_count) * v_in_copy[k] + g_lambda_2 * v_out[item_p][k];
			grad *= g_mult_st;

			//grad_out_sq_sum[item_p][k] += SQUARE(grad);
			//v_out[item_p][k] -= (g_eta / g_c) * grad / sqrt(grad_out_sq_sum[item_p][k] + 1e-12);

			update_adagrad(grad, item_p, k, v_out, grad_out_sq_sum);
		}
	}
}

/**************************************************************************
*
* Update for multi-modal embeddings,  OBSOLETE
*
***************************************************************************/
void Update(int basket_idx, const vector<Basket>& baskets, double **v_in, double **v_out, double **grad_in_sq_sum, double **grad_out_sq_sum, double *nll)
{
	/***************/
	/* Update v_in */
	/***************/
	int basket_size = (int)baskets[basket_idx].size();
	int item_idx = std::rand() % basket_size;
	int item_k = baskets[basket_idx][item_idx]; // center item

	*nll = 0.0;
	vector<double> v_in_grad(g_dim, 0);

	for (int i = 0; i < basket_size; ++i) {

		if (i != item_idx) {

			int item_m = baskets[basket_idx][i]; // item to predict
			double p1 = sigmoid(-inner_prod(v_out[item_m], v_in[item_k], g_dim));
			p1 = LIMIT(p1);
			*nll += -log(1 - p1);

			for (int k = 0; k < g_dim; ++k) {
				v_in_grad[k] = -p1 * v_out[item_m][k];
				//v_in[item_k][k] -= -g_eta * p_1 * v_out[item_m][k];
			}
		}
	}

	vector<int> neg_items = GetNegativeItems(g_neg_count * (basket_size - 1));

	for (int j = 0; j < (int)neg_items.size(); ++j) {
		int item_r = neg_items[j];
		double p2 = sigmoid(inner_prod(v_out[item_r], v_in[item_k], g_dim));
		p2 = LIMIT(p2);
		*nll += -log(1 - p2);

		for (int k = 0; k < g_dim; ++k) {
			v_in_grad[k] += p2 * v_out[item_r][k];
			//v_in[item_k][k] -= g_eta * p_2 * v_out[item_r][k];
		}
	}

	for (int k = 0; k < g_dim; ++k) {
		if (k == V_IN_CONST_IDX)
			continue;

		//grad_in_sq_sum[item_k][k] = GAMMA * grad_in_sq_sum[item_k][k] + (1 - GAMMA) * SQUARE(v_in_grad[k]);
		grad_in_sq_sum[item_k][k] += SQUARE(v_in_grad[k]);
		v_in[item_k][k] -= g_eta * v_in_grad[k] / sqrt(grad_in_sq_sum[item_k][k] + 1e-12);

		//v_in[item_k][k] -= g_eta * v_in_grad[k];
	}

	/***************/
	/* Update v_in */
	/***************/
	map<int, int> pos_updates;
	map<int, int> neg_updates;
	map<int, int> all_updates;

	for (int i = 0; i < (int)baskets[basket_idx].size(); ++i) {

		int item_m = baskets[basket_idx][i]; // item to predict
		//double p_1 = sigmoid(-inner_prod(v_out[item_m], v_in[item_k], g_dim));
		//p_1 = LIMIT(p_1);

		if (i != item_idx) {

			INCREMENT(pos_updates, item_m);
			all_updates[item_m] = 1;

			//for (int k = 0; k < g_dim; ++k) {
			//	v_out[item_m][k] -= -g_eta * p_1 * v_in[item_k][k];
			//}
		}
	}

	for (int j = 0; j < (int)neg_items.size(); ++j) {
		int item_r = neg_items[j];
		//double p_2 = sigmoid(inner_prod(v_out[item_r], v_in[item_k], g_dim));
		//p_2 = LIMIT(p_2);

		INCREMENT(neg_updates, item_r);
		all_updates[item_r] = 1;

		//for (int k = 0; k < g_dim; ++k) {
		//	v_out[item_r][k] -= g_eta * p_2 * v_in[item_k][k];
		//}
	}

	for (auto r : all_updates) {

		int item_p = r.first; // item to predict
		int pos_count = GET(pos_updates, item_p, 0);
		int neg_count = GET(neg_updates, item_p, 0);
		double p1 = 0.0, p2 = 0.0;

		if (pos_count) {
			p1 = sigmoid(-inner_prod(v_out[item_p], v_in[item_k], g_dim));
			p1 = LIMIT(p1);
		}

		if (neg_count) {
			p2 = sigmoid(inner_prod(v_out[item_p], v_in[item_k], g_dim));
			p2 = LIMIT(p2);
		}

		for (int k = 0; k < g_dim; ++k) { // NB!!! start from 1

			if (k == V_OUT_CONST_IDX)
				continue;

			double grad = (-p1 * pos_count + p2 * neg_count) * v_in[item_k][k];

			//grad_out_sq_sum[item_p][k] = GAMMA * grad_out_sq_sum[item_p][k] + (1 - GAMMA) * SQUARE(grad);
			grad_out_sq_sum[item_p][k] += SQUARE(grad);
			v_out[item_p][k] -= g_eta * grad / sqrt(grad_out_sq_sum[item_p][k] + 1e-12) ;

			//v_out[item_p][k] -= g_eta * grad;
		}
	}
}

/**************************************************************************
*
* Generate train pairs from baskets.
*
***************************************************************************/
void GetTrainPairs(const vector<Basket>& baskets, vector<pair<int, int> > *train_pairs)
{
	for (auto basket : baskets) {
		if (basket.size() >= 2) {
			for (auto item_num1 : basket) {
				for (auto item_num2: basket) {
					if (item_num1 != item_num2) {
						train_pairs->push_back(make_pair(item_num1, item_num2));
					}
				}
			}
		}
	}
}

/**************************************************************************
*
* Calculate improvement for quality score.
*
***************************************************************************/
double GetImprovement()
{
	if ((int)g_scores.size() < g_stop_count + 1)
		return 1.0;

	double prev_max = *std::max_element(g_scores.begin(), g_scores.end() - g_stop_count);
	double max = *std::max_element(g_scores.begin(), g_scores.end());

	if (prev_max)
		return max / prev_max - 1;
	else
		return 0.0;
}

/**************************************************************************
*
* Function to be run in a separate thread for updateing weights.
*
***************************************************************************/
void async_updater(int thread_num, const vector<Basket>& baskets, const vector<pair<int, int> >& train_pairs, const vector<ViewObject>& views, int samples,
          double **v_in, double **v_out, double **grad_in_sq_sum, double **grad_out_sq_sum,
          double **v_in2, double **v_out2, double **grad_in_sq_sum2, double **grad_out_sq_sum2,
          double *nll, int *nll_count, double *mse, int *mse_count)
{
	*nll = 0;
	*mse = 0;
	*mse_count = 0;
	*nll_count = 0;
	std::uniform_int_distribution<int> distr1(0, views.size() - 1);
	std::uniform_int_distribution<int> distr2(0, train_pairs.size() - 1);
	std::uniform_int_distribution<int> distr3(0, g_item_count - 1);

	std::bernoulli_distribution bad_coin(g_views_importance / (1 + g_views_importance));
	std::bernoulli_distribution bad_coin_ni(g_ni);

	std::bernoulli_distribution bad_coin2(g_fake_ratio);

	// multithreading ???? max samples ???
	if (g_views_sample)
		samples *= (1 + g_views_importance);

	int i = 0;

	while (i < samples) {

		if (g_stop_threads)
			break;

		int neg_updates = 1, views_updates = 1;

		if (g_views_sample == 0) { // no views
			neg_updates = 1;
			views_updates = 0;
		}
		else {
			if (bad_coin(g_generators[thread_num])) {
				views_updates = 1;
				neg_updates = 0;
			}
			else {
				views_updates = 0;
				neg_updates = 1;
			}
		}

		double part_nll = 0, part_mse = 0;

		for (int j = 0; j < views_updates; ++j) {

			int ni = bad_coin_ni(g_generators[thread_num]);

			if (ni) {
				int view_idx = distr1(g_generators[thread_num]);
				UpdateViews(views[view_idx], V_IN_CONST_IDX, -1, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, w_in, w_out, grad_w_in_sq_sum, grad_w_out_sq_sum, &part_mse);

				*mse += part_mse;
				(*mse_count)++;
			}
			else {

				int view_idx = distr1(g_generators[thread_num]);

				if (!g_simmetric_views) {
					if (g_lambda_2) {
						UpdateReg(v_in, grad_in_sq_sum, views[view_idx].Item1, &v_in_ts);
						UpdateReg(v_in2, grad_in_sq_sum2, views[view_idx].Item2, &v_in2_ts);
					}

					UpdateViews(views[view_idx], V_IN_CONST_IDX, -1, v_in, v_in2, grad_in_sq_sum, grad_in_sq_sum2, w_in, w_in2, grad_w_in_sq_sum, grad_w_in_sq_sum2, &part_mse);
				}
				else {
					UpdateViews(views[view_idx], V_IN_CONST_IDX, V_IN_CONST_IDX, v_in, v_in, grad_in_sq_sum, grad_in_sq_sum, w_in, w_in, grad_w_in_sq_sum, grad_w_in_sq_sum, &part_mse);
				}

				*mse += part_mse;
				(*mse_count)++;

				view_idx = distr1(g_generators[thread_num]);

				if (!g_simmetric_views) {
					if (g_lambda_2) {
						UpdateReg(v_out, grad_out_sq_sum, views[view_idx].Item1, &v_out_ts);
						UpdateReg(v_out2, grad_out_sq_sum2, views[view_idx].Item2, &v_out2_ts);
					}

					UpdateViews(views[view_idx], V_OUT_CONST_IDX, -1, v_out, v_out2, grad_out_sq_sum, grad_out_sq_sum2, w_out, w_out2, grad_w_out_sq_sum, grad_w_out_sq_sum2, &part_mse);
				}
				else
					UpdateViews(views[view_idx], V_OUT_CONST_IDX, V_OUT_CONST_IDX, v_out, v_out, grad_out_sq_sum, grad_out_sq_sum, w_out, w_out, grad_w_out_sq_sum, grad_w_out_sq_sum, &part_mse);

				*mse += part_mse;
				(*mse_count)++;
			}
		}

		for (int j = 0; j < neg_updates; ++j) {
			int pair_idx = distr2(g_generators[thread_num]);

			if (!g_rank)
				UpdateClass(train_pairs[pair_idx].first, train_pairs[pair_idx].second, baskets, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, &part_nll);
			else
				UpdateRank(train_pairs[pair_idx].first, train_pairs[pair_idx].second, baskets, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, &part_nll);

			//if (g_l2)
			//	g_c *= (1 - g_l2 / train_pairs.size());
		}

		int cnt;

		if (g_fake_ratio) {
			if (g_fake_ratio < 1)
				cnt = (int)bad_coin2(g_generators[thread_num]);
			else
				cnt = (int)g_fake_ratio;

			for (int m = 0; m < cnt; ++m) {
				int fake_item1 = distr3(g_generators[thread_num]);
				int fake_item2 = distr3(g_generators[thread_num]);

				if (!g_rank)
					UpdateClass(fake_item1, fake_item2, baskets, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, &part_nll);
				else
					UpdateRank(fake_item1, fake_item2, baskets, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, &part_nll);
			}
		}

		/*if (g_l2) {
			for (int i = 0; i < g_item_count; ++i) {
				for (int k = 0; k < g_dim; ++k) {
					double grad;

					grad = g_l2 * v_in[i][k];
					update_adagrad(grad, i, k, v_in, grad_in_sq_sum);

					grad = g_l2 * v_out[i][k];
					update_adagrad(grad, i, k, v_out, grad_out_sq_sum);

					grad = g_l2 * v_in2[i][k];
					update_adagrad(grad, i, k, v_in2, grad_in_sq_sum2);

					grad = g_l2 * v_out2[i][k];
					update_adagrad(grad, i, k, v_out2, grad_out_sq_sum2);
				}
			}
		}*/

		*nll += part_nll;
		(*nll_count)++;
		g_time++;
		i++;
	}

	for (int i = 0; i < g_item_count; ++i) {
		for (int k = 0; k < g_dim; ++k) {
			v_in[i][k] *= g_c;
			v_out[i][k] *= g_c;
			v_in2[i][k] *= g_c;
			v_out2[i][k] *= g_c;
		}
	}

	g_c = 1;
}

/**************************************************************************
*
* Train multi-modal embeddings.
*
**************************************************************************/
void FitMME(const vector<Basket>& baskets, const vector<Basket>& valBaskets, const vector<ViewObject>& views, double ***p_v_in, double ***p_v_out)
{
	double **v_in = init_matrix(g_item_count, g_dim);
	double **v_out = init_matrix(g_item_count, g_dim);
	double **grad_in_sq_sum = init_matrix(g_item_count, g_dim);
	double **grad_out_sq_sum = init_matrix(g_item_count, g_dim);

	double **v_in2 = init_matrix(g_item_count, g_dim);
	double **v_out2 = init_matrix(g_item_count, g_dim);
	double **grad_in_sq_sum2 = init_matrix(g_item_count, g_dim);
	double **grad_out_sq_sum2 = init_matrix(g_item_count, g_dim);

	v_in_ts = vector<int>(g_item_count, 0);
	v_out_ts = vector<int>(g_item_count, 0);
	v_in2_ts = vector<int>(g_item_count, 0);
	v_out2_ts = vector<int>(g_item_count, 0);

	if (g_use_features || g_views_features) {
		int size = token_nums.size();

		w_in = init_matrix(size, g_dim);
		w_out = init_matrix(size, g_dim);
		grad_w_in_sq_sum = init_matrix(size, g_dim);
		grad_w_out_sq_sum = init_matrix(size, g_dim);

		w_in2 = init_matrix(size, g_dim);
		w_out2 = init_matrix(size, g_dim);
		grad_w_in_sq_sum2 = init_matrix(size, g_dim);
		grad_w_out_sq_sum2 = init_matrix(size, g_dim);
	}

	*p_v_in = v_in;
	*p_v_out = v_out;

	std::default_random_engine generator;
	std::normal_distribution<double> distribution(0.0, 0.1);

	for (int i = 0; i < g_item_count; ++i) {
		for (int j = 0; j < g_dim; ++j) {
			v_in[i][j] = distribution(generator);
			v_out[i][j] = distribution(generator);
		}

		if (V_IN_CONST_IDX > 0)
			v_in[i][V_IN_CONST_IDX] = 1;

		if (V_OUT_CONST_IDX > 0)
			v_out[i][V_OUT_CONST_IDX] = 1;

		//v_in2[i][1 - V_IN_CONST_IDX] = 1;
		//v_out2[i][1 - V_OUT_CONST_IDX] = 1;
	}

	if (g_use_features || g_views_features) {
		int size = token_nums.size();

		for (int i = 0; i < size; ++i) {
			for (int j = 0; j < g_dim; ++j) {
				w_in[i][j] = distribution(generator);
				w_out[i][j] = distribution(generator);
			}
		}
	}

/*	vector<pair<int, int> > all_items_sessions;

	for (int i = 0; i < (int)baskets.size(); ++i) {
		for (int j = 0; j < (int)baskets[i].size(); ++j) {
			all_items_sessions.push_back(pair<int, int>(i, j));
		}
	}

	cout << all_items_sessions.size() << endl; */

	vector<pair<int, int> > train_pairs;
	GetTrainPairs(baskets, &train_pairs);

	assert(train_pairs.size() != 0);

	vector<pair<int, int> > val_pairs;
	vector<vector<int> > val_neg_items;

	for (int repl = 0; repl < 3; ++repl) {
		GetTrainPairs(valBaskets, &val_pairs);

		for (int i = 0; i < (int)val_pairs.size(); ++i) {
			val_neg_items.push_back(GetNegativeItems(g_neg_count));
		}
	}

	cout << "train pairs " << train_pairs.size() << endl;

	for (int i = 0; i < g_threads; ++i)
		g_generators.push_back(std::default_random_engine(i));

	for (int epoch = 0; epoch < g_iterations_max; ++epoch) {
		vector<thread> threads;
		int nlls_count[g_threads], mses_count[g_threads];
		double nlls[g_threads], mses[g_threads];

		g_stop_threads = false;

		for (int i = 0; i < g_threads; ++i) {
			nlls[i] = 0;

			int samples = (i == 0 ? train_pairs.size() / g_threads : INT_MAX);
			samples = max(samples, 1);

			threads.push_back(thread(async_updater, i, baskets, train_pairs, views, samples,
							 v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, v_in2, v_out2, grad_in_sq_sum2, grad_out_sq_sum2,
							 &nlls[i], &nlls_count[i], &mses[i], &mses_count[i]));
		}

		double nll = 0.0, mse = 0.0;
		int nll_count = 0, mse_count = 0;

		for (int i = 0; i < g_threads; ++i) {

			threads[i].join();

			if (i == 0)
				g_stop_threads = true;

			nll += nlls[i];
			mse += mses[i];
			mse_count += mses_count[i];
			nll_count += nlls_count[i];
		}

		nll /= nll_count;
		mse = (mse_count ? mse / mse_count : 0.0);

		double test_nll = 0; //CalcNLL(val_pairs, val_neg_items, v_in, v_out);
		double recall = 0, ndcg = 0;

		CalcRecall(val_pairs, 20, v_in, v_out, &recall, &ndcg);

		//g_scores.push_back(1.0 / test_nll);
		g_scores.push_back(ndcg);
		double imp = GetImprovement();

		cout << "epoch " << epoch <<  " " << nll << " " << test_nll << " " << sqrt(mse) << " " << recall << " " << ndcg <<  " " << imp <<  " " << (time(NULL) - g_start_time) << endl;
		//cout << g_c << endl;

		if (val_pairs.size() > 0 && imp < g_termination)
			break;
	}

/*	for (int epoch = 0; epoch < g_iterations_max; ++epoch) {

		double test_nll = CalcNLL(val_pairs, val_neg_items, v_in, v_out);
		double recall, wrecall;
		CalcRecall(val_pairs, 20, v_in, v_out, &recall, &wrecall);

		//g_scores.push_back(recall);
		g_scores.push_back(1.0 / test_nll);
		double imp = GetImprovement();

		nll /= train_pairs.size();
		mse = (mse_count ? mse / mse_count : 0.0);

		cout << "epoch " << epoch <<  " " << nll << " " << test_nll << " " << sqrt(mse) << " " << recall << " " << wrecall <<  " " << imp << endl;

		if (imp < 1e-4)
			break;
	}
*/
/*	for (int epoch = 0; epoch < 200; ++epoch) {

		double nll = 0.0;

		for (int i = 0; i < (int)baskets.size(); ++i) {

			int basket_idx = std::rand() % baskets.size();

			if (baskets[basket_idx].size() >= 2) {
				double part_nll = 0.0;
				Update(basket_idx, baskets, v_in, v_out, grad_in_sq_sum, grad_out_sq_sum, &part_nll);

				nll += part_nll;
			}
		}

		cout << "epoch " << epoch <<  " " << nll << endl;
	}
*/

	free_matrix(grad_in_sq_sum, g_item_count);
	free_matrix(grad_out_sq_sum, g_item_count);
	free_matrix(v_in2, g_item_count);
	free_matrix(v_out2, g_item_count);
	free_matrix(grad_in_sq_sum2, g_item_count);
	free_matrix(grad_out_sq_sum2, g_item_count);
}

/**************************************************************************
*
* Write embeddings of items to a file.
*
**************************************************************************/
void SaveToFile(string filename, double **v, const vector<string>& item_ids)
{
	ofstream ofs(filename.c_str());

	for (int i = 0; i < (int)item_ids.size(); ++i) {
		ofs << item_ids[i];

		for (int k = 0; k < g_dim; ++k)
			ofs << " " << v[i][k];

		ofs << endl;
	}

	ofs.close();
}

/**************************************************************************
*
* Parse command line options.
*
**************************************************************************/
void ParseOptions(const po::variables_map& vm)
{
	g_lambda_2 = vm["lambda-2"].as<float>();
	g_l2 = vm["l2"].as<float>();
	g_iterations_max = vm["iterations"].as<int>();
	g_eta = vm["learning-rate"].as<float>();
	g_neg_count = vm["neg-sample"].as<int>();
	g_dim = vm["dim"].as<int>();
	g_views_sample = vm["views-sample"].as<int>();
	g_views_importance = vm["vi"].as<float>();
	g_termination = vm["termination"].as<float>();
	g_threads = vm["threads"].as<int>();
	g_simmetric_views = vm.count("sim");
	g_recommend_new = vm.count("new");
	g_rank = vm.count("rank");
	g_norm_v_out = vm.count("norm");
	g_stop_count = vm["stop"].as<int>();
	g_negative_sampling = vm["neg-sampling"].as<int>();
	g_mult_st = vm["mult-stochastic"].as<float>();
	g_views_features = vm.count("views-f");
	g_dw = vm["dw"].as<float>();
	g_fake_ratio = vm["fake"].as<float>();
	g_naive = vm.count("naive");
	g_ni = vm["ni"].as<float>();

	if (g_mult_st > 1) {
		g_mult_mf = 1 / g_mult_st;
		g_mult_st = 1;
	}
}

int run_main(int argc, char **argv)
{
	std::srand(11);
	g_start_time = time(NULL);

	po::options_description general_desc("General options");
	general_desc.add_options()
      	("help,h", "produce help message")
       	("baskets,b", po::value<string>(), "baskets: train set")
       	("views,v", po::value<string>()->default_value(""), "baskets: test set")
       	("val", po::value<string>()->default_value(""), "baskets validation dataset")
       	("item-count", po::value<string>(), "a file for item counts for Recall calculation")
       	("item-features", po::value<string>(), "list of item features")
		("iterations,i", po::value<int>()->default_value(500), "maximum number of iterations")
		("threads,t", po::value<int>()->default_value(1), "number of threads")
		("dim", po::value<int>()->default_value(100), "embedding dimensionality")
		("learning-rate,l", po::value<float>()->default_value(0.05), "learning rate")
		("neg-sample,n", po::value<int>()->default_value(20), "number of negative samples")
       	("views-sample", po::value<int>()->default_value(1), "number of views samples")
		("termination", po::value<float>()->default_value(1e-3), "termination threshold")
		("stop", po::value<int>()->default_value(10), "termination iter count")
		("neg-sampling", po::value<int>()->default_value(1), "sample negative items with this power")
		("mult-stochastic,m", po::value<float>()->default_value(1), "")
		("rank", "use ranking")
		("new", "include new items in rec. lists");

	po::options_description exp_desc("Experimental options");
	exp_desc.add_options()
		("lambda-2", po::value<float>()->default_value(0.0), "L2 regularization")
		("l2", po::value<float>()->default_value(0.0), "L2 regularization, v2")
		("fake", po::value<float>()->default_value(0.0), "")
		("vi", po::value<float>()->default_value(1.0), "L2 regularization")
		("neg-reg", po::value<int>()->default_value(0), "")
		("output,o", po::value<string>()->default_value(""), "output files prefix")
		("norm", "norm v_out")
		("views-f", "")
		("all-items", "")
		("dw", po::value<float>()->default_value(1.0), "")
		("sim", "use simmetric views vectors")
		("naive", "")
		("ni", po::value<float>()->default_value(0.0));

	general_desc.add(exp_desc);

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, general_desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << general_desc << endl;
		return 0;
	}

	ParseOptions(vm);

	vector<Basket> baskets;
	vector<string> item_ids;
	map<string, int> item_nums;

	const string basketsFilename = vm["baskets"].as<string>();
	ReadBaskets(basketsFilename, &baskets, &item_ids, &item_nums, nullptr);

	vector<ViewObject> views;

	if (vm["views"].as<string>().size())
		ReadViews(vm["views"].as<string>(), &item_ids, &item_nums, &token_nums, &views);
	else
		g_views_sample = 0;

	for (auto basket : baskets)
		for (auto item_num : basket)
			g_items_with_counts.push_back(item_num);

	int neg_reg = vm["neg-reg"].as<int>();

	if (!vm.count("all-items")) {
		for (int item_num = 0; item_num < (int)item_ids.size(); ++item_num)  {
			g_train_items.push_back(item_num);

			for (int i = 0; i < neg_reg; ++i)
				g_items_with_counts.push_back(item_num);
		}
	}

	const string valBasketsFilename = vm["val"].as<string>();
	vector<Basket> valBaskets;

	if (valBasketsFilename.size())
		ReadBaskets(valBasketsFilename, &valBaskets, &item_ids, &item_nums, &g_new_items);

	if (vm.count("all-items")) {
		for (int item_num = 0; item_num < (int)item_ids.size(); ++item_num)  {
			g_train_items.push_back(item_num);

			for (int i = 0; i < neg_reg; ++i)
				g_items_with_counts.push_back(item_num);
		}
	}

	g_item_count = item_ids.size();

	double norm = 0;

	for (auto r : views)
		norm += SQUARE(r.PMI);

	cout << "views PMI norm " << sqrt(norm / views.size()) << endl;
	//RemoveBias(&views);

	g_item_weights.resize(g_item_count);
	if (vm.count("item-count"))
		ReadUnaryCount(vm["item-count"].as<string>(), item_nums, &g_item_weights);
	else
		std::fill(g_item_weights.begin(), g_item_weights.end(), 1);

	g_use_features = vm.count("item-features");

	if (vm.count("item-features"))
		ReadItemFeatures(vm["item-features"].as<string>(), item_nums, &token_nums, &item_features);

	cout << "train baskets " << baskets.size() << endl;
	cout << "val baskets " << valBaskets.size() << endl;
	cout << "train items " << g_train_items.size() << endl;
	cout << "items " << item_ids.size() << endl;
	cout << "new items " << g_new_items.size() << endl;
	cout << "items with counts " << g_items_with_counts.size() << endl;
	cout << "output prefix " << vm["output"].as<string>() << endl;
	cout << "item features " << item_features.size() << endl;
	cout << "token nums " << token_nums.size() << endl;

	double **v_in, **v_out;
	FitMME(baskets, valBaskets, views, &v_in, &v_out);

	if (g_use_features || g_views_features) {
		for (int i = 0; i < g_item_count; ++i) {
			for (auto t : item_features[i]) {
				for (int k = 0; k < g_dim; ++k) {
					v_in[i][k] += w_in[t][k];
					v_out[i][k] += w_out[t][k];
				}
			}
		}
	}

	string output = vm["output"].as<string>();

	SaveToFile(string("v_in") + output + string(".txt"), v_in, item_ids);
	SaveToFile(string("v_out") + output + string(".txt"), v_out, item_ids);

	if (g_use_features || g_views_features) {
		SaveToFile(string("w_in") + output + string(".txt"), w_in, token_ids);
		SaveToFile(string("w_out") + output + string(".txt"), w_out, token_ids);
	}

	GetPredictions(string("predictions") + output, item_ids, v_in, v_out);

	free_matrix(v_in, g_item_count);
	free_matrix(v_out, g_item_count);

	return 0;
}


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

#include "drake/bindings/pydrake/common/eigen_geometry_pybind.h"
#include "drake/bindings/pydrake/common/value_pybind.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
#include "drake/bindings/pydrake/pydrake_pybind.h"
#include "drake/multibody/math/spatial_algebra.h"
#include "drake/systems/rendering/frame_velocity.h"
#include "drake/systems/rendering/multibody_position_to_geometry_pose.h"
#include "drake/systems/rendering/pose_aggregator.h"
#include "drake/systems/rendering/pose_vector.h"

namespace drake {
namespace pydrake {

PYBIND11_MODULE(rendering, m) {
  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
  using namespace drake::systems;
  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
  using namespace drake::systems::rendering;
  constexpr auto& doc = pydrake_doc.drake.systems.rendering;

  m.doc() = "Bindings for the rendering portion of the Systems framework.";

  py::module::import("pydrake.systems.framework");

  using T = double;

  py::class_<PoseVector<T>, BasicVector<T>> pose_vector(
      m, "PoseVector", doc.PoseVector.doc);
  pose_vector  // BR
      .def(py::init(), doc.PoseVector.ctor.doc_0args)
      .def(py::init<const Eigen::Quaternion<T>&,
               const Eigen::Translation<T, 3>&>(),
          py::arg("rotation"), py::arg("translation"),
          doc.PoseVector.ctor.doc_2args)
      .def("get_transform", &PoseVector<T>::get_transform,
          doc.PoseVector.get_transform.doc)
      .def("set_transform", &PoseVector<T>::set_transform,
          doc.PoseVector.set_transform.doc)
      .def("get_translation", &PoseVector<T>::get_translation,
          doc.PoseVector.get_translation.doc)
      .def("set_translation", &PoseVector<T>::set_translation,
          doc.PoseVector.set_translation.doc)
      .def("get_rotation", &PoseVector<T>::get_rotation,
          doc.PoseVector.get_rotation.doc)
      .def("set_rotation", &PoseVector<T>::set_rotation,
          doc.PoseVector.set_rotation.doc);

  pose_vector.attr("kSize") = int{PoseVector<T>::kSize};

  py::class_<FrameVelocity<T>, BasicVector<T>> frame_velocity(
      m, "FrameVelocity", doc.FrameVelocity.doc);
  frame_velocity  // BR
      .def(py::init(), doc.FrameVelocity.ctor.doc_0args)
      .def(py::init<const multibody::SpatialVelocity<T>&>(),
          py::arg("velocity"), doc.FrameVelocity.ctor.doc_1args)
      .def("get_velocity", &FrameVelocity<T>::get_velocity,
          doc.FrameVelocity.get_velocity.doc)
      .def("set_velocity", &FrameVelocity<T>::set_velocity, py::arg("velocity"),
          doc.FrameVelocity.set_velocity.doc);

  frame_velocity.attr("kSize") = int{FrameVelocity<T>::kSize};

  py::class_<PoseBundle<T>> pose_bundle(m, "PoseBundle", doc.PoseBundle.doc);
  pose_bundle
      .def(py::init<int>(), py::arg("num_poses"), doc.PoseBundle.ctor.doc)
      .def("get_num_poses", &PoseBundle<T>::get_num_poses,
          doc.PoseBundle.get_num_poses.doc)
      .def("get_transform", &PoseBundle<T>::get_transform,
          doc.PoseBundle.get_transform.doc)
      .def("set_transform", &PoseBundle<T>::set_transform,
          doc.PoseBundle.set_transform.doc)
      .def("get_velocity", &PoseBundle<T>::get_velocity,
          doc.PoseBundle.get_velocity.doc)
      .def("set_velocity", &PoseBundle<T>::set_velocity,
          doc.PoseBundle.set_velocity.doc)
      .def("get_name", &PoseBundle<T>::get_name, doc.PoseBundle.get_name.doc)
      .def("set_name", &PoseBundle<T>::set_name, doc.PoseBundle.set_name.doc)
      .def("get_model_instance_id", &PoseBundle<T>::get_model_instance_id,
          doc.PoseBundle.get_model_instance_id.doc)
      .def("set_model_instance_id", &PoseBundle<T>::set_model_instance_id,
          doc.PoseBundle.set_model_instance_id.doc);
  AddValueInstantiation<PoseBundle<T>>(m);

  py::class_<PoseVelocityInputPorts<T>>(
      m, "PoseVelocityInputPorts", doc.PoseVelocityInputPorts.doc)
      // N.B. We use lambdas below since we cannot use `def_readonly` with
      // reference members.
      .def_property_readonly(
          "pose_input_port",
          [](PoseVelocityInputPorts<T>* self) -> const InputPort<T>& {
            return self->pose_input_port;
          },
          doc.PoseVelocityInputPorts.pose_input_port.doc)
      .def_property_readonly(
          "velocity_input_port",
          [](PoseVelocityInputPorts<T>* self) -> const InputPort<T>& {
            return self->velocity_input_port;
          },
          doc.PoseVelocityInputPorts.velocity_input_port.doc);

  py::class_<PoseAggregator<T>, LeafSystem<T>>(
      m, "PoseAggregator", doc.PoseAggregator.doc)
      .def(py::init<>(), doc.PoseAggregator.ctor.doc)
      .def("AddSingleInput", &PoseAggregator<T>::AddSingleInput,
          py_rvp::reference_internal, doc.PoseAggregator.AddSingleInput.doc)
      .def("AddSinglePoseAndVelocityInput",
          &PoseAggregator<T>::AddSinglePoseAndVelocityInput,
          doc.PoseAggregator.AddSinglePoseAndVelocityInput.doc)
      .def("AddBundleInput", &PoseAggregator<T>::AddBundleInput,
          py_rvp::reference_internal, doc.PoseAggregator.AddBundleInput.doc);

  py::class_<MultibodyPositionToGeometryPose<T>, LeafSystem<T>>(m,
      "MultibodyPositionToGeometryPose",
      doc.MultibodyPositionToGeometryPose.doc)
      .def(py::init<const multibody::MultibodyPlant<T>&, bool>(),
          py::arg("plant"), py::arg("input_multibody_state") = false,
          // Keep alive, reference: `self` keeps `plant` alive.
          py::keep_alive<1, 2>(),
          doc.MultibodyPositionToGeometryPose.ctor
              .doc_2args_plant_input_multibody_state);

  // TODO(eric.cousineau): Add more systems as needed.
}

}  // namespace pydrake
}  // namespace drake


#include "partition/partitioner.hpp"
#include "partition/bisection_graph.hpp"
#include "partition/bisection_to_partition.hpp"
#include "partition/cell_storage.hpp"
#include "partition/compressed_node_based_graph_reader.hpp"
#include "partition/edge_based_graph_reader.hpp"
#include "partition/io.hpp"
#include "partition/multi_level_partition.hpp"
#include "partition/recursive_bisection.hpp"
#include "partition/remove_unconnected.hpp"

#include "extractor/io.hpp"

#include "util/coordinate.hpp"
#include "util/geojson_debug_logger.hpp"
#include "util/geojson_debug_policies.hpp"
#include "util/integer_range.hpp"
#include "util/json_container.hpp"
#include "util/log.hpp"

#include <algorithm>
#include <iterator>
#include <vector>

#include <boost/assert.hpp>

#include "util/geojson_debug_logger.hpp"
#include "util/geojson_debug_policies.hpp"
#include "util/json_container.hpp"
#include "util/timing_util.hpp"

namespace osrm
{
namespace partition
{

void LogGeojson(const std::string &filename, const std::vector<std::uint32_t> &bisection_ids)
{
    // reload graph, since we destroyed the old one
    auto compressed_node_based_graph = LoadCompressedNodeBasedGraph(filename);

    util::Log() << "Loaded compressed node based graph: "
                << compressed_node_based_graph.edges.size() << " edges, "
                << compressed_node_based_graph.coordinates.size() << " nodes";

    groupEdgesBySource(begin(compressed_node_based_graph.edges),
                       end(compressed_node_based_graph.edges));

    auto graph =
        makeBisectionGraph(compressed_node_based_graph.coordinates,
                           adaptToBisectionEdge(std::move(compressed_node_based_graph.edges)));

    const auto get_level = [](const std::uint32_t lhs, const std::uint32_t rhs) {
        auto xored = lhs ^ rhs;
        std::uint32_t level = log(xored) / log(2.0);
        return level;
    };

    std::vector<std::vector<util::Coordinate>> border_vertices(33);

    for (NodeID nid = 0; nid < graph.NumberOfNodes(); ++nid)
    {
        const auto source_id = bisection_ids[nid];
        for (const auto &edge : graph.Edges(nid))
        {
            const auto target_id = bisection_ids[edge.target];
            if (source_id != target_id)
            {
                auto level = get_level(source_id, target_id);
                border_vertices[level].push_back(graph.Node(nid).coordinate);
                border_vertices[level].push_back(graph.Node(edge.target).coordinate);
            }
        }
    }

    util::ScopedGeojsonLoggerGuard<util::CoordinateVectorToMultiPoint> guard(
        "border_vertices.geojson");
    std::size_t level = 0;
    for (auto &bv : border_vertices)
    {
        if (!bv.empty())
        {
            std::sort(bv.begin(), bv.end(), [](const auto lhs, const auto rhs) {
                return std::tie(lhs.lon, lhs.lat) < std::tie(rhs.lon, rhs.lat);
            });
            bv.erase(std::unique(bv.begin(), bv.end()), bv.end());

            util::json::Object jslevel;
            jslevel.values["level"] = util::json::Number(level++);
            guard.Write(bv, jslevel);
        }
    }
}

int Partitioner::Run(const PartitionConfig &config)
{
    auto compressed_node_based_graph =
        LoadCompressedNodeBasedGraph(config.compressed_node_based_graph_path.string());

    util::Log() << "Loaded compressed node based graph: "
                << compressed_node_based_graph.edges.size() << " edges, "
                << compressed_node_based_graph.coordinates.size() << " nodes";

    groupEdgesBySource(begin(compressed_node_based_graph.edges),
                       end(compressed_node_based_graph.edges));

    auto graph =
        makeBisectionGraph(compressed_node_based_graph.coordinates,
                           adaptToBisectionEdge(std::move(compressed_node_based_graph.edges)));

    util::Log() << " running partition: " << config.max_cell_sizes.front() << " " << config.balance
                << " " << config.boundary_factor << " " << config.num_optimizing_cuts << " "
                << config.small_component_size
                << " # max_cell_size balance boundary cuts small_component_size";
    RecursiveBisection recursive_bisection(graph,
                                           config.max_cell_sizes.front(),
                                           config.balance,
                                           config.boundary_factor,
                                           config.num_optimizing_cuts,
                                           config.small_component_size);

    // Up until now we worked on the compressed node based graph.
    // But what we actually need is a partition for the edge based graph to work on.
    // The following loads a mapping from node based graph to edge based graph.
    // Then loads the edge based graph tanslates the partition and modifies it.
    // For details see #3205

    std::vector<extractor::NBGToEBG> mapping;
    extractor::io::read(config.cnbg_ebg_mapping_path.string(), mapping);
    util::Log() << "Loaded node based graph to edge based graph mapping";

    auto edge_based_graph = LoadEdgeBasedGraph(config.edge_based_graph_path.string());
    util::Log() << "Loaded edge based graph for mapping partition ids: "
                << edge_based_graph->GetNumberOfEdges() << " edges, "
                << edge_based_graph->GetNumberOfNodes() << " nodes";

    // TODO: node based graph to edge based graph partition id mapping should be done split off.

    // Partition ids, keyed by node based graph nodes
    const auto &node_based_partition_ids = recursive_bisection.BisectionIDs();

    // Partition ids, keyed by edge based graph nodes
    std::vector<NodeID> edge_based_partition_ids(edge_based_graph->GetNumberOfNodes(),
                                                 SPECIAL_NODEID);

    // Only resolve all easy cases in the first pass
    for (const auto &entry : mapping)
    {
        const auto u = entry.u;
        const auto v = entry.v;
        const auto forward_node = entry.forward_ebg_node;
        const auto backward_node = entry.backward_ebg_node;

        // This heuristic strategy seems to work best, even beating chosing the minimum
        // border edge bisection ID
        edge_based_partition_ids[forward_node] = node_based_partition_ids[u];
        if (backward_node != SPECIAL_NODEID)
            edge_based_partition_ids[backward_node] = node_based_partition_ids[v];
    }

    std::vector<Partition> partitions;
    std::vector<std::uint32_t> level_to_num_cells;
    std::tie(partitions, level_to_num_cells) =
        bisectionToPartition(edge_based_partition_ids, config.max_cell_sizes);

    auto num_unconnected = removeUnconnectedBoundaryNodes(*edge_based_graph, partitions);
    util::Log() << "Fixed " << num_unconnected << " unconnected nodes";

    util::Log() << "Edge-based-graph annotation:";
    for (std::size_t level = 0; level < level_to_num_cells.size(); ++level)
    {
        util::Log() << "  level " << level + 1 << " #cells " << level_to_num_cells[level]
                    << " bit size " << std::ceil(std::log2(level_to_num_cells[level] + 1));
    }

    TIMER_START(packed_mlp);
    MultiLevelPartition mlp{partitions, level_to_num_cells};
    TIMER_STOP(packed_mlp);
    util::Log() << "MultiLevelPartition constructed in " << TIMER_SEC(packed_mlp) << " seconds";

    TIMER_START(cell_storage);
    CellStorage storage(mlp, *edge_based_graph);
    TIMER_STOP(cell_storage);
    util::Log() << "CellStorage constructed in " << TIMER_SEC(cell_storage) << " seconds";

    TIMER_START(writing_mld_data);
    io::write(config.mld_partition_path, mlp);
    io::write(config.mld_storage_path, storage);
    TIMER_STOP(writing_mld_data);
    util::Log() << "MLD data writing took " << TIMER_SEC(writing_mld_data) << " seconds";

    return 0;
}

} // namespace partition
} // namespace osrm


/**
 * @author Enrique Fernández
 * @author Gérald Lelong
 */

#include <cmath>
#include <tf/transform_datatypes.h>
#include <urdf_parser/urdf_parser.h>
#include <boost/assign.hpp>
#include <stdexcept>
#include <string>
#include <ackermann_controller/ackermann_controller.h>

using hardware_interface::VelocityJointInterface;
using hardware_interface::PositionJointInterface;
using hardware_interface::JointStateInterface;

namespace ackermann_controller {

AckermannController::AckermannController()
    : open_loop_(false)
    , command_struct_()
    , steering_angle_instead_of_angular_speed_(true)
    , wheelbase_(1.0)
    , cmd_vel_timeout_(0.5)
    , velocity_rolling_window_size_(10)
    , odom_frame_id_("odom")
    , base_frame_id_("base_link")
    , base_link_("base_link")
    , enable_odom_tf_(true)
{
}

bool AckermannController::init(
    hardware_interface::RobotHW * hw,
    ros::NodeHandle& root_nh,
    ros::NodeHandle& controller_nh)
{
    VelocityJointInterface * vhw = hw->get<VelocityJointInterface>();
    PositionJointInterface * phw = hw->get<PositionJointInterface>();
    JointStateInterface * shw = hw->get<JointStateInterface>();

    const std::string complete_ns = controller_nh.getNamespace();
    std::size_t id = complete_ns.find_last_of("/");
    name_ = complete_ns.substr(id + 1);

    if (!initParams(controller_nh))
    {
        return false;
    }

    try
    {
        boost::shared_ptr<urdf::ModelInterface> urdf_model = getURDFModel(root_nh);

        std::vector<std::string>::const_iterator it;
        std::vector<std::string> spinning_joint_names = getJointNames(controller_nh, "spinning_joints");
        std::vector<std::string> odometry_joint_names = getJointNames(controller_nh, "odometry_joints");
        std::vector<std::string> steering_joint_names = getJointNames(controller_nh, "steering_joints");

        for (it = spinning_joint_names.begin(); it != spinning_joint_names.end(); ++it)
        {
            ActuatedWheel wheel(*it, base_link_, urdf_model, vhw->getHandle(*it));
            ROS_INFO_STREAM_NAMED(name_,
                                  "Found spinning joint " << wheel.name_
                                  << " with lateral deviation " << wheel.lateral_deviation_
                                  << " and radius " << wheel.radius_);
            spinning_joints_.push_back(wheel);
        }

        for (it = odometry_joint_names.begin(); it != odometry_joint_names.end(); ++it)
        {
            Wheel wheel(*it, base_link_, urdf_model, shw->getHandle(*it));
            ROS_INFO_STREAM_NAMED(name_,
                                  "Found odometry joint " << wheel.name_
                                  << " with lateral deviation " << wheel.lateral_deviation_
                                  << " and radius " << wheel.radius_);
            odometry_joints_.push_back(wheel);
        }

        for (it = steering_joint_names.begin(); it != steering_joint_names.end(); ++it)
        {
            ActuatedJoint steering_joint(*it, base_link_, urdf_model, phw->getHandle(*it));
            ROS_INFO_STREAM_NAMED(name_,
                                  "Found steering joint " << steering_joint.name_
                                  << " with lateral deviation " << steering_joint.lateral_deviation_);
            steering_joints_.push_back(steering_joint);
        }
    }
    catch(const std::runtime_error& e)
    {
        ROS_ERROR_STREAM_NAMED(name_, e.what());
        return false;
    }

    odometry_.setWheelbase(wheelbase_);
    odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size_);

    setOdomPubFields(root_nh, controller_nh);

    sub_command_ = controller_nh.subscribe("cmd_vel", 1, &AckermannController::cmdVelCallback, this);

    return true;
}

void AckermannController::update(const ros::Time& time, const ros::Duration& period)
{
    updateOdometry(time, period);
    moveRobot(time, period);
}

void AckermannController::updateOdometry(const ros::Time& time, const ros::Duration& period)
{
    if (open_loop_)
    {
        odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
    }
    else
    {
        odometry_.update(steering_joints_, odometry_joints_, time);
    }

    if (last_state_publish_time_ + publish_period_ < time)
    {
        last_state_publish_time_ += publish_period_;

        const geometry_msgs::Quaternion orientation(
            tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

        if (odom_pub_->trylock())
        {
            odom_pub_->msg_.header.stamp = time;
            odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
            odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
            odom_pub_->msg_.pose.pose.orientation = orientation;
            odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinear();
            odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
            odom_pub_->unlockAndPublish();
        }

        if (enable_odom_tf_ && tf_odom_pub_->trylock())
        {
            geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
            odom_frame.header.stamp = time;
            odom_frame.transform.translation.x = odometry_.getX();
            odom_frame.transform.translation.y = odometry_.getY();
            odom_frame.transform.rotation = orientation;
            tf_odom_pub_->unlockAndPublish();
        }
    }
}

void AckermannController::moveRobot(const ros::Time& time, const ros::Duration& period)
{
    // Retrieve current velocity command and time step:
    Commands curr_cmd = *(command_.readFromRT());
    const double dt = (time - curr_cmd.stamp).toSec();

    // Brake if cmd_vel has timeout:
    if (dt > cmd_vel_timeout_)
    {
        curr_cmd.lin = 0.0;
        curr_cmd.ang = 0.0;
    }

    // Limit velocities and accelerations:
    const double cmd_dt(period.toSec());

    limiter_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);

    last1_cmd_ = last0_cmd_;
    last0_cmd_ = curr_cmd;

    double angle = 0.0;

    if(steering_angle_instead_of_angular_speed_)
    {
        angle = curr_cmd.ang;
    }
    else
    {
        angle = curr_cmd.lin * tan(curr_cmd.ang) / wheelbase_;
    }

    // Compute steering angles
    for (std::vector<ActuatedJoint>::iterator it = steering_joints_.begin(); it != steering_joints_.end(); ++it)
    {
        double steering_angle = std::atan(wheelbase_ * std::tan(angle)/std::abs(wheelbase_ + it->lateral_deviation_ * std::tan(angle)));
        it->setCommand(steering_angle);
    }

    // Compute wheels velocities
    for (std::vector<ActuatedWheel>::iterator it = spinning_joints_.begin(); it != spinning_joints_.end(); ++it)
    {
        double velocity = (curr_cmd.lin * (1.0 + 2.0 * it->lateral_deviation_ * std::tan(curr_cmd.ang) / (2.0 * wheelbase_))) / it->radius_;
        it->setCommand(velocity);
    }
}

void AckermannController::starting(const ros::Time& time)
{
    brake();

    // Register starting time used to keep fixed rate
    last_state_publish_time_ = time;

    odometry_.init(time);
}

void AckermannController::stopping(const ros::Time& /*time*/)
{
    brake();
}

void AckermannController::brake()
{
    const double velocity = 0.0;

    std::vector<ActuatedWheel>::iterator it;
    for (it = spinning_joints_.begin(); it != spinning_joints_.end(); ++it)
    {
        it->setCommand(velocity);
    }
}

void AckermannController::cmdVelCallback(const geometry_msgs::Twist& command)
{
    if (isRunning())
    {
        command_struct_.ang = command.angular.z;
        command_struct_.lin = command.linear.x;
        command_struct_.stamp = ros::Time::now();
        command_.writeFromNonRT(command_struct_);

        ROS_DEBUG_STREAM_NAMED(
            name_,
            "Added values to command. "
            << "Ang: "   << command_struct_.ang << ", "
            << "Lin: "   << command_struct_.lin << ", "
            << "Stamp: " << command_struct_.stamp);
    }
    else
    {
        ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
    }
}

bool AckermannController::initParams(ros::NodeHandle& controller_nh)
{
    // Odometry related:
    double publish_rate;
    controller_nh.param("publish_rate", publish_rate, 50.0);
    ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
                          << publish_rate << "Hz.");
    publish_period_ = ros::Duration(1.0 / publish_rate);

    controller_nh.param("open_loop", open_loop_, open_loop_);
    ROS_INFO_STREAM_NAMED(name_, "Open loop is " << (open_loop_?"enabled":"disabled"));

    controller_nh.param("wheelbase", wheelbase_, wheelbase_);
    ROS_INFO_STREAM_NAMED(name_, "Wheelbase set to " << wheelbase_);

    controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size_, velocity_rolling_window_size_);
    ROS_INFO_STREAM_NAMED(
        name_, "Velocity rolling window size of " << velocity_rolling_window_size_ << ".");

    // Twist command related:
    controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
    ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
                          << cmd_vel_timeout_ << "s.");

    controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
    ROS_INFO_STREAM_NAMED(name_, "Odom frame_id set to " << odom_frame_id_);

    controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
    ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);

    controller_nh.param("base_model_link", base_link_, base_link_);
    ROS_INFO_STREAM_NAMED(name_, "Base base_model_link set to " << base_link_);

    controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
    ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));

    controller_nh.param("steering_angle_instead_of_angular_speed", steering_angle_instead_of_angular_speed_, steering_angle_instead_of_angular_speed_);
    ROS_INFO_STREAM_NAMED(name_, "Steering angle instead of angular speed is " << (steering_angle_instead_of_angular_speed_?"enabled":"disabled"));

    // Velocity and acceleration limits:
    controller_nh.param("has_velocity_limits"    , limiter_.has_velocity_limits    , limiter_.has_velocity_limits    );
    controller_nh.param("has_acceleration_limits", limiter_.has_acceleration_limits, limiter_.has_acceleration_limits);
    controller_nh.param("has_deceleration_limits", limiter_.has_deceleration_limits, limiter_.has_deceleration_limits);
    controller_nh.param("has_jerk_limits"        , limiter_.has_jerk_limits        , limiter_.has_jerk_limits        );
    controller_nh.param("max_velocity"           , limiter_.max_velocity           ,  limiter_.max_velocity          );
    controller_nh.param("min_velocity"           , limiter_.min_velocity           , -limiter_.max_velocity          );
    controller_nh.param("max_acceleration"       , limiter_.max_acceleration       ,  limiter_.max_acceleration      );
    controller_nh.param("min_acceleration"       , limiter_.min_acceleration       , -limiter_.max_acceleration      );
    controller_nh.param("max_deceleration"       , limiter_.max_deceleration       ,  limiter_.max_deceleration      );
    controller_nh.param("min_deceleration"       , limiter_.min_deceleration       , -limiter_.max_deceleration      );
    controller_nh.param("max_jerk"               , limiter_.max_jerk               ,  limiter_.max_jerk              );
    controller_nh.param("min_jerk"               , limiter_.min_jerk               , -limiter_.max_jerk              );

    return true;
}

boost::shared_ptr<urdf::ModelInterface> AckermannController::getURDFModel(const ros::NodeHandle& nh)
{
    const std::string model_param_name = "robot_description";
    std::string robot_model_str = "";

    if (!nh.hasParam(model_param_name) || !nh.getParam(model_param_name, robot_model_str))
    {
        throw std::runtime_error("Robot description couldn't be retrieved from param server.");
    }

    boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));

    return model;
}

std::vector<std::string> AckermannController::getJointNames(
    ros::NodeHandle& controller_nh,
    const std::string& param)
{
    std::vector<std::string> names;

    XmlRpc::XmlRpcValue wheel_list;
    if (!controller_nh.getParam(param, wheel_list))
    {
        throw std::runtime_error(std::string("Couldn't retrieve param '") + param + "'.");
    }

    if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
        if (wheel_list.size() == 0)
        {
            throw std::runtime_error(param + " is an empty list");
        }

        for (int i = 0; i < wheel_list.size(); ++i)
        {
            if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
            {
                throw std::runtime_error(param + " child isn't a string.");
            }
        }

        names.resize(wheel_list.size());
        for (int i = 0; i < wheel_list.size(); ++i)
        {
            names[i] = static_cast<std::string>(wheel_list[i]);
        }
    }
    else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
    {
        names.push_back(wheel_list);
    }
    else
    {
        throw std::runtime_error(param + " is neither a list of strings nor a string.");
    }

    return names;
}

void AckermannController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
{
    // Get and check params for covariances
    XmlRpc::XmlRpcValue pose_cov_list;
    controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
    ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
    ROS_ASSERT(pose_cov_list.size() == 6);
    for (int i = 0; i < pose_cov_list.size(); ++i)
        ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

    XmlRpc::XmlRpcValue twist_cov_list;
    controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
    ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
    ROS_ASSERT(twist_cov_list.size() == 6);
    for (int i = 0; i < twist_cov_list.size(); ++i)
        ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

    // Setup odometry realtime publisher + odom message constant fields
    odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
    odom_pub_->msg_.header.frame_id = odom_frame_id_;
    odom_pub_->msg_.child_frame_id = base_frame_id_;
    odom_pub_->msg_.pose.pose.position.z = 0;
    odom_pub_->msg_.pose.covariance = boost::assign::list_of
                                      (static_cast<double>(pose_cov_list[0])) (0)  (0)  (0)  (0)  (0)
                                      (0)  (static_cast<double>(pose_cov_list[1])) (0)  (0)  (0)  (0)
                                      (0)  (0)  (static_cast<double>(pose_cov_list[2])) (0)  (0)  (0)
                                      (0)  (0)  (0)  (static_cast<double>(pose_cov_list[3])) (0)  (0)
                                      (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[4])) (0)
                                      (0)  (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[5]));
    odom_pub_->msg_.twist.twist.linear.y  = 0;
    odom_pub_->msg_.twist.twist.linear.z  = 0;
    odom_pub_->msg_.twist.twist.angular.x = 0;
    odom_pub_->msg_.twist.twist.angular.y = 0;
    odom_pub_->msg_.twist.covariance = boost::assign::list_of
                                       (static_cast<double>(twist_cov_list[0])) (0)  (0)  (0)  (0)  (0)
                                       (0)  (static_cast<double>(twist_cov_list[1])) (0)  (0)  (0)  (0)
                                       (0)  (0)  (static_cast<double>(twist_cov_list[2])) (0)  (0)  (0)
                                       (0)  (0)  (0)  (static_cast<double>(twist_cov_list[3])) (0)  (0)
                                       (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[4])) (0)
                                       (0)  (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[5]));
    tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
    tf_odom_pub_->msg_.transforms.resize(1);
    tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
    tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
    tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
}
}

PLUGINLIB_EXPORT_CLASS(ackermann_controller::AckermannController, controller_interface::ControllerBase)


#ifndef CONFIGPARSER_HPP
#define CONFIGPARSER_HPP

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/algorithm/string.hpp>

#include <vector>
#include <string>
#include <iostream>
#include <stdexcept>
#include <sstream>

#include "consts.hpp"

namespace pt = boost::property_tree;
namespace ba = boost::algorithm;

struct bunchdata{
  double x, y, z, px, py, pz, t, q;
  double dx, dy, dz, dpx, dpy, dpz, dt;
  std::string xdist, ydist, zdist, pxdist, pydist, pzdist, tdist;
  size_t npart;
};

class ConfigParser{
public:
  std::vector<bunchdata> bunches;
  double eta2;
  double lambda0;
  double int_time;
  std::string outfile;
  bool radt_mode;
  size_t nsteps;

  ConfigParser(std::string filename): filename(filename){
    parse_config();
  }

  void parse_config(){
    try{
      pt::read_ini(filename, config);
    }
    catch (const std::exception& e){
      std::ostringstream err;
      err << "Failed to parse " << filename << " check your syntax!\n";
      throw std::runtime_error(err.str());
    }

    for(const auto &it : config){
      if(ba::starts_with(it.first, "Bunch")){ handle_bunch_block(it.first, it.second);}
      else if( it.first == "Output"){ handle_output_block(it.first, it.second);}
      else if( it.first == "Bubble"){ handle_bubble_block(it.first, it.second);}
      else { handle_unknown_block(it.first, it.second);}
    }
  }

  void handle_bunch_block(std::string name, pt::ptree block){
    bunchdata currbunch;
    std::cout << "Parsing Bunch Spec:\n\n";
    try{
      currbunch.npart = block.get<size_t>("npart");
    } catch (const std::exception& e){
      std::string err = "Error: Number of particles (npart) not specified in bunch.\n";
      throw std::runtime_error(err);
    }
    try{
      currbunch.q = block.get<double>("q");
    } catch (const std::exception& e){
      currbunch.q = (double)currbunch.npart * k_e;
      std::cout << "Warning: No bunch charge specified, assuming npart * e = "
        << currbunch.q << "C\n";
    }
    currbunch.xdist = block.get("xdist", "");
    currbunch.ydist = block.get("ydist", "");
    currbunch.zdist = block.get("zdist", "");
    currbunch.pxdist = block.get("pxdist", "");
    currbunch.pydist = block.get("pydist", "");
    currbunch.pzdist = block.get("pzdist", "");
    currbunch.tdist = block.get("tdist", "");

    currbunch.x = block.get("x", 0.);
    currbunch.y = block.get("y", 0.);
    currbunch.z = block.get("z", 0.);
    currbunch.px = block.get("px", 0.);
    currbunch.py = block.get("py", 0.);
    currbunch.pz = block.get("pz", 0.);
    currbunch.t = block.get("t", 0.);

    currbunch.dpx = block.get("dpx", 0.);
    currbunch.dpy = block.get("dpy", 0.);
    currbunch.dpz = block.get("dpz", 0.);
    currbunch.dx = block.get("dx", 0.);
    currbunch.dy = block.get("dy", 0.);
    currbunch.dz = block.get("dz", 0.);
    currbunch.dt = block.get("dt", 0.);

    std::cout << "Read bunch parameters:"
      << "\n\tnpart = " << currbunch.npart
      << "\n\tq = " << currbunch.q
      << "\n\tt = " << currbunch.t << "\tdt = " << currbunch.dt << "\tdist: " << currbunch.tdist
      << "\n\tx = " << currbunch.x << "\tdx = " << currbunch.dx << "\tdist: " << currbunch.xdist
      << "\n\ty = " << currbunch.y << "\tdy = " << currbunch.dy << "\tdist: " << currbunch.ydist
      << "\n\tz = " << currbunch.z << "\tdz = " << currbunch.dz << "\tdist: " << currbunch.zdist
      << "\n\tpx = " << currbunch.px << "\tdpx = " << currbunch.dpx << "\tdist: " << currbunch.pxdist
      << "\n\tpy = " << currbunch.py << "\tdpy = " << currbunch.dpy << "\tdist: " << currbunch.pydist
      << "\n\tpz = " << currbunch.pz << "\tdpz = " << currbunch.dpz << "\tdist: " << currbunch.pzdist
      << std::endl << std::endl;

    this->bunches.push_back(currbunch);
  }

  void handle_output_block(std::string name, pt::ptree block){
    try{
      this->nsteps = block.get<size_t>("nsteps");
    } catch (const std::exception& e){
      std::string err = "Error: Number of timesteps (nsteps) not specified.\n";
      throw std::runtime_error(err);
    }
    try{
      this->outfile = block.get<std::string>("filename");
    } catch (const std::exception& e){
      std::string err = "Error: filename not specified.\n";
      throw std::runtime_error(err);
    }
    try{
      this->radt_mode = block.get<bool>("radt_mode");
    } catch (const std::exception& e){
      this->radt_mode = false;
    }

    this->int_time = block.get("int_time", 1.0);

    if(this->radt_mode){
      std::cout << "Writing to " << this->outfile
        << " using radt normalized units.";
    } else {
      std::cout << "Writing to " << filename
        << " using SI units.";
    }
    std::cout << "\n\n";
  }

  void handle_bubble_block(std::string name, pt::ptree block){
    try{
      this->eta2 = block.get<double>("eta2");
    } catch (const std::exception& e){
      std::string err = "Error: eta2 not specified.\n";
      throw std::runtime_error(err);
    }
    try{
      this->lambda0 = block.get<double>("lambda0");
    } catch (const std::exception& e){
      std::string err = "Error: lambda0 not specified.\n";
      throw std::runtime_error(err);
    }
    std::cout << "Read Bubble Parameters:"
      << "\n\teta2 = " << this->eta2
      << "\n\tlambda0 = " << this->lambda0
      << std::endl << std::endl;
  }

  void handle_unknown_block(std::string name, pt::ptree block){
    std::cout << "Warning: Unknown block" << name << "ignoring...\n";
  }

protected:
  pt::ptree config;
  std::string filename;

};

#endif


#define BOOST_TEST_MODULE BoostComputeScan
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/external/boost_compute.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(boost_compute_inclusive_scan)
{
    const size_t n = 1024;

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    vex::compute::inclusive_scan(X, Y);

    std::partial_sum(x.begin(), x.end(), x.begin());

    check_sample(Y, [&](size_t idx, double a) {
            BOOST_CHECK_CLOSE(a, x[idx], 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(boost_compute_exclusive_scan)
{
    const size_t n = 1024;

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    vex::compute::exclusive_scan(X, Y);

    double sum = 0;
    for(auto xi = x.begin(); xi != x.end(); ++xi) {
        double next = sum + *xi;
        *xi = sum;
        sum = next;
    }

    check_sample(Y, [&](size_t idx, double a) {
            BOOST_CHECK_CLOSE(a, x[idx], 1e-8);
            });
}

BOOST_AUTO_TEST_SUITE_END()


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Cartography.h>
#include <vw/InterestPoint.h>
#include <vw/Math.h>
#include <vw/Mosaic/ImageComposite.h>
#include <asp/ControlNetTK/Equalization.h>
#include <asp/Core/Common.h>
#include <asp/Core/Macros.h>
#include <limits>

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
namespace po = boost::program_options;

using std::cout;
using std::endl;
using std::string;

using namespace vw;
using namespace vw::cartography;
using namespace vw::ip;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

template <int dim>
class HomogeneousTransformFunctor : public UnaryReturnSameType {
  Matrix<double,dim+1,dim+1> m_trans;

public:
  HomogeneousTransformFunctor(Matrix<double,dim+1,dim+1> trans) : m_trans(trans) {}

  inline Vector<double,dim> operator()(Vector<double,dim> pt) const {
    if (pt == Vector<double,dim>())
      return pt;

    Vector<double,dim+1> pt_h;
    subvector(pt_h, 0, dim) = pt;
    pt_h[dim] = 1;

    Vector<double,dim+1> result = m_trans * pt_h;
    if (result[dim] != 1)
      result /= result[dim];
    return subvector(result,0,dim);
  }
};

void match_orthoimages( string const& left_image_name,
                        string const& right_image_name,
                        std::vector<InterestPoint> & matched_ip1,
                        std::vector<InterestPoint> & matched_ip2,
                        size_t const& max_points )
{

  vw_out() << "\t--> Finding Interest Points for the orthoimages\n";

  fs::path left_image_path(left_image_name), right_image_path(right_image_name);
  string left_ip_file = change_extension(left_image_path, ".vwip").string();
  string right_ip_file = change_extension(right_image_path, ".vwip").string();
  string match_file = (left_image_path.branch_path() / (fs::basename(left_image_path) + "__" + fs::basename(right_image_path) + ".match")).string();

  // Building / Loading Interest point data
  if ( fs::exists(match_file) ) {

    vw_out() << "\t    * Using cached match file.\n";
    read_binary_match_file(match_file, matched_ip1, matched_ip2);
    vw_out() << "\t    * " << matched_ip1.size() << " matches\n";

  } else {
    std::vector<InterestPoint> ip1_copy, ip2_copy;

    if ( !fs::exists(left_ip_file) ||
         !fs::exists(right_ip_file) ) {

      // Worst case, no interest point operations have been performed before
      vw_out() << "\t    * Locating Interest Points\n";
      DiskImageView<PixelGray<float32> > left_disk_image(left_image_name);
      DiskImageView<PixelGray<float32> > right_disk_image(right_image_name);

      // Interest Point module detector code.
      OBALoGInterestOperator obalog_detector(0.03);
      IntegralInterestPointDetector<OBALoGInterestOperator> detector( obalog_detector, 200 );
      std::list<InterestPoint> ip1, ip2;
      vw_out() << "\t    * Processing " << left_image_name << "...\n" << std::flush;
      ip1 = detect_interest_points( left_disk_image, detector );
      vw_out() << "Located " << ip1.size() << " points.\n";
      vw_out() << "\t    * Processing " << right_image_name << "...\n" << std::flush;
      ip2 = detect_interest_points( right_disk_image, detector );
      vw_out() << "Located " << ip2.size() << " points.\n";

      vw_out() << "\t    * Generating descriptors..." << std::flush;
      SGradDescriptorGenerator descriptor;
      descriptor( left_disk_image, ip1 );
      descriptor( right_disk_image, ip2 );
      vw_out() << "done.\n";

      // Writing out the results
      vw_out() << "\t    * Caching interest points: "
               << left_ip_file << " & " << right_ip_file << std::endl;
      write_binary_ip_file( left_ip_file, ip1 );
      write_binary_ip_file( right_ip_file, ip2 );

    }

    vw_out() << "\t    * Using cached IPs.\n";
    ip1_copy = read_binary_ip_file(left_ip_file);
    ip2_copy = read_binary_ip_file(right_ip_file);

    vw_out() << "\t    * Matching interest points\n";
    ip::DefaultMatcher matcher(0.6);

    matcher(ip1_copy, ip2_copy, matched_ip1, matched_ip2,
            false, TerminalProgressCallback( "asp", "\t    Matching: "));
    ip::remove_duplicates(matched_ip1, matched_ip2);
    vw_out(InfoMessage) << "\t    " << matched_ip1.size() << " putative matches.\n";
    asp::cnettk::equalization( matched_ip1, matched_ip2, max_points );
    vw_out(InfoMessage) << "\t    " << matched_ip1.size() << " thinned matches.\n";

    vw_out() << "\t    * Caching matches: " << match_file << "\n";
    write_binary_match_file( match_file, matched_ip1, matched_ip2);
  }

}

struct Options : public asp::BaseOptions {
  Options() : dem1_nodata(std::numeric_limits<double>::quiet_NaN()), dem2_nodata(std::numeric_limits<double>::quiet_NaN()) {}
  // Input
  string dem1_name, dem2_name, ortho1_name, ortho2_name;
  double dem1_nodata, dem2_nodata;
  size_t max_points;

  // Output
  string output_prefix;
};

void handle_arguments( int argc, char *argv[], Options& opt ) {
  po::options_description general_options("");
  general_options.add_options()
    ("max-match-points", po::value(&opt.max_points)->default_value(800), "The max number of points that will be enforced after matching.")
    ("default-value", po::value(&opt.dem1_nodata), "The value of missing pixels in the first dem")
    ("output-prefix,o", po::value(&opt.output_prefix), "Specify the output prefix");
  general_options.add( asp::BaseOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("dem1", po::value(&opt.dem1_name), "Explicitly specify the first dem")
    ("dem2", po::value(&opt.dem2_name), "Explicitly specify the second dem")
    ("ortho1", po::value(&opt.ortho1_name), "Explicitly specify the first orthoimage")
    ("ortho2", po::value(&opt.ortho2_name), "Explicitly specify the second orthoimage");

  po::positional_options_description positional_desc;
  positional_desc.add("dem1", 1);
  positional_desc.add("ortho1", 1);
  positional_desc.add("dem2", 1);
  positional_desc.add("ortho2", 1);

  std::string usage("<dem1> <ortho1> <dem2> <ortho2>");
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options,
                             positional, positional_desc, usage );

  if ( opt.dem1_name.empty() || opt.dem2_name.empty() ||
       opt.ortho1_name.empty() || opt.ortho2_name.empty() )
    vw_throw( ArgumentErr() << "Missing input files.\n"
              << usage << general_options );
  if ( opt.output_prefix.empty() )
    opt.output_prefix = change_extension(fs::path(opt.dem1_name), "").string();
}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    DiskImageResourceGDAL ortho1_rsrc(opt.ortho1_name), ortho2_rsrc(opt.ortho2_name),
      dem1_rsrc(opt.dem1_name), dem2_rsrc(opt.dem2_name);

    // Pull out dem nodatas
    if ( opt.dem1_nodata == std::numeric_limits<double>::quiet_NaN() &&
         dem1_rsrc.has_nodata_read() ) {
      opt.dem1_nodata = dem1_rsrc.nodata_read();
      vw_out() << "\tFound DEM1 input nodata value: "
               << opt.dem1_nodata << endl;
    }
    if ( dem2_rsrc.has_nodata_read() ) {
      opt.dem2_nodata = dem2_rsrc.nodata_read();
      vw_out() << "\tFound DEM2 input nodata value: "
               << opt.dem2_nodata << endl;
    } else {
      vw_out() << "\tMissing nodata value for DEM2. Using DEM1 nodata value.\n";
      opt.dem2_nodata = opt.dem1_nodata;
    }

    typedef DiskImageView<double> dem_type;
    dem_type dem1_dmg(opt.dem1_name), dem2_dmg(opt.dem2_name);

    typedef InterpolationView<EdgeExtensionView<dem_type, ConstantEdgeExtension>, BilinearInterpolation> bilinear_type;
    typedef InterpolationView<EdgeExtensionView<dem_type, ConstantEdgeExtension>, NearestPixelInterpolation> nearest_type;
    bilinear_type dem1_interp =
      interpolate(dem1_dmg, BilinearInterpolation(), ConstantEdgeExtension());
    bilinear_type dem2_interp =
      interpolate(dem2_dmg, BilinearInterpolation(), ConstantEdgeExtension());
    nearest_type dem1_nearest =
      interpolate(dem1_dmg, NearestPixelInterpolation(), ConstantEdgeExtension());
    nearest_type dem2_nearest =
      interpolate(dem2_dmg, NearestPixelInterpolation(), ConstantEdgeExtension());

    GeoReference ortho1_georef, ortho2_georef, dem1_georef, dem2_georef;
    read_georeference(ortho1_georef, ortho1_rsrc);
    read_georeference(ortho2_georef, ortho2_rsrc);
    read_georeference(dem1_georef, dem1_rsrc);
    read_georeference(dem2_georef, dem2_rsrc);

    std::vector<InterestPoint> matched_ip1, matched_ip2;

    match_orthoimages(opt.ortho1_name, opt.ortho2_name,
                      matched_ip1, matched_ip2, opt.max_points);

    vw_out() << "\t--> Rejecting outliers using RANSAC.\n";
    std::vector<Vector4> ransac_ip1, ransac_ip2;
    for (size_t i = 0; i < matched_ip1.size(); i++) {
      Vector2 point1 = ortho1_georef.pixel_to_lonlat(Vector2(matched_ip1[i].x, matched_ip1[i].y));
      Vector2 point2 = ortho2_georef.pixel_to_lonlat(Vector2(matched_ip2[i].x, matched_ip2[i].y));

      Vector2 dem_pixel1 = dem1_georef.lonlat_to_pixel(point1);
      Vector2 dem_pixel2 = dem2_georef.lonlat_to_pixel(point2);

      if ( dem1_nearest(dem_pixel1.x(),dem_pixel1.y()) != opt.dem1_nodata &&
           dem2_nearest(dem_pixel2.x(),dem_pixel2.y()) != opt.dem2_nodata ) {

        double alt1 = dem1_georef.datum().radius(point1.x(), point1.y()) + dem1_interp(dem_pixel1.x(), dem_pixel1.y());
        double alt2 = dem2_georef.datum().radius(point2.x(), point2.y()) + dem2_interp(dem_pixel2.x(), dem_pixel2.y());

        Vector3 xyz1 = lon_lat_radius_to_xyz(Vector3(point1.x(), point1.y(), alt1));
        Vector3 xyz2 = lon_lat_radius_to_xyz(Vector3(point2.x(), point2.y(), alt2));

        ransac_ip1.push_back(Vector4(xyz1.x(), xyz1.y(), xyz1.z(), 1));
        ransac_ip2.push_back(Vector4(xyz2.x(), xyz2.y(), xyz2.z(), 1));
      } else {
        vw_out() << "Actually dropped something.\n";
      }
    }

    std::vector<size_t> indices;
    Matrix<double> trans;
    math::RandomSampleConsensus<math::AffineFittingFunctorN<3>,math::L2NormErrorMetric>
      ransac( math::AffineFittingFunctorN<3>(), math::L2NormErrorMetric(), 10);
    trans = ransac(ransac_ip1, ransac_ip2);
    indices = ransac.inlier_indices(trans, ransac_ip1, ransac_ip2);

    vw_out() << "\t    * Ransac Result: " << trans << "\n";
    vw_out() << "\t                     # inliers: " << indices.size() << "\n";

    { // Saving transform to human readable text
      std::string filename = (fs::path(opt.dem1_name).branch_path() / (fs::basename(fs::path(opt.dem1_name)) + "__" + fs::basename(fs::path(opt.dem2_name)) + "-Matrix.txt")).string();
      std::ofstream ofile( filename.c_str() );
      ofile << std::setprecision(15) << std::flush;
      ofile << "# inliers: " << indices.size() << endl;
      ofile << trans << endl;
      ofile.close();
    }

    ImageViewRef<PixelMask<double> > dem1_masked(create_mask(dem1_dmg, opt.dem1_nodata));

    ImageViewRef<Vector3> point_cloud =
      lon_lat_radius_to_xyz(project_point_image(dem_to_point_image(dem1_masked, dem1_georef), dem1_georef, false));
    ImageViewRef<Vector3> point_cloud_trans =
      per_pixel_filter(point_cloud, HomogeneousTransformFunctor<3>(trans));

    DiskImageResourceGDAL point_cloud_rsrc(opt.output_prefix + "-PC.tif",
                                           point_cloud_trans.format(),
                                           opt.raster_tile_size,
                                           opt.gdal_options);
    block_write_image(point_cloud_rsrc, point_cloud_trans,
                      TerminalProgressCallback("asp", "\t--> Transforming: "));
  } ASP_STANDARD_CATCHES;

  return 0;
}


#include <iostream>
#include <boost/format.hpp>
// Qt headers
#include <QtGui>
#include <CGAL/Qt/GraphicsViewNavigation.h>
#include <QLineF>
#include <QRectF>
#include <QApplication>
#include <QGraphicsScene>
#include <QGraphicsView>
// CGAL headers
#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/utility.h>
// GraphicsView items and event filters (input classes)
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Largest_empty_iso_rectangle_2.h>
// the two base classes
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsRectItem>
#include <QGraphicsLineItem>
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Segment_2 Segment_2;
using namespace std;

struct Coords
{
   int w, x, y, z;
};

int main(int argc, char **argv)
{
	CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
	std::vector<Point_2> points;
	std::vector<Coords> pointList;

	QApplication app(argc, argv);
	QGraphicsScene scene;

	FILE *fp = NULL;
	fp = fopen("../src/input.txt","r");
	int startx,starty,endx,endy;
	while(fscanf(fp,"%d %d %d %d",&startx,&starty,&endx,&endy)!=EOF){

		pointList.push_back({startx,starty,endx,endy});

	}

	for (int i=0;i<pointList.size();i++) {
		for (int j=i+1;j<pointList.size();j++) {

			scene.addLine(QLineF(pointList[i].w,pointList[i].x,pointList[i].y,pointList[i].z));
			scene.addLine(QLineF(pointList[j].w,pointList[i].x,pointList[j].y,pointList[i].z));
			scene.setSceneRect(0,0, 100, 100);

		}

	}

	QGraphicsView* view = new QGraphicsView(&scene);
	CGAL::Qt::GraphicsViewNavigation navigation;
	view->installEventFilter(&navigation);
	view->viewport()->installEventFilter(&navigation);
	view->setRenderHint(QPainter::Antialiasing);
	view->show();

    return app.exec();
}