/*
Versions
1. solve.ox, November 2000. Anders Karlstrom
2. solve_.ox, April 20. 2003 Anders Karlstrom and Edward Morey

This program creates a number of functions that are used by the program ExpectedCV.ox

The functions created in this program are read in by that program

This program cannot be run by itself (it uses a number of functions defined in ExpectedCV.ox),
so should be viewed as part of that program.

It defines the functions zfunc(), zbrac(), eprint(), and rtbis().

The user does not have to modify this file for other applications.

zbrac brackets the roots of the equations zfunc(x)=0
rtbis returnes the roots of the same equation. The method used is bisection
 */



/* ----------------------------------------------------------
zfunc(x)=0 is the function to be solved.
in: x is a Nx1 vector
out: the value of zfunc(x)
-------------------------------------------------------------*/
zfunc(const x)

{
  decl tmp, u0, u1;
  u0=Indirects(p0,q0,y0) ;
  u1=Indirects(p1,q1,x)  ;	
  tmp= u1 - u0;
  return tmp;
}


/* ----------------------------------------------------------
zbrac brackets the roots of the equations zfunc(x)=0
in: xx1 initial lower guess
    xx2 initial upper guess
	funktion function to be solved 

out: Nx2 vector which brackets the equation funktion(x)=0
-------------------------------------------------------------*/
zbrac(const xx1, const xx2, const ret, const funktion)
{
  decl j,f1,f2;
  decl x1, x2;
  decl debug=0;
  x1=xx1; x2=xx2;

  
    println("Trying to find bracket the roots: zbrac");
  if (x1==x2) {eprint("zbrac: Error 11 (x1=x2)\n"); }

  f1=funktion(x1);
  f2=funktion(x2);
  
  for (j = 0; j < MAXNTRY; ++j)
  {	 
	print(".");
	{
	  x1=  x1 + ((f1 .* f2) .> 0) .* (  (abs(f1) .< abs(f2)) .?  ( ZFACTOR * (x1-x2) ) .: 0    )  ;		 //(abs(f1) .< abs(f2)) .*
	  f1=(abs(f1) .< abs(f2)) .? funktion(x1) .: f1;
    }
 
	{
	  x2=  x2 + ((f1 .* f2) .> 0) .* ( (abs(f1) .> abs(f2)) .?  ZFACTOR * (x2-x1) .: 0 )     ;
      f2=(abs(f1) .> abs(f2)) .?  funktion(x2) .: f2 ;
    }
	if (debug) print(x2[0:6][]);   //x1 vid förbättring, x2 vid försämring
	if (( f1 .* f2) < 0)
    {
	  ret[0]=1;
	  println("zbrac ok");
	  return x1 ~ x2;	  // success
    }
  }
  
  eprint("zbrac: Warning 26 (MAXNTRY reached)\n");
    	ret[0]=0;
  return x1 ~ x2;	 // failure
}

/* ----------------------------------------------------------
rtbis finds the roots of the equations zfunc(x)=0, which is a Nx1 vector
in: xx_ Nx2 vector which brackets the roots
	funktion function to be solved 

out: Nx1 vector of roots the equations zfunc(x)=0
-------------------------------------------------------------*/
rtbis(const xx_, const res, const funktion)
{
  decl m_rtbis, j, dx,f,fmid,xmid;
  decl x1,x2; decl debug=0;
  decl xx1=xx_;
  
  println("Trying to find roots: rtbis");
  if (columns(xx1) >2)
  	{  x1=xx1[][0:mNrAlternatives-1]; x2=xx1[][mNrAlternatives:];
	}
  else
  { 	x1 = xx1[][0] ; x2=xx1[][1];
  }
  if (debug) print("RTBIS called with", x1[0: ][],x2[0: ][], x2[0: ][]-x1[0: ][]);
  fmid=funktion(x2);
  f=funktion(x1)	;

  decl tmp= f .* fmid;
  
  if (!( tmp < 0)) {println("RTBIS: Warning 25 (root must be bracketed in rtbis") ; }

  m_rtbis=(f .< 0) .? x1 .: x2 ;
  
  if (debug) print("mrtbis",m_rtbis[0:5][]);
  
  dx=(f .< 0) .? (x2-x1) .: (x1-x2) ;
  if (debug) print("dx=",dx[0:5][]);

  for (j = 0; j < MAXBIS; ++j)
  {
	print(".");
	dx= 0.5 * dx;
	xmid= m_rtbis + dx;
	fmid=funktion(xmid);

	m_rtbis = (fmid .> 0) .? m_rtbis .: xmid ;

//	m_rtbis= (fmid .> 0) .* m_rtbis + (fmid .<= 0) .* xmid;

	if (debug)
	{  println("RTBIS: iter=",j);
	   print(m_rtbis[0:10][] - y0[0:10][]);
	}
//	print("abs(dx)",abs(dx)[0:5][],max(abs(dx)));
	if ( (abs(dx) < BISERR) .|| (fmid .== 0) )
	{
	  m_rtbis = m_rtbis .* ( (abs(m_rtbis - y0)) .> rounderror )	+ y0 .* ( ( abs(m_rtbis - y0) ) .<= rounderror );
	  if (debug)
	  {  println("rtbis ok, C ~ u0(y)-u1(y+c)");
	     print(m_rtbis[0:5][] - y0[0:5][]," \n", -funktion(m_rtbis)[0:10][]);
	  }
	  res[0]=m_rtbis;
	  if (!(-funktion(m_rtbis) < BISERR)) goto errh;
	  return 1 ; 
	}
  }
:errh
  print("rtbis: Error 47     (Could not locate all roots?)");
  println("rtbis NOT ok, C ~ u0(y)-u1(y+c)");
  print(-funktion(m_rtbis)[0:100][]);
  return 0;
}