//********************************************************
// DFDCPseudo_factory.cc - factory producing first-order 
// reconstructed points for the FDC
// Author: Craig Bookwalter (craigb at jlab.org)
// Date:   March 2006
// UVX cathode-wire matching revised by Simon Taylor, Aug 2006
//********************************************************

#include "DFDCPseudo_factory.h"

#define HALF_CELL 0.5
#define MAX_DEFLECTION 0.15
#define X0 0
#define QA 1
#define K2 2
#define ITER_MAX 100
#define TOLX 1e-4
#define TOLF 1e-4
#define A_OVER_H 0.475
#define ALPHA 1e-4 // rate parameter for Newton step backtracking algorithm

///
/// DFDCCathodeCluster_gLayer_cmp(): 
/// non-member function passed to std::sort() to sort DFDCCathodeCluster pointers 
/// by their gLayer attributes.
///
bool DFDCCathodeCluster_gLayer_cmp(const DFDCCathodeCluster* a, 
				   const DFDCCathodeCluster* b) {
	return a->gLayer < b->gLayer;
}

///
/// DFDCAnode_gLayer_cmp(): 
/// non-member function passed to std::sort() to sort DFDCHit pointers 
/// for the anode wires by their gLayer attributes.
///
bool DFDCAnode_gLayer_cmp(const DFDCHit* a, const DFDCHit* b) {
	return a->gLayer < b->gLayer;
}

///
/// DFDCPseudo_factory::DFDCPseudo_factory():
/// default constructor -- initializes log file
///
DFDCPseudo_factory::DFDCPseudo_factory() {
	logFile = new ofstream("DFDCPseudo_factory.log");
	_log = new JStreamLog(*logFile, "PSEUDO");
	*_log << "File initialized." << endMsg;
}

///
/// DFDCPseudo_factory::~DFDCPseudo_factory():
/// default destructor -- closes log file
///
DFDCPseudo_factory::~DFDCPseudo_factory() {
	logFile->close();
	delete logFile;
	delete _log;
}

///
/// DFDCPseudo_factory::evnt():
/// this is the place that anode hits and DFDCCathodeClusters are organized into pseudopoints.
///
jerror_t DFDCPseudo_factory::evnt(JEventLoop* eventLoop, int eventNo) {
	vector<const DFDCHit*> fdcHits;
	vector<const DFDCHit*> xHits;
	vector<const DFDCCathodeCluster*> cathClus;
	vector<const DFDCCathodeCluster*> uClus;
	vector<const DFDCCathodeCluster*> oneLayerU;
	vector<const DFDCCathodeCluster*> vClus;
	vector<const DFDCCathodeCluster*> oneLayerV;
	vector<const DFDCHit*> oneLayerX;

	eventLoop->Get(fdcHits);
	eventLoop->Get(cathClus);
	
	// Ensure clusters are in order of ascending Z position.
	std::sort(cathClus.begin(), cathClus.end(), DFDCCathodeCluster_gLayer_cmp);
	
	// Sift through hits and select out anode hits.
	for (unsigned int i=0; i < fdcHits.size(); i++)
		if (fdcHits[i]->type == 0)
			xHits.push_back(fdcHits[i]);
	// Make sure the wires are also in order of ascending z position
	std::sort(xHits.begin(), xHits.end(), DFDCAnode_gLayer_cmp);
			
	// Sift through clusters and put U and V clusters into respective vectors.
	for (unsigned int i=0; i < cathClus.size(); i++) {
		if (cathClus[i]->plane == 1)
			vClus.push_back(cathClus[i]);
		else
			uClus.push_back(cathClus[i]);
	}

	vector<const DFDCCathodeCluster*>::iterator uIt = uClus.begin();
	vector<const DFDCCathodeCluster*>::iterator vIt = vClus.begin();
	vector<const DFDCHit*>::iterator xIt = xHits.begin();
	
	// For each layer, get its sets of V, X, and U hits, and then pass them to the geometrical
	// organization routine, DFDCPseudo_factory::makePseudo()
	for (int iLayer=1; iLayer <= 24; iLayer++) {
		for (; ((uIt != uClus.end() && (*uIt)->gLayer == iLayer)); uIt++)
			oneLayerU.push_back(*uIt);
		for (; ((vIt != vClus.end() && (*vIt)->gLayer == iLayer)); vIt++)
			oneLayerV.push_back(*vIt);
		for (; ((xIt != xHits.end() && (*xIt)->gLayer == iLayer)); xIt++)
			oneLayerX.push_back(*xIt);
		makePseudo(oneLayerX, oneLayerU, oneLayerV,iLayer);
		oneLayerU.clear();
		oneLayerV.clear();
		oneLayerX.clear();
	}
	
	return NOERROR;
}

/// 
/// DFDCPseudo_factory::makePseudo():
/// performs UV+X matching to create pseudopoints
///
void DFDCPseudo_factory::makePseudo(vector<const DFDCHit*>& x,
				    vector<const DFDCCathodeCluster*>& u,
				    vector<const DFDCCathodeCluster*>& v,
				    int layer)
{
  if ((u.size() == 0) || (v.size() == 0) || (x.size() == 0))
    return;
  vector<const DFDCHit*>::iterator xIt;
  vector<const DFDCCathodeCluster*>::iterator uIt;
  vector<const DFDCCathodeCluster*>::iterator vIt;
  centroid_t temp;
  float E1,E2,pos1,pos2;
  
  // Loop over all U and V clusters looking for peaks
  upeaks.clear();
  for (uIt = u.begin(); uIt != u.end(); uIt++){
    vector<const DFDCHit*>::const_iterator strip=(*uIt)->members.begin();
    switch((*uIt)->members.size()){
    case 0: // Make sure we have data!!
      break;
    case 1: // One isolated hit in the cluster:  use element number itself
      temp.pos=(*strip)->element;
      temp.q=2.*((*strip)->q);  // Each cathode view should see half the 
                                 // anode charge
      upeaks.push_back(temp);
      break;
    case 2: //Two adjacent hits: use average for the centroid
      pos1=(*strip)->element;
      pos2=(*(strip+1))->element;
      E1=(*strip)->q;
      E2=(*(strip+1))->q;      
      temp.pos=(pos1*E1+pos2*E2)/(E1+E2);
      temp.q=2.*(E1+E2);
      upeaks.push_back(temp);
      break;
    default:      
      for (strip=(*uIt)->members.begin();
	   strip!=(*uIt)->members.end();strip++){
	FindCentroid((*uIt)->members,strip,upeaks);
      }
      break;
    }
  }	
  vpeaks.clear();
  for (vIt = v.begin(); vIt != v.end(); vIt++){
    vector<const DFDCHit*>::const_iterator strip=(*vIt)->members.begin();
    switch((*vIt)->members.size()){
    case 0: // Make sure we have data!!
      break;
    case 1: // One isolated hit in the cluster:  use element number itself
      temp.pos=(*strip)->element;
      temp.q=2.*((*strip)->q);
      temp.numstrips=1;
      vpeaks.push_back(temp);
      break;
    case 2: //Two adjacent hits: use average for the centroid
      pos1=(*strip)->element;
      pos2=(*(strip+1))->element;
      E1=(*strip)->q;
      E2=(*(strip+1))->q;      
      temp.pos=(pos1*E1+pos2*E2)/(E1+E2);
      temp.q=2.*(E1+E2);
      temp.numstrips=2;
      vpeaks.push_back(temp);
      break;
    default:      
      for (strip=(*vIt)->members.begin();
	   strip!=(*vIt)->members.end();strip++){	
	FindCentroid((*vIt)->members,strip,vpeaks);
      }
      break;
    }
  }
  if (upeaks.size()*vpeaks.size()>0){
    //Loop over all u and v centroids looking for matches with wires
    for (unsigned int i=0;i<upeaks.size();i++){
      for (unsigned int j=0;j<vpeaks.size();j++){
	// In the layer local coordinate system, wires are quantized 
	// in the x-direction and y is along the wire.
	float x_from_strips=DFDCGeometry::getXLocalStrips(upeaks[i].pos,
							  vpeaks[j].pos);
	float y_from_strips=DFDCGeometry::getYLocalStrips(upeaks[i].pos,
							  vpeaks[j].pos);
	for(xIt=x.begin();xIt!=x.end();xIt++){
	  float x_from_wire=DFDCGeometry::getWireR(*xIt);
	  if (fabs(x_from_wire-x_from_strips)<WIRE_SPACING/2.){
	    int status=upeaks[i].numstrips+vpeaks[j].numstrips;
	    float xres=WIRE_SPACING/2./sqrt(12.);
	    float yres=fabs(x_from_wire-x_from_strips);
            float cosangle,sinangle;	   
 
	    yres=x_from_wire-x_from_strips;
	   
	    DFDCPseudo* newPseu = new DFDCPseudo;
	    newPseu->w      = x_from_wire;
	    newPseu->dw     = xres;
	    newPseu->s      = y_from_strips;
	    newPseu->ds     = yres;
	    newPseu->wire   = DFDCGeometry::GetDFDCWire(layer, (*xIt)->element);
	    newPseu->time   = (*xIt)->t;
	    newPseu->dist   = newPseu->time*DRIFT_SPEED;
	    newPseu->status = status;

	    sinangle=newPseu->wire->udir(0);
	    cosangle=newPseu->wire->udir(1);
	    newPseu->x=(newPseu->w)*cosangle+(newPseu->s)*sinangle;
	    newPseu->y=-(newPseu->w)*sinangle+(newPseu->s)*cosangle;

	    DMatrix V(2,2); 
	    double sigx2=HALF_CELL*HALF_CELL/3.;
	    double sigy2=MAX_DEFLECTION*MAX_DEFLECTION/3.;
	    V(0,0)=sigx2*cosangle*cosangle+sigy2*sinangle*sinangle;
	    V(1,1)=sigx2*sinangle*sinangle+sigy2*cosangle*cosangle;
	    V(0,1)=V(1,0)=(sigy2-sigx2)*sinangle*cosangle;

	    newPseu->cov.ResizeTo(V);
	    newPseu->cov=V;

	    _data.push_back(newPseu);
	  } // match in x
	} // xIt loop
      } // vpeaks loop
    } // upeaks loop
  } // if we have peaks in both u and v views
}			

//
/// DFDCPseudo_factory::FindCentroid()
///   Uses the Newton-Raphson method for solving the set of non-linear
/// equations describing the charge distribution over 3 strips for the peak 
/// position x0, the anode charge qa, and the "width" parameter k2.  
/// See Numerical Recipes in C p.379-383.
/// Updates list of centroids. 
///
jerror_t DFDCPseudo_factory::FindCentroid(const vector<const DFDCHit*>& H,
                       vector<const DFDCHit *>::const_iterator peak,
                       vector<centroid_t>&centroids){
  centroid_t temp;
  // Make sure we do not exceed the range of the vector
  if (peak>H.begin() && peak+1!=H.end()){
    float err_diff1=0.,err_diff2=0.;

    // Some code for checking for significance of fluctuations.
    // Currently disabled.
    //float dq1=(*(peak-1))->dq;
    //float dq2=(*peak)->dq;
    //float dq3=(*(peak+1))->dq;
    //err_diff1=sqrt(dq1*dq1+dq2*dq2);
    //err_diff2=sqrt(dq2*dq2+dq3*dq3);
   
    // Check for a peak in three adjacent strips
    if ((*peak)->q-(*(peak-1))->q > err_diff1
                && (*peak)->q-(*(peak+1))->q > err_diff2){
      // Define some matrices for use in the Newton-Raphson iteration
      DMatrix J(3,3);  //Jacobean matrix
      DMatrix F(3,1),N(3,1),X(3,1),par(3,1);
      DMatrix newpar(3,1);
      int i=0;
      double sum=0.;
 
      // Initialize the matrices to some suitable starting values
      par(X0,0)=double((*peak)->element);
      par(K2,0)=1.;
      for (vector<const DFDCHit*>::const_iterator j=peak-1;j<=peak+1;j++){
        X(i,0)=double((*j)->element);
        N(i++,0)=double((*j)->q);
        sum+=double((*j)->q);
      }
      par(QA,0)=2.*sum;
      newpar=par;
    
      // Newton-Raphson procedure
      double errf=0.,errx=0;
      for (int iter=1;iter<=ITER_MAX;iter++){
        errf=0.;
        errx=0.;

        // Compute Jacobian matrix: J_ij = dF_i/dx_j.
        for (i=0;i<3;i++){
          double argp=par(K2,0)*(par(X0,0)-X(i,0)+A_OVER_H);
          double argm=par(K2,0)*(par(X0,0)-X(i,0)-A_OVER_H);
           
          J(i,QA)=-(tanh(argp)-tanh(argm))/4.;
          J(i,K2)=-par(QA,0)/4.*(argp/par(K2,0)*(1.-tanh(argp)*tanh(argp))
                                 -argm/par(K2,0)*(1.-tanh(argm)*tanh(argm)));
          J(i,X0)=-par(QA,0)*par(K2,0)/4.
            *(tanh(argm)*tanh(argm)-tanh(argp)*tanh(argp)); 
	  F(i,0)=N(i,0)-par(QA,0)/4.*(tanh(argp)-tanh(argm));
	  if (fabs(F(i,0))>errf) errf=fabs(F(i,0));
        }
	// Check for convergence
	if (errf<TOLF){	
	  temp.pos=par(X0,0);
	  temp.q=par(QA,0);
	  temp.numstrips=3;
	  centroids.push_back(temp);
  
	  return NOERROR;
	}

        // Check that J is invertible
        TDecompLU lu(J);
        if (lu.Decompose()==false){
          *_log << ":FindCentroid: Singular matrix"<< endMsg;
           
          return UNRECOVERABLE_ERROR; // error placeholder
        }

	// Find the new set of parameters
        jerror_t error;
	if ((error=FindNewParmVec(N,X,F,J,par,newpar))!=NOERROR){
	  *_log << ":FindNewParmVec:  error=" << error << endMsg;

	  return error;
	}

        //Check for convergence
        for (i=0;i<3;i++){
          if (fabs(par(i,0)-newpar(i,0))>errx) errx=fabs(par(i,0)-newpar(i,0));
	}
        if (errx<TOLX){
          temp.pos=par(X0,0);
          temp.q=par(QA,0);
          temp.numstrips=3;
          centroids.push_back(temp);
        
          return NOERROR;
        }
	par=newpar;
      }
    }
  }
  else return VALUE_OUT_OF_RANGE;
    
  return INFINITE_RECURSION; // error placeholder
}
     


///
/// DFDCPseudo_factory::FindNewParmVec()
///   Routine used by FindCentroid to backtrack along the direction
/// of the Newton step if the step is too large in order to avoid conditions
/// where the iteration procedure starts to diverge.
/// The procedure uses a scalar quantity f= 1/2 F.F for this purpose.
/// Algorithm described in Numerical Recipes in C, pp. 383-389.
///
 
jerror_t DFDCPseudo_factory::FindNewParmVec(DMatrix N,DMatrix X,DMatrix F,
				       DMatrix J,DMatrix par,
				       DMatrix &newpar){
  // Invert the J matrix
  DMatrix InvJ(DMatrix::kInverted,J);
  
  // Find the full Newton step
  DMatrix fullstep(3,1);
  fullstep=InvJ*F; fullstep*=-1;

  // find the rate of decrease for the Newton-Raphson step
  DMatrix slope(1,1);
  slope=(-1.0)*DMatrix(DMatrix::kTransposed,F)*F; //dot product

  // This should be a negative number...
  if (slope(0,0)>=0){
    return VALUE_OUT_OF_RANGE;
  }
  
  double lambda=1.0;  // Start out with full Newton step
  double lambda_temp,lambda2=lambda;
  DMatrix f(1,1),f2(1,1),newF(3,1),newf(1,1);

  // Compute starting values for f=1/2 F.F 
  f=0.5*(DMatrix(DMatrix::kTransposed,F)*F); // dot product

  for (;;){
    newpar=par+lambda*fullstep;

    // Compute the value of the vector F and f=1/2 F.F with the current step
    for (int i=0;i<3;i++){
      double argp=newpar(K2,0)*(newpar(X0,0)-X(i,0)+A_OVER_H);
      double argm=newpar(K2,0)*(newpar(X0,0)-X(i,0)-A_OVER_H);
      newF(i,0)=N(i,0)-newpar(QA,0)/4.*(tanh(argp)-tanh(argm));
    }
    newf=0.5*(DMatrix(DMatrix::kTransposed,newF)*newF); // dot product
 
    if (lambda<0.1) {  // make sure the step is not too small
      newpar=par;
      return NOERROR;
    } // Check if we have sufficient function decrease
    else if (newf(0,0)<=f(0,0)+ALPHA*lambda*slope(0,0)){
      return NOERROR;
    }
    else{
      // g(lambda)=f(par+lambda*fullstep)
      if (lambda==1.0){//first attempt: quadratic approximation for g(lambda)
        lambda_temp=-slope(0,0)/2./(newf(0,0)-f(0,0)-slope(0,0));
      }
      else{ // cubic approximation for g(lambda)
        double temp1=newf(0,0)-f(0,0)-lambda*slope(0,0);
        double temp2=f2(0,0)-f(0,0)-lambda2*slope(0,0);
        double a=(temp1/lambda/lambda-temp2/lambda2/lambda2)/(lambda-lambda2);
        double b=(-lambda2*temp1/lambda/lambda+lambda*temp2/lambda2/lambda2)
          /(lambda-lambda2);
        if (a==0.0) lambda_temp=-slope(0,0)/2./b;
        else{
          double disc=b*b-3.0*a*slope(0,0);
          if (disc<0.0) lambda_temp=0.5*lambda;
          else if (b<=0.0) lambda_temp=(-b+sqrt(disc))/3./a;
          else lambda_temp=-slope(0,0)/(b+sqrt(disc));
        }
        // ensure that we are headed in the right direction...
        if (lambda_temp>0.5*lambda) lambda_temp=0.5*lambda;
      }
    }
    lambda2=lambda;
    f2=newf;
    // Make sure that new version of lambda is not too small
    lambda=(lambda_temp>0.1*lambda ? lambda_temp : 0.1*lambda);
  } 
}

//------------------
// toString
//------------------
const string DFDCPseudo_factory::toString(void)
{
	// Ensure our Get method has been called so _data is up to date
	Get();
	if(_data.size()<=0)return string(); // don't print anything if we have no data!

	printheader("layer: wire:   r(cm): phi(rad):  z(cm): time(ns):      w(cm):     s(cm):   status:");

	for(unsigned int i=0; i<_data.size(); i++){
		DFDCPseudo *hit = _data[i];
		//const DFDCWire *w = hit->wire;
		DVector3 pos = hit->wire->origin + hit->wire->udir*hit->s;
		double phi = pos.Phi();
		if(phi<0.0)phi+=2.0*M_PI;

		printnewrow();
		printcol("%d",		hit->wire->layer);
		printcol("%d",		hit->wire->wire);
		printcol("%3.1f", pos.Perp());
		printcol("%1.3f", phi);
		printcol("%3.1f", pos.Z());
		printcol("%3.1f",	hit->time);
		printcol("%3.1f",	hit->w);
		printcol("%1.4f",	hit->s);
		printcol("%d",		hit->status);
		printrow();
	}

	return _table;

}