///////////////////////////////////////////////////////////////////////////////
//
///////////////////////////////////////////////////////////////////////////////
#include "TRPhiWt.hh"
#include "Stntuple/loop/TStnModule.hh"
#include "TMinuit.h"
#include <iostream>
#include "TMath.h"
#include "TF1.h"
#include <cmath>
#include "TROOT.h"
#include "Stntuple/base/TMatrix55.hh"

ClassImp(TRPhiWt)

TRPhiWt *TRPhiWt::gRPhiWt;

TRPhiWt::TRPhiWt(const char* name, const char* title, const char* filename)
  : TStnModule(name,title),
    fTr(NULL), fTl(NULL), fHit(NULL), fI(NULL), fSi(NULL),
    fT(NULL), fR(NULL),
    fNStrTyp(0),fNStrMin(NULL),fNStrMax(NULL),fLoadFile(false),fMinPt(15.0),
    fAnchorType(-1), fGeo(NULL), fMaxQ(-1.0)
{
  fFilename=filename;
  gRPhiWt=this;
}

TRPhiWt::~TRPhiWt() {
  delete []fNStrMin;
  delete []fNStrMax;
  delete fGeo;
}

void TRPhiWt::UseAlignment(const std::string &geoFile) {
  fGeo = new TSiAlign(geoFile);
}


void TRPhiWt::SetNStripPerClus(int nType, const int *nmin, const int *nmax){
  fNStrTyp=nType;
  fNStrMin = new int[nType];
  fNStrMax = new int[nType];
  for (int i=0; i<nType; i++){
    fNStrMin[i]=nmin[i]; 
    fNStrMax[i]=nmax[i];
  }
}

int TRPhiWt::ClusterType(int nstr) {
  int ctype=-1;
  for (int i=0; i<fNStrTyp && ctype<0; i++){
    if (nstr>=fNStrMin[i] && nstr<=fNStrMax[i]) {
      ctype = i;
    }
  }
  return ctype;
}

void TRPhiWt::GetSigmas(TMinuit *m, double *sigma, double *error){
  for (int i=0; i<4; i++){
    double sig,err;
    m->GetParameter(i+1,sig,err);
    sigma[i]=sig;
    if (error) error[i]=err;
  }
}

void TRPhiWt::AssignSigmas(const double *sigs, const int *n, double *layer_sigma){
  for (int i=0; i<3; i++){
    layer_sigma[i] = sigs[n[i]];
  }
}

int TRPhiWt::BeginJob() {

  RegisterDataBlock("TrackBlock"    ,"TStnTrackBlock"    ,&fTr );
  RegisterDataBlock("TrackLinkBlock","TStnTrackLinkBlock",&fTl );
  RegisterDataBlock("SvxDataBlock"  ,"TSvxDataBlock"     ,&fHit);
  RegisterDataBlock("SiIsectBlock"  ,"TSiIsectBlock"     ,&fI  );
  RegisterDataBlock("SiStripsBlock" ,"TSiStripBlock"     ,&fSi );
  BookHistograms();
  
  return 0;
}
  
int TRPhiWt::BeginRun(){
  return 0;
}

void  TRPhiWt::BookHistograms(){
  if (fLoadFile) {
    fFile=new TFile(fFilename.c_str(),"UNKNOWN","rphiHits");
    fFile->cd();
    fT = (TTree *)fFile->Get("rphi");
    fT->SetBranchAddress("p",&fp);
  }
  else {
    fFile=new TFile(fFilename.c_str(),"RECREATE","rphiHits");
    fT=new TTree("rphi","Pedestals");
    fT->Branch("p",&fp,"w/I:pt/D:r:er:x[3]:y[3]:n[3]/I:nst[3]:q[3]/F:dx[3]/D");
  }

  fR = new TH1F("r","Residual After Cuts",140,-70.0,70.0);
  fR->GetXaxis()->SetTitle("Residual at L2 (microns)");
  fR_full = new TH1F("r_full","Residual Full Sample",140,-70.0,70.0);
  fR_full->GetXaxis()->SetTitle("Residual at L2 (microns)");
  fS = new TH1F("s","Residual/Err After Cuts",100,-3.0,3.0);
  fS_full = new TH1F("s_full","Residual/Err Full Sample",100,-3.0,3.0);

  fPR = new TH2F("pr","Residual vs Phi",10,-0.3,0.3,140,-70.0,70.0);
  fPS = new TH2F("ps","Residual/Err vs Phi",10,-0.3,0.3,100,-3.0,3.0);
  fWR = new TH2F("wr","Residual vs Wedge",12,-0.5,11.5,140,-70.0,70.0);
  fWS = new TH2F("ws","Residual/Err vs Wedge",12,-0.5,11.5,100,-3.0,3.0);

  fD = new TH1F("d","Diff b/w circle and approx at L2",100,-1.0,1.0);
  fD->GetXaxis()->SetTitle("microns");
  fD0= new TH1F("d0","Diff b/w circle and straight line at L2",100,-10.0,10.0);
  fD0->GetXaxis()->SetTitle("microns");
}

int TRPhiWt::Event(int ientry){
  if (fLoadFile) return 0;

  fTr->Clear();
  fTr->GetEntry(ientry);
  if (fTr->NTracks()<=0) return 0;
  bool goodTrack=false;
  for (int i=0; i<fTr->NTracks() && !goodTrack; i++){
    if (PassesTrackCuts(i)){
      goodTrack=true;
    }
  }
  if (!goodTrack) return 0;


  if (fMaxQ>0) {
    fSi->Clear();
    if (!fSi->GetEntry(ientry)) return 0;
    fSi->InitEvent();
  }

  fHit->Clear();
  if (!fHit->GetEntry(ientry)) return 0;
  fHit->InitEvent();

  fTl->Clear();
  if (!fTl->GetEntry(ientry)) return 0;
  if (fTl->SiHitLinkTrk()->NLinksTotal()<=0) return 0;

  fI->Clear();
  if (!fI->GetEntry(ientry)) return 0;

  for (int i=0; i<fTr->NTracks(); i++){
    TStnTrack *t=fTr->Track(i);
    if (PassesTrackCuts(i,1)){
      t->Print();

      // Fill the baby ntuple
      fp.w = -1;
      for (int j=0; j<fTl->SiHitLinkTrk()->NLinks(i) && fp.w<0; j++) {
	int ihit = fTl->SiHitLinkTrk()->Index(i,j);
	TStnSiHit *s=fHit->SiHit(ihit);
	int il = s->DigiCode()->Layer();
	if (il==1) {
	  fp.w = s->DigiCode()->PhiWedge();
	}
      }
      fp.pt = t->Pt();
      fp.r  = 1/(2*t->C0());
      TMatrix55 &m = *(t->Cov());
      fp.er = 2*fp.r*fp.r*std::sqrt(m(1,1));
      int n=0;

      // Find the center and normal of inner ladder and rotate to wedge 3
      double rot = 0;
      TVector3 cnt;
      bool found = false;
      for (int j=0; j<fTl->SiHitLinkTrk()->NLinks(i) && !found && fGeo; j++){
	TStnSiDigiCode *digi=fHit->SiHit(fTl->SiHitLinkTrk()->Index(i,j))->DigiCode();
	if (digi->Layer()==1 && digi->Side()==0) {
	  rot = M_PI/2 - fGeo->GetNormal(digi)->Phi();
	  cnt = *(fGeo->GetCenter(digi));
	  found = true;
	}
      }
  
      // Loop over hits on desired layers, transform coords to wedge-3-like and save
      for (int j=0; j<fTl->SiHitLinkTrk()->NLinks(i); j++){
	int ihit = fTl->SiHitLinkTrk()->Index(i,j);
	TStnSiHit *s=fHit->SiHit(ihit);
	int il = s->DigiCode()->Layer();
	if (il>0 && il<4 && s->DigiCode()->Side()==0) {
	  TVector3 v(*(s->Global()));
	  if (fGeo) {
	    v -= cnt;
	    v.RotateZ(rot);
	  }
	  else {
	    v.RotateZ( (3-fp.w)*(M_PI/6.0) );
	  }
	  fp.x[il-1] = v.X();
	  fp.y[il-1] = v.Y();
	  fp.n[il-1] = ClusterType(s->Nstrip());
	  fp.nst[il-1] = s->Nstrip();
	  fp.q[il-1] = s->Qtot();
	  fp.dx[il-1] = 0.0;

	  if (fMaxQ>0) {
	    float q;
	    double dx;
	    RecalculateClusterCentroid(ihit,dx,q);
	    fp.x[il-1] += dx;
	    fp.q[il-1] = q;
	    fp.dx[il-1] = dx;
	  }
	  n++;
	}
      }
      if (n==3){
	fT->Fill();
      }
      else {
	std::cout << "Say WHAT? " << n << std::endl;
      }
    }
  }
  
  
  

  return 0;
}

bool TRPhiWt::PassesTrackCuts(int itrk, int flag){
  bool pass=false;
  TStnTrack *t=fTr->Track(itrk);
  if (t->Algorithm() !=11 ) return pass;

  //  if (t->Charge()<0.0) return pass;

  //  if (t->Pt()>15.0 && t->NSvxRPhiHits()==5 && t->NSvxSASHits()==2 && t->NSvxZHits()==3) {
  if (t->Pt()>fMinPt && t->NSvxRPhiHits()>=5 && t->NSvxSASHits()>=2 ) {
    if (flag==0) {
      pass = true;
    }
    else {
      if (fTl->SiIsectLinkTrk()->NLinks(itrk)>0) {
	bool first=true;
	int w=-1;
	int b=-1;
	int isecL1=-1;
	int digiL1=-1;
	// Check that track and all hits are in one wedge and one barrel
	for (int i=0; i<fTl->SiIsectLinkTrk()->NLinks(itrk); i++){
	  int is = fTl->SiIsectLinkTrk()->Index(itrk,i);
	  if (fI->SiIsect(is)->DigiCode()->Layer()<6 && fI->SiIsect(is)->DigiCode()->Layer()>0) {
	    if (first) {
	      w = fI->SiIsect(is)->DigiCode()->PhiWedge();
	      b = fI->SiIsect(is)->DigiCode()->Barrel();
	      first = false;
	    }
	    else if (fI->SiIsect(is)->DigiCode()->PhiWedge() != w ||
		     fI->SiIsect(is)->DigiCode()->Barrel()   != b    ) {
	      pass = false;
	      return pass;
	    }
	    if (fI->SiIsect(is)->DigiCode()->Layer()==1) {
	      isecL1=is;
	      digiL1=fI->SiIsect(is)->DigiCode()->fDigiCode;
	    }
	  }
	}
	if (isecL1<0) return false;
	for (int i=0; i<fTl->SiHitLinkTrk()->NLinks(itrk); i++){
	  int ihit = fTl->SiHitLinkTrk()->Index(itrk,i);
	  if (fHit->SiHit(ihit)->DigiCode()->Layer()<6  &&
	      fHit->SiHit(ihit)->DigiCode()->Layer()>0 &&
	      fHit->SiHit(ihit)->DigiCode()->Side()==0 ) {
	    if (fHit->SiHit(ihit)->DigiCode()->PhiWedge() != w ||
		fHit->SiHit(ihit)->DigiCode()->Barrel()   != b    ) {
	      pass = false;
	      return pass;
	    }
	    // Check that there are no bad hits
	    if (! fHit->SiHit(ihit)->Good()) {
	      pass = false;
	      return pass;
	    }
	  }
	}

	
	// Check that track has hits with the right cluster mult
	if (fTl->SiHitLinkTrk()->NLinks(itrk)>0) {
	  int nhit_good=0;
	  for (int i=0; i<fTl->SiHitLinkTrk()->NLinks(itrk); i++){
	    int ihit = fTl->SiHitLinkTrk()->Index(itrk,i);
	    int ilay = fHit->SiHit(ihit)->DigiCode()->Layer();
	    if (ilay>0 && ilay<4 &&
		fHit->SiHit(ihit)->DigiCode()->Side()==0) {
	      if (ClusterType(fHit->SiHit(ihit)->Nstrip())<0) {
		//		fHit->SiHit(ihit)->Print();
		pass = false;
		return pass;
	      }
	      else {
		nhit_good++;
	      }
	      if (fAnchorType>=0) {
		if ( (ilay==1 && ClusterType(fHit->SiHit(ihit)->Nstrip())!=fAnchorType) ||
		     (ilay==3 && ClusterType(fHit->SiHit(ihit)->Nstrip())!=fAnchorType) ) {
		  pass=false;
		  return pass;
		}
	      }
	    }
	  }
	  if (nhit_good==3) pass = true;

	  // Finally check that track is isolated from other tracks at L1
	  for (int jtrk=0; jtrk<fTr->NTracks(); jtrk++) {
	    if (itrk != jtrk) {
	      
	      for (int i=0; i<fTl->SiIsectLinkTrk()->NLinks(jtrk); i++){
		int isec = fTl->SiIsectLinkTrk()->Index(jtrk,i);
		TStnSiIsect *sec = fI->SiIsect(isec);
		if (sec != NULL) {
		  if (sec->DigiCode()->fDigiCode == digiL1) {
		    
		    float iso = (sec->fGlobal - fI->SiIsect(isecL1)->fGlobal).Perp();
		    if (iso < 0.0060*10) {
		      pass = false;
		      return false;
		    }
		  }
		}
	      }
	    }
	  }
	  

	}
	else {
	  pass = false; // No sihits on track
	}
      }
      else {
	pass = false; // No track intersections
      }
    }
  }
  return pass;
}

int TRPhiWt::EndJob(){
  FitWt();
  if (!fLoadFile) fFile->Write();
  return 0;
}

void TRPhiWt::FitWt() {  
  TMinuit *m = new TMinuit(5);
  m->SetFCN(Likelihood);

  double arglist[10];
  int ierflg = 0;

  arglist[0] = 3.0; // verbose
  m->mnexcm("SET PRI", arglist ,1,ierflg);
  
  arglist[0] = 0.5; // neg-log-like
  m->mnexcm("SET ERR", arglist ,1,ierflg);
  
  arglist[0] = 2.0; // slower fit, get errors correct
  m->mnexcm("SET STRAT", arglist ,1,ierflg);
  
  m->mnparm(0, "mean", 0.0, 0.0001, 0,0,ierflg);
  m->mnparm(1, "sigma_1", 0.002, 0.0001, 0,0,ierflg);
  m->mnparm(2, "sigma_2", 0.002, 0.0001, 0,0,ierflg);
  m->mnparm(3, "sigma_3", 0.003, 0.0001, 0,0,ierflg);
  m->mnparm(4, "sigma_4plus", 0.004, 0.0001, 0,0,ierflg);

  arglist[0] = 1;
  m->mnexcm("FIX", arglist ,1,ierflg);

  for (int i=2+fNStrTyp; i<=5; i++) {
    arglist[0] = i;
    m->mnexcm("FIX", arglist ,1,ierflg);
  }

  // First fit with 2.5 sigma cut on prelim parameters
  double err,mean;
  m->GetParameter(0,mean,err);

  double sigs[4],sigs_error[4];
  GetSigmas(m, sigs);

  fC=new TTree("cut","Tree after cuts");
  fC->Branch("c",&fc,"w/I:pt/D:r:er:x[3]:y[3]:n[3]/I:nst[3]:q[3]/F:dx[3]/D");
  int n=CutTree(mean,sigs,fMinPt);

  m->mnexcm("MINIMIZE", arglist ,0,ierflg);

//   arglist[0] = 1;
//   m->mnexcm("REL", arglist ,1,ierflg);
  m->mnexcm("MINIMIZE", arglist ,0,ierflg);
  m->mnexcm("MINOS", arglist ,0,ierflg);

  double mean_best,sigma_best[4];

  // Make graphs of resolution and width of pull vs iteration
  for (int i=0; i<fNStrTyp; i++){
    char tit[80];
    sprintf(tit,"clus%d",i);
    fG[i] = new TGraphErrors(10);
    fG[i]->SetName(tit);
    fG[i]->SetMarkerStyle(20+i);
    fG[i]->SetMarkerSize(0.8);
    fG[i]->SetMarkerColor(i+1);
    fG[i]->SetLineColor(i+1);
  }
  fW = new TGraphErrors(10);
  fW->SetName("sigma_clus");
  fW->SetMarkerStyle(20);
  fW->SetMarkerSize(0.8);
  fW->SetMarkerColor(1);
  fW->SetLineColor(1);
   
  // 10 iterations after first fit
  for (int iter=0; iter<10; iter++) {
    m->GetParameter(0,mean,err);
    GetSigmas(m, sigs);
    n=CutTree(mean,sigs,fMinPt);
    m->mnexcm("MINIMIZE", arglist ,0,ierflg);
    m->mnexcm("MINOS", arglist ,0,ierflg);

    m->GetParameter(0,mean,err);
    GetSigmas(m, sigs, sigs_error);
    for (int i=0; i<fNStrTyp; i++){
      fG[i]->SetPoint(iter,(double)iter,sigs[i]*10000);
      fG[i]->SetPointError(iter,0.0,sigs_error[i]*10000);
      std::cout << "\n\n*** N, MEAN, SIGMA = "<< n << " "<< mean << " " << sigs[i] << "\n\n"<< std::endl;
    }

    fS->Reset();
    for (int j=0; j<(int)fC->GetEntries(); j++){
      fC->GetEntry(j);
      float r = residL(fc.x,fc.y,fc.r);

      double layer_sigmas[3];
      AssignSigmas(sigs, fc.n, layer_sigmas);
      float s = (r-mean)/std::sqrt(sigma_residL2(fc.x,fc.y,fc.r,layer_sigmas));
      fS->Fill(s);
    }
    if (fS->GetEntries()<500) {
      fS->Fit("gaus","OL");
    }
    else {
      fS->Fit("gaus","O");
    }
    TF1 *f = fS->GetFunction("gaus");
    fW->SetPoint(iter, (double)iter, f->GetParameter(2) );
    fW->SetPointError(iter, 0.0, f->GetParError(2) );
    
    mean_best=mean;
    GetSigmas(m,sigma_best);
  }

  // Recut the tree using the pt>15 sample
  n=CutTree(mean_best,sigma_best,fMinPt);

  m->mnexcm("MINIMIZE", arglist ,0,ierflg);
  m->mnexcm("MINOS", arglist ,0,ierflg);
  m->GetParameter(0,mean,err);
  GetSigmas(m, sigs);
  n=CutTree(mean,sigs,fMinPt);

  
  fR->Reset();
  fS->Reset();

  for (int i=0; i<(int)fC->GetEntries(); i++){
    fC->GetEntry(i);
    float x_check = fc.x[0]-fc.y[0]*(fc.x[2]-fc.x[0])/(fc.y[2]-fc.y[0])+(fc.x[2]-fc.x[0])*fc.y[1]/(fc.y[2]-fc.y[0])-fc.x[1];
    float r = residL(fc.x,fc.y,fc.r);
    fR->Fill(r*10000);
    double layer_sigmas[3];
    AssignSigmas(sigs, fc.n, layer_sigmas);
    float s = (r-mean)/std::sqrt(sigma_residL2(fc.x,fc.y,fc.r,layer_sigmas));
    fS->Fill(s);
    fD->Fill(10000 * (r-residC(fc.x,fc.y,fc.r)));
    fD0->Fill(10000 * (r-x_check));
  }
  for (int i=0; i<(int)fT->GetEntries(); i++){
    fT->GetEntry(i);
    float r = residL(fp.x,fp.y,fp.r);
    double layer_sigmas[3];
    AssignSigmas(sigs, fp.n, layer_sigmas);
    float s = (r-mean)/std::sqrt(sigma_residL2(fp.x,fp.y,fp.r,layer_sigmas));
    fS_full->Fill(s);
    fR_full->Fill(r*10000);

    fPR->Fill(std::atan2(fp.x[2]-fp.x[0],fp.y[2]-fp.y[0]),r*10000);
    fPS->Fill(std::atan2(fp.x[2]-fp.x[0],fp.y[2]-fp.y[0]),s);
    fWR->Fill(fp.w*1.0,r*10000);
    fWS->Fill(fp.w*1.0,s);
  }
  
}

void TRPhiWt::Likelihood(Int_t &npar, Double_t *gin, Double_t &f, 
			 Double_t *par, Int_t iflag) {
  //  TRPhiWt *g = (TRPhiWt *)gROOT->FindObject("RPhiWt");
  TRPhiWt *g = gRPhiWt;

  double neglike=0;
         
  if (par[1]<0 || par[2]<0 || par[3]<0 || par[4]<0) {
    neglike = 1.0E10;
  }
  else {
    for (int i=0; i<(int)g->fC->GetEntries(); i++){
      g->fC->GetEntry(i);
      
      double r_xcheck = g->resid(g->fc.x, g->fc.y);
      double r_xxcheck = g->residC(g->fc.x, g->fc.y, g->fc.r);
      double r = g->residL(g->fc.x, g->fc.y, g->fc.r);
      //      std::cout << r_xcheck - r << r_xxcheck - r << std::endl;
//       if (std::abs(r_xxcheck - r)>1.0E-4) std::cout << r_xxcheck << " "
// 						    << std::atan2(g->fc.y[2]-g->fc.y[1],g->fc.x[2]-g->fc.x[0]) << " "
// 						    << g->fc.pt << " "
// 						    <<" "<< r_xxcheck - r << std::endl;
      double layer_sigmas[3];
      g->AssignSigmas(&par[1],g->fc.n, layer_sigmas);
      double s = g->sigma_residL2(g->fc.x, g->fc.y, g->fc.r, layer_sigmas);
      double s_xcheck = g->sigma_residL2_old(g->fc.x, g->fc.y, g->fc.r, par[1]);
      if (s>0) {
	//	if (std::abs(r-par[0])/std::sqrt(s)<3.0) {
	  //	neglike += g->logGaussianZeroMean(r, s);
	  neglike += g->logGaussian(r, s, par[0]);
	  //	}
      }
    }
  }
  f = neglike;
}

double TRPhiWt::a(const double *x, const double *y) const {
  return x[0] - y[0]*(x[2]-x[0])/(y[2]-y[0]);
}

double TRPhiWt::b(const double *x, const double *y) const {
  return (x[2]-x[0])/(y[2]-y[0]);
}

double TRPhiWt::sigma_a2(const double *x, const double *y, double s) const {
  double n = s*s * (y[2]*y[2] + y[0]*y[0]);

  double d = (y[2]-y[0])*(y[2]-y[0]);

  return n/d;
}

double TRPhiWt::sigma_b2(const double *x, const double *y, double s) const {
  double n = 2*s*s;
  double d = (y[2]-y[0])*(y[2]-y[0]);

  return n/d;
}

double TRPhiWt::cov_ab(const double *x, const double *y, double s) const {
  double n = -s*s * (y[2]+y[0]);

  double d = (y[2]-y[0])*(y[2]-y[0]);

  return n/d;
}

double TRPhiWt::resid(const double *x, const double *y) const {
  return a(x,y) + b(x,y)*y[1] - x[1];
}

double TRPhiWt::sigma_resid2(const double *x, const double *y, double s) const {
  double n = s*s * ( (y[1]-y[2])*(y[1]-y[2]) + (y[1]-y[0])*(y[1]-y[0]) + (y[2]-y[0])*(y[2]-y[0]));
  
  double d = (y[2]-y[0])*(y[2]-y[0]);

  double err2 = n/d;

  double x_check = y[1]*y[1] * sigma_b2(x,y,s) + 2*y[1]*cov_ab(x,y,s) + sigma_a2(x,y,s) + s*s;

  return err2;
}

double TRPhiWt::residC(const double *x, const double *y, double r) const {
  double xc,yc;
  circleFrom2PointsAndRadius(x,y,r, xc,yc);
  double xpred = std::sqrt(r*r - (y[1]-yc)*(y[1]-yc)) + xc;
  double xpred2 = -std::sqrt(r*r - (y[1]-yc)*(y[1]-yc)) + xc;

  double d = xpred - x[1];
  double d2 = xpred2 - x[1];
  
  return (std::abs(d)<std::abs(d2) ? d : d2);
}

double TRPhiWt::A(const double *x, const double *y, double r) const {
  return std::sqrt( (x[2]-x[0])*(x[2]-x[0]) + (y[2]-y[0])*(y[2]-y[0]) );
}

double TRPhiWt::sL(const double *x, const double *y, double r, double xL) const {
  return std::sqrt( (xL-x[0])*(xL-x[0]) + (y[1]-y[0])*(y[1]-y[0]) );
}

double TRPhiWt::residL(const double *x, const double *y, double r) const {
  double xL = a(x,y) + b(x,y)*y[1]; // line fit prediction

  double A_S = A(x,y,r)-sL(x,y,r,xL);
  double S = sL(x,y,r,xL);

  double a = -std::atan2(x[2]-x[0],y[2]-y[0]); // angle of perp to line between x1,y1 and x3,y3
  a += (r < 0 ? -M_PI : 0);


  double d_lim = S*A_S/(2*std::abs(r));
  double d = std::abs(r) - std::sqrt(4*r*r - 4*S*A_S)/2; // small addition to line fit
  //  d *= std::cos(a);
  d *= (r < 0 ? -1 : 1);
  
  return xL + d - x[1];
}

double TRPhiWt::sigma_residL2_old(const double *x, const double *y, double r, double s) const {
  double xL = a(x,y) + b(x,y)*y[1]; // line fit prediction
  double errLine2 = sigma_resid2(x,y,s);

  double B = A(x,y,r)-sL(x,y,r,xL);
  double S = sL(x,y,r,xL);
  
  double err2 = (x[1]-x[0])*(x[1]-x[0])*B*B/(S*S)
    - (x[1]-x[0])*(x[2]-x[1])
    + (x[2]-x[1])*(x[2]-x[1])*S*S/(B*B);
  err2 *= s*s/(2*r*r);

  return err2 + errLine2;
}

double TRPhiWt::sigma_residL2(const double *x, const double *y, double r, const double *s) const {
  double xL = a(x,y) + b(x,y)*y[1]; // line fit prediction
  double B = A(x,y,r)-sL(x,y,r,xL);
  double S = sL(x,y,r,xL);
  
  double s1 = ( (y[2]-y[1])/(y[2]-y[0]) - (x[1]-x[0])*B/(2*r*S) ) * s[0];

  double s2 = (  ( (x[1]-x[0])*B/S - (x[2]-x[1])*S/B )/(2*r) - 1  ) * s[1];

  double s3 = ( (y[1]-y[0])/(y[2]-y[0]) + (x[2]-x[1])*S/(2*r*B) ) * s[2];

  return s1*s1 + s2*s2 + s3*s3;
}
 
double TRPhiWt::logGaussianZeroMean(double r, double sigma_r2) {
  double n = r*r/(2*sigma_r2);

  double d = std::log( std::sqrt(sigma_r2*2*M_PI) );

  return n + d;
}

double TRPhiWt::logGaussian(double r, double sigma_r2, double mean) {
  double n = (r-mean)*(r-mean)/(2*sigma_r2);

  double d = std::log( std::sqrt(sigma_r2*2*M_PI) );

  //  d += std::log( 1-TMath::Erf(mean/(std::sqrt(2*sigma_r2))) );

  return n + d;
}

void TRPhiWt::circleFrom2PointsAndRadius(const double *x, const double *y, //input points
					 double r, // input radius
					 double &xc, double &yc) const { // output center of circle
  // Use 3-point formula for circle. 3rd point we figure out from known radius
  double x1=x[0];
  double y1=y[0];
  double x3=x[2];
  double y3=y[2];

  double h = std::abs(r) - std::sqrt(std::abs(4*r*r - ( (x3-x1)*(x3-x1) + (y3-y1)*(y3-y1) ) ))/2; // saggita

  double a = -std::atan2(x3-x1,y3-y1); // angle of perp to line between x1,y1 and x3,y3
  a += (r < 0 ? -M_PI : 0);
  double x2 = (x1+x3)/2 + h*std::cos(a);
  double y2 = (y1+y3)/2 + h*std::sin(a);

  double Part1 = ((x2-x1) * (x2+x1) + (y2-y1) * (y2 + y1)) / (2.0 * (x2-x1));
  double Part2 = ((x3-x1) * (x3+x1) + (y3-y1) * (y3 + y1)) / (2.0 * (x3-x1));
  double Part3 = (y2 - y1) / (x2 - x1);
  double Part4 = (y3 - y1) / (x3 - x1);
  double Y = (Part2 - Part1) / (Part4 - Part3);
  double X = Part2 - Part4 * Y;
  double Radius = std::sqrt ( (x3 - X)*(x3 - X) + (y3 - Y)*(y3 - Y) );


  //  std::cout << std::abs(r-Radius) << std::endl;
  
  xc = X;
  yc = Y;
}

int TRPhiWt::CutTree(double mean, const double *sigma, double minpt) {
  fC->Reset();
  for (int i=0; i<(int)fT->GetEntries(); i++){
    fT->GetEntry(i);

    double layer_sigmas[3];
    AssignSigmas(sigma, fp.n, layer_sigmas);

    double r = residL(fp.x, fp.y, fp.r);
    double s = sigma_residL2(fp.x, fp.y, fp.r, layer_sigmas);
    
    if (std::abs(r-mean)/std::sqrt(s)<2.5
	&& fp.pt >= minpt
	) {
      fc.w = fp.w;
      fc.pt = fp.pt;
      fc.r = fp.r;
      fc.er = fp.er;
      for (int j=0; j<3; j++) {
	fc.x[j] = fp.x[j];
	fc.y[j] = fp.y[j];
	fc.n[j] = fp.n[j];
	fc.nst[j] = fp.nst[j];
	fc.q[j] = fp.q[j];
	fc.dx[j] = fp.dx[j];
      }
      fC->Fill();
    }
  }

  return (int)fC->GetEntries();
}


void TRPhiWt::RecalculateClusterCentroid(int ihit, double &dx, float &q) {
  q = 0;
  dx = 0;
  double centroid=0;
  double centroid_orig=0;

  TStnSiHit *hit = fHit->SiHit(ihit);

  int nstp = fHit->SiStripLinkHit()->NLinks(ihit);
  if (nstp<1) return;
  for (int is=0; is<nstp; is++) {
    
    // Get the index of this strip into the TClonesArray in TSvxDataBlock
    int istp = fHit->SiStripLinkHit()->Index(ihit,is);
    TStnSiStrip *stp = fSi->SiStrip(istp);
    
    float qstp = std::min( stp->fADC + stp->fPed,  fMaxQ );
    qstp -= stp->fPed;
    q += qstp;
    centroid += qstp * stp->fStrip;
    centroid_orig += stp->fADC * stp->fStrip;
  }
  centroid /= q;
  centroid_orig /= hit->Qtot();
  double offset = hit->StripNum() - centroid_orig;
  centroid += offset;

  dx = centroid - hit->StripNum();

  double pitch = (hit->DigiCode()->Layer()==2) ? 0.0062 : 0.0060;
  pitch *= (hit->DigiCode()->LadderSeg()==0) ? -1.0 : 1.0;

  dx *= pitch;
}