#include <string>
#include <iostream>
#include <iomanip>

#include "TRandom3.h"

#include "JGeometry3D/JPosition3D.hh"
#include "JGeometry3D/JDirection3D.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JGeometry3DTestkit.hh"
#include "JMath/JMathToolkit.hh"

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


/**
 * \file
 *
 * Test program for 3D geometry.
 *
 * \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;

    zap['n'] = make_field(numberOfEvents) = 100;
    zap['e'] = make_field(precision)      = 1e-7;
    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);

  {
    const JPosition3D ux(1.0, 0.0, 0.0);
    const JPosition3D uy(0.0, 1.0, 0.0);
    const JPosition3D uz(0.0, 0.0, 1.0);

    ASSERT(equals(JPosition3D(ux).rotate(JRotation3X(0.5*PI)),  ux, precision), "Test rotation around x-axis");
    ASSERT(equals(JPosition3D(uy).rotate(JRotation3X(0.5*PI)),  uz, precision), "Test rotation around x-axis");
    ASSERT(equals(JPosition3D(uz).rotate(JRotation3X(0.5*PI)), -uy, precision), "Test rotation around x-axis");

    ASSERT(equals(JPosition3D(ux).rotate(JRotation3Y(0.5*PI)), -uz, precision), "Test rotation around y-axis");
    ASSERT(equals(JPosition3D(uy).rotate(JRotation3Y(0.5*PI)),  uy, precision), "Test rotation around y-axis");
    ASSERT(equals(JPosition3D(uz).rotate(JRotation3Y(0.5*PI)),  ux, precision), "Test rotation around y-axis");

    ASSERT(equals(JPosition3D(ux).rotate(JRotation3Z(0.5*PI)),  uy, precision), "Test rotation around z-axis");
    ASSERT(equals(JPosition3D(uy).rotate(JRotation3Z(0.5*PI)), -ux, precision), "Test rotation around z-axis");
    ASSERT(equals(JPosition3D(uz).rotate(JRotation3Z(0.5*PI)),  uz, precision), "Test rotation around z-axis");
  }

  for (int counter = 0; counter != numberOfEvents; ++counter) {

    STATUS("event: " << setw(10) << counter << '\r'); DEBUG(endl);

    const JQuaternion3D Qi = getRandom<JQuaternion3D>();
    const JRotation3D   Ri(Qi);

    JPosition3D first  = getRandom<JPosition3D>();
    JPosition3D second = first;
    
    second.rotate(Qi);
    second.rotate_back(Ri);

    ASSERT(equals(first, second, precision), "Test of compatibility between quaternion and rotation matrix.");

    second.rotate(Ri);
    second.rotate_back(Qi);

    ASSERT(equals(first, second, precision), "Test of compatibility between rotation matrix and quaternion.");
  }
  STATUS(endl);

  return 0;
}