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

#include "TRandom3.h"

#include "JGeometry3D/JDirection3D.hh"
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JGeometry3DTestkit.hh"
#include "JGeometry3D/JEigen3D.hh"
#include "JMath/JMathToolkit.hh"
#include "JMath/JConstants.hh"

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


/**
 * \file
 *
 * Test program for quaternion interpolations.
 *
 * \author mdejong
 */
int main(int argc, char**argv)
{
  using namespace std;
  using namespace JPP;

  int            numberOfEvents;
  double         precision;
  UInt_t         seed;
  int            debug;

  try {

    JParser<> zap("Test program for quaternion interpolations.");

    zap['n'] = make_field(numberOfEvents)      = 100;
    zap['e'] = make_field(precision)           = 1.0e-10;
    zap['S'] = make_field(seed)                = 0;
    zap['d'] = make_field(debug)               = 3;

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


  gRandom->SetSeed(seed);

  {
    for (double alpha : { 0.0, 90.0, 180.0 }) {

      const JQuaternion3D Q[] = { 
	JQuaternion3X(alpha * PI / 180.0),
	JQuaternion3Y(alpha * PI / 180.0),
	JQuaternion3Z(alpha * PI / 180.0)
      };

      for (int i = 0; i != sizeof(Q)/sizeof(Q[0]); ++i) {
	for (int j = 0; i != j; ++j) {

	  JQuaternion3D q(Q[i]);

	  q.conjugate();
	  q.mul(Q[j]);

	  const double beta = atan2(sqrt(q.getB() * q.getB() +
					 q.getC() * q.getC() +
					 q.getD() * q.getD()),
				    q.getA()) * 180.0 / PI;

	  DEBUG("angle [deg] " << getAngle(Q[i], Q[j]) << ' ' << beta << endl);

	  ASSERT(fabs(getAngle(Q[i], Q[j]) - beta) <= precision, "test of angle");
	}
      }
    }
  }

  ASSERT(numberOfEvents > 0);
  
  for (int i = 0; i != numberOfEvents; ++i) {

    {
      const JQuaternion3D Q = getRandom<JQuaternion3D>();

      for (JDirection3D p0 : { JDirection3D(1.0, 0.0, 0.0), JDirection3D(0.0, 1.0, 0.0), JDirection3D(0.0, 0.0, 1.0) }) {

	const JQuaternion3D::decomposition result(Q, p0);

	DEBUG(FIXED(12,9) << Q                      << endl);
	DEBUG(FIXED(12,9) << result.getQuaternion() << endl);

	ASSERT(Q.equals(result.getQuaternion(), precision), "test of decomposition");
      }
    }

    {
      const JQuaternion3D Qa = getRandom<JQuaternion3D>();
      const JQuaternion3D Qb = getRandom<JQuaternion3D>();
      const JQuaternion3D Qc = interpolate(Qa, Qb, 0.5);

      for (JDirection3D p0 : { JDirection3D(1.0, 0.0, 0.0), JDirection3D(0.0, 1.0, 0.0), JDirection3D(0.0, 0.0, 1.0) }) {

	JDirection3D pa(p0);
	JDirection3D pb(p0);
	JDirection3D pc(p0);

	pa.rotate(Qa).normalise();
	pb.rotate(Qb).normalise();
	pc.rotate(Qc).normalise();

	const double ab =  acos(pa.getDot(pb)) * 180.0 / PI;
	const double ac =  acos(pa.getDot(pc)) * 180.0 / PI;
	const double bc =  acos(pb.getDot(pc)) * 180.0 / PI;

	DEBUG(FIXED(12,9) << ab << endl);
	DEBUG(FIXED(12,9) << ac << endl);
	DEBUG(FIXED(12,9) << bc << endl);

	ASSERT(fabs(ac - bc) <= precision, "test of interpolation");
      }
    }

    {
      vector<JQuaternion3D> buffer;

      for (int i = -2; i <= +2; ++i) {
	for (int j = -2; j <= +2; ++j) {
	  for (int k = -2; k <= +2; ++k) {

	    JQuaternion3D q(JQuaternion3X(i * 1.0),
			    JQuaternion3Y(j * 1.0),
			    JQuaternion3Z(k * 1.0));
			
	    buffer.push_back(q.conjugate());
	  }
	}
      }

      JQuaternion3D R = getAverage(buffer.begin(), buffer.end());

      for (JDirection3D p0 : { JDirection3D(1.0, 0.0, 0.0), JDirection3D(0.0, 1.0, 0.0), JDirection3D(0.0, 0.0, 1.0) }) {

	JDirection3D p1(p0);

	p1.rotate(R).normalise();

	DEBUG(p0 << endl);
	DEBUG(p1 << endl);

	ASSERT(p1.equals(p0, precision), "test of averaging");
      }
    }

    {
      vector<JQuaternion3D> buffer;

      for (int i : { -1, +1}) {

	JQuaternion3D q(JQuaternion3X(i * 1.0));
			
	buffer.push_back(q.conjugate());
      }

      JQuaternion3D R = getAverage(buffer.begin(), buffer.end());

      for (JDirection3D p0 : { JDirection3D(1.0, 0.0, 0.0), JDirection3D(0.0, 1.0, 0.0), JDirection3D(0.0, 0.0, 1.0) }) {

	JDirection3D p1(p0);

	p1.rotate(R).normalise();

	DEBUG(p0 << endl);
	DEBUG(p1 << endl);

	ASSERT(p1.equals(p0, precision), "test of averaging");
      }
    }
  }
}