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

#include "TROOT.h"
#include "TFile.h"
#include "TH1D.h"
#include "TProfile.h"
#include "TRandom.h"

#include "km3net-dataformat/offline/Head.hh"
#include "km3net-dataformat/offline/Evt.hh"
#include "km3net-dataformat/tools/time_converter.hh"
#include "JDAQ/JDAQEventIO.hh"
#include "JDAQ/JDAQTimesliceIO.hh"
#include "JAAnet/JHead.hh"
#include "JAAnet/JHeadToolkit.hh"
#include "JAAnet/JAAnetToolkit.hh"
#include "JPhysics/JConstants.hh"
#include "JGeometry3D/JDirection3D.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JGeometry3D/JGeometry3DToolkit.hh"
#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JModuleRouter.hh"
#include "JDetector/JPMTRouter.hh"
#include "JTrigger/JHit.hh"
#include "JTrigger/JFrame.hh"
#include "JTrigger/JTimeslice.hh"
#include "JTrigger/JHitL0.hh"
#include "JTrigger/JHitL1.hh"
#include "JTrigger/JBuildL1.hh"
#include "JTrigger/JBuildL2.hh"
#include "JTrigger/JAlgorithm.hh"
#include "JTrigger/JMatch1D.hh"
#include "JTrigger/JMatch3B.hh"
#include "JSupport/JTriggeredFileScanner.hh"
#include "JSupport/JFileRecorder.hh"
#include "JSupport/JMonteCarloFileSupportkit.hh"
#include "JSupport/JSupport.hh"

#include "JFit/JMatrixNZ.hh"
#include "JFit/JFitToolkit.hh"

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


/**
 * \file
 *
 * Example program to test chi2 calculations based on JFIT::JMatrixNZ.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;
  using namespace KM3NETDAQ;

  JTriggeredFileScanner<> inputFile;
  JLimit_t&      numberOfEvents = inputFile.getLimit();
  string         outputFile;
  string         detectorFile;
  double         Tmax_ns;
  double         roadWidth_m;
  double         ctMin;
  double         alpha_deg;
  double         sigma_ns;
  int            numberOfOutliers;
  bool           useTrue;
  int            debug;

  try { 

    JParser<> zap("Example program to test chi2 calculations of co-variance matrix including direction uncertainty.");
    
    zap['f'] = make_field(inputFile);
    zap['o'] = make_field(outputFile)          = "matrixnz.root";
    zap['a'] = make_field(detectorFile);
    zap['n'] = make_field(numberOfEvents)      = JLimit::max();
    zap['T'] = make_field(Tmax_ns)             =  20.0;               // [ns]
    zap['R'] = make_field(roadWidth_m)         = 150.0;               // [m]    
    zap['C'] = make_field(ctMin)               =   0.0;               //   
    zap['A'] = make_field(alpha_deg);
    zap['S'] = make_field(sigma_ns);
    zap['O'] = make_field(numberOfOutliers);
    zap['U'] = make_field(useTrue);
    zap['d'] = make_field(debug)               = 1;
    
    zap(argc, argv);
  }
  catch(const exception& error) {
    FATAL(error.what() << endl);
  }


  using namespace KM3NETDAQ;


  const unsigned int NUMBER_OF_HITS      =  6;
  const double       STANDARD_DEVIATIONS =  3.0;                        // [unit]
  const double       HIT_OFF             =  1.0e3 * sigma_ns*sigma_ns;  // [ns^2]


  JDetector detector;

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

  const JModuleRouter moduleRouter(detector);
  const JPMTRouter    pmtRouter   (detector);

  const JPosition3D   center = get<JPosition3D>(getHeader(inputFile));


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


  TH1D h0("h0", NULL, 50,  0.0,  20.0);
  TH1D h1("h1", NULL, 50,  0.0,  20.0);

  TH1D p0("p0", NULL, 50,  0.0,   1.0);
  TH1D p1("p1", NULL, 50,  0.0,   1.0);

  vector<double> X;

  {
    double x = -0.5;
    
    for ( ; x <  10.0; x +=  1.0) { X.push_back(x); }
    for ( ; x <  25.0; x +=  2.0) { X.push_back(x); }
    for ( ; x < 100.0; x +=  5.0) { X.push_back(x); }
  }

  TProfile n0("n0", NULL, X.size() - 1, X.data());
  TProfile n1("n1", NULL, X.size() - 1, X.data());


  typedef vector<JHitL1>  JData_t;
  const JBuildL2<JHitL1>  buildL2(JL2Parameters(2, Tmax_ns, ctMin));
  const JMatch3B<JHitL1>  match3B(roadWidth_m, Tmax_ns);
  const JMatch1D<JHitL1>  match1D(roadWidth_m, Tmax_ns);

  JHit::setSlewing(!useTrue);

  while (inputFile.hasNext()) {

    STATUS("event: " << setw(10) << inputFile.getCounter() << '\r'); DEBUG(endl);

    JTriggeredFileScanner<>::multi_pointer_type ps = inputFile.next();

    const JDAQEvent* tev   = ps;
    const Evt*       event = ps;
    
    const time_converter converter(*event, *tev);
	
    vector<Trk>::const_iterator muon = find_if(event->mc_trks.begin(), event->mc_trks.end(), is_muon);

    if (muon != event->mc_trks.end()) {

      const JRotation3D R(getDirection(*muon));

      JLine1Z tz(getPosition(*muon), converter.putTime(muon->t));

      tz.add(center);
      tz.rotate(R);

      const double theta = alpha_deg * PI / 180.0;
      const double phi   = gRandom->Uniform(0.0, 2*PI);
      
      const JRotation3D Rs(JAngle3D(theta,phi));


      JData_t data;

      if (!useTrue) {

	buildL2(*tev, moduleRouter, back_inserter(data));
	
	data.erase(unique(data.begin(), data.end(), equal_to<JDAQModuleIdentifier>()), data.end());

      } else {

	map<int, vector<JHitL0> > zbuf;

	for (vector<Hit>::const_iterator i = event->mc_hits.begin(); i != event->mc_hits.end(); ++i) {
	  
	  if (!is_noise(*i)) {
	    
	    const JPMTAddress&    address = pmtRouter.getAddress(i->pmt_id);
	    const JPMTIdentifier& id      = pmtRouter.getIdentifier(address);
	    const JPMT&           pmt     = pmtRouter.getPMT(address);
	    const double          t1      = converter.putTime(getTime(*i));

	    zbuf[address.first].push_back(JHitL0(JDAQPMTIdentifier(id.getModuleID(), id.getPMTAddress()), pmt, t1));
	  }
	}

	for (map<int, vector<JHitL0> >::iterator i = zbuf.begin(); i != zbuf.end(); ++i) {

	  sort(i->second.begin(), i->second.end(), less<JHit>());
	  
	  data.push_back(i->second[0]);
	}
      }


      // 3D cluster

      data.erase(clusterizeWeight(data.begin(), data.end(), match3B), data.end());


      // 1D cluster
      
      for (JData_t::iterator i = data.begin(); i != data.end(); ++i) {
	i->rotate(R);
      }

      data.erase(clusterizeWeight(data.begin(), data.end(), match1D), data.end());


      // move true muon to center of mass of hits

      tz.setZ(JWeighedCenter3D(data.begin(), data.end()).getZ(), getSpeedOfLight());


      // set coordinate origin to true muon position

      for (JData_t::iterator i = data.begin(); i != data.end(); ++i) {
	i->sub(tz.getPosition());
      }

      tz.setPosition(JVector3D(0,0,0));

      
      // apply rotational smearing to hits

      for (JData_t::iterator i = data.begin(); i != data.end(); ++i) {
	i->rotate_back(Rs);
      }


      if (data.size() >= NUMBER_OF_HITS + numberOfOutliers) {

	JData_t::iterator __end = data.end();

	for (int n = 0; n < numberOfOutliers && distance(data.begin(), __end) > NUMBER_OF_HITS; ++n) {
	  
	  double            ymax = 0.0;
	  JData_t::iterator kill = __end;
	  
	  for (JData_t::iterator i = data.begin(); i != __end; ++i) {
	    
	    const double y = fabs(i->getT() - tz.getT(*i)) / sigma_ns;
	    
	    if (y > ymax) {
	      ymax = y;
	      kill = i;
	    }
	  }
	  
	  if (ymax >= STANDARD_DEVIATIONS)
	    iter_swap(kill, --__end);
	  else 
	    break;
	}

	const double chi2 = getChi2(tz, data.begin(), __end, sigma_ns);
	const int    N    = distance(data.begin(), __end);
	
	h0.Fill(chi2 / N);
        p0.Fill(TMath::Prob(chi2, N));
	n0.Fill((double) data.size(), (double) N / (double) data.size()); 
      }


      if (data.size() >= NUMBER_OF_HITS + numberOfOutliers) {

	JMatrixNZ V;
	JVectorNZ Y;

	V.set(tz, data.begin(), data.end(), alpha_deg, sigma_ns);
	Y.set(tz, data.begin(), data.end());

	V.invert();

	unsigned int N = data.size();
	
	for (int n = 0; n < numberOfOutliers && N > NUMBER_OF_HITS; ++n, --N) {
	  
	  double ymax =  0.0;
	  int    kill = -1;
	  
	  for (unsigned int i = 0; i != V.size(); ++i) {
	      
	    const double y = getChi2(Y, V, i);
	    
	    if (y > ymax) {
	      ymax = y;
	      kill = i;
	    }
	  }

	  if (ymax >= STANDARD_DEVIATIONS * STANDARD_DEVIATIONS)
	    V.update(kill, HIT_OFF);
	  else 
	    break;
	}
	  
	const double chi2 = V.getDot(Y);
	
	h1.Fill(chi2 / N);
        p1.Fill(TMath::Prob(chi2, N));
	n1.Fill((double) data.size(), (double) N / (double) data.size());
      }
    }
  }
  STATUS(endl);

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