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

#include "TRandom3.h"

#include "JGeometry3D/JPosition3D.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JGeometry3D/JQuaternion3D.hh"

#include "JUTM/JUTMPosition.hh"

#include "JDetector/JCompass.hh"

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


/**
 * \file
 *
 * Example compass operations.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

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

  try {

    JParser<> zap;

    zap['n'] = make_field(numberOfEvents)      = 100;
    zap['e'] = make_field(precision)           = 1.0e-8;
    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 JCompass c1(0.5*PI, 0.0, 0.0);

    const JUTMPosition pointer[] = { JNorth_t, JEast_t, JSouth_t, JWest_t, JNorth_t };

    for (int i = 0; i != 4; ++i) {

      JPosition3D u = pointer[i];

      DEBUG(u << " - > ");

      u.rotate(c1.getRotation());

      DEBUG(u << endl);

      ASSERT(u.equals(pointer[i+1], precision), "compass definitions");
    }
  }

  for (int count = 0; count < numberOfEvents; ++count) {

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

    const JCompass c1(gRandom->Uniform(-1.0*PI,+1.0*PI),
		      gRandom->Uniform(-0.5*PI,+0.5*PI),
		      gRandom->Uniform(-0.5*PI,+0.5*PI));

    const JRotation3D    R1 = c1.getRotation();
    const JQuaternion3D  Q1(R1);
    const JQuaternion3D  Q2 = c1.getQuaternion();
    const JCompass       c2(Q1);

    DEBUG("compass    [1] " << setprecision(3) << c1 << endl);
    DEBUG("compass    [2] " << setprecision(3) << c2 << endl);
    DEBUG("quaternion [1] " << setprecision(5) << Q1 << endl);
    DEBUG("quaternion [2] " << setprecision(5) << Q2 << endl);

    DEBUG(R1 << endl);

    ASSERT(Q1.equals(+Q2, precision) || 
	   Q1.equals(-Q2, precision), "compass to rotation to quaternion versus compass to quaternion");

    ASSERT(c1.equals(c2, precision), "compass to quaternion to compass");

    const JPosition3D pos[] = {
      JVector3X_t,
      JVector3Y_t,
      JVector3Z_t,
    };

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

      JPosition3D p0 = pos[i];
      JPosition3D p1 = p0;

      p1.rotate(R1);

      JPosition3D p2 = p1;

      p2.rotate_back(Q1);

      DEBUG(showpos << FIXED(8,3) << p0 << " -> " << flush);
      DEBUG(showpos << FIXED(8,3) << p1 << " -> " << flush);
      DEBUG(showpos << FIXED(8,3) << p2 << endl);

      ASSERT(p0.equals(p2, precision));
    }
  }

  return 0;
}