/* Trivial solver */

#include <math.h>
#include <stdio.h>
#include "twin_solve.h"

/****************************************************************/

#define SCALE 0.00001

static double find_deriv (func_to_solve_t fn, double at)
{
  double dx = max (abs(at) * SCALE, SCALE);
  double y2 = fn (at + dx);
  double y1 = fn (at - dx);
  double deriv = (y2 - y1) / (dx + dx);
  db_printf (log_file, "find_deriv: at=%f, dx=%f, y1=%f, y2=%f, deriv=%f\n",
	     at, dx, y1, y2, deriv);
  return (deriv);
}  /* find_deriv */

/****************************************************************/

#define MIN_STEPS 2
#define MAX_STEPS 50
#define MAX_ERR   (1.0e-10)
#define MAX_JUMP  3

double solve_func (func_to_solve_t fn, double goal, double guess)
{
  db_printf (log_file, "solve_func called; goal=%f, guess=%f\n", goal, guess);

  double max_epsilon = max (abs(goal) * MAX_ERR, MAX_ERR);
  double max_y = goal + max_epsilon;
  double min_y = goal - max_epsilon;
  double y;
  double x = guess;

  for (int steps = 0; steps < MAX_STEPS; steps++)
    {
      y = fn (x);
      double epsilon = y - goal;
      if (steps >= MIN_STEPS && abs(epsilon) < max_epsilon)
	return (x);

      double deriv = find_deriv (fn, x);

      db_printf (log_file, " .. step=%d, x=%f, y=%f, epsilon=%f, deriv=%f\n",
		 steps, x, y, epsilon, deriv);

      if (deriv IS 0.0)
	{
	  fprintf (stderr, "solve_func:  Zero derivative at x=%f, epsilon=%f -- giving up\n",
		   x, epsilon);
	  return (x);
	}  /* if */
      double jump = -epsilon / deriv;
      if (abs(jump) > MAX_JUMP && abs(jump) > abs(MAX_JUMP * x))
	{
	  double new_jump = abs(jump) * MAX_JUMP / jump;	/* max_jump * sign(jump) */
	  db_printf (log_file, " .. Overjump: jump=%f, using %f\n",
		     jump, new_jump);
	  jump = new_jump;
	}
      x += jump;
    }

  return (x);				/* If we fall off due to too many steps, return this. */
}  /* solve_func */