package iterators;

import java.io.*;
import main.BetterTokenizer;
import java.lang.*;
import main.Model;
import main.GeneralInput;
import parameters.ParameterSet;


public class PowellIterator extends ModelIterator implements Runnable {


	//  Used in findBracket
	static final float 	GOLD = 1.618034f;	// magnification ratio for subsequent intervals in findBracket
	static final float	GLIMIT = 100.0f;	// maximum magnification allowed for parabolic fit step
	static final float	TINY = 1.0e-10f;

	// Used in Brent
	static final float 	ITMAX = 100;
	static final float 	CGOLD = 0.3819660f;
	static final float 	ZEPS = 1.0e-5f;

	//  Used in linMin
	static final float	TOL = 1.0e-3f;



	static final float 	HUGE = 1.0e20f;



	float	[][]  	drct;  	// array of current minimization directions.
	float 	[]		dub;	// current upper bounds (in arc length) along current directions
	float 	[]		dlb;	// current lower bounds (in arc length) along current directions
	float 			f_tol;	// stop if fractional decrease in objective
							// function after one iteration step is less than ftol
				

	public PowellIterator() {}	// Have a no argument initializer so we can do new_by_name

	public void loadParameters(BetterTokenizer tokenizer) throws Exception {
		super.loadParameters(tokenizer);	// The grunt work is done by parent class		
	}		
		
	
	// Put any parameters specific to this iterator class in here, as if clauses.
	protected void loadParameter(String info, BetterTokenizer tokenizer) throws Exception {
		if(info.equals("tolerance")) {GeneralInput.nextToken(tokenizer); f_tol = (float)tokenizer.nval; }
		else super.loadParameter(info, tokenizer);
	}

	
	/*****************************************************************
	// ******* This version of F for diagnostic purposes only
	public float F(float [] thePoint)  {
	numFunctionCalls++;
	return (10 + SQR(thePoint[0]) + SQR(thePoint[1]));
	}
	// ******* This version of F for diagnostic purposes only
	//*****************************************************************/

//  MODIFICATION emm 4/19 
	public ModelIterator copy()  throws Exception {
		PowellIterator newIter = new PowellIterator();
		newIter.init(this.network, this.model);
		newIter.f_tol = this.f_tol;
		newIter.nParsTV = this.nParsTV;
		newIter.parsTV = this.parsTV.copy();
		newIter.theFunction = this.theFunction.copy();
		return newIter;
	}
// END MODIFICATION

	private void findBoundaries(int ivec)  {  // finds constraint boundaries along direction ivec.

		float maxstep = HUGE;
		float dum;
		float [] lb = parsTV.getLowerBounds();
		float [] ub = parsTV.getUpperBounds();
		
		//	assume at least for now that current p[] lies within all bounds
		
		// find upper bound.
		for(int i = 0; i < nParsTV; i++)  {
			if(drct[ivec][i] > 0.0f)  {	// headed towards upper bound
				if((dum = (ub[i] - p[i])/drct[ivec][i]) < maxstep)
					maxstep = dum;
			}  else if(drct[ivec][i] < 0.0f){	// headed towards lower bound
				if((dum = (lb[i] - p[i])/drct[ivec][i]) < maxstep)
					maxstep = dum;
			}
		}
		dub[ivec] = maxstep;
		
		// now find lower bound.
		maxstep = -(HUGE);
		for(int i = 0; i < nParsTV; i++)  {
			if(drct[ivec][i] > 0.0f)  {	// headed towards lower bound
				if((dum = (lb[i] - p[i])/drct[ivec][i]) > maxstep)
					maxstep = dum;
			}  else if(drct[ivec][i] < 0.0f) {	// headed towards upper bound
				if((dum = (ub[i] - p[i])/drct[ivec][i]) > maxstep)
					maxstep = dum;
			}
		}
		dlb[ivec] = maxstep;
	}
			
			

	private float checkBounds(float x, int ivec)  {   // check if x exceeds any bounds.  Return
													// x if not, dub[ivec] if hit upper, 
													// dlb[ivec] if hit lower.

		float maxstep = x;
		float dum;
		
		if(x > dub[ivec])  return dub[ivec];
		else if (x < dlb[ivec])	return dlb[ivec];
		else return(x);
	}
		
			
	private float f1dim(float x, int ivec)  {  // compute f at at distance x along direction drct[ivec] from p

		float [] thePoint = new float [nParsTV];
		
		for(int j = 0; j < nParsTV; j++)  
			thePoint[j] = p[j] + x*drct[ivec][j];
		return F(thePoint);
	}

			
	private boolean findBracket(float [] pts,float [] f_at_pts,int ivec)  {

		float  ulim,u,r,q,fu,dum,a,b,c,fa,fb,fc;
		
/*		for(float inc = 0f; inc < 0.4f; inc+=0.001)
			f1dim(pts[1] + inc,ivec);
*/
		a = pts[0]; b = pts[1]; c = pts[2];
		fa = f_at_pts[0];
		fb = f1dim(b,ivec);
		if(fb > fa)  {  // switch a and b to aim downhill
			dum = a; a = b; b = dum;
			dum = fa; fa = fb; fb = dum;
		}
		c = b + GOLD*(b - a);

		if((dum = checkBounds(c,ivec)) != c)  { 	// means we hit a constraint		
			pts[0] = a;
			pts[1] = b;
			pts[2] = dum;
			f_at_pts[0] = fa;
			f_at_pts[1] = fb;
			f_at_pts[2] = f1dim(dum,ivec);			
			if(f_at_pts[2] > fb)  return true;  else return false;
		}  else fc = f1dim(c,ivec);

		
		while(fb > fc)  {	// otherwise keep returning here until we bracket. 
			r = (b - a)*(fb - fc);	// compute u by parabolic extrapolation from a,b,c.
			q = (b - c)*(fb - fa);
			u = b - ((b - c)*q - (b - a)*r)/(2.0f*SIGN(Math.max(Math.abs(q - r),TINY),q - r));
			
			ulim = b + GLIMIT*(c - b);  // we won't step farther than this no matter what.
			
			if((b - u)*(u - c) > 0.0f)  {  // parabolic u is between b and c. try it.
				fu = f1dim(u,ivec);
				if(fu < fc)  {	// got a minimum between b and c.
					pts[0] = b;
					pts[1] = u;
					pts[2] = c;
					f_at_pts[0] = fb;
					f_at_pts[1] = fu;
					f_at_pts[2] = fc;
					return true;
				} else if(fu > fb)  {	// got a minimum between a and u.
					pts[0] = a;
					pts[1] = b;
					pts[2] = u;
					f_at_pts[0] = fa;
					f_at_pts[1] = fb;
					f_at_pts[2] = fu;
					return true;
				}
				
				u = c + GOLD*(c - b);		// parabolic fit no good.  Use default magnification.
				
			}  else if((u - a)/(b - a) <= 1)  {	// reject this u and use default magnification
				u = c + GOLD*(c - b);
				
			}  else if((dum = checkBounds(u,ivec)) != u)  { 	// means we hit a constraint.
					pts[0] = b;
					pts[1] = c;
					pts[2] = dum;
					f_at_pts[0] = fb;
					f_at_pts[1] = fc;
					f_at_pts[2] = f1dim(dum,ivec);
					if(f_at_pts[2] > fc)  return true;  else return false;
					
			}  else if((c - u)*(u - ulim) > 0.0f)  {	// parabolic fit between c and ulim
				fu = f1dim(u,ivec);
				if(fu < fc)  {
					b = c;  c = u; u = u + GOLD*(u - b);  
					fb = fc; fc = fu;
				}
			}  else  {  // u > ulim.  limit parabolic u to ulim.
				u = ulim;
			}
			
			if((dum = checkBounds(u,ivec)) != u)  { 	// means we hit a constraint.
				pts[0] = b;
				pts[1] = c;
				pts[2] = dum;
				f_at_pts[0] = fb;
				f_at_pts[1] = fc;
				f_at_pts[2] = f1dim(dum,ivec);
				if(f_at_pts[2] > fc)  return true;  else return false;
			}  else  fu = f1dim(u,ivec);
			
			a = b; b = c; c = u;
			fa = fb; fb = fc; fc = fu;
		}
		pts[0] = a;
		pts[1] = b;
		pts[2] = c;
		f_at_pts[0] = fa;
		f_at_pts[1] = fb;
		f_at_pts[2] = fc;
		return true;
	}
			
		
	private float brent(float [] pts, float f_at_x, int ivec, float tol)  {

		int iter;
		float a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,ax,bx,cx;
		float e = 0.0f;	// the distance moved step before last.
		
System.out.println("starting brent");

		d = 0.0f;
		ax = pts[0]; bx = pts[1]; cx = pts[2];
		a = (ax < cx) ? ax: cx;	// put a and b in ascending order.
		b = (ax > cx) ? ax: cx;
		x = w = v = bx;					// start with middle point
		fw = fv = fx = f_at_x;		// and its function value.
		for (iter = 1; iter < ITMAX; iter++)  {
			xm = 0.5f*(a + b);
			tol2 = 2.0f*(tol1 = tol*Math.abs(x) + ZEPS);
//	System.out.println("x:\t" + x + "\txm:\t" + xm + "\ta:\t" + a + "\tb:\t" + b + 
//						"\tb - a:\t" + (b - a) + "\ttol2:\t" + tol2);
			if (Math.abs(x - xm) <= (tol2 - 0.5f*(b - a)))  {	// test for done here.
				pts[1] = x;
				System.out.println("brent exited after" + iter + "iterations");
				return fx;
			}
			if( Math.abs(e) > tol1)  {	// 	construct a trial parabolic fit.
				r = (x - w)*(fx - fv);
				q = (x - v)*(fx - fw);
				p = (x - v)*q - (x - w)*r;
				q = 2.0f*(q - r);
				if (q > 0.0f)  p = -p;
				q = Math.abs(q);
				etemp = e;
				e = d;
//System.out.println("x:\t" + x + "\tw:\t" + w + "\tv:\t" + v + "\tfx:\t" + fx + "\tfw:\t" + fw + "\tfv:\t" + fv + 
//				"\tr:\t" + r + "\tq:\t" + q + "\tp:\t" + p);
					
//System.out.println("etemp:\t" + etemp + "\tp/q:\t" + p/q + "\tu:\t" + (x + p/q));
				if( Math.abs(p) >= Math.abs(0.5f*q*etemp) || p <= q*(a - x) || p >= q*(b - x))  {
					d = CGOLD*(e = (x >= xm ? a - x: b - x));
//					System.out.println("golden mean:  d = \t" + d + "\te = \t" + e);
				}
				//  These above conditions determine the acceptibility of the parabolic fit.
				// Now take the golden section step into the larger of the two segments.
				
				else {
					d = p/q;
					u = x + d;
					if(u - a < tol2 || b - u < tol2)
						d = SIGN(tol1,xm - x);
				}
			}  else  {
//				System.out.println("golden mean:  e was\t" + e);
				d = CGOLD*(e = (x >= xm ? a - x: b - x));
//				System.out.println("now e is\t" + e + "\td is \t" + d);
			}
			u = Math.abs(d) >= tol1 ? x + d : x + SIGN(tol1,d);
			fu = f1dim(u,ivec);
//	System.out.println("u:\t" + u + "\tfu:\t" + fu);
//	System.out.println();
			if (fu <= fx)  {	// now decide what to do with our function evaluation.
				if(u >= x) a = x; else b = x;
				v = w; w = x; x = u;
				fv = fw; fw = fx; fx = fu;
			}  else  {
				if(u < x) a = u; else b = u;
				if(fu <= fw || w == x)  {
					v = w; w = u; fv = fw; fw = fu;
				} else if(fu <= fv || v == x || v == w)  {
					v = u; fv = fu;
				}
			}
		}
		System.out.println("too many iterations in brent");
		pts[1] = x;
		return fx;
	}

		
		
	private float linMin(float [] xi,int ivec, float f_at_p)  {  // minimizes F along direction drct[ivec] 
										// starting at p.  Resets p and returns F(p).

		float [] pts = {0.0f,0.01f,0.0f};
		float [] f_at_pts = {f_at_p,0.0f,0.0f};
		float  xmin, ret;
		int j;
		
		if(findBracket(pts,f_at_pts,ivec)) {
			println("Found a bracketing triplet: a = " + pts[0] + " f[a] = " + 
							f_at_pts[0] + "\tb = " + pts[1] + " f[b] = " + f_at_pts[1] +
							"\tc = " + pts[2] + " f[c] = " + f_at_pts[2]);
			ret = brent(pts,f_at_pts[1],ivec,TOL);
		}
		else {
			ret = f_at_pts[2];						// hit a constraint boundary; use that as minimum
			println("hit a constraint boundary. Using it as the new minimum.");
		}
		for(j = 0; j < nParsTV; j++)  {
			xi[j] *= pts[1];
			p[j] += xi[j];
		}
		for(j = 0; j < nParsTV; j++)	findBoundaries(j);
		
		return ret;
	}

	// The iterators start() function is called to start the iterator searching,
	// and it makes a thread and starts that going.  So this run() function should
	// be the top level of the iterator.
	
	
	
	public void reset()  {  // do this before every new run

		int i;
// ELI 5/28 - using super.reset() instead of some of this
/*		if(verbose)  {
			try {
				ps = new PrintStream(new FileOutputStream(outFileName));
			} catch(IOException e) { ps = null; }
		}		
		nParsTV = parsTV.getNumParams();		// set local values of nParsTV 
		p = parsTV.getParVals();  			// and p from parsTV
		bombed = false;
		numFunctionCalls = 0;
*/
		super.reset();
		
		drct = new float [nParsTV] [nParsTV];
		for(i = 0; i < nParsTV; i++)     //  initialize to unit vectors
			for(int j = 0; j < nParsTV; j++)
				if(j == i)  drct[i][j] = 1.0f;  else drct[i][j] = 0.0f;
				
		dub = new float [nParsTV];
		for(i = 0; i < nParsTV; i++)	dub[i] = parsTV.getUpperBound(i);
		
		dlb = new float [nParsTV];
		for(i = 0; i < nParsTV; i++)	dlb[i] = parsTV.getLowerBound(i);
		
		for(i = 0; i < nParsTV; i++) 	findBoundaries(i);	// initialize boundaries
		
	}
		
//ELI 3/23 Changed from run to doRun
	public void doRun()  {	// minimize F from starting point p[]
										// with a set of initial directions drct[][].  At
										// the end p[] is at the best point and f_at_p
										// is set to F(p).

		int i, j, ibig;
		float del, fp, fptt=0, t, f_at_p=0f;

		float [] dirTemp, pt, ptt; 
	
		reset();
		dirTemp = new float [nParsTV];
		pt = new float [nParsTV];
		ptt = new float [nParsTV];
		f_at_p = F(p);	// can't assume we already have this value.
		for(j = 0; j < nParsTV; j++)  {	// save the initial point
			pt[j] = p[j];
		}
		for(int iter = 1;;iter++)  {
			
			println("");
			println("Start a new round of minimizations.");
			fp = f_at_p;
			ibig = 0;
			del = 0.0f;	// will be the biggest function increase.
			for(i = 0; i < nParsTV; i++)  {  //loop over all directions in the set.
				println("MINIMIZING ALONG DIRECTION " + i + ":");
			 	for(j = 0; j < nParsTV; j++)  dirTemp[j] = drct[i][j];	// copy the direction.
			 	fptt = f_at_p;
				f_at_p = linMin(dirTemp,i,f_at_p);		// minimize along it and set f_at_p.
			 	print("Linemin done.  p = (" + p[0]);
			 	for(int par = 1; par < nParsTV; par++) 
			 		print("," + p[par]);
			 	println(");\tf[p] = " + f_at_p);
			 	if(Math.abs(fptt - f_at_p) > del)  {	// record if it's the largest decrease so far.
			 		del = Math.abs(fptt - f_at_p);
			 		ibig = i;
			 	}
			}
			if(2.0f*Math.abs(fp - f_at_p) <= f_tol*(Math.abs(fp) + Math.abs(f_at_p)))  	{  // termination criterion.
				finalScore = f_at_p;
				super.stopRun();
				return;			
			}
			if(iter == ITMAX)  System.out.println("powell exceeding maximum iterations");
			for(j = 0; j < nParsTV; j++)  {
				ptt[j] = 2.0f*p[j] - pt[j];			// Construct the extrapolated point and the 
			 	dirTemp[j] = p[j] - pt[j];			// average direction moved.  Save the old 
			 	pt[j] = p[j];						// starting point.
			}
			fptt = F(ptt);
			if(fptt < fp)  {
				t = 2.0f*(fp - 2.0f*f_at_p + fptt)*SQR(fp - f_at_p - del) - del*SQR(fp - fptt);
				if(t < 0.0f)  {		// don't always change the direction set.
			 		for(j = 0; j < nParsTV; j++)  {			// save the new direction and...
			 			drct[ibig][j] = drct[nParsTV-1][j];
			 			drct[nParsTV-1][j] = dirTemp[j];
			 		}
			 		println("");
			 		println("added a new direction and minimized along it.");
			 		f_at_p = linMin(drct[nParsTV-1],nParsTV-1,f_at_p);	// move to its minimum.
			 		print("Linemin done.  p = (" + p[0]);
			 		for(int par = 1; par < nParsTV; par++) 
			 			print("," + p[par]);
			 		println(");\tf[p] = " + f_at_p);
			 	}
			}
		}
	}
			
			 				
	
	private float SIGN(float a, float b)  {

		if(b >= 0)  return Math.abs(a);  else return -(Math.abs(a));
	}
	
	private float SQR(float x)  {
		return x*x;
	}
	
}