#include <iostream>
#include <iomanip>
#include <limits>
#include <cmath>

#include "TROOT.h"
#include "TFile.h"
#include "TH2D.h"

#include "JAcoustics/JSoundVelocity.hh"
#include "JLang/JException.hh"
#include "JTools/JQuantile.hh"

#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"


namespace {

  using JACOUSTICS::JAbstractSoundVelocity;
  using JLANG::JException;
  using JMATH::JMath;


  /**
   * Auxiliary fata structure for high-precision sinus of angle.
   *
   * If sin_t::option is <tt>true</tt>, value is offset by one; else value is offset by zero.
   */
  struct sin_t {
    /**
     * Get sin.
     *
     * \return                      sin
     */
    double gets() const
    { 
      return (option ? 1.0 + value: value);
    } 

    /**
     * Get cos.
     *
     * \return                      cos
     */
    double getc() const
    { 
      return (option ? sqrt(2.0 + value) * sqrt(0.0 - value) : sqrt(1.0 + value) * sqrt(1.0 - value));
    } 

    bool   option;    //!< option
    double value;     //!< value
  };


  /**
   * Auxiliary data structure for tracing of sound ray.
   */
  struct JBellhop_t {
    /**
     * Default constructor.
     */
    JBellhop_t() :
      s({false,0.0}),
      x(0.0),
      z(0.0),
      t(0.0),
      d(0.0)
    {}

    /**
     * Constructor.
     *
     * \param   s                   sin angle with respect to vertical
     * \param   x                   x-position [m]
     * \param   z                   z-position [m]
     * \param   t                   time       [s]
     * \param   d                   distance   [m]
     */
    JBellhop_t(const sin_t& s,
	       const double x,
	       const double z,
	       const double t,
	       const double d) :
      s(s),
      x(x),
      z(z),
      t(t),
      d(d)
    {}

    sin_t  s;   //!< sinus angle with respect to vertical
    double x;   //!< x-position [m]
    double z;   //!< z-position [m]
    double t;   //!< time       [s]
    double d;   //!< distance   [m]
  };


  /**
   * Data structure for tracing of sound ray.
   */
  struct JBellhop {

    static constexpr double  STEP_SIZE_M           =  0.001;      //!< step size [m]
    static constexpr size_t  NUMBER_OF_ITERATIONS  =  1000;       //!< maximum number of iterations


    /**
     * Constructor.
     *
     * \param  V             sound velocity
     * \param  precision     precision
     */
    JBellhop(const JAbstractSoundVelocity& V, const double precision) :
      V(V),
      precision(precision)
    {}


    /**
     * Estimate sinus of angle of sound ray with respect to vertical at start position to reach end position.
     *
     * \param  x0            x-position at start
     * \param  z0            z-position at start
     * \param  x1            x-position at end
     * \param  z1            z-position at end
     * \return               sinus of angle of sound ray
     */
    sin_t operator()(const double x0,
		     const double z0,
		     const double x1,
		     const double z1) const
    {
      using namespace std;

      const double st = fabs(x1 - x0) / hypot(x1 - x0, z1 - z0);

      double st_min = (z1 > z0 ? st : 0.0);
      double st_max = (z1 > z0 ? 1.0 : st);

      if (fabs(x1 - x0) > fabs(z1 - z0)) {

	st_min -= 1.0;   // precision
	st_max -= 1.0;   //

	for (size_t i = 0; i != NUMBER_OF_ITERATIONS; ++i) {

	  double s0 = 0.5*(st_min + st_max);

	  double s  = s0;
	  double x  = x0;
	  double z  = z0;

	  while ((x - x1) * copysign(1.0, x1 - x0) < -0.5 * STEP_SIZE_M) {

	    const double c  = sqrt(2.0 + s) * sqrt(-s);
	    const double dx = copysign(STEP_SIZE_M,                 x1 - x0);
	    const double dz = copysign(STEP_SIZE_M * c / (1.0 + s), z1 - z0);
	    
	    s += (1.0 + s) * (V(z + dz) - V(z)) / V(z + 0.5*dz);
	    x += dx;
	    z += dz;

	    if        (s >   0.0) {

	      s = 0.0;

	    } else if (s <= -1.0) {

	      z = numeric_limits<double>::max();

	      break;
	    }
	  }

	  if (fabs(z - z1) <= precision) {
	    return { true, s0 };
	  }

	  if ((z - z1) * copysign(1.0, z1 - z0) > 0.0)
	    st_min = s0;
	  else
	    st_max = s0;
	}

      } else {

	for (size_t i = 0; i != NUMBER_OF_ITERATIONS; ++i) {

	  double s0 = 0.5*(st_min + st_max);

	  double s  = s0;
	  double x  = x0;
	  double z  = z0;

	  while ((z - z1) * copysign(1.0, z1 - z0) < -0.5 * STEP_SIZE_M) {

	    const double c  = sqrt((1.0 + s) * (1.0 - s));
	    const double dx = copysign(STEP_SIZE_M * s / c, x1 - x0);
	    const double dz = copysign(STEP_SIZE_M,         z1 - z0);
	  
	    s += s * (V(z + dz) - V(z)) / V(z + 0.5*dz);
	    x += dx;
	    z += dz;
	    
	    if        (s >= 1.0) {

	      x = numeric_limits<double>::max();

	      break;

	    } else if (s <  0.0) {

	      s = 0.0;
	    }
	  }
	  
	  if (fabs(x - x1) <= precision) {
	    return { false, s0 };
	  }

	  if ((x - x1) * copysign(1.0, x1 - x0) < 0.0)
	    st_min = s0;
	  else
	    st_max = s0;
	}
      }

      THROW(JException, "Reached maximum number of iterations " << NUMBER_OF_ITERATIONS);
    }


    /**
     * Sound ray tracing.
     *
     * \param  s0            sinus angle of sound ray
     * \param  x0            x-position at start
     * \param  z0            z-position at start
     * \param  x1            x-position at end
     * \param  z1            z-position at end
     * \return               sound ray trace
     */    
    JBellhop_t operator()(const sin_t& s0,
			  const double x0,
			  const double z0,
			  const double x1,
			  const double z1) const
    {
      using namespace std;
      
      double x = x0;
      double z = z0;
      double s = s0.value;
      double t = 0.0;
      double d = 0.0;

      if (s0.option) {
	
	while ((x - x1) * copysign(1.0, x1 - x0) < -0.5 * STEP_SIZE_M) {

	  const double c  = sqrt(2.0 + s) * sqrt(-s);
	  const double dx = copysign(STEP_SIZE_M,                 x1 - x0);
	  const double dz = copysign(STEP_SIZE_M * c / (1.0 + s), z1 - z0);
	  const double v  = V(z + 0.5*dz);
	  
	  s += (1.0 + s) * (V(z + dz) - V(z)) / v;
	  x += dx;
	  z += dz;
	  d += sqrt(dx*dx + dz*dz);
	  t += sqrt(dx*dx + dz*dz) / v;

	  if      (s >  0.0)
	    s = 0.0;
	  else if (s < -1.0)
	    s = -1.0;
	}

      } else {

	while ((z - z1) * copysign(1.0, z1 - z0) < -0.5 * STEP_SIZE_M) {

	  const double c  = sqrt((1.0 + s) * (1.0 - s));
	  const double dx = copysign(STEP_SIZE_M * s / c, x1 - x0);
	  const double dz = copysign(STEP_SIZE_M,         z1 - z0);
	  const double v  = V(z + 0.5*dz);

	  s += s * (V(z + dz) - V(z)) / v;
	  x += dx;
	  z += dz;
	  d += sqrt(dx*dx + dz*dz);
	  t += sqrt(dx*dx + dz*dz) / v;

	  if      (s > 1.0)
	    s = 1.0;
	  else if (s < 0.0)
	    s = 0.0;
	}
      }

      return JBellhop_t({s0.option,s}, x, z, t, d);
    }


    const JAbstractSoundVelocity& V;
    double precision;
  };
}


/**
 * \file
 *
 * Example program for sound ray tracing.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

  string                  outputFile;
  JSoundVelocity          V              = getSoundVelocity;    // default sound velocity
  double                  precision;
  int                     debug;

  try {

    JParser<> zap("Example program for sound ray tracing.");

    zap['o'] = make_field(outputFile)          = "";
    zap['V'] = make_field(V,            "sound velocity")                    = JPARSER::initialised();
    zap['e'] = make_field(precision)        = 1.0e-3;
    zap['d'] = make_field(debug)            = 2;

    zap(argc, argv);
  }
  catch(const exception &error) {
    FATAL(error.what() << endl);
  }


  V.set(-3500);

  const JBellhop bellhop(V, precision);

  if (outputFile == "") {

    double x0, z0;
    double x1, z1;
    /*
    x0 =    0.0;  z0 =  0.0;
    x1 = 2000.0;  z1 = 10.0;

    {
    */
    //
    for ( ; ; ) {

      cout << "> " << flush;
      cin  >> x0 >> z0 >> x1 >> z1;
      //

      try {
	
	const double     D      = hypot(x1 - x0, z1 - z0);
	const sin_t      s0     = bellhop(x0, z0, x1, z1); 
	const JBellhop_t result = bellhop(s0, x0, z0, x1, z1);
      
	cout << "s  " << SCIENTIFIC(12,5) << (x1 - x0) / D                    << endl;
	cout << "s0 " << SCIENTIFIC(12,5) << s0.value << ' ' << s0.option     << endl;
	cout << "s1 " << SCIENTIFIC(12,5) << result.s.value                   << endl;
	cout << "x' " << FIXED     (12,5) << result.x             << " [m]"   << endl;
	cout << "z' " << FIXED     (12,5) << result.z             << " [m]"   << endl;
	cout << "D  " << FIXED     (12,5) << D                    << " [m]"   << endl;
	cout << "D' " << FIXED     (12,5) << result.d             << " [m]"   << endl;
	cout << "t  " << FIXED     (12,5) << V.getTime(D, z0, z1) << " [s]"   << endl;
	cout << "t' " << FIXED     (12,5) << result.t             << " [s]"   << endl;


	const double v0 = V(z0);
	const double v1 = V(z1);
	const double b  = (v1 - v0) / (z1 - z0);
	const double c  =  v1 / v0;
	const double u  = (v0 + v1) / v1;
	const double v  = (v0 - v1) / v1;
	const double st = s0.value;

	cout << "v0 " << FIXED     (12,5) << v0                   << " [m/s]" << endl;
	cout << "v1 " << FIXED     (12,5) << v1                   << " [m/s]" << endl;

	double dx = 0.0;

	if (s0.option) {

	  dx = (sqrt(2.0 + st) * sqrt(0.0 - st) - sqrt(1.0 + c + c * st) * sqrt(1.0 - c - c * st)) * v0 / (1.0 + st) / b;

	} else {

	  dx = (sqrt(1.0 + st) * sqrt(1.0 - st) - sqrt(1.0   +   c * st) * sqrt(1.0   -   c * st)) * v0 / (   st   ) / b;
	}

	cout << "x' " << FIXED     (12,5) << dx                   << " [m]"   << endl;

	const double x =  s0.getc();
	const double y = (s0.option ? c * sqrt(u + st) * sqrt(v - st) : sqrt(1.0 + c * st) * sqrt(1.0 - c * st));
	
	cout << "t' " << FIXED     (12,5) << (atanh(x) - atanh(y)) / b        << endl;
      }
      catch(const exception& error) {
	cerr << error.what() << endl;
      }
    }

  } else {
    
    const double x0 =    0.0;
    const double z0 =    0.0;

    JQuantile Qx("x");
    JQuantile Qz("z");

    TFile out(outputFile.c_str(), "recreate");

    TH2D h2("h2", NULL, 50, 1.0, 3.5, 50, -1.0, 3.0);

    for (int ix = 1; ix <= h2.GetXaxis()->GetNbins(); ++ix) {
      for (int iy = 1; iy <= h2.GetYaxis()->GetNbins(); ++iy) {
      
	const double x1 = pow(10.0, h2.GetXaxis()->GetBinCenter(ix));
	const double z1 = pow(10.0, h2.GetYaxis()->GetBinCenter(iy));

	STATUS("x1 " << FIXED(12,6) << x1                   << " [m]" << ' ');  DEBUG(endl);
	STATUS("z1 " << FIXED(12,6) << z1                   << " [m]" << '\r'); DEBUG(endl);

	try {

	  const double     D      = hypot(x1 - x0, z1 - z0);
	  const sin_t      s0     = bellhop(x0, z0, x1, z1); 
	  const JBellhop_t result = bellhop(s0, x0, z0, x1, z1);

	  Qx.put(result.x - x1);
	  Qz.put(result.z - z1);

	  h2.SetBinContent(ix, iy, (result.t - V.getTime(D, z0, z1)) * 1.0e6);  // [us]
	}
	catch(const exception& error) {
	  ERROR(error.what() << endl);
	}
      }
    }

    Qx.print(cout);
    Qz.print(cout);

    out.Write();
    out.Close();
  }
}