#include <string>
#include <iostream>
#include <iomanip>
#include <limits>
#include <map>

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

#include "JROOT/JGraph.hh"
#include "JROOT/JRootToolkit.hh"
#include "JROOT/JManager.hh"

#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JDetectorAddressMapToolkit.hh"
#include "JDetector/JModuleRouter.hh"

#include "JSupport/JMultipleFileScanner.hh"
#include "JCompass/JEvt.hh"
#include "JCompass/JSupport.hh"

#include "JDynamics/JDynamics.hh"

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


namespace {

  using JUTC::JUTCTimeRange;
  using JGEOMETRY3D::JQuaternion3D;

  /**
   * Auxiliary method to get UTC time range of calibration data.
   *
   * \param  __begin                   begin of calibration data
   * \param  __end                     end   of calibration data
   * \return                           UTC time range
   */
  template<class T>
  static inline JUTCTimeRange getUTCTimeRange(T __begin, T __end)
  {
    JUTCTimeRange range = JUTCTimeRange::DEFAULT_RANGE();

    for (T i = __begin; i != __end; ++i) {
      if (!i->second.empty()) {
	range.combine(JUTCTimeRange(i->second.getXmin(), i->second.getXmax()));
      }
    }
 
    return range;
  }
  
  /**
   * Get twist angle.
   *
   * \param  Q                         quaternion
   * \return                           twist angle [rad]
   */
  inline double getTwist(const JQuaternion3D& Q)
  {
    using namespace JPP;

    const JQuaternion3D::decomposition q(Q, JVector3Z_t);

    double phi = (q.twist.getD() > 0.0 ? +2.0 : -2.0) * acos(q.twist.getA());

    while (phi > +PI) { phi -= 2*PI; }
    while (phi < -PI) { phi += 2*PI; }

    return phi;
  }
}


/**
 * \file
 *
 * Example program to plot orientation data.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

  JMultipleFileScanner<JCOMPASS::JOrientation> inputFile;
  JLimit_t&       numberOfEvents = inputFile.getLimit();
  string          detectorFile;
  string          outputFile;
  double          Tmax_s;
  int             debug;

  try {

    JParser<> zap("Example program to plot orientation data.");

    zap['f'] = make_field(inputFile,    "output of JBallarat[.sh]");
    zap['n'] = make_field(numberOfEvents)                                    = JLimit::max();
    zap['a'] = make_field(detectorFile);
    zap['o'] = make_field(outputFile)    = "gold.root";
    zap['T'] = make_field(Tmax_s);
    zap['d'] = make_field(debug)         = 2;

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


  JDetector detector;

  try {
    load(detectorFile, detector);
  }
  catch(const JException& error) {
    FATAL(error);
  }

  const JModuleRouter router(detector);

  if (!hasDetectorAddressMap(detector.getID())) {
    FATAL("No detector address map for detector identier " << detector.getID() << endl);
  }

  const JDetectorAddressMap& demo = getDetectorAddressMap(detector.getID());


  JDynamics dynamics(detector, Tmax_s);

  STATUS("loading input from file(s) " << inputFile << "... " << flush);

  dynamics.load(inputFile);

  STATUS("OK" << endl);


  map<int, JGraph_t> Z0;

  for (JDynamics::JOrientation::const_iterator module = dynamics.orientation.begin(); module != dynamics.orientation.end(); ++module) {

    if (router.getModule(module->first).getFloor() != 0) {

      STATUS("Creating graphics for input data " << setw(10) << module->first << flush << '\r');

      JQuaternion3D Q0 = getRotation(getModule(demo.get(module->first)), router.getModule(module->first));

      Q0.conjugate();

      JGraph_t& z0 = Z0[module->first];

      for (JDynamics::JOrientation::function_type::const_iterator i = module->second.begin(); i != module->second.end(); ++i) {

	const JQuaternion3D Q1 = router.getModule(module->first).getQuaternion() * i->getY();

	z0.put(i->getX(), getTwist(Q1 * Q0));
      }
    }
  }
  STATUS(endl);

  map<int, TH1D*> H1;

  for (JDynamics::JOrientation::const_iterator module = dynamics.orientation.begin(); module != dynamics.orientation.end(); ++module) {

    if (router.getModule(module->first).getFloor() != 0 && module->second.size() > 1u) {

      STATUS("Creating graphics for input data " << setw(10) << module->first << flush << '\r');

      TH1D* h1 = H1[module->first] = new TH1D(MAKE_CSTRING("U[" << module->first << "].twist"), NULL, 500, -180.0, +180.0);

      for (JDynamics::JOrientation::function_type::const_iterator i1 = module->second.begin(), i0 = i1++; i1 != module->second.end(); ++i0, ++i1) {

	JQuaternion3D Q0 = i0->getY();
	JQuaternion3D Q1 = i1->getY();

	h1->Fill(getTwist(Q1 * Q0.conjugate()) * 180.0 / PI);
      }
    }
  }
  STATUS(endl);

  map<int, TH1D*> H0;

  for (JDynamics::JOrientation::const_iterator module = dynamics.orientation.begin(); module != dynamics.orientation.end(); ++module) {

    STATUS("Creating graphics for compiled data " << setw(10) << module->first << flush << '\r');

    if (router.getModule(module->first).getFloor() != 0 && !module->second.empty()) {

      JQuaternion3D Q0 = getRotation(getModule(demo.get(module->first)), router.getModule(module->first));

      Q0.conjugate();

      const Double_t xmin = module->second.getXmin();
      const Double_t xmax = module->second.getXmax();
      const Int_t    nx   = (Int_t) ((xmax - xmin) / Tmax_s);

      TH1D* h0 = H0[module->first] = new TH1D(MAKE_CSTRING("H[" << module->first << "].twist"), NULL, nx, xmin, xmax);

      for (Int_t i = 1; i <= h0->GetXaxis()->GetNbins(); ++i) {

        const Double_t      x = h0->GetXaxis()->GetBinCenter(i);
        const JQuaternion3D Q = module->second(x) * Q0;

        h0->SetBinContent(i, getTwist(Q));
      }
    }
  }
  STATUS(endl);

  const Double_t xmin = getUTCTimeRange(dynamics.orientation.begin(), dynamics.orientation.end()).getLowerLimit();
  const Double_t xmax = getUTCTimeRange(dynamics.orientation.begin(), dynamics.orientation.end()).getUpperLimit();
  const Int_t    nx   = (Int_t) ((xmax - xmin) / Tmax_s);

  JManager<int, TH1D> X0(new TH1D("X[%].twist", NULL, nx, xmin, xmax));

  for (Int_t i = 1; i <= X0->GetXaxis()->GetNbins(); ++i) {

    STATUS("Creating graphics for detector data " << FIXED(5,1) << (double) (i * 100) / (double) X0->GetXaxis()->GetNbins() << "%" << flush << '\r');

    dynamics.update(X0->GetXaxis()->GetBinCenter(i));

    for (JDetector::const_iterator module = dynamics.begin(); module != dynamics.end(); ++module) {

      if (module->getFloor() != 0) {
      
	TH1D* x0 = X0[module->getID()];

	const JQuaternion3D Q = getRotation(router.getModule(module->getID()), *module);

	x0->SetBinContent(i, getTwist(Q));
      }
    }
  }
  STATUS(endl);


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

  for (map<int, TH1D*>::const_iterator i = H0.begin(); i != H0.end(); ++i) {
    out << *i->second;
  }

  for (map<int, TH1D*>::const_iterator i = H1.begin(); i != H1.end(); ++i) {
    out << *i->second;
  }

  out << X0;

  for (map<int, JGraph_t>::const_iterator i = Z0.begin(); i != Z0.end(); ++i) {
    out << JGraph(i->second, MAKE_CSTRING("G[" << i->first << "].twist"));
  }

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