#include <iostream>
#include <iomanip>
#include <vector>
#include <map>

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

#include "JDB/JAHRS.hh"
#include "JDB/JSupport.hh"

#include "JROOT/JGraph.hh"

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

#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JDirection3D.hh"

#include "JLang/JComparator.hh"
#include "JSupport/JMultipleFileScanner.hh"

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


/**
 * \file
 *
 * Example program to monitor AHRS data.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;
  
  JMultipleFileScanner<JAHRS> inputFile;
  JLimit_t&       numberOfEvents = inputFile.getLimit();
  string          outputFile;
  string          detectorFile;
  double          Tmax_s;
  size_t          numberOfModules;
  int             debug;

  try {

    JParser<> zap("Example program to monitor AHRS data.");
    
    zap['f'] = make_field(inputFile);
    zap['n'] = make_field(numberOfEvents)      = JLimit::max();
    zap['o'] = make_field(outputFile)          = "ahrs.root";
    zap['a'] = make_field(detectorFile);
    zap['T'] = make_field(Tmax_s)              = 600.0;   // [s]
    zap['N'] = make_field(numberOfModules)     = 10;
    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);

  typedef vector<JAHRS>          buffer_type;    // data type
  typedef map<int, buffer_type>  map_type;       // string -> data

  map_type  data;

  for (int counter = 0; inputFile.hasNext(); ++counter) {

    STATUS("counter: " << setw(8) << counter << '\r' << flush); DEBUG(endl);

    const JAHRS* parameters = inputFile.next();

    if (router.hasModule(parameters->DOMID)) {

      const JLocation location = router.getModule(parameters->DOMID);

      data[location.getString()].push_back(*parameters);
    }
  }
  STATUS(endl);


  map<int, JGraph_t>  ZO;
  map<int, JGraph_t>  ZA;

  for (map_type::iterator string = data.begin(); string != data.end(); ++string) {

    if (!string->second.empty()) {

      sort(string->second.begin(), string->second.end(), make_comparator(&JAHRS::UNIXTIME));

      for (buffer_type::const_iterator p = string->second.begin(); p != string->second.end(); ) {

	set<int> buffer;

	buffer_type::const_iterator q = p;

	for ( ; q != string->second.end() && q->UNIXTIME - p->UNIXTIME <= Tmax_s * 1.0e3; ++q) {
	  buffer.insert(q->DOMID);
	}

	if (buffer.size() > numberOfModules) {

	  JQuaternion3D Q;

	  for (buffer_type::const_iterator i = p; i != q; ++i) {
	    Q += JCompass(*i, JAHRSCalibration()).getQuaternion();
	  }

	  Q.normalise();

	  JDirection3D u(0.0, 0.0, 1.0);

	  u.rotate(Q);

	  const double t1  =  p->UNIXTIME * 1.0e-3  +  0.5*Tmax_s;

	  ZO[string->first].put(t1, atan2(u.getDY(), u.getDX()));
	  ZA[string->first].put(t1, sqrt(u.getDX()*u.getDX() + u.getDY()* u.getDY()));
	}

	p = q;
      }
    }
  }


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

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

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

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