// DTrack_factory_MMI.cc  $Id$
//

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

#include "DTrack_factory_MMI.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 "JFactoryGeneratorDTrack_MMI.h"
#include "MyTrajectory.h"
#include "MyTrajectoryBfield.h"
#include "chisqMin.h"
#include "parabolaResidFunc.h"
#include "combinedResidFunc.h"
#include "combinedResidFunc.h"
#include "globalGslFuncs.h"

extern "C"{
void InitPlugin(JApplication *app){
	InitJANAPlugin(app);
	app->AddFactoryGenerator(new JFactoryGeneratorDTrack_MMI());
}
}

//------------------------------------------------------------------
// DTrack_factory_MMI 
//------------------------------------------------------------------
DTrack_factory_MMI::DTrack_factory_MMI():debug_level(1)
{
}

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

//------------------------------------------------------------------
// init 
//------------------------------------------------------------------
jerror_t DTrack_factory_MMI::init(void)
{
  cout << "init called\n";
  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 DTrack_factory_MMI::fini(void)
{
  cout << "fini called\n";
  rootfile->Write();
  signatureFile->close();
  delete signatureFile;
  return NOERROR;
}

//------------------------------------------------------------------
// brun 
//------------------------------------------------------------------
jerror_t DTrack_factory_MMI::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 DTrack_factory_MMI::erun()
{
 return NOERROR;
}

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

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

  //if (eventnumber != 434 ) 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));
  if (size_fdc < 14) return NOERROR;

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

  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 = (pseudopoints[0])->x;
  double yfirst = (pseudopoints[0])->y;
  double zfirst = (pseudopoints[0])->wire->origin(2);
  double sinthxfirst = xfirst/zfirst; // approximately
  double sinthyfirst = yfirst/zfirst;

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

    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 << ' '
		   << sinthxfirst << ' '
		   << sinthyfirst << ' '
		   << ppEnd(1) << ' ' 
		   << ppEnd(2) << ' '
		   << ppEnd(3) << ' '
		   << ppEnd(4) << ' '
		   << ppEnd(5) << ' '
		   << ppFit.getChi2() << endl;

		//------------------ Code to copy fit results into a DTrack object ---------------
		// Copy fit results into pos and mom DVector3 objects.
		// I'm not exactly sure of the parameterization here, but I'm assuming it is:
		//
		// ppEnd(1) = x at origin
		// ppEnd(2) = y at arigin
		// ppEnd(3) = 1/p
		// ppEnd(4) = px/pz at origin
		// ppEnd(5) = py/pz at origin
		//
		// I actually don't think the above is quite correct, but it at least makes the
		// following understandable.
		DVector3 pos(ppEnd(1), ppEnd(2), 65.0); // 65.0 is z position of center of target
		DVector3 mom;
		double theta = atan(sqrt(pow(ppEnd(4), 2.0) + pow(ppEnd(5), 2.0)));
		double phi = atan2(ppEnd(5), ppEnd(4));
		mom.SetMagThetaPhi(1.0/ppEnd(3), theta, phi);

		// Create a DTrack object with the fit results
		DTrack *track = new DTrack;
		track->q			= +1.0; // Don't see where this is set
		track->p			= mom.Mag();
		track->theta	= mom.Theta();
		track->phi		= mom.Phi();
		if(track->phi<0.0)track->phi+=2.0*M_PI;
		track->x			= pos.X();
		track->y			= pos.Y();
		track->z			= pos.Z();
		track->candidateid = -1;
		track->chisq	= ppFit.getChi2();
		track->rt		= NULL;
		
		// Fill in DKinematicData part
		track->setMass(0.0);
		track->setMomentum(mom);
		track->setPosition(pos);
		track->setCharge(track->q);

		// Add DTrack to factory's object container where it can be accessed by other factories
		_data.push_back(track);
		
		//---------------------------------------------------------------------------------

  } catch (int code) {
    cout << "==========fit error = " << code << "===========" << endl;
  }


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

  return NOERROR;
 
}