#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <armadillo>
#pragma warning(disable:4996)
using namespace std;
using namespace arma;

int rand_int(int n) {
	return randi<int>(distr_param(0, n - 1));
}
uvec random_sample(uvec x, int k) {
	int n = x.n_elem;
	uvec sample(k);
	if (k == 1)
		sample(0) = x(rand_int(n));
	else {
		uvec pom = shuffle(x);
		sample = pom(span(0, k - 1));
	}
	return sample;
}
uvec u_diff_v(uvec u, uvec v) {
	int n = u.n_elem;
	uvec r(n);
	int k = 0;
	for (int i = 0; i < n; i++)
	{
		if (!any(u(i) == v)) {
			r(k) = u(i);
			k++;
		}
	}
	if (k == 0)
		return uvec();
	else
		return r(span(0, k - 1));
}
urowvec exchange(urowvec x, uvec ind) {
	urowvec x_e = x;
	int p = ind(0);
	int d = ind(1);
	x_e(p) = x(d);
	x_e(d) = x(p);
	return x_e;
}
void all_ind(umat& I, int n) {
	int k = 0;
	for (unsigned int i = 0; i < n; i++) {
		for (unsigned int j = 0; j < n; j++)
		{
			if (i != j) {
				I.col(k) = { i,j };
				k++;
			}
		}
	}
}
urowvec inversion(urowvec x, uvec ind) {
	int n = x.n_elem;
	int l = min(ind);
	int d = max(ind);
	urowvec x_i;
	if (l == 0 && d == n - 1) {
		x_i = reverse(x);
		return x_i;
	}
	if (l == 0) {
		x_i = join_horiz(reverse(x(span(l, d))), x(span(d + 1, n - 1)));
		return x_i;
	}
	if (d == n - 1) {
		x_i = join_horiz(x(span(0, l - 1)), reverse(x(span(l, d))));
		return x_i;
	}
	x_i = join_horiz(join_horiz(x(span(0, l - 1)), reverse(x(span(l, d)))), x(span(d + 1, n - 1)));
	return x_i;
}
urowvec insertion(urowvec x, uvec ind) {
	int e = ind(0);
	int p = ind(1);
	int n = x.n_elem;
	urowvec x_i = x;
	x_i.replace(x(p), x(e));
	if (e < p) {
		if (e > 0) {
			x_i = join_horiz(join_horiz(x(span(0, e - 1)), x_i(span(e + 1, p))), x(span(p, n - 1)));
		}
		else {
			x_i = join_horiz(x_i(span(e + 1, p)), x(span(p, n - 1)));
		}
	}
	else {
		if (e < n - 1) {
			x_i = join_horiz(join_horiz(x_i(span(0, p)), x(span(p, e - 1))), x(span(e + 1, n - 1)));
		}
		else {
			x_i = join_horiz(x_i(span(0, p)), x(span(p, e - 1)));
		}
	}
	return x_i;
}
void mat_rastojanja(mat& C, mat koord) {
	int n = koord.n_rows;
	for (int i = 0; i < n; i++)
	{
		for (int j = 0; j < n; j++)
		{
			double delta_x = koord(i, 0) - koord(j, 0);
			double delta_y = koord(i, 1) - koord(j, 1);
			C(i, j) = sqrt(delta_x*delta_x  + delta_y*delta_y);
		}
	}
	C = round(C);// Optimalna rešenja koja su na sajtu dobijena su pri radu iskljucivo sa celim brojevima. Zato sva rastojanja zaokruzujemo na ceo broj.
}
//f-ja racuna kapacitet rute ruta, vektor d sadrzi potrebe korisnika
double kapacitet_rute(urowvec ruta, vec d) {
	double kapacitet = 0;
	int l = ruta.n_elem;
	for (int i = 0; i < l; i++)
	{
		kapacitet += d(ruta(i));
	}
	return kapacitet;
}
urowvec inicijalno_resenje(int n, int p, int Q, vec d) {
	umat rute = zeros<umat>(p,n);
	vec d_sort = sort(d, "descend");
	uvec d_ind_sort = sort_index(d, "descend");
	cout << d_sort << endl;
	for (int i = 0; i < p; i++)
	{
		rute(i, 1) = d_ind_sort(i); //prvih p korisnika sa najvecim kapacitetom se rasporedjuje svaki u po jedno vozilo tj rutu
	}
	//rasporedjujemo ostale korisnike iz vektora d_ind_sort (), bez (poslednjeg-to je skladiste)
	for (int i = p; i < n-1; i++)
	{
		//pokusavamo da smestimo i-tog korisnika redom u svaku rutu; smestamo ga u prvu rutu u koju staje (po kapacitetu)
		for (int j = 0; j < p; j++)
		{
			if (kapacitet_rute(rute.row(j),d) + d_sort(i) <= Q) {
				uvec nule = find(rute.row(j) == 0);
				rute(j,nule(1)) = d_ind_sort(i);
				break;
			}
		}
	}
	//transformisemo matricu rute u vektor sol, tj. nadovezujemo vrste matrice rute
	urowvec ruta=rute.row(0);
	uvec nule = find(ruta == 0);
	urowvec sol=ruta(span(0,nule(1)-1));
	for (int i = 1; i < p; i++)
	{
		ruta = rute.row(i);
		nule = find(ruta == 0);
		ruta = ruta(span(0, nule(1)-1));
		sol = join_horiz(sol, ruta);
	}
	return sol;
}

//f-ja proverava kapacitete svih ruta u okviru resenja sol
//ukoliko ruta bar jednog vozila prevazilazi kapacitet Q, f-ja vraca 1. Inace vraca 0
int provera_kapaciteta(urowvec sol, vec d, int Q) {
	int ind = 0;
	uvec indexi0 = find(sol == 0);
	urowvec ruta;
	int l = indexi0.n_elem; //broj ruta
	for (int i = 0; i < l; i++)
	{
		if (i == l - 1 && sol(0) == 0) {
			ruta = sol(span(indexi0(i), sol.n_elem - 1));

		}
		else {
			if (i == l - 1 && sol(0) != 0) {
				ruta = join_horiz(sol(span(indexi0(i), sol.n_elem - 1)), sol(span(0, indexi0(0) - 1)));
			}
			else
				ruta = sol(span(indexi0(i), indexi0(i + 1) - 1));	
		}		
		if (kapacitet_rute(ruta, d) > Q) {
			ind = 1;
			break;
		}
	}
	return ind;
}

double f_cilja(mat C, urowvec sol) {
	double f = 0;
	uvec indexi0 = find(sol == 0);
	int l = indexi0.n_elem;
	urowvec ruta;
	for (int i = 0; i < l; i++){
		if (i == (l - 1) && sol(0) == 0) {
			ruta = sol(span(indexi0(i), sol.n_elem - 1));
		}
		else
		{if(i==(l-1) && sol(0)!=0)
			ruta=join_horiz(sol(span(indexi0(i), sol.n_elem - 1)),sol(span(0,indexi0(0)-1)));
		else
			ruta = sol(span(indexi0(i), indexi0(i + 1) - 1));
		}
		//racunamo troskove obilaska tekuce rute
		for (int j = 0; j < ruta.n_elem; j++)
		{	if(j== ruta.n_elem-1)
			f += C(ruta(ruta.n_elem - 1), 0);
		else
			f += C(ruta(j), ruta(j + 1));
		}
	}
	return f;
}
//Nasumicni element iz SIMPGEN okoline-potrebno za SA
urowvec SIMPGEN(urowvec sol) {

	int n = sol.n_elem;
	urowvec sol_SIMPGEN;
	uvec v = linspace<uvec>(0, n - 1, n);
	uvec ind = random_sample(v, 4);
	uvec sort_ind = sort(ind);
	uvec AB = { sort_ind(0), sort_ind(3) };
	uvec CD = {sort_ind(1), sort_ind(2)};
	sol_SIMPGEN = inversion(sol, AB);
	sol_SIMPGEN = inversion(sol_SIMPGEN, CD);
	return sol_SIMPGEN;
}
//SA:
//sol_i je pocetno resenje
//T0 su pocetna i krajnja temperatura
//parametar alpha je koeficijent hladjenja
//parametar max_iter je broj iteracija na jednom temperaturnom nivou
//parametar beta omugucava da se povecava broj iteracija na nekom temperaturnom nivou kako pretraga odmice
double SA(urowvec sol_i, double f_i, double T0, double Tf, int max_iter, double alpha, double beta,vec d, int Q,mat C) {
	urowvec best = sol_i;
	double best_f = f_i;
	double T = T0; 
	urowvec new_sol;
	double new_f;
	double delta;
	while (T > Tf) {
		for (int i = 0; i < max_iter; i++)
		{
			new_sol = SIMPGEN(sol_i);
			while (provera_kapaciteta(new_sol, d, Q)==1) { //proveravamo dopustivost resenja nakon SIMPGEN transformacije
				new_sol = SIMPGEN(sol_i);
			}
			new_f = f_cilja( C,new_sol);
			delta = new_f - f_i;
			if (delta <= 0) {
				sol_i = new_sol;
				f_i = new_f;
			}
			else {
				double rand = randu<double>();
				if (rand < exp(-delta / T)) {
					sol_i = new_sol;
					f_i = new_f;
				}
			}
			if (new_f < best_f) {
				best_f = new_f;
				best = new_sol;
			}
		}
		//podesavamo temperaturu
		T=T*alpha;
		//podesavamo broj iteracija na svakoj T
		vec v = { 200,beta * max_iter };//ne dopustamo da broj iteracija predje 200
		max_iter = min(v);
	}
	cout << best << endl;
	return best_f;
}




//******** TS************
//f-ja proverava da li potez (par indeksa potez) pripada Tabu listi, vraca 1 ukoliko pripada i 0 ako ne
int potez_u_TL(umat TL, uvec potez) {
	int l = TL.n_cols; //trenutna duzina TL
	int rez = 0;

	if (l == 0) {
		return rez;
	}
	for (int i = 0; i < l; i++)
	{
		if (all(TL.col(i) == potez)) {
			rez = 1;
			break;
		}
	}
	return rez;
}
//f-ja pretrazuje okolinu tekuceg resenja x i trzi najboljeg suseda u smislu f-je cilja
//indikator je 1,2,3 za operator zamene/inverzije/umetanja pri kreiranju okoline
//vraca najboljeg suseda
//aspiracijski kriterijum postoji i kada je upotrebljen inikatorA se postavlja na 1
//potez koji vodi do nadjenog najboljeg suseda se upisuje u uvec potez
urowvec pretrazi_okolinu(urowvec x, double f_best, mat C, umat TL, vec d, int Q, int indikator, int& indikatorA, uvec& potez) {
	urowvec newx;
	int n = x.n_elem;
	umat I = zeros<umat>(2, n * n - n);
	all_ind(I, n);
	umat P = zeros<umat>(n * n - n,n);
	vec F = zeros<vec>(n * n - n, 1);
	indikatorA = 0;
	//pretrazujemo okolinu resenja x
	for (int i = 0; i < n * n - n; i++)
	{
		uvec ind = I.col(i);
		switch (indikator)
		{
		case 1:
			P.row(i) = exchange(x, ind);
			break;
		case 2:
			P.row(i) = inversion(x, ind);
			break;
		case 3:
			P.row(i) = insertion(x, ind);
			break;
		}
		if (provera_kapaciteta(P.row(i), d, Q) == 0) { //dopustivo je resenje
			if (potez_u_TL(TL, ind) == 0 || f_cilja(C, P.row(i)) < 0.95 * f_best) { //ne pripada Tl, ili zadovoljava aspiracijski kriterijum
				F(i) = f_cilja(C, P.row(i));
			}
			else {
				F(i) = datum::inf; //za resenja u TL postavljamo brednost na beskonacno, da ih ne bismo razmatrali
			}
		}	
		else{
			F(i) = datum::inf; //isto i za resenja koja nisu dopustiva
		}
	}
	//trazimo najbolje resenje iz formirane okoline
	int ind_min = F.index_min();
	newx = P.row(ind_min);
	potez = I.col(ind_min);//ovaj potez treba da se doda u TL
	if (F(ind_min) < f_best & potez_u_TL(TL,potez) == 1) {
		indikatorA = 1;
	}

	return newx;
}
void izbaci_potez_iz_TL(umat& TL, uvec potez) {
	int r = TL.n_cols;
	int i;
	for (i = 0; i < r; i++)
	{
		if (all(TL.col(i) == potez))
			break;
	}
	//potez se nalaza u i-toj koloni

	for (int k = i+1; k < r; k++)
	{
		TL.col(k - 1) = TL.col(k);
	}
	TL = TL.cols(0, r - 2);
}

double TS(urowvec sol, double f, int r, int max_iterTS, mat C, vec d, int Q) {
	urowvec x_best = sol;
	double f_best = f;
	umat TL;
	uvec potez;
	int indikatorA = 0;
	urowvec newx;
	double newf;

	int iter = 0;
	while (iter < max_iterTS) {
		iter++;
		newx = pretrazi_okolinu(sol, f_best, C, TL, d, Q, 1, indikatorA, potez);
		
		if (indikatorA == 1)
			izbaci_potez_iz_TL(TL, potez);
		newf = f_cilja(C, newx);
		if (newf < f_best) {
			f_best = newf;
			x_best = newx;
		}
		TL = join_rows(potez, TL);//dodajemo potez na pocetak TL
		if (TL.n_cols > r) {
			TL = TL.cols(0, r - 1);
		}
		sol = newx;
	}
	cout << x_best << endl;
	return f_best;
}



int main(int argc, char** argv) {
	ifstream data("p-n16-k8.txt");
	int n, p, Q;
	data >> n;
	data >> p;
	data >> Q;
	mat koord = zeros<mat>(n, 2);
	uvec rbr = zeros<uvec>(n, 1);
	vec d = zeros<vec>(n, 1);

	for (int i = 0; i < n; i++)
	{
		data >> rbr(i);
		data >> koord(i, 0);
		data >> koord(i, 1);
	}
	for (int i = 0; i < n; i++)
	{
		data >> rbr(i);
		data >> d(i);
	}
	cout << koord << endl;
	cout << "Podaci su uspesno ucitani." << endl;
	mat C = zeros<mat>(n, n);
	mat_rastojanja(C, koord);

	//***inicijalno resenje
	urowvec sol_i = inicijalno_resenje(n, p, Q, d);
	cout << "Inicijalno resenje: " << sol_i << endl;
	double f_i = f_cilja(C, sol_i);
	cout << f_i << endl;

	//******SA parametri**********
	double T0 = 200; //polazna temperatura
	double Tf = 0.1; //krajnja temperatura
	double alpha = 0.96; //parametar hladjenja
	double max_iter = 20; //broj iteracija koje provedemo na istoj temperaturi (na svakih max_iter iteracija, temperatura se snizava)
	double beta = 1.05;
	
	double f_SA = SA(sol_i, f_i, T0, Tf, max_iter, alpha, beta, d, Q, C);
	cout << "SA: vrednost f - ja cilja: " << f_SA << endl;


	//*******TS parametri********
	int r = 10;//duzina TL
	int max_iterTS = 600;//kriterijum zaustavljanja
	double f_TS = TS(sol_i, f_i, r, max_iterTS, C, d, Q);
	cout << "TS: vrednost f - ja cilja: " << f_TS << endl;

}