// MyProcessor.cc
//

#include <iostream>
#include <vector>
#include <string>
#include <CLHEP/Matrix/Matrix.h>
#include <CLHEP/Matrix/Vector.h>

#include "MyProcessor.h"
#include "FDC/DFDCHit.h"
#include "FDC/DFDCPseudo_factory.h"
#include "FDC/DFDCSegment_factory.h"
#include "CDC/DCDCTrackHit.h"
#include "TRACKING/DMCThrown.h"
#include "TRACKING/DMCTrackHit.h"
#include "DANA/DApplication.h"

#include "MyTrajectory.h"
#include "MyTrajectoryBfield.h"
#include "chisqMin.h"
#include "parabolaResidFunc.h"
#include "combinedResidFunc.h"
#include "combinedResidFunc.h"
#include "globalGslFuncs.h"

//------------------------------------------------------------------
// MyProcessor 
//------------------------------------------------------------------
MyProcessor::MyProcessor():debug_level(2)
{
}

//------------------------------------------------------------------
// ~MyProcessor 
//------------------------------------------------------------------
MyProcessor::~MyProcessor()
{
  cout << "MyProcessor destructor called\n";
}

//------------------------------------------------------------------
// init 
//------------------------------------------------------------------
jerror_t MyProcessor::init(void)
{
  rootfile = new TFile("fitter.root", "RECREATE");
  hPsHits = new TH1F("hPsHits", "pseudohit multiplicity", 25, -0.5, 24.5);
  hPTot = new TH1F("hPTot", "total momentum", 50, 0.0, 5.0);
  signatureFile = new ofstream("fitter_signature.txt");
  return NOERROR;
}

//------------------------------------------------------------------
// fini
//------------------------------------------------------------------
jerror_t MyProcessor::fini(void)
{
  cout << "fini called\n";
  rootfile->Write();
  signatureFile->close();
  delete signatureFile;
  return NOERROR;
}

//------------------------------------------------------------------
// brun 
//------------------------------------------------------------------
jerror_t MyProcessor::brun(JEventLoop *eventLoop, int runnumber)
{
DApplication* dapp=dynamic_cast<DApplication*>(eventLoop->GetJApplication());
  bfield = dapp->GetBfield();
  JFactory_base *base = eventLoop->GetFactory("DFDCSegment");
	segment_factory = dynamic_cast<DFDCSegment_factory*>(base);
	return NOERROR;
}

//------------------------------------------------------------------
// erun 
//------------------------------------------------------------------
jerror_t MyProcessor::erun()
{
 return NOERROR;
}

//------------------------------------------------------------------
// evnt 
//------------------------------------------------------------------

jerror_t MyProcessor::evnt(JEventLoop *eventLoop, int eventnumber)
{

  //if (eventnumber != 1) return NOERROR;

  if (debug_level > 1) cout << "----------- New Event "
			     << eventnumber << " -------------" << endl;
  vector<const DFDCHit*>fdchits;
  //eventLoop->Get(fdchits);  

  vector<const DFDCPseudo*>pseudopoints;
  eventLoop->Get(pseudopoints);  

  vector<const DCDCTrackHit*>trackhits;
  eventLoop->Get(trackhits);  
  
  vector<const DMCTrackHit*>truepoints;
  eventLoop->Get(truepoints);

  vector<const DMCThrown*>thrown;
  eventLoop->Get(thrown);

  int size_fdc =  pseudopoints.size();
  if (debug_level > 1) cout << "number of pseudopoints = " << size_fdc << endl;
  hPsHits->Fill(float(size_fdc));

  int size_cdc = trackhits.size();
  if (debug_level > 1) cout << "number of trackhits = " << size_cdc << endl;

  if (size_fdc < 6) return NOERROR;

  fdcDataFile = new ofstream("fdcData.txt");
  cdcDataFile = new ofstream("cdcData.txt");
  trajFile = new ofstream("fdcTraj.txt");

  for (int i = 0; i < size_fdc; i++) {
    *fdcDataFile << i + 1 << " " << (pseudopoints[i])->x << " "
	      << (pseudopoints[i])->y << " "
	      << (pseudopoints[i])->wire->origin(2) << endl;
  }

  for (int i = 0; i < size_cdc; i++) {
    const DCDCTrackHit *trkhit = trackhits[i];
    const DCDCWire *wire = trkhit->wire;
    double x = wire->origin.X();
    double y = wire->origin.Y();
    double z = wire->origin.Z();
    double phi_wire = atan2(y, x);
    double phi = phi_wire + PIOVER2;
    double theta = wire->stereo;
    *cdcDataFile << i + 1 << " " << x << " " << y << " " << z << " "
		 << theta << " " << phi << endl;
  }

  HepVector B(3);
  B(1) = 0.0; B(2) = 0.0; B(3) = 4.0;

  double xfirst, yfirst, zfirst;
  if (size_fdc > 0) {
    xfirst = (pseudopoints[0])->x;
    yfirst = (pseudopoints[0])->y;
    zfirst = (pseudopoints[0])->wire->origin(2);
  } else if (size_cdc > 0) {
    const DCDCTrackHit *trkhit = trackhits[size_cdc - 1];
    const DCDCWire *wire = trkhit->wire;
    xfirst = wire->origin.X();
    yfirst = wire->origin.Y();
    zfirst = wire->origin.Z();
  } else {
    int code = 32;
    throw code;
  }
  double delta_z = zfirst - 65.0;
  double rPerp = sqrt(xfirst*xfirst + yfirst*yfirst);
  double thetaInitial = atan2(rPerp, delta_z);
  double phiInitial = atan2(yfirst, xfirst);

  vector<HepVector*>* trajPtr;
  HepVector trajPoint(3);
 
  try {
    MyTrajectoryBfield helixTraj(bfield);
    combinedResidFunc prf(&pseudopoints, &trackhits, &helixTraj, debug_level);
    residFuncPtr = &prf;
    chisqMin ppFit(&prf, debug_level);
    HepVector ppStart(5), ppEnd(5);
    ppStart(1) = 0.0;
    ppStart(2) = 0.0;
    ppStart(3) = thetaInitial;
    ppStart(4) = phiInitial;
    ppStart(5) = 0.0;

    helixTraj.swim(ppStart); // swim with initial parameters
    trajPtr = helixTraj.getTrajectory();
    for (unsigned int i = 0; i < trajPtr->size(); i++) {
      trajPoint = *((*trajPtr)[i]);
      *trajFile << "0 " << i + 1 << " " << trajPoint(1) << " " << trajPoint(2) << " "
		<< trajPoint(3) << endl;
    }
    trajPtr->clear();

    ppFit.setStartParams(ppStart);
    ppFit.fit();
    ppFit.getParams(ppEnd);
    hPTot->Fill(1.0/ppEnd(3));
    helixTraj.swim(ppEnd); // swim with final parameters
    trajPtr = helixTraj.getTrajectory();
    for (unsigned int i = 0; i < trajPtr->size(); i++) {
      trajPoint = *((*trajPtr)[i]);
      *trajFile << "1 " << i + 1 << " " << trajPoint(1) << " " << trajPoint(2) << " "
		<< trajPoint(3) << endl;
    }
    *signatureFile << eventnumber << ' '
		   << size_fdc << ' '
		   << size_cdc << ' '
		   << thetaInitial << ' '
		   << phiInitial << ' '
		   << ppEnd(1) << ' ' 
		   << ppEnd(2) << ' '
		   << ppEnd(3) << ' '
		   << ppEnd(4) << ' '
		   << ppEnd(5) << ' '
		   << ppFit.getChi2() << endl;
  } catch (int code) {
    cout << "==========fit error = " << code << "===========" << endl;
  }


  fdcDataFile->close();
  cdcDataFile->close();
  trajFile->close();
  delete fdcDataFile;
  delete cdcDataFile;
  delete trajFile;

  return NOERROR;
 
}