// MyProcessor.cc
//

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

#include "MyProcessor.h"
#include "FDC/DFDCHit_factory.h"
#include "FDC/DFDCPseudo_factory.h"
#include "FDC/DFDCSegment_factory.h"
#include "TRACKING/DMCTrackHit.h"
#include "TRACKING/DMCThrown_factory.h"
#include "TRACKING/DTrackLinker_factory.h"
#include "DANA/DApplication.h"

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

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

//------------------------------------------------------------------
// ~MyProcessor 
//------------------------------------------------------------------
MyProcessor::~MyProcessor()
{
}

//------------------------------------------------------------------
// init 
//------------------------------------------------------------------
jerror_t MyProcessor::init(void)
{
  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 < 0) return NOERROR;
  dataFile = new ofstream("fdcData.txt");
  trajFile = new ofstream("fdcTraj.txt");

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

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

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

  vector<const DTrackLinker*>links;
  eventLoop->Get(links);

  double size =  pseudopoints.size();
  cout << "number of pseudopoints = " << size << endl;
  if (size < 5) return NOERROR;

  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);
    pseudoResidFunc prf(&pseudopoints, &helixTraj);
    residFuncPtr = &prf;
    chisqMin ppFit(&prf);
    HepVector ppStart(5), ppEnd(5);
    ppStart(1) = 0.0;
    ppStart(2) = 0.0;
    ppStart(3) = 0.0;
    ppStart(4) = sinthxfirst;
    ppStart(5) = sinthyfirst;
    ppFit.setStartParams(ppStart);
    ppFit.fit();
    ppFit.getEndParams(ppEnd);
    helixTraj.swim(ppEnd); // swim with final parameters
    trajPtr = helixTraj.getTrajectory();
    //    cout << "size = " << trajPtr->size() << endl;
    if (trajPtr->size() >= 3) {
      //      cout
      //	<< "first point = " << *((*trajPtr)[0]) << endl
      //	<< "second point = " << *((*trajPtr)[1]) << endl
      //	<< "third point = " << *((*trajPtr)[2]) << endl;
    }
    for (unsigned int i = 0; i < trajPtr->size(); i++) {
      trajPoint = *((*trajPtr)[i]);
      *trajFile << i + 1 << " " << trajPoint(1) << " " << trajPoint(2) << " "
		<< trajPoint(3) << endl;
    }
  } catch (int code) {
    cout << "==========fit error = " << code << "===========" << endl;
  }


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

  // test of B field map

  {

    double x=0.0, y = 0.0, z = 65.0, Bx, By, Bz;
    bfield->GetField(x, y, z, Bx, By, Bz);
    cout << "r = " << x << ',' << y << ',' << z
	 << " B = " << Bx << ',' << By << ',' << Bz << endl;
    
  }
  
  return NOERROR;
 
}