// 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(1)
{
}

//------------------------------------------------------------------
// ~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 != 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 =  pseudopoints.size();
  if (debug_level > 1) cout << "number of pseudopoints = " << size << endl;
  hPsHits->Fill(float(size));
  if (size < 14) return NOERROR;

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

  dataFile = new ofstream("fdcData.txt");
  trajFile = new ofstream("fdcTraj.txt");

  for (int i = 0; i < size; i++) {
    *dataFile << i + 1 << " " << (pseudopoints[i])->x << " "
	      << (pseudopoints[i])->y << " "
	      << (pseudopoints[i])->wire->origin(2) << 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 << ' ' << sinthxfirst << ' '
		   << sinthyfirst << ' ' << ppEnd(1) << ' ' << ppEnd(2) << ' '
		   << ppEnd(3) << ' ' << ppEnd(4) << ' ' << ppEnd(5) << endl;
  } catch (int code) {
    cout << "==========fit error = " << code << "===========" << endl;
  }


  dataFile->close();
  trajFile->close();
  delete dataFile;
  delete trajFile;

  return NOERROR;
 
}