//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcLaserFitTask
//     
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//	Felix Boehmer    	TUM     (original author)
//
//-----------------------------------------------------------


// This Class' Header ------------------
#include "TpcLaserFitTask.h"


// C++ headers
#include <iostream>
#include <vector>
#include <map>
#include <string>

// Collaborating Class Headers --------
#include "TClonesArray.h"
#include "FairRootManager.h"
#include "TVector3.h"
#include "TVectorD.h"
#include "TArrayD.h"
#include "TMath.h"
#include "TF2.h"
#include "TH2D.h"
#include "TH1D.h"
#include "TCanvas.h"
#include "TpcDigiPar.h"
#include "TpcDigiMapper.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"

#include "TpcLaser.h"
#include "TpcLaserGrid.h"
#include "TpcRefResidualCollection.h"
#include "TpcResidual.h"
#include "BiCubSpline.h"
#include "BiCubSplineFitter.h"
#include "SplineTF2Interface.h"

#include "GFAbsTrackRep.h"
#include "GFTrack.h"

#include "TPolyMarker3D.h"


// Class Member definitions -----------

TpcLaserFitTask::TpcLaserFitTask()		//default constructor
  : FairTask("TPC Laser Fit"),
    fPersistence(kFALSE),
    fPlot(kFALSE),
    fDoErrors(kFALSE),
    fNknotsZ(6),
    fNknotsR(3),
    fdevMapR(NULL),
    fdevMapPerp(NULL),
    frecoMapR(NULL),
    frecoMapPerp(NULL),
    fResBranchName("TpcTrackResiduals"),
    fTrackBranchName("TpcGFLaserTrack"),
    fSplineBranchName("TpcLaserSplineFits")
    
{
  double phi[9] = {0,0,0,0,0,0,0,0,1};
  fRotPhi.ResizeTo(3,3);
  fRotPhi = TMatrixT<double>(3,3,phi);
  double theta[9] = {0,0,0,0,1,0,0,0,0};
  fRotTheta.ResizeTo(3,3);
  fRotTheta = TMatrixT<double>(3,3,theta);
  fUnity.ResizeTo(3,3);
  fUnity[0][0] = 1.; fUnity[1][1] = 1.; fUnity[2][2] = 1.; 
}



TpcLaserFitTask::~TpcLaserFitTask()
{
  ;
}

InitStatus
TpcLaserFitTask::Init()
{

 //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==0)
    {
      Error("TpcLaserFitTask::Init","RootManager not instantiated!");
      return kERROR;
    }

  // Get input collection
  fTrackArray = (TClonesArray*) ioman->GetObject(fTrackBranchName);
  fResArray = (TClonesArray*) ioman->GetObject(fResBranchName);
  
  if(fResArray==NULL) {
    TString err("Residual-Array (");
    err+=fResBranchName;
    err+=") not found!";
    Error("TpcLaserFitTask::Init",err);
    return kERROR;
  }
  if(fTrackArray==NULL) {
    TString err("Track-Array (");
    err+=fTrackBranchName;
    err+=") not found!";
    Error("TpcLaserFitTask::Init",err);
    return kERROR;
  }
  
  //read in parameters
  fzMin = fpar->windowMin();
  fzMax = fpar->windowMax();
  frMin=fpar->getRMin();
  frMax=fpar->getRMax();

  std::cout<<"TpcLaserFitTask::Init(): parameters initialized: \n"
	   <<"fzMin = "<<fzMin
	   <<"\n_zMax = "<<fzMax
	   <<"\n_rMin = "<<frMin
	   <<"\n_rMax = "<<frMax<<std::endl;
    
  // create and register output array
  fSplineArray = new TClonesArray("BiCubSpline");
  ioman->Register(fSplineBranchName,"Tpc",fSplineArray,fPersistence);
  // fstatArray = new TClonesArray("TpcLaserStat"); 
  // ioman->Register("TpcLaserStat","Tpc",fstatArray,fPersistence);
  // ftrackArray = new TClonesArray("TpcLaserTrack"); 
  // ioman->Register("TpcLaserTrack","Tpc",ftrackArray,fPersistence);
  // ffitStatArray = new TClonesArray("TpcLaserFitTaskStat"); 
  // ioman->Register("TpcLaserFitTaskStat","Tpc",ffitStatArray,fPersistence);
  

  // initialize mesh for Spline fitting
  fNlamdaZ = fNknotsZ + 8;   //4 knots on each side outside the data area
  fNlamdaR = fNknotsR + 8;

  //stretch the range a bit to make sure we have 4 knots on each side 
  //safely OUTSIDE the data area
    
  double lengthZ = (fzMax-fzMin);
  double lengthR = (frMax-frMin);  //data area dimensions

  double meshWidthZ = lengthZ/((double)(fNknotsZ-2));
  double meshWidthR = lengthR/((double)(fNknotsR-2));
  
  //distribute knots such that the data area ends at the middle of the outer knot bins
  
  double fzMin_mod = fzMin - 0.5*meshWidthZ;
  double frMin_mod = frMin - 0.5*meshWidthR;

  for(int i=0; i<fNlamdaZ; ++i)
    fknotsZ.push_back(fzMin_mod + (i-4)*meshWidthZ);
  
  for(int i=0; i<fNlamdaR; ++i)
    fknotsR.push_back(frMin_mod + (i-4)*meshWidthR);
  
  fdevMapR = new BiCubSpline(fknotsZ, fknotsR);
  fdevMapPerp = new BiCubSpline(fknotsZ, fknotsR);
  frecoMapR = new BiCubSpline(fknotsZ, fknotsR);
  frecoMapPerp = new BiCubSpline(fknotsZ, fknotsR);

  std::cout<<"\n--------- TpcLaserFitTask::Init(): Spline mesh initialized -----------"
	   <<std::endl<<"Knots in Z: ";
  for(unsigned int i=0; i<fknotsZ.size(); i++) {
    if(i%4==0 && i>0)
      std::cout<<"\n            " ;
    std::cout<<fknotsZ[i]<<",  ";
  }
  std::cout<<std::endl;
  std::cout<<std::endl<<"Knots in R: ";
  for(unsigned int i=0; i<fknotsR.size(); i++) {
    if(i%4==0 && i>0)
      std::cout<<"\n            " ;
    std::cout<<fknotsR[i]<<",  ";
  }
  std::cout<<"\n-------------------------------------------------------------------------\n"<<std::endl;

  //init rotation matrices
  
    
  return kSUCCESS;
}


void 
TpcLaserFitTask::SetParContainers() {

  std::cout<<"TpcLaserFitTask::SetParContainers"<<std::endl;
  std::cout.flush();

  // Get run and runtime database
  FairRun* run = FairRun::Instance();
  if ( ! run ) Fatal("SetParContainers", "No analysis run");

  FairRuntimeDb* db = run->GetRuntimeDb();
  if ( ! db ) Fatal("SetParContainers", "No runtime database");

  // Get Tpc digitisation parameter container
  fpar= (TpcDigiPar*) db->getContainer("TpcDigiPar");
  if (! fpar ) Fatal("SetParContainers", "TpcDigiPar not found");
}



//TODO: Error in z has to be taken into account!!!

void
TpcLaserFitTask::Exec(Option_t* opt) 
{
  
  std::cout<<"TpcLaserFitTask::Exec(): Starting Laser Reconstruction ..."<<std::endl;
  
  unsigned int nBinsR = 35;
  unsigned int nBinsZ = 100;
  double binWidthR = (frMax-frMin)/(double)nBinsR;
  double binWidthZ = (fzMax-fzMin)/(double)nBinsZ;
  
  
  TH1D* histTheta = new TH1D("histTheta", "Laser Track #Theta distribution",
			     360,0,360);

  TH2D* histErrDr = new TH2D("histErrDr", "Mean errors of radial residuals",
			     nBinsZ,fzMin,fzMax,nBinsR,frMin,frMax);
  histErrDr->GetXaxis()->SetTitle("z (cm)");
  histErrDr->GetYaxis()->SetTitle("r (cm)");
  TH2D* histErrDrWeight = new TH2D("histErrDrWeight", "Number of measurements of radial residuals",
				   nBinsZ,fzMin,fzMax,nBinsR,frMin,frMax);
  histErrDrWeight->GetXaxis()->SetTitle("z (cm)");
  histErrDrWeight->GetYaxis()->SetTitle("r (cm)");
  TH2D* histErrDphi = new TH2D("histErrDphi", "Mean errors of phi residuals",
			       nBinsZ,fzMin,fzMax,nBinsR,frMin,frMax);
  histErrDphi->GetXaxis()->SetTitle("z (cm)");
  histErrDphi->GetYaxis()->SetTitle("r (cm)");
  TH2D* histErrDphiWeight = new TH2D("histErrDphiWeight", "Number of measurements of phi residuals",
				     nBinsZ,fzMin,fzMax,nBinsR,frMin,frMax);
  histErrDphiWeight->GetXaxis()->SetTitle("z (cm)");
  histErrDphiWeight->GetYaxis()->SetTitle("r (cm)");
  TH2D* histMeanClErr = new TH2D("histMeanClErr", "Mean cluster error",
				     nBinsZ,fzMin,fzMax,nBinsR,frMin,frMax);
  histMeanClErr->GetXaxis()->SetTitle("z (cm)");
  histMeanClErr->GetYaxis()->SetTitle("r (cm)");
  
  TMatrixT<double> histMapR(nBinsR,nBinsZ);
  TMatrixT<double> histMapPhi(nBinsR,nBinsZ);

  
			     

  const std::map<unsigned int, TpcLaser*>* laserMap = TpcLaserGrid::Instance()->GetLasers();
  const std::map<unsigned int, GFAbsTrackRep*>* repMap = TpcLaserGrid::Instance()->GetTracks();
  
  //Loop over all GFLaserTracks
  unsigned int nResCol = fResArray->GetEntriesFast();
  std::cout<<"      Looping over "<<nResCol<<" residual collections ... "<<std::endl;
  bool correctBranch=false;
  TString refname;
  for(unsigned int iResC=0; iResC<nResCol; iResC++) {
    TpcRefResidualCollection* rc = (TpcRefResidualCollection*) (*fResArray)[iResC];
    //this check runs only once
    if(!correctBranch) {
      refname = TString(rc->getRefBranch().c_str());
      if(refname == fTrackBranchName)
	correctBranch = true;
      else {
	TString err("Inconsistent input! Residuals refer to ");
	err+=refname;
	err+=", but TrackBranchName is ";
	err+=fTrackBranchName;
	err+="! Aborting.";
	Fatal("TpcLaserFitTask::Exec()", err);
      }
    }
    
    int trackID = rc->getRefIndex();
    if(trackID<0) {
      Fatal("TpcLaserFitTask::Exec()", "ResidualCollection does not contain valid trackID");
    }

    //get the corresponding track:
    GFTrack* tr = (GFTrack*) (*fTrackArray)[trackID];
    
    //loop over all residuals
    unsigned int nRes = rc->getSize();
    for(unsigned int iRes=0; iRes<nRes; iRes++) {
      const TpcResidual* r = rc->getResidual(iRes);
      TVector3 pP = r->getRefPos();
      TVector3 resV3 = r->getResXYZ();
      TVector3 hPos = r->getHitPos();
      TVector3 dirPP = pP;
      dirPP.SetZ(0.);           //project to r-phi plane
      dirPP.SetMag(1.);
      double dr = dirPP*resV3;  //project the residual on the radial vector pointing towards pP
      //rotate counter-clockwise 90 deg so we can get the other projection:
      dirPP.RotateZ(TMath::Pi()/2);
      double dperp = dirPP*resV3;
      
      double errDR = 1.;
      double errPERP = 1.;
     
      if(fDoErrors) {
	//residual weighting with error:
	TMatrixT<double> cov = r->getHitCov();
	// for(int i=0; i<3; i++)
	//   for(int j=0; j<3; j++) {
	//     cov[i][j] = 0.;
	//     if(i==j)
	//       cov[i][j] = 2;
	//   }

	
	double meanErr = (2./3.)*(TMath::Sqrt(cov[0][0]) + TMath::Sqrt(cov[1][1]) + TMath::Sqrt(cov[2][2]));
		
	//align with track direction and blow up error along track:
	TVector3 trackDir = tr->getCardinalRep()->getMom();
	double phi = trackDir.Phi();
	double theta = trackDir.Theta();
	histTheta->Fill(theta*180./TMath::Pi());
	
	//std::cout<<"COV BEFORE rotation:"<<std::endl;
	//cov.Print();
	//std::cout<<"   rotate phi: "<<phi*180/(TMath::Pi())<<", theta: "<<theta*180/(TMath::Pi())<<std::endl;
	rotateCov(cov, phi, theta+TMath::Pi()/2.);  //now aligned with X-Axis
	//cov.Print();
	double lengthR = (frMax-frMin);
	cov[0][0] = (lengthR*lengthR)/4;   //blow up, semi-axis = chamberdim/2
	//cov.Print();
	rotateCovBack(cov, -phi, -(theta+TMath::Pi()/2.));  //rotate back
	//cov.Print();
	
	//rotate according to residual position : (only r-phi plane matters)
	rotateCov(cov, pP.Phi(), 0.);
	//cov.Print();
	
	//extract weighting factor from ellipse projection:
	errDR = TMath::Sqrt(cov[0][0]);
	errPERP = TMath::Sqrt(cov[1][1]);

	//std::cout<<"ERRORS: errDR: "<<errDR<<", errPHI: "<<errPERP<<std::endl;
	
	if(errDR < 0 || errPERP < 0) 
	  Fatal("TpcLaserFitTask::Exec()", "NEGATIVE DIAGONAL COV ELEMENT!");

	//Fill histograms
	double rp = pP.Perp();
	double zp = pP.Z();
	int rBin = floor((rp-frMin)/binWidthR) + 1;
	int zBin = floor((zp-fzMin)/binWidthZ) + 1;
	if(rBin>=nBinsR)  //can happen if the calculated projection point is actually outside the volume
	  rBin=nBinsR-1;
	if(rBin<1)  
	  rBin=1;
	if(zBin>=nBinsZ)
	  zBin=nBinsZ-1;
	if(zBin<1)  
	  zBin=1;
	
	histMeanClErr->SetBinContent(zBin,rBin,histMeanClErr->GetBinContent(zBin,rBin) + meanErr);
	histErrDr->SetBinContent(zBin,rBin,histErrDr->GetBinContent(zBin,rBin)+errDR);
	histMapR[rBin-1][zBin-1] += 1;
	histErrDphi->SetBinContent(zBin,rBin,histErrDphi->GetBinContent(zBin,rBin)+errPERP);
	histMapPhi[rBin-1][zBin-1] += 1;
	
      }

      std::vector<double>* mapR = new std::vector<double>(4,0.);
      std::vector<double>* mapPerp = new std::vector<double>(4,0.);
      std::vector<double>* recoR = new std::vector<double>(4,0.);
      std::vector<double>* recoPerp = new std::vector<double>(4,0.);
      
      
      mapR->at(0)=pP.Z(); mapR->at(1)=pP.Perp(); mapR->at(2)=dr; mapR->at(3)=errDR;
      mapPerp->at(0)=pP.Z(); mapPerp->at(1)=pP.Perp(); mapPerp->at(2)=dperp; 
      mapPerp->at(3)=errPERP;
     
      recoR->at(0)=pP.Z(); recoR->at(1)=hPos.Perp(); recoR->at(2)=dr; recoR->at(3)=errDR;
      recoPerp->at(0)=pP.Z(); recoPerp->at(1)=hPos.Perp(); recoPerp->at(2)=dperp;
      recoPerp->at(3)=errPERP;
      
      fdevMapR_data.push_back(mapR);
      fdevMapPerp_data.push_back(mapPerp);
      frecoMapR_data.push_back(recoR);
      frecoMapPerp_data.push_back(recoPerp);
      
    } //end loop over residuals
    
  }

  
  
  // for(int i=0; i<Nlasers; i++) {
  //   std::vector<TpcLaserStat*>* statList = (laserTrackList[i])->getStatList();  
  //   unsigned int size = statList->size();
  //   for(unsigned int j=0; j<size; j++) {
  //     Int_t statsize = fstatArray->GetEntriesFast();
  //     new((*fstatArray)[statsize]) TpcLaserStat(*(statList->at(j)));
  //   }
  // }
  
  // Fit using BiCubic Splines

  std::cout<<"\n\nTpcLaserFitTask::Exec(): starting fits ..............."<<std::endl;

  BiCubSplineFitter* fit_devMapR = new BiCubSplineFitter(fdevMapR, &fdevMapR_data);
  BiCubSplineFitter* fit_devMapPerp = new BiCubSplineFitter(fdevMapPerp, &fdevMapPerp_data);
  BiCubSplineFitter* fit_recoMapR = new BiCubSplineFitter(frecoMapR, &frecoMapR_data);
  BiCubSplineFitter* fit_recoMapPerp = new BiCubSplineFitter(frecoMapPerp, &frecoMapPerp_data);

  // Perform QR decomposition
  fit_devMapR->decompose();
  fit_devMapPerp->decompose();
  fit_recoMapR->decompose();
  fit_recoMapPerp->decompose();

  // Obtain solution
  TVectorD sol_devMapR = fit_devMapR->solve();
  TVectorD sol_devMapPerp = fit_devMapPerp->solve();
  TVectorD sol_recoMapR = fit_recoMapR->solve();
  TVectorD sol_recoMapPerp = fit_recoMapPerp->solve();

  //extract the right number of elements
  TArrayD res_devMapR = TArrayD((fNlamdaZ-4)*(fNlamdaR-4));
  TArrayD res_devMapPerp = TArrayD((fNlamdaZ-4)*(fNlamdaR-4));
  TArrayD res_recoMapR = TArrayD((fNlamdaZ-4)*(fNlamdaR-4));
  TArrayD res_recoMapPerp = TArrayD((fNlamdaZ-4)*(fNlamdaR-4));
  
  for(int i=0; i<(fNlamdaZ-4)*(fNlamdaR-4); ++i) {
    res_devMapR[i] = sol_devMapR[i];
    res_devMapPerp[i] = sol_devMapPerp[i];
    res_recoMapR[i] = sol_recoMapR[i];
    res_recoMapPerp[i] = sol_recoMapPerp[i];
  }
  
  // Convert to standard array and finally set coefficients of Spline
  
  double* par_devMapR = res_devMapR.GetArray();
  double* par_devMapPerp = res_devMapPerp.GetArray();  
  double* par_recoMapR = res_recoMapR.GetArray();
  double* par_recoMapPerp = res_recoMapPerp.GetArray();

  std::cout<<"\nTpcLaserFitTask::Exec(): transporting fit results to Splines ....."<<std::endl;
  
  std::vector<BiCubSpline*> splines;
  fdevMapR->setCoeffsByArray(par_devMapR);           splines.push_back(fdevMapR);
  fdevMapPerp->setCoeffsByArray(par_devMapPerp);     splines.push_back(fdevMapPerp);
  frecoMapR->setCoeffsByArray(par_recoMapR);         splines.push_back(frecoMapR);
  frecoMapPerp->setCoeffsByArray(par_recoMapPerp);   splines.push_back(frecoMapPerp);

  //write to tree: MIND THE ORDER
  for(int ispl=0; ispl<4; ispl++) {
    new ((*fSplineArray)[ispl]) BiCubSpline(*splines[ispl]);
  }

  
  if(fPlot) {    
    std::cout<<"\nTpcLaserFitTask::Exec(): creating plots .........."<<std::endl;

    TPolyMarker3D* poly_devMapR = new TPolyMarker3D(fdevMapR_data.size());
    TPolyMarker3D* poly_devMapPerp = new TPolyMarker3D(fdevMapPerp_data.size());
    TPolyMarker3D* poly_recoMapR = new TPolyMarker3D(frecoMapR_data.size());
    TPolyMarker3D* poly_recoMapPerp = new TPolyMarker3D(frecoMapPerp_data.size());
  
    for(int i=0; i<fdevMapR_data.size(); ++i) {
      std::vector<double>* temp = fdevMapR_data[i];
      poly_devMapR->SetNextPoint(temp->at(0), temp->at(1), temp->at(2));
    }
    
    for(int i=0; i<fdevMapPerp_data.size(); ++i) {
      std::vector<double>* temp = fdevMapPerp_data[i];
      poly_devMapPerp->SetNextPoint(temp->at(0), temp->at(1), temp->at(2));
    }
    
    for(int i=0; i<frecoMapR_data.size(); ++i) {
      std::vector<double>* temp = frecoMapR_data[i];
      poly_recoMapR->SetNextPoint(temp->at(0), temp->at(1), temp->at(2));
    }
    
    for(int i=0; i<frecoMapPerp_data.size(); ++i) {
      std::vector<double>* temp = frecoMapPerp_data[i];
      poly_recoMapPerp->SetNextPoint(temp->at(0), temp->at(1), temp->at(2));
    }
    
    //normalize error histos:
    for(unsigned int ir=0; ir<nBinsR; ir++) 
      for(unsigned int iz=0; iz<nBinsZ; iz++) {
	if(histMapR[ir][iz] > 0.5) {  //awkward, but just means 1 or more entries
	  histErrDr->SetBinContent(iz+1,ir+1,histErrDr->GetBinContent(iz+1,ir+1)/histMapR[ir][iz]);
	  //histMeanClErr->SetBinContent(iz+1,ir+1,histMeanClErr->GetBinContent(iz+1,ir+1)/histMapR[ir][iz]);
	}
	histErrDrWeight->SetBinContent(iz+1,ir+1,histMapR[ir][iz]);
	if(histMapPhi[ir][iz] > 0.5) {
	  histErrDphi->SetBinContent(iz+1,ir+1,histErrDphi->GetBinContent(iz+1,ir+1)/histMapPhi[ir][iz]);
	  histMeanClErr->SetBinContent(iz+1,ir+1,histMeanClErr->GetBinContent(iz+1,ir+1)/histMapPhi[ir][iz]);
	}
	histErrDphiWeight->SetBinContent(iz+1,ir+1,histMapPhi[ir][iz]);
      }
	
    
    TCanvas* canv = new TCanvas();
    canv->Divide(2,2);
    canv->cd(1);
    poly_devMapR->Draw();
    canv->cd(2);
    poly_devMapPerp->Draw();
    canv->cd(3);
    poly_recoMapR->Draw();
    canv->cd(4);
    poly_recoMapPerp->Draw();

    TCanvas* canv2 = new TCanvas();
    canv2->Divide(2,2);
    
    canv2->cd(1);
    (fdevMapR->getTF2(fzMin,fzMax,frMin,frMax))->Draw("SURF1");
    canv2->cd(2);
    (fdevMapPerp->getTF2(fzMin,fzMax,frMin,frMax))->Draw("SURF1");
    canv2->cd(3);
    (frecoMapR->getTF2(fzMin,fzMax,frMin,frMax))->Draw("SURF1");
    canv2->cd(4);
    (frecoMapPerp->getTF2(fzMin,fzMax,frMin,frMax))->Draw("SURF1");

    TCanvas* canv3 = new TCanvas();
    canv3->Divide(2,2);
    canv3->cd(1);
    histErrDr->Draw("colz");
    canv3->cd(2);
    histErrDphi->Draw("colz");
    canv3->cd(3);
    histErrDrWeight->Draw("colz");
    canv3->cd(4);
    histErrDphiWeight->Draw("colz");
    
    TCanvas* canv4 = new TCanvas();
    histMeanClErr->Draw("colz");

    TCanvas* canv5 = new TCanvas();
    histTheta->Draw();
    
    
    
  }
  
}

//rotation around Z-axis about phi, then about (new) Y-axis about theta
void 
TpcLaserFitTask::rotateCov(TMatrixT<double>& cov, 
			   double phi, double theta) {

  fRotPhi[0][0] = TMath::Cos(phi);
  fRotPhi[0][1] = -TMath::Sin(phi);
  fRotPhi[1][0] = TMath::Sin(phi);
  fRotPhi[1][1] = TMath::Cos(phi);

   
  //Rotation around z-axis (phi)
  cov = fRotPhi.T() * (cov * fRotPhi);
  
  //Now rotate around the NEW y-axis
  TVector3 y(0.,1.,0.);
  y.RotateZ(phi);
  //now rotate around this axis:
  double entries[9] = {0.   ,-y.Z(), y.Y(), 
		       y.Z(), 0.   ,-y.X(),
		      -y.Y(), y.X(), 0.  };
  TMatrixT<double>* crossY = new TMatrixT<double>(3,3,entries);
  double entries2[9] = {y.X()*y.X(), y.X()*y.Y(), y.X()*y.Z(),
			y.Y()*y.X(), y.Y()*y.Y(), y.Y()*y.Z(),
			y.Z()*y.X(), y.Z()*y.Y(), y.Z()*y.Z()};
  TMatrixT<double>* tensY = new TMatrixT<double>(3,3,entries2);
  TMatrixT<double> rotTheta(3,3);
  rotTheta = TMath::Cos(theta)*fUnity + TMath::Sin(theta)*(*crossY) + (1-TMath::Cos(theta))*(*tensY);
  cov = rotTheta.T() * (cov * rotTheta);
  
  delete crossY;
  delete tensY;
  //std::cout<<"Covariance matrix after rotation:"<<std::endl;
  //cov.Print();
  
}

//rotation around Y about theta, then around (new) Z-axis about phi
void 
TpcLaserFitTask::rotateCovBack(TMatrixT<double>& cov, 
			       double phi, double theta) {

  fRotTheta[0][0] = TMath::Cos(theta);
  fRotTheta[0][2] = TMath::Sin(theta);
  fRotTheta[2][0] = -TMath::Sin(theta);
  fRotTheta[2][2] = TMath::Cos(theta);

  cov = fRotTheta.T() * (cov * fRotTheta);
  
  //Now rotate around the NEW z-axis
  TVector3 z(0.,0.,1.);
  z.RotateY(theta);
  //now rotate around this axis:
  double entries[9] = {0.   ,-z.Z(), z.Y(), 
		       z.Z(), 0.   ,-z.X(),
		      -z.Y(), z.X(), 0.  };
  TMatrixT<double>* crossZ = new TMatrixT<double>(3,3,entries);
  double entries2[9] = {z.X()*z.X(), z.X()*z.Y(), z.X()*z.Z(),
			z.Y()*z.X(), z.Y()*z.Y(), z.Y()*z.Z(),
			z.Z()*z.X(), z.Z()*z.Y(), z.Z()*z.Z()};
  TMatrixT<double>* tensZ = new TMatrixT<double>(3,3,entries2);
  TMatrixT<double> rotPhi(3,3);
  rotPhi = TMath::Cos(phi)*fUnity + TMath::Sin(phi)*(*crossZ) + (1-TMath::Cos(phi))*(*tensZ);
  cov = rotPhi.T() * (cov * rotPhi);
  
  delete crossZ;
  delete tensZ;
 
  
}


ClassImp(TpcLaserFitTask)