//compile with: g++ RTPlearning.cpp nrutil.cpp -o PS -O2 -w
// working if at each episode the glow matrix is initialized to zero
#include <stdio.h> 
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <math.h>
#include <string>
#include <sstream>
#include <fstream>
#include <bits/stdc++.h>
using namespace std;

#include "RTPlearning.h"
#include "nrutil.h"

#define PI 3.14159265358979323846264338

 
int main (int argc, char**argv) {
	// for parallelization and restarting (when restarting a parallel simulation, N_runs should not change)
	k_bunch = atoi(argv[1]);
	
	
	// reading various parameters
	read_parameters ();
	
	// environment parameters
	Nphi = 2;							// number of phases
	M = 2 + int((R_max-R_target)/Dr);	// number of distance bins
	Nomegas = 2;						// number of values of the additional perceptor
	Ns = Nphi*M*Nomegas;				// number of states
	Na = 2;								// number of actions
	
	// other settings
	srand (12345+k_bunch*N_episodes+k_bunch*N_episodes+N_episodes0);									// initialize random number generator
	
	// arrays initialization
	array_initialization();
	
	// utilities initialization
	v_factor1 = dvector(0,Nt_single_episode);
	v_factor2 = dvector(0,Nt_single_episode);
	v_factor3 = dvector(0,Nt_single_episode);
	H2 = dvector(0,Na-1);
	visited_times = i3tensor(0,Ns-1,0,Na-1,0,Nt_single_episode);
	v_factor1[0]=1.0;
	for (int i=1; i<=Nt_single_episode; i++){
		v_factor1[i]=pow((1.0-gammma),i*1.0);
	}
	v_factor2[0] = 0.0;
	v_factor2[1] = gammma;
	for (int i=2; i<=Nt_single_episode; i++){
		v_factor2[i] = v_factor2[i-1] + gammma*v_factor1[i-1];
	}
	v_factor3[0]=1.0;
	for (int i=1; i<=Nt_single_episode; i++){
		v_factor3[i]=pow((1.0-eta),i*1.0);
	}

	////////////////////////////////////////////////////////////////////
	// run learning for several independent processes
	for (int run=0; run<N_runs; run++){
		printf("  %d out of %d; ep: %d to %d  (Pe = %5.1f, bunch %d)\n" , run+1,N_runs,N_episodes0+1,N_episodes0+N_episodes,Pe, k_bunch);

		ProjectiveSimulationOptimized(run);

		for (int s=0; s<Ns; s++){
			Pvalues[run][s] = H[s][1]/(H[s][0]+H[s][1]);
			Svalues[run][s] = H[s][0];
			avg_Pvalues[s] += Pvalues[run][s];
			for (int a=0; a<Na; a++){
				avg_state_action_values[s][a]+=H[s][a];
				avg_glow[s][a]+=G[s][a];
			}
		}
	}

	// compute final averages
	for (int s = 0; s<Ns; s++){
		avg_Pvalues[s] = avg_Pvalues[s]/N_runs;
		for (int a=0; a<Na; a++){
			avg_state_action_values[s][a] = avg_state_action_values[s][a]/N_runs;
			avg_glow[s][a] = avg_glow[s][a]/N_runs;
		}
	}
	
	for (int epindex=Nep0_arrays; epindex<=Nep_arrays; epindex++){
		for (int run=0; run<N_runs; run++){
			if (target_times[run][epindex]>0.0) {
				avg_target_times[epindex]+=target_times[run][epindex];
				number_targets[epindex]+=1;
				int ir = rindex(initial_radii[run][epindex]);
				number_targets_rini[ir][epindex]+=1;
				avg_target_times_rini[ir][epindex]+=target_times[run][epindex];
			}
		}
		if (number_targets[epindex]>0) {
			avg_target_times[epindex]=avg_target_times[epindex]/number_targets[epindex];
		}
		for (int i = 0; i<M; i++) {
			if (number_targets_rini[i][epindex]>0) {
				avg_target_times_rini[i][epindex]=avg_target_times_rini[i][epindex]/number_targets_rini[i][epindex];
			}
		}
		
		for (int s = 0; s<Ns; s++){
			avg_Pvalues_episode[epindex][s]=avg_Pvalues_episode[epindex][s]/N_runs;
			avg_Hvalues_episode[epindex][s][0]=avg_Hvalues_episode[epindex][s][0]/N_runs;
			avg_Hvalues_episode[epindex][s][1]=avg_Hvalues_episode[epindex][s][1]/N_runs;
		}
	}

	// saving results
	print_averages ();
	print_data ();
}

////////////////////////////////////////////////////////////////////////
void read_parameters () {
	FILE* fp = fopen("Parameters.txt", "r");
	if (fp == NULL) {
		exit(EXIT_FAILURE);
	}
	char str [20];
	char* line = NULL;
	size_t len = 0;
	int k = -1;
	while ((getline(&line, &len, fp)) != -1) {
		// using printf() in all tests for consistency
		k++;
		printf("%d %s", k,line);
		
		if (k==1) {
			sscanf (line,"%s %*s %lf",str,&L);
			//~ printf("        L = %11.8f\n", L);
		} else if (k==2) {
			sscanf (line,"%s %*s %lf",str,&tau);
			//~ printf("        tau = %11.8f\n", tau);
		} else if (k==4) {
			sscanf (line,"%s %*s %lf",str,&Pe);
			//~ printf("        Pe = %11.8f\n", Pe);
		} else if (k==5) {
			sscanf (line,"%s %*s %lf",str,&ell);
			//~ printf("        ell = %11.8f\n", ell);
		} else if (k==6) {
			sscanf (line,"%s %*s %lf",str,&mu);
			//~ printf("        mu = %11.8f\n", mu);
		} else if (k==7) {
			sscanf (line,"%s %*s %lf",str,&R_target);
			//~ printf("        R = %11.8f\n", R_target);
		} else if (k==9) {
			sscanf (line,"%s %*s %lf",str,&dt);
			//~ printf("        dt = %11.8f\n", dt);
		} else if (k==11) {
			sscanf (line,"%s %s %*s %d",str,str,&N_runs);
			//~ printf("        N runs = %d\n", N_runs);
		} else if (k==12) {
			sscanf (line,"%s %s %*s %d",str,str,&N_episodes);
			//~ printf("        N episodes = %d\n", N_episodes);
		} else if (k==13) {
			sscanf (line,"%s %s %*s %d",str,str,&N_episodes0);
			//~ printf("        N episodes = %d\n", N_episodes0);	
		} else if (k==14) {
			sscanf (line,"%s %s %*s %d",str,str,&Nep_last);
			//~ printf("        N episodes last = %d\n", Nep_last);
		} else if (k==15) {
			sscanf (line,"%s %*s %lf",str,&time_single_episode);
			//~ printf("        T = %11.8f\n", time_single_episode);
		} else if (k==16) {
			sscanf (line,"%s %*s %lf",str,&Dr);
			//~ printf("        Dr = %11.8f\n", Dr);	
		} else if (k==17) {
			sscanf (line,"%s %*s %lf",str,&R_max);
			//~ printf("        R max = %11.8f\n", R_max);
		} else if (k==18) {
			sscanf (line,"%s %*s %lf",str,&reward);
			//~ printf("        reward = %11.8f\n", reward);
		} else if (k==19) {
			sscanf (line,"%s %*s %lf",str,&gammma);
			//~ printf("        gamma = %11.8f\n", gammma);
		} else if (k==20) {
			sscanf (line,"%s %*s %lf",str,&eta);
			//~ printf("        eta = %11.8f\n", eta);
		} else if (k==21) {
			sscanf (line,"%s %s %s %*s %d",str,str,str,&const_initial_policy);
			//~ printf("        const_initial_policy = %d\n", const_initial_policy);
		}
		if (const_initial_policy == 1) {
			if (k==22) {
				sscanf (line,"%s %*s %lf",str,&pBP);
				//~ printf("        pBP = %11.8f\n", pBP);
			} else if (k==23) {
				sscanf (line,"%s %*s %lf",str,&pABP);
				//~ printf("        pABP = %11.8f\n", pABP);
			}
		}
	}
	fclose(fp);
	if (line) {
		free(line);
	}
	
	// ... it follows
	D = L*L/(4*tau);		// diffusion coefficient for the passive particle (kept the same also for the ABP phase)
	v = Pe * L / tau;		// self-propulsion velocity (it must be such that typically the particle do not meet the target)
	D_theta = v/(ell*L);	// rotational diffusion coefficient
	Nt_single_episode = int(time_single_episode/dt);
	
	N_runs0 = k_bunch*N_runs;
	Nep0_arrays = ep_index(N_episodes0)+1;
	Nep_arrays = ep_index(N_episodes0+N_episodes);
	if (Nep_arrays<=0) {
		printf("error arrays from %d tp %d\n",Nep0_arrays,Nep_arrays);
	}
	
	// check and print parameters
	//~ printf("typical step length due to diffusion, to activity =  %f L ,%f L\n",sqrt(4*D*dt),v*dt);
	//~ printf("bunch %d\n",k_bunch);
	//~ printf("running from episode %d to episode %d\n",N_episodes0,N_episodes0+N_episodes-1);
}

void array_initialization(){
	target_times = dmatrix(0,N_runs-1,Nep0_arrays,Nep_arrays);
	initial_radii = dmatrix(0,N_runs-1,Nep0_arrays,Nep_arrays);
	initial_thetas = dmatrix(0,N_runs-1,Nep0_arrays,Nep_arrays);
	target_times_lastepisodes = dmatrix(0,N_runs-1,N_episodes-Nep_last+1,N_episodes);
	initial_radii_lastepisodes = dmatrix(0,N_runs-1,N_episodes-Nep_last+1,N_episodes);
	initial_thetas_lastepisodes = dmatrix(0,N_runs-1,N_episodes-Nep_last+1,N_episodes);
	H = dmatrix(0,Ns-1,0,Na-1);				// h-values (the number of states should match with Nt_max_phase_duration)
	H0 = dmatrix(0,Ns-1,0,Na-1);			// initial h-values (the number of states should match with Nt_max_phase_duration)
	G = dmatrix(0,Ns-1,0,Na-1);				// Glow matrix
	Pvalues = dmatrix(0,N_runs-1,0,Ns-1);
	Svalues = dmatrix(0,N_runs-1,0,Ns-1);
	avg_Pvalues = dvector(0,Ns-1);
	avg_Pvalues_episode = dmatrix(Nep0_arrays,Nep_arrays,0,Ns-1);
	avg_Hvalues_episode = d3tensor(Nep0_arrays,Nep_arrays,0,Ns-1,0,Na-1);
	avg_state_action_values = dmatrix(0,Ns-1,0,Na-1);
	avg_glow = dmatrix(0,Ns-1,0,Na-1);
	avg_target_times = dvector(Nep0_arrays,Nep_arrays);
	number_targets = ivector(Nep0_arrays,Nep_arrays);
	number_targets_rini = imatrix(0,M-1,Nep0_arrays,Nep_arrays);
	avg_target_times_rini = dmatrix(0,M-1,Nep0_arrays,Nep_arrays);
	changes_in_G = imatrix(0,Ns-1,0,Na-1);
	
	
	// H0 values
	for (int s = 0; s<Ns; s++){
		int phi = int(s/(Ns/2));
		int omega = int((s%(Ns/2))/(Ns/4));
		int ir = s%M;
		if (phi==0) {
			H0[s][0]=1.0-pBP;
			H0[s][1]=pBP;
		} else if (phi==1) {
			H0[s][0]=1.0-pABP;
			H0[s][1]=pABP;
		}
	}
	
	// average switching probability values, average H values, average glow values
	for (int s = 0; s<Ns; s++){
		avg_Pvalues[s]=0.0;
		for (int a=0; a<Na; a++){
			avg_state_action_values[s][a]=0.0;
			avg_glow[s][a]=0.0;
		}
	}
	
	// average switching probabilities, average values of H at the saving episodes
	for (int epindex=Nep0_arrays; epindex<=Nep_arrays; epindex++){
		for (int s = 0; s<Ns; s++){
			avg_Pvalues_episode[epindex][s]=0.0;
			avg_Hvalues_episode[epindex][s][0]=0.0;
			avg_Hvalues_episode[epindex][s][1]=0.0;
		}
	}
	
	// average target time (if target is found), number of found targets (also as a function of the initial radius)
	for (int epindex=Nep0_arrays; epindex<=Nep_arrays; epindex++){
		avg_target_times[epindex] = 0.;
		number_targets[epindex] = 0;
		for (int i = 0; i<M; i++) {
			number_targets_rini[i][epindex] = 0;
			avg_target_times_rini[i][epindex] = 0.;
		}
	}
	
	// target times, initial radii, initial directions (2 \pi n  means pointing towards the target) at the saving episodes and in the last Nep_last episodes)
	for (int run=0; run<N_runs; run++){
		for (int epindex=Nep0_arrays; epindex<=Nep_arrays; epindex++){
			target_times[run][epindex] = 0.;
			initial_radii[run][epindex] = 0.;
			initial_thetas[run][epindex] = 0.;
		}
		for (int ep=N_episodes-Nep_last+1; ep<= N_episodes; ep++){
			target_times_lastepisodes[run][ep] = 0.;
			initial_radii_lastepisodes[run][ep] = 0.;
			initial_thetas_lastepisodes[run][ep] = 0.;
		}
	}
	
	// read the H learned at the previous step
	if (N_episodes0 != 0) {// if it is a restart
		H_restart = d3tensor(0,N_runs,0,Ns-1,0,Na-1);
		read_H_restart();
	}
}

void print_averages () {
	std::stringstream sstr;
	sstr << "AvgHvalues_bunch" << k_bunch << "_episode" <<  N_episodes0+N_episodes  << ".dat";
	const std::string tmp = sstr.str();
	const char* cstr = tmp.c_str();
	FILE * out = fopen(cstr,"w");
	for (int s = 0; s<Ns; s++){
		fprintf(out,"%5d %2d  %14.8f \n" , s,0,avg_state_action_values[s][0]);
		fprintf(out,"%5d %2d  %14.8f \n" , s,1,avg_state_action_values[s][1]);
	}
	fclose(out);

	std::stringstream sstr2;
	sstr2 << "AvgGvalues_bunch" << k_bunch << "_episode" <<  N_episodes0+N_episodes  << ".dat";
	const std::string tmp2 = sstr2.str();
	const char* cstr2 = tmp2.c_str();
	FILE * out2 = fopen(cstr2,"w");
	for (int s = 0; s<Ns; s++){
		fprintf(out2,"%5d %2d  %12.2f \n" , s,0,avg_glow[s][0]);
		fprintf(out2,"%5d %2d  %12.2f \n" , s,1,avg_glow[s][1]);
	}
	fclose(out2);

	std::stringstream sstr3;
	sstr3 << "AvgPvalues_bunch" << k_bunch << "_episode" <<  N_episodes0+N_episodes  << ".dat";
	const std::string tmp3 = sstr3.str();
	const char* cstr3 = tmp3.c_str();
	FILE * out3 = fopen(cstr3,"w");
	for (int s = 0; s<Ns/2; s++){
		fprintf(out3,"%5d  %10.8f %10.8f \n" , s,avg_Pvalues[s],avg_Pvalues[s+Ns/2] );
	}
	fclose(out3);
	
	std::stringstream sstr4;
	sstr4 << "AvgTargetTimes_bunch" << k_bunch << ".dat";
	const std::string tmp4 = sstr4.str();
	const char* cstr4 = tmp4.c_str();
	FILE * out4 = fopen(cstr4,"a");
	for (int ep = 1; ep<=N_episodes; ep++){
		if (ep<=10 || (ep>10 && ep<=100 && ep%10==0) || (ep>100 && ep<=1000 && ep%100==0) || (ep>1000 && ep<=10000 && ep%1000==0) || (ep>10000 && ep<=100000 && ep%10000==0) || (ep>100000 && ep<=1000000 && ep%100000==0) ){
			int epindex = ep_index(N_episodes0+ep);
			if (epindex>0){
				fprintf(out4,"%7d  %10.6f %6d\n" , N_episodes0+ep,avg_target_times[epindex],number_targets[epindex]);
			}
		}
	}
	fclose(out4);
	
	std::stringstream sstr5;
	sstr5 << "AvgPvaluesEpisode_bunch" << k_bunch << ".dat";
	const std::string tmp5 = sstr5.str();
	const char* cstr5 = tmp5.c_str();
	FILE * out5 = fopen(cstr5,"a");
	for (int ep = 1; ep<=N_episodes; ep++){
		if (ep<=10 || (ep>10 && ep<=100 && ep%10==0) || (ep>100 && ep<=1000 && ep%100==0) || (ep>1000 && ep<=10000 && ep%1000==0) || (ep>10000 && ep<=100000 && ep%10000==0) || (ep>100000 && ep<=1000000 && ep%100000==0)){
			int epindex = ep_index(N_episodes0+ep);
			if (epindex>0){
				for (int s = 0; s<Ns/2; s++){
					fprintf(out5,"%7d %5d  %10.8f %10.8f \n" , N_episodes0+ep,s,avg_Pvalues_episode[epindex][s],avg_Pvalues_episode[epindex][s+Ns/2] );
				}
			}
		}
	}
	fclose(out5);
	
	std::stringstream sstr6;
	sstr6 << "AvgHvaluesEpisode_bunch" << k_bunch << ".dat";
	const std::string tmp6 = sstr6.str();
	const char* cstr6 = tmp6.c_str();
	FILE * out6 = fopen(cstr6,"a");
	for (int ep = 1; ep<=N_episodes; ep++){
		if (ep<=10 || (ep>10 && ep<=100 && ep%10==0) || (ep>100 && ep<=1000 && ep%100==0) || (ep>1000 && ep<=10000 && ep%1000==0) || (ep>10000 && ep<=100000 && ep%10000==0) || (ep>100000 && ep<=1000000 && ep%100000==0)){
			int epindex = ep_index(N_episodes0+ep);
			if (epindex>0){
				for (int s = 0; s<Ns/2; s++){
					fprintf(out6,"%7d %5d  %10.8f %10.8f  %10.8f %10.8f \n" , N_episodes0+ep,s,avg_Hvalues_episode[epindex][s][0],avg_Hvalues_episode[epindex][s][1],avg_Hvalues_episode[epindex][s+Ns/2][0],avg_Hvalues_episode[epindex][s+Ns/2][1] );
				}
			}
		}
	}
	fclose(out6);
	
	std::stringstream sstr7;
	sstr7 << "AvgTargetTimes_rini_bunch" << k_bunch << ".dat";
	const std::string tmp7 = sstr7.str();
	const char* cstr7 = tmp7.c_str();
	FILE * out7 = fopen(cstr7,"a");
	for (int i = 0; i<M; i++) {
		for (int ep = 1; ep<=N_episodes; ep++){
			if (ep<=10 || (ep>10 && ep<=100 && ep%10==0) || (ep>100 && ep<=1000 && ep%100==0) || (ep>1000 && ep<=10000 && ep%1000==0) || (ep>10000 && ep<=100000 && ep%10000==0) || (ep>100000 && ep<=1000000 && ep%100000==0)){
				int epindex = ep_index(N_episodes0+ep);
				if (epindex>0){
					fprintf(out7,"%5d %7d  %10.6f %6d\n" , i,N_episodes0+ep,avg_target_times_rini[i][epindex],number_targets_rini[i][epindex]);
				}
			}
		}
	}
	fclose(out7);
}

void print_data () {
	std::stringstream sstr2;
	sstr2 << "Pvalues_bunch" << k_bunch << "_episode" <<  N_episodes0+N_episodes  << ".dat";
	const std::string tmp2 = sstr2.str();
	const char* cstr2 = tmp2.c_str();
	FILE * out2 = fopen(cstr2,"w");
	for (int run=0; run<N_runs; run++){
		for (int s = 0; s<Ns/2; s++){
			fprintf(out2,"%5d %5d  %10.8f %10.8f  %10.4f %10.4f\n" , N_runs0+run,s,Pvalues[run][s],Pvalues[run][s+Ns/2],Svalues[run][s],Svalues[run][s+Ns/2]);
		}
	}
	fclose(out2);
	
	std::stringstream sstr3;
	sstr3 << "TargetTimes_bunch" << k_bunch << ".dat";
	const std::string tmp3 = sstr3.str();
	const char* cstr3 = tmp3.c_str();
	FILE * out3 = fopen(cstr3,"a");
	for (int run=0; run<N_runs; run++){
		for (int ep=1; ep<=N_episodes; ep++){
			if (ep<=10 || (ep>10 && ep<=100 && ep%10==0) || (ep>100 && ep<=1000 && ep%100==0) || (ep>1000 && ep<=10000 && ep%1000==0) || (ep>10000 && ep<=100000 && ep%10000==0) || (ep>100000 && ep<=1000000 && ep%100000==0)){
				int epindex = ep_index(N_episodes0+ep);
				if (epindex>0){
					fprintf(out3,"%5d %7d %10.4f %10.5f %10.5f\n" , N_runs0+run,N_episodes0+ep,target_times[run][epindex],initial_radii[run][epindex],initial_thetas[run][epindex]);
				}
			}
		}
	}
	fclose(out3);
	
	std::stringstream sstr4;
	sstr4 << "TargetTimes_LastEpisodes_bunch" << k_bunch << ".dat";
	const std::string tmp4 = sstr4.str();
	const char* cstr4 = tmp4.c_str();
	FILE * out4 = fopen(cstr4,"a");
	for (int run=0; run<N_runs; run++){
		for (int ep=N_episodes-Nep_last+1; ep<=N_episodes; ep++){
			fprintf(out4,"%5d %7d %10.4f %10.5f %10.5f\n" , N_runs0+run,N_episodes0+ep,target_times_lastepisodes[run][ep],initial_radii_lastepisodes[run][ep],initial_thetas_lastepisodes[run][ep]);
		}
	}
	fclose(out4);
}

void read_H_restart () {
	std::stringstream sstr2;
	sstr2 << "Pvalues_bunch" << k_bunch << "_episode" <<  N_episodes0  << ".dat";
	const std::string tmp2 = sstr2.str();
	const char* cstr2 = tmp2.c_str();
	FILE* fp = fopen(cstr2, "r");
	if (fp == NULL) {
		exit(EXIT_FAILURE);
	}
	char str [20];
	char* line = NULL;
	size_t len = 0;
	int k = -1;
	
	int run;
	int rrun;
	int ss;
	double probBP;
	double probABP;
	double HstayBP;
	double HstayABP;
	
	while ((getline(&line, &len, fp)) != -1) {
		// using printf() in all tests for consistency
		k++;
		sscanf (line,"%d %d %lf %lf %lf %lf",&rrun,&ss,&probBP,&probABP,&HstayBP,&HstayABP);
		run = rrun % N_runs;
		H_restart[run][ss][0] = HstayBP;
		H_restart[run][ss+Ns/2][0] = HstayABP;
		H_restart[run][ss][1] = HstayBP*probBP/(1.0-probBP);
		H_restart[run][ss+Ns/2][1] = HstayABP*probABP/(1.0-probABP);
	}
	fclose(fp);
	if (line) {
		free(line);
	}
}

int rindex(double r){
	int ir = -1;
	if (r <= R_target){
		ir = 0;
	} else if (r > R_max) {
		ir = M-1;
	} else {
		ir = 1 + int((r-R_target)/Dr);
		if (ir >= M-1 || ir<1) {
			printf("Check! State error \n" );
		}
	}
	return ir;
}

int ep_index (int ep) {
	int i;
	i = -1;
	if (ep<=10){
		i = ep;
	} else if (ep==20){
		i = 11;
	} else if (ep==30){
		i = 12;
	} else if (ep==40){
		i = 13;
	} else if (ep==50){
		i = 14;
	} else if (ep==60){
		i = 15;
	} else if (ep==70){
		i = 16;
	} else if (ep==80){
		i = 17;
	} else if (ep==90){
		i = 18;
	} else if (ep==100){
		i = 19;
	} else if (ep==200){
		i = 20;
	} else if (ep==300){
		i = 21;
	} else if (ep==400){
		i = 22;
	} else if (ep==500){
		i = 23;
	} else if (ep==600){
		i = 24;
	} else if (ep==700){
		i = 25;
	} else if (ep==800){
		i = 26;
	} else if (ep==900){
		i = 27;
	} else if (ep==1000){
		i = 28;
	} else if (ep==2000){
		i = 29;
	} else if (ep==3000){
		i = 30;
	} else if (ep==4000){
		i = 31;
	} else if (ep==5000){
		i = 32;
	} else if (ep==6000){
		i = 33;
	} else if (ep==7000){
		i = 34;
	} else if (ep==8000){
		i = 35;
	} else if (ep==9000){
		i = 36;
	} else if (ep==10000){
		i = 37;
	} else if (ep==20000){
		i = 38;
	} else if (ep==30000){
		i = 39;
	} else if (ep==40000){
		i = 40;
	} else if (ep==50000){
		i = 41;
	} else if (ep==60000){
		i = 42;
	} else if (ep==70000){
		i = 43;
	} else if (ep==80000){
		i = 44;
	} else if (ep==90000){
		i = 45;
	} else if (ep==100000){
		i = 46;
	} else if (ep==200000){
		i = 47;
	} else if (ep==300000){
		i = 48;
	} else if (ep==400000){
		i = 49;
	} else if (ep==500000){
		i = 50;
	} else if (ep==600000){
		i = 51;
	} else if (ep==700000){
		i = 52;
	} else if (ep==800000){
		i = 53;
	} else if (ep==900000){
		i = 54;
	} else if (ep==1000000){
		i = 55;
	} else if (ep==2000000){
		i = 56;
	} else if (ep==3000000){
		i = 57;
	} else if (ep==4000000){
		i = 58;
	} else if (ep==5000000){
		i = 59;
	} else if (ep==6000000){
		i = 60;
	} else if (ep==7000000){
		i = 61;
	} else if (ep==8000000){
		i = 62;
	} else if (ep==9000000){
		i = 63;
	} else if (ep==10000000){
		i = 64;
	}
	
	return i;
}

void ProjectiveSimulationOptimized(int run){
	// the state s=(phi,r) include the phase phi=BP or ABP and the distance r discretized as explained in the method section of the paper
	// the action a is stay in current phase (a=0) or switch phase(a=1)
	int phi;							// phase
	int ir;								// index running over the partition of distances
	int omega;							// index of additional perceptor
	double x,y,r;						// particle position and radial distance from the target
	double theta;						// self-propulsion direction
	double x_target,y_target;			// target position
	
	double sigma = sqrt(2*D*dt);
	double sigma_theta = sqrt(2*D_theta*dt);
	double vdt = v*dt;
	
	double theta2;
	double phiold,rold;
	double dx,dy;
	double p;							// probability of switching given that we are in a given state
	int n_targets,s,a,tep;
	double target_time_ep;
	double factor1, factor2;
	int ks,epindex;
	
	// target position
	x_target = 0.0;
	y_target = 0.0;

	// initial h-values matrix
	if (N_episodes0 ==0 ) {
		for (int ss = 0; ss<Ns; ss++){
			H[ss][0]=H0[ss][0];
			H[ss][1]=H0[ss][1];
		}
	} else {
		for (int ss = 0; ss<Ns; ss++){
			H[ss][0]=H_restart[run][ss][0];
			H[ss][1]=H_restart[run][ss][1];
		}
	}
	// initial G-values matrix
	for (int ss = 0; ss<Ns; ss++){
		G[ss][0]=0.0;
		G[ss][1]=0.0;
		changes_in_G[ss][0]=0;
		changes_in_G[ss][1]=0;
	}
	
	// loop over episodes
	for (int episode=1; episode<=N_episodes; episode++){
		epindex = ep_index(N_episodes0+episode);
		// initialize particle position and self-propulsion direction at the beginning of the episode
		theta2 = 2*PI*(rand()/((double) RAND_MAX));
		r_ini = R_max * sqrt( rand()/((double) RAND_MAX) );
		while (r_ini <= R_target) {
			r_ini = R_max * sqrt( rand()/((double) RAND_MAX) );
		}
		x = r_ini*cos(theta2);
		y = r_ini*sin(theta2);
		theta = 2*PI*(rand()/((double) RAND_MAX));
		if (epindex>=Nep0_arrays){
			initial_radii[run][epindex]=r_ini;
			initial_thetas[run][epindex]=theta-theta2;
		}
		if (episode>N_episodes-Nep_last) {
			initial_radii_lastepisodes[run][episode]=r_ini;
			initial_thetas_lastepisodes[run][episode]=theta-theta2;
		}
		
		// initialize the state
		ir = rindex(r_ini);
		rold=r_ini;
		phi = 0;
		phiold = 0;
		omega = 0;
		s = ir + omega*M + phi*Nomegas*M;
		
		
		// other initializations for the episode
		n_targets = 0;
		target_time_ep = 0.0;
		
		// initialize to zero the glow-matrix every 20 episodes
		if (episode%20==0){
			for (int ss = 0; ss<Ns; ss++){
				G[ss][0]=0.0;
				G[ss][1]=0.0;
				changes_in_G[ss][0]=0;
				changes_in_G[ss][1]=0;
			}
		}
		
		// run dynamics
		for (int t=1; t<=Nt_single_episode; t++){
			// find the action (a=1 switch)
			a = 0;
			H2[0] = v_factor1[t-1]*H[s][0] + v_factor2[t-1]*H0[s][0];
			H2[1] = v_factor1[t-1]*H[s][1] + v_factor2[t-1]*H0[s][1];
			p = H2[1]/(H2[1]+H2[0]);
			if ((rand()/((double) RAND_MAX)) <= p){
				a = 1;
			}
			
			// update matrices that I need to update Glow matrix when I find the target
			changes_in_G[s][a]+=1;
			ks = changes_in_G[s][a]-1;
			visited_times[s][a][ks]=t;
			////////////////////////
			
			
			// update the phase
			if (a==1) {
				phiold = phi;
				phi = (phi+1) % Nphi;
			}	else {
				phiold = phi;
			}
			// update position
			double u1 = rand ()/(( double ) RAND_MAX );
			double u2 = rand ()/(( double ) RAND_MAX );
			double z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
			if (z1<-6660.0 || z1>6660.0){
				u1 = rand ()/(( double ) RAND_MAX );
				u2 = rand ()/(( double ) RAND_MAX );
				z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
			}
			x += phiold*vdt*cos(theta) + sigma * z1; // - mudt*cos(vU*x)
			u1 = rand ()/(( double ) RAND_MAX );
			u2 = rand ()/(( double ) RAND_MAX );
			z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
			if (z1<-6660.0 || z1>6660.0){
				u1 = rand ()/(( double ) RAND_MAX );
				u2 = rand ()/(( double ) RAND_MAX );
				z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
			}
			y += phiold*vdt*sin(theta) + sigma * z1; // - mudt*cos(vU*y)
			// update self-propulsion direction
			if (a==0) {
				u1 = rand ()/(( double ) RAND_MAX );
				u2 = rand ()/(( double ) RAND_MAX );
				z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
				if (z1<-6660.0 || z1>6660.0){
					u1 = rand ()/(( double ) RAND_MAX );
					u2 = rand ()/(( double ) RAND_MAX );
					z1 = sqrt ( -2.* log ( u1 )) * cos (2.* PI * u2 );
				}
				theta += sigma_theta * z1;
			} else {
				theta = 2*PI*(rand()/((double) RAND_MAX));
			}
			
			// determine new state
			r = sqrt(x*x+y*y);
			ir = rindex(r);
			omega = 0;
			if (r < rold) {
				omega=1;
			}
			rold = r;
			s = ir + omega*M + phi*Nomegas*M;
			
			// check if we have reached the target and we can detect it
			if (ir == 0 && phi==0) {
				target_time_ep = t*dt;
				n_targets = 1;
				tep = t;
				// interrupt episode
				break;
			} else {
				tep = t;
			}
			
		} // end of episode

		// update G-values needed for the update of the H-matrix
		for (int i=0; i<Ns; i++){
			for (int j=0; j<Na; j++){
				if (changes_in_G[i][j]>=1){
					for (int k=0; k<changes_in_G[i][j]; k++){
						if (k==0){
							G[i][j] *= v_factor3[visited_times[i][j][k]];
						} else {
							G[i][j] *= v_factor3[visited_times[i][j][k]-visited_times[i][j][k-1]];
						}
						G[i][j] += 1;
					}
					G[i][j] *= v_factor3[Nt_single_episode-visited_times[i][j][changes_in_G[i][j]-1]];
				} else {
					G[i][j] *= v_factor3[Nt_single_episode];
				}
			}
			changes_in_G[i][0]=0;
			changes_in_G[i][1]=0;
		}
		
		if (n_targets == 1) {
			// update h-values with rewards if target was found
			for (int i=0; i<Ns; i++){
				H[i][0] = v_factor1[tep]*H[i][0] + v_factor2[tep]*H0[i][0] + reward*G[i][0];
				H[i][1] = v_factor1[tep]*H[i][1] + v_factor2[tep]*H0[i][1] + reward*G[i][1];
				//~ H[i][0] = v_factor1[tep]*H[i][0] + v_factor2[tep]*H0[i][0] + reward*(2*(Nt_single_episode-tep)/Nt_single_episode)*G[i][0];
				//~ H[i][1] = v_factor1[tep]*H[i][1] + v_factor2[tep]*H0[i][1] + reward*(2*(Nt_single_episode-tep)/Nt_single_episode)*G[i][1];
			}
			//~ printf("target found at apisode %d and time %d\n",episode,tep);
		} else {
			for (int i=0; i<Ns; i++){
				H[i][0] = v_factor1[Nt_single_episode]*H[i][0] + v_factor2[Nt_single_episode]*H0[i][0];
				H[i][1] = v_factor1[Nt_single_episode]*H[i][1] + v_factor2[Nt_single_episode]*H0[i][1];
			}
		}
		
		// save search time during the episode
		if (epindex>=Nep0_arrays){
			if (n_targets > 0){
				target_times[run][epindex]=target_time_ep;
			} else {
				target_times[run][epindex] = 0.0;	// just for convenience
			}
		
			// update average Probability values at the end of the episode
			for (int s = 0; s<Ns; s++){
				avg_Pvalues_episode[epindex][s] += H[s][1]/(H[s][0]+H[s][1]);
				avg_Hvalues_episode[epindex][s][0] += H[s][0];
				avg_Hvalues_episode[epindex][s][1] += H[s][1];
			}
			
		}
		
		if (episode>N_episodes-Nep_last && n_targets > 0) {
			target_times_lastepisodes[run][episode]=target_time_ep;
		}

	}
}