#include "arx.h"


namespace bdm {



	
void str_bitset ( bvec &out, ivec ns, int nbits ) {
	
for ( int i = 0; i < ns.length(); i++ ) {
		out ( ns ( i ) - 2 ) = 1;
	}
}


double seloglik1 ( str_aux in ) {
// This is the loglikelihood (non-constant part) - this should be used in
// frequent computation
	int len = length ( in.d );
	int p1  = in.posit1 - 1;

	double i1 = -0.5 * in.nu * log ( in.d ( p1 ) ) - 0.5 * sum ( log ( in.d.right ( len - p1 - 1 ) ) );
	double i0 = -0.5 * in.nu0 * log ( in.d0 ( p1 ) ) - 0.5 * sum ( log ( in.d0.right ( len - p1 - 1 ) ) );
	return i1 - i0;
//DEBUGGing print:
//fprintf('SELOGLIK1: str=%s loglik=%g\n', strPrintstr(in), l);*/
}


void sedydr ( mat &r, mat &f, double &Dr, double &Df, int R/*,int jl,int jh ,mat &rout, mat &fout, double &Drout, double &Dfoutint &kr*/ ) {
	/*SEDYDR dyadic reduction, performs transformation of sum of 2 dyads
	%
	% [rout, fout, Drout, Dfout, kr] = sedydr(r,f,Dr,Df,R,jl,jh);
	% [rout, fout, Drout, Dfout] = sedydr(r,f,Dr,Df,R);
	%
	% Description: dyadic reduction, performs transformation of sum of
	%  2 dyads r*Dr*r'+ f*Df*f' so that the element of r pointed by R is zeroed
	%
	%     r    : column vector of reduced dyad
	%     f    : column vector of reducing dyad
	%     Dr   : scalar with weight of reduced dyad
	%     Df   : scalar with weight of reducing dyad
	%     R    : scalar number giving 1 based index to the element of r,
	%            which is to be reduced to
	%            zero; the corresponding element of f is assumed to be 1.
	%     jl   : lower index of the range within which the dyads are
	%            modified (can be omitted, then everything is updated)
	%     jh   : upper index of the range within which the dyads are
	%            modified (can be omitted then everything is updated)
	%     rout,fout,Drout,dfout : resulting two dyads
	%     kr   : coefficient used in the transformation of r
	%            rnew = r + kr*f
	%
	% Description: dyadic reduction, performs transformation of sum of
	%            2 dyads r*Dr*r'+ f*Df*f' so that the element of r indexed by R is zeroed
	% Remark1: Constant mzero means machine zero and should be modified
	%           according to the precision of particular machine
	% Remark2: jl and jh are, in fact, obsolete. It takes longer time to
	%           compute them compared to plain version. The reason is that we
	%           are doing vector operations in m-file. Other reason is that
	%           we need to copy whole vector anyway. It can save half of time for
	%           c-file, if you use it correctly. (please do tests)
	%
	% Note: naming:
	%       se = structure estimation
	%       dydr = dyadic reduction
	%
	% Original Fortran design: V. Peterka 17-7-89
	% Modified for c-language: probably R. Kulhavy
	% Modified for m-language: L. Tesar 2/2003
	% Updated: Feb 2003
	% Project: post-ProDaCTool
	% Reference: none*/

	/*if nargin<6;
	   update_whole=1;
	else
	   update_whole=0;
	end;*/

	double mzero = 1e-32;

	if ( Dr < mzero ) {
		Dr = 0;
	}

	double r0   = r ( R, 0 );
	double kD   = Df;
	double kr   = r0 * Dr;


	Df   = kD + r0 * kr;

	if ( Df > mzero ) {
		kD = kD / Df;
		kr = kr / Df;
	} else {
		kD = 1;
		kr = 0;
	}

	Dr = Dr * kD;

// Try to uncomment marked stuff (*) if in numerical problems, but I don't
// think it can make any difference for normal healthy floating-point unit
//if update_whole;
	r = r - r0 * f;
//   rout(R) = 0;   // * could be needed for some nonsense cases(or numeric reasons?), normally not
	f = f + kr * r;
//   fout(R) = 1;   // * could be needed for some nonsense cases(or numeric reasons?), normally not
	/*else;
	   rout = r;
	   fout = f;
	   rout(jl:jh) = r(jl:jh) - r0 * f(jl:jh);
	   rout(R) = 0;
	   fout(jl:jh) = f(jl:jh) + kr * rout(jl:jh);
	end;*/
}



/*mat*/ void seswapudl ( mat &L, vec &d , int i/*, vec &dout*/ ) {
	/*%SESWAPUDL swaps information matrix in decomposition V=L^T diag(d) L
	%
	%   [Lout, dout] = seswapudl(L,d,i);
	%
	% L : lower triangular matrix with 1's on diagonal of the decomposistion
	% d : diagonal vector of diagonal matrix of the decomposition
	% i : index of line to be swapped with the next one
	% Lout : output lower triangular matrix
	% dout : output diagional vector of diagonal matrix D
	%
	% Description:
	%  Lout' * diag(dout) * Lout = P(i,i+1) * L' * diag(d) * L * P(i,i+1);
	%
	%  Where permutation matrix P(i,j) permutates columns if applied from the
	%  right and line if applied from the left.
	%
	% Note: naming:
	%       se = structure estimation
	%       lite = light, simple
	%       udl = U*D*L, or more precisely, L'*D*L, also called as ld
	%
	% Design  : L. Tesar
	% Updated : Feb 2003
	% Project : post-ProDaCTool
	% Reference: sedydr*/

	int j = i + 1;

	double pomd = d ( i );
	d ( i ) = d ( j );
	d ( j ) = pomd;

	L.swap_rows ( i, j );
	L.swap_cols ( i, j );

	
//% We must be working with LINES of matrix L !

    mat r = L.get_row ( i );
	r.transpose();
	mat f = L.get_row ( j );
	f.transpose();

    double Dr = d ( i );
	double Df = d ( j );

	sedydr ( r, f, Dr, Df, j );

    double r0 = r ( i, 0 );
	Dr = Dr * r0 * r0;
	r  = r / r0;

    mat pom_mat = r.transpose();
	L.set_row ( i, pom_mat.get_row ( 0 ) );
	pom_mat = f.transpose();
	L.set_row ( j, pom_mat.get_row ( 0 ) );

	d ( i )   = Dr;
	d ( j )   = Df;

	L ( i, i ) = 1;
	L ( j, j ) = 1;
}


void str_bitres ( bvec &out, ivec ns, int nbits ) {

  for ( int i = 0; i < ns.length(); i++ ) {
		out ( ns ( i ) - 2 ) = 0;
	}
}

str_aux sestrremove ( str_aux in, ivec removed_elements ) {
//% Removes elements from regressor
	
	int n_strL = length ( in.strL );
	str_aux out = in;
	
    for ( int i = 0; i < removed_elements.length(); i++ ) {

		int f = removed_elements ( i );
		int posit1 = ( find ( out.strL == 1 ) ) ( 0 );
		int positf = ( find ( out.strL == f ) ) ( 0 );
		for ( int g = positf - 1; g > posit1 - 1; g-- ) {
			//% BEGIN: We are swapping g and g+1 NOW!!!!
			seswapudl ( out.L, out.d, g );
			seswapudl ( out.L0, out.d0, g );

			int pom_strL = out.strL ( g );
			out.strL ( g ) = out.strL ( g + 1 );
			out.strL ( g + 1 ) = pom_strL;
			//% END
		}
	
	
	}
	out.posit1 = ( find ( out.strL == 1 ) ) ( 0 ) + 1;
	out.strRgr = out.strL.right ( n_strL - out.posit1 );
	out.strMis = out.strL.left ( out.posit1 - 1 );
	str_bitres ( out.bitstr, removed_elements, out.nbits );
	out.loglik = seloglik1 ( out );

	return out;
}


ivec setdiff ( ivec a, ivec b ) {
	ivec pos;

	for ( int i = 0; i < b.length(); i++ ) {
		pos = find ( a == b ( i ) );
		for ( int j = 0; j < pos.length(); j++ ) {
			a.del ( pos ( j ) - j );
		}
	}
	return a;
}




void add_new ( Array<str_aux> &global_best, str_aux newone, int nbest ) {
// Eventually add to global best, but do not go over nbest values
// Also avoids repeating things, which makes this function awfully slow

	int  addit, i = 0;
	if ( global_best.length() >= nbest ) {
		//logliks = [global_best.loglik];

	for ( int j = 1; j < global_best.length(); j++ ) {
			if ( global_best ( j ).loglik < global_best ( i ).loglik ) {
				i = j;
			}
		}
	if ( global_best ( i ).loglik < newone.loglik ) {
			//         if ~any(logliks == new.loglik);
			addit = 1;


	for (int j = 0; j < global_best.length(); j++){
		if (newone.bitstr == global_best(j).bitstr){
         addit = 0;
         break;
		}
	}
	if ( addit ) {
		global_best ( i ) = newone;
				// DEBUGging print:
				// fprintf('ADDED structure, add_new: %s, loglik=%g\n', strPrintstr(new), new.loglik);
		}
	 }
	} else
		global_best = concat ( global_best, newone );

}






str_aux sestrinsert ( str_aux in, ivec inserted_elements ) {
// Moves elements into regressor
	int n_strL = in.strL.length();
	str_aux out = in;
	for ( int j = 0; j < inserted_elements.length(); j++ ) {
		int f = inserted_elements ( j );
		int posit1 = ( find ( out.strL == 1 ) ) ( 0 );
		int positf = ( find ( out.strL == f ) ) ( 0 );
		for ( int g = positf; g <= posit1 - 1; g++ ) {

			// BEGIN: We are swapping g and g+1 NOW!!!!
			seswapudl ( out.L,  out.d,  g );
			seswapudl ( out.L0, out.d0, g );

			int pom_strL = out.strL ( g );
			out.strL ( g ) = out.strL ( g + 1 );
			out.strL ( g + 1 ) = pom_strL;

			// END
		}
	}

	out.posit1 = ( find ( out.strL == 1 ) ) ( 0 ) + 1;
	out.strRgr = out.strL.right ( n_strL - out.posit1 );
	out.strMis = out.strL.left ( out.posit1 - 1 );
	str_bitset ( out.bitstr, inserted_elements, out.nbits );
	out.loglik = seloglik1 ( out );

	return out;
}

double seloglik2 ( str_aux in ) {
// This is the loglikelihood (constant part) - this should be added to
// everything at the end. It needs some computation, so it is useless to
// make it for all the stuff
	double logpi = log ( pi );

	double i1 = lgamma ( in.nu / 2 ) - 0.5 * in.nu * logpi;
	double i0 = lgamma ( in.nu0 / 2 ) - 0.5 * in.nu0 * logpi;
	return i1 - i0;
}




ivec straux1 ( ldmat Ld, double nu, ldmat Ld0, double nu0, ivec belief, int nbest, int max_nrep, double lambda, int order_k, Array<str_aux> &rgrsout/*, stat &statistics*/ ) {
// see utia_legacy/ticket_12/ implementation and str_test.m

	const mat &L = Ld._L();
	const vec &d = Ld._D();

	const mat &L0 = Ld0._L();
	const vec &d0 = Ld0._D();

	int n_data = d.length();

	ivec belief_out = find ( belief == 4 ) + 2; // we are avoiding to put this into regressor
	ivec belief_in  = find ( belief == 1 ) + 2; // we are instantly keeping this in regressor

	str_aux full;

	full.d0  = d0;
	full.nu0 = nu0;
	full.L0  = L0;
	full.L   = L;
	full.d   = d;
	full.nu0 = nu0;
	full.nu  = nu;
	full.strL = linspace ( 1, n_data );
	full.strRgr = linspace ( 2, n_data );
	full.strMis = ivec ( 0 );
	full.posit1 = 1;
	full.bitstr.set_size ( n_data - 1 );
	full.bitstr.clear();
	str_bitset ( full.bitstr, full.strRgr, full.nbits );
	full.loglik = seloglik1 ( full );    // % loglikelihood


//% construct full and empty structure
	full = sestrremove ( full, belief_out );
	str_aux empty = sestrremove ( full, setdiff ( full.strRgr, belief_in ) );

//% stopping rule calculation:

	bmat local_max ( 0, 0 );
	int to, muto = 0;

//% statistics:
//double cputime0 = cputime;
//if nargout>=3;

	CPU_Timer timer;
	timer.start();
	ivec mutos ( max_nrep + 2 );
	vec maxmutos ( max_nrep + 2 );
	mutos.zeros();
	maxmutos.zeros();


//end;
//% ----------------------

//% For stopping-rule calculation
//%so       = 2^(n_data -1-length(belief_in)- length(belief_out)); % do we use this ?
//% ----------------------

	ivec all_str = linspace ( 1, n_data );
	Array<str_aux> global_best ( 1 );
	global_best ( 0 ) = full;


//% MAIN LOOP is here.

	str_aux   best;
	for ( int n_start = -1; n_start <= max_nrep; n_start++ ) {
		str_aux  last, best;

		to = n_start + 2;

		if ( n_start == -1 ) {
			//% start from the full structure
			last = full;
		} else {
			if ( n_start == 0 )
				//% start from the empty structure
				last = empty;

			else {
				//% start from random structure
            
		
		
			  ivec last_str = find ( to_bvec( concat( 0, floor_i ( 2 * randun ( n_data - 1 )) ) ) ) + 1;// this creates random vector consisting of indexes, and sorted
			  last = sestrremove ( full, setdiff ( all_str, concat( concat( 1 , last_str ), empty.strRgr ) ) );

			}
		}
		//% DEBUGging print:
		//%fprintf('STRUCTURE generated            in loop %2i was %s\n', n_start, strPrintstr(last));

		//% The loop is repeated until likelihood stops growing (break condition
		//% used at the end;


		while ( 1 ) {
			//% This structure is going to hold the best elements
			best = last;
			//% Nesting by removing elements (enpoorment)
			ivec  removed_items = setdiff ( last.strRgr, belief_in );

			ivec removed_item;
			str_aux newone;

			for ( int i = 0; i < removed_items.length(); i++ ) {
				removed_item = vec_1 ( removed_items ( i ) );
				newone = sestrremove ( last, removed_item );
				if ( nbest > 1 ) {
					add_new ( global_best, newone, nbest );
				}
				if ( newone.loglik > best.loglik ) {
					best = newone;
				}
			}
			//% Nesting by adding elements (enrichment)
			ivec added_items = setdiff( last.strMis, belief_out );
			ivec added_item;

			for ( int j = 0; j < added_items.length(); j++ ) {
				added_item = vec_1 ( added_items ( j ) );
				newone = sestrinsert ( last, added_item );
				if ( nbest > 1 ) {
					add_new ( global_best, newone, nbest );
				}
				if ( newone.loglik > best.loglik ) {
					best = newone;
				}
			}


			//% Break condition if likelihood does not change.
			if ( best.loglik <= last.loglik )
				break;
			else
				//% Making best structure last structure.
				last = best;
			}


		// % DEBUGging print:
		//%fprintf('STRUCTURE found (local maxima) in loop %2i was %s randun_seed=%11lu randun_counter=%4lu\n', n_start, strPrintstr(best), randn('seed'), RANDUN_COUNTER);

		//% Collecting of the best structure in case we don't need the second parameter
		if ( nbest <= 1 ) {
			if ( best.loglik > global_best ( 0 ).loglik ) {
				global_best = best;
			}
		}

		//% uniqueness of the structure found
		int append = 1;

		for ( int j = 0; j  < local_max.rows() ; j++ ) {
			if ( best.bitstr == local_max.get_row ( j ) ) {
				append = 0;
				break;
			}
		}


		if ( append ) {
			local_max.append_row ( best.bitstr );
			muto = muto + 1;
		}

		//% stopping rule:
		double maxmuto = ( to - order_k - 1 ) / lambda - to + 1;
		if ( to > 2 ) {
			if ( maxmuto >= muto ) {
				//% fprintf('*');
				break;
			}
		}

		// do statistics if necessary:
		//if (nargout>=3){
		mutos ( to - 1 )    = muto;
		maxmutos ( to - 1 ) = maxmuto;
		//}
	}

//% Aftermath: The best structure was in: global_best

//% Updating loglikelihoods: we have to add the constant stuff

	for ( int f = 0 ; f < global_best.length(); f++ ) {
		global_best ( f ).loglik = global_best ( f ).loglik + seloglik2 ( global_best ( f ) );
	}

//% Making first output parameter:

	int max_i = 0;
	for ( int j = 1; j < global_best.length(); j++ )
		if ( global_best ( max_i ).loglik < ( global_best ( j ).loglik ) ) max_i = j;

	best = global_best ( max_i );

//% Making the second output parameter

	vec logliks ( global_best.length() );
	for ( int j = 0; j < logliks.length(); j++ )
		logliks ( j ) = global_best ( j ).loglik;

	ivec i = sort_index ( logliks );
	rgrsout.set_length ( global_best.length() );

	for ( int j = global_best.length() - 1; j >= 0; j-- )
		rgrsout ( j ) = global_best ( i ( j ) );

//if (nargout>=3);

	str_statistics statistics;

	statistics.allstrs = 2 ^ ( n_data - 1 - length ( belief_in ) - length ( belief_out ) );
	statistics.nrand   = to - 2;
	statistics.unique  = muto;
	statistics.to  = to;
	statistics.cputime_seconds = timer.get_time();
	statistics.itemspeed       = statistics.to / statistics.cputime_seconds;
	statistics.muto = muto;
	statistics.mutos = mutos;
	statistics.maxmutos = maxmutos;
//end;

	return best.strRgr;

}






}