//$$ solution.cpp                    // solve routines

// Copyright (C) 1994: R B Davies


#define WANT_STREAM                  // include.h will get stream fns
#define WANT_MATH                    // include.h will get math fns

#include "include.h"
#include "boolean.h"
#include "myexcept.h"

#include "solution.h"

unsigned long SolutionException::Select;

SolutionException::SolutionException(const char* a_what) : Exception()
{
   Select = Exception::Select;
   AddMessage("Error detected by solution package\n");
   AddMessage(a_what); AddMessage("\n");
   if (a_what) Tracer::AddTrace();
};

inline Real square(Real x) { return x*x; }

void OneDimSolve::LookAt(int V)
{
   lim--;
   if (!lim) Throw(SolutionException("Does not converge"));
   Last = V;
   Real yy = function(x[V]) - YY;
   Finish = (fabs(yy) < acc);
   y[V] = vpol*yy;
}

void OneDimSolve::HFlip() { hpol=-hpol; State(U,C,L); }

void OneDimSolve::VFlip()
   { vpol = -vpol; y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2]; }

void OneDimSolve::Flip()
{
   hpol=-hpol; vpol=-vpol; State(U,C,L);
   y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2];
}

void OneDimSolve::State(int I, int J, int K) { L=I; C=J; U=K; }

void OneDimSolve::Linear(int I, int J, int K)
{
   x[J] = (x[I]*y[K] - x[K]*y[I])/(y[K] - y[I]);
   // cout << "Linear\n";
}

void OneDimSolve::Quadratic(int I, int J, int K)
{
   // result to overwrite I
   Real YJK, YIK, YIJ, XKI, XKJ;
   YJK = y[J] - y[K]; YIK = y[I] - y[K]; YIJ = y[I] - y[J];
   XKI = (x[K] - x[I]);
   XKJ = (x[K]*y[J] - x[J]*y[K])/YJK;
   if ( square(YJK/YIK)>(x[K] - x[J])/XKI ||
      square(YIJ/YIK)>(x[J] - x[I])/XKI )
   {
      x[I] = XKJ;
      // cout << "Quadratic - exceptional\n";
   }
   else
   {
      XKI = (x[K]*y[I] - x[I]*y[K])/YIK;
      x[I] = (XKJ*y[I] - XKI*y[J])/YIJ;
      // cout << "Quadratic - normal\n";
   }
}

Real OneDimSolve::Solve(Real Y, Real X, Real Dev, int Lim)
{
   enum Loop { start, captured1, captured2, binary, finish };
   Tracer et("OneDimSolve::Solve");
   lim=Lim;
   if (Dev==0.0)Throw(SolutionException("Dev is zero"));
   L=0; C=1; U=2; vpol=1; hpol=1; y[C]=0.0; y[U]=0.0;
   if (Dev<0.0) { hpol=-1; Dev = -Dev; }
   YY=Y;                                // target value
   x[L] = X;                            // initial trial value
   Loop TheLoop = start;
   for (;;)
   {
      switch (TheLoop)
      {
      case start:
	 LookAt(L); if (Finish) { TheLoop = finish; break; }
	 if (y[L]>0.0) VFlip();               // so Y[L] < 0
	 x[U] = X + Dev * hpol;
	 LookAt(U); if (Finish) { TheLoop = finish; break; }
	 if (y[U] > 0.0) { TheLoop = captured1; break; }
	 if (y[U] == y[L])
	    Throw(SolutionException("Function is flat"));
	 if (y[U] < y[L]) HFlip();             // Change direction
	 State(L,U,C);
	 for (i=0; i<20; i++)
	 {
	    // cout << "Searching for crossing point\n";
	    // Have L C then crossing point, Y[L]<Y[C]<0
	    x[U] = x[C] + Dev*hpol;
	    LookAt(U); if (Finish) { TheLoop = finish; break; }
	    if (y[U] > 0) { TheLoop = captured2; break; }
	    if (y[U] < y[C])
	       Throw(SolutionException("Function is not monotone"));
	    Dev *= 2.0;
	    State(C,U,L);
	 }
	 if (TheLoop != start ) break;
	 Throw(SolutionException("Can't locate a crossing point"));

      case captured1:
	 // cout << "Captured - 1\n";
	 // We have 2 points L and U with crossing between them
	 Linear(L,C,U);                   // linear interpolation
					  // - result to C
	 LookAt(C); if (Finish) { TheLoop = finish; break; }
	 if (y[C] > 0.0) Flip();            // Want y[C] < 0
	 if (y[C] < 0.5*y[L]) { State(C,L,U); TheLoop = binary; break; }

      case captured2:
	 // cout << "Captured - 2\n";
	 // We have L,C before crossing, U after crossing
	 Quadratic(L,C,U);                // quad interpolation
					  // - result to L
	 LookAt(L); if (Finish) { TheLoop = finish; break; }
	 if ((x[L] - x[C])*hpol <= 0.0 || (x[L] - x[U])*hpol >= 0.0)
	    { State(C,L,U); TheLoop = captured1; break; }
	 State(C,L,U);
	 // cout << "Through first stage\n";
	 if (y[C] > 0.0) Flip();
	 if (y[C] > 0.5*y[L]) { TheLoop = captured2; break; }
	 else { State(C,L,U); TheLoop = captured1; break; }

      case binary:
	 // We have L, U around crossing - do binary search
	 // cout << "Binary\n";
	 for (i=3; i; i--)
	 {
	    x[C] = 0.5*(x[L]+x[U]);
	    LookAt(C); if (Finish) { TheLoop = finish; break; }
	    if (y[C]>0.0) State(L,U,C); else State(C,L,U);
	 }
	 if (TheLoop != binary) break;
	 TheLoop = captured1; break;

      case finish:
	 return x[Last];

      }
   }
}

