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

#include "TROOT.h"
#include "TFile.h"
#include "TH1D.h"
#include "TRandom3.h"

#include "JLang/JException.hh"
#include "JMath/JConstants.hh"
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"
#include "JGeometry3D/JDirection3D.hh"
#include "JCompass/JHit.hh"
#include "JCompass/JModel.hh"
#include "JFit/JSimplex.hh"

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


/**
 * Test program for fit of quaternions.
 */
int main(int argc, char**argv)
{
  using namespace std;
  using namespace JPP;

  string       outputFile;
  int          numberOfEvents;
  double       X;
  double       Y;
  double       Z;
  double       alpha;
  JPosition3D  sigma;
  UInt_t       seed;
  bool         fit;
  int          debug;

  try {

    JParser<> zap("Test program for fit of quaternions.");

    zap['o'] = make_field(outputFile)          = "twist.root";
    zap['n'] = make_field(numberOfEvents)      = 1000;
    zap['x'] = make_field(X,               "tilt angle around x-axis [deg]") = 0.0;
    zap['y'] = make_field(Y,               "tilt angle around y-axis [deg]") = 0.0;
    zap['z'] = make_field(Z,               "tilt angle around z-axis [deg]") = 0.0;
    zap['a'] = make_field(alpha,           "twist angle [deg/m]");
    zap['s'] = make_field(sigma,           "(x,y,z) resolution [deg]");
    zap['S'] = make_field(seed)                = 0;
    zap['F'] = make_field(fit,             "enable fit");
    zap['d'] = make_field(debug)               = 2;

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


  gRandom->SetSeed(seed);


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

  TH1D hc("chi2/NDF", NULL, 100, 0.0,  2.0);
  TH1D h0("tilt",     NULL, 100, 0.0,  2.0);
  TH1D h1("twist",    NULL, 100, 0.0,  0.1);


  // model

  const JModel model(JQuaternion3D(JQuaternion3X(X     * PI / 180.0),
				   JQuaternion3Y(Y     * PI / 180.0),
				   JQuaternion3Z(Z     * PI / 180.0)),
		     JQuaternion3D(JQuaternion3Z(alpha * PI / 180.0)));

  STATUS("Q0   " << FIXED(12,8) << model.Q0 << endl);
  STATUS("Q1   " << FIXED(12,8) << model.Q1 << endl);
  

  JSimplex<JModel> simplex;

  JSimplex<JModel>::MAXIMUM_ITERATIONS = 10000;

  simplex.debug = debug;

  const JChi2 getChi2(EM_NORMAL);


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

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

    // generate data

    const int    N   = 18;                         // number of floors
    const double z1  = 30.0;                       // height of first floor
    const double dz  = 10.0;                       // distance between floors
    const double eps = 1.0e-6;                     // minimal sigma

    vector<JHit> data;

    for (int i = 1; i <= N; ++i) {

      const double z = z1 + (i - 1) * dz;          // height of floor i

      data.push_back(JHit(i, z, model(z), max(eps, sigma.getLength())));
    }

    // smearing

    for (vector<JHit>::iterator i = data.begin(); i != data.end(); ++i) {
      i->mul(JQuaternion3D(JQuaternion3X(gRandom->Gaus(0.0, sigma.getX()) * PI / 180.0),
			   JQuaternion3Y(gRandom->Gaus(0.0, sigma.getY()) * PI / 180.0),
			   JQuaternion3Z(gRandom->Gaus(0.0, sigma.getZ()) * PI / 180.0)));
    }

    for (vector<JHit>::const_iterator i = data.begin(); i != data.end(); ++i) {
      DEBUG(FIXED(4,1) << i->getZ() << ' ' << FIXED(12,8) << i->getQuaternion() << endl);
    }


    JModel result(data.begin(), data.end());      // prefit

    DEBUG("R0   " << FIXED(12,8) << result.Q0 << ' ' << getAngle(model.Q0, result.Q0) << endl);
    DEBUG("R1   " << FIXED(12,8) << result.Q1 << ' ' << getAngle(model.Q1, result.Q1) << endl);

    if (fit) {

      simplex.value = result;                     // start value

      simplex.step.resize(4);

      simplex.step[0] = JModel(JQuaternion3X(0.50 * PI / 180.0), JQuaternion3D::getIdentity());
      simplex.step[1] = JModel(JQuaternion3Y(0.50 * PI / 180.0), JQuaternion3D::getIdentity());
      simplex.step[2] = JModel(JQuaternion3Z(0.50 * PI / 180.0), JQuaternion3D::getIdentity());
      simplex.step[3] = JModel(JQuaternion3D::getIdentity(), JQuaternion3Z(0.05 * PI / 180.0));

      const double chi2 = simplex(getChi2, data.begin(), data.end());  
      const int    ndf  = data.size() * 4 - simplex.step.size();

      DEBUG("number of iterations " << simplex.numberOfIterations << endl);

      result = simplex.value;                     // final value

      DEBUG("chi2 " << chi2 << '/' << ndf << endl);
      DEBUG("R0   " << FIXED(12,8) << simplex.value.Q0 << ' ' << getAngle(model.Q0, result.Q0) << endl);
      DEBUG("R1   " << FIXED(12,8) << simplex.value.Q1 << ' ' << getAngle(model.Q1, result.Q1) << endl);

      hc.Fill(chi2 / ndf);
    }

    //h0.Fill(getAngle(model.Q0, result.Q0));
    h0.Fill(getAngle(JDirection3D(0.0, 0.0, 1.0).rotate(model .Q0), 
		     JDirection3D(0.0, 0.0, 1.0).rotate(result.Q0)));
    h1.Fill(getAngle(model.Q1, result.Q1));
  }
  STATUS(endl);

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