// ----------------------------------------------------------------------------
// $Id: LCDEventShape.cxx,v 1.1 2001/05/10 19:32:04 toshi Exp $
// ----------------------------------------------------------------------------
//
// $Log: LCDEventShape.cxx,v $
// Revision 1.1  2001/05/10 19:32:04  toshi
// C++ file name convention has been changed from .C to .cxx .
//
//
// V0.0           :            Thrust finder utility translated from 
//                             Java routines written by G.Bower. 
// V0.2 Jul 07/99 : M.Iwasaki  Change Mod to Mag function in TVector3
//                             so as to use root 2.2.x
// V0.3 Sep 23/99 : M.Iwasaki  Fix setEvent memory leak, 
//                             apply nessesary modifications in Thrust, Major, 
//                             Minor axis, and Thrust, and add ~EventSape().
// V0.4 May 12/00 : M.Iwasaki  Make necessary modifications.
//                                        
// V1.0 Aug   /00 : M.Iwasaki  Change Class name from EventShape to 
//                             LCDEventShape.
//
// V1.1 Aug 29/00 : M.Iwasaki  Remove Thrust(), and add GetThrust(), GetMajor(),
//                             and GetMiner(), so as to provide thrust, Major, 
//                             and Miner values, respectively.
//                             change class names:: thrustAxis -> GetThrustAxis etc..

#include "LCDEventShape.h"

ClassImp(LCDEventShape)

Int_t LCDEventShape::m_maxpart = 1000;

 LCDEventShape::LCDEventShape():
  m_dSphMomPower(2.0),m_dDeltaThPower(0),
  m_iFast(4),m_dConv(0.0001),m_iGood(2)
{
  m_dAxes.ResizeTo(4,4);
}
//______________________________________________________________

 LCDEventShape::~LCDEventShape(){
}
//______________________________________________________________

// Input the particle 3(4)-vector list
// e: 3-vector  TVector3       ..(px,py,pz) or
//    4-vector  TLorentzVector ..(px,py,pz,E) 
// Even input the TLorentzVector, we don't use Energy 
// 
 void LCDEventShape::SetPartList(TObjArray* e)
{	
  //To make this look like normal physics notation the
  //zeroth element of each array, mom[i][0], will be ignored
  //and operations will be on elements 1,2,3...
  TMatrixD mom(m_maxpart,6);
  Double_t tmax = 0;
  Double_t phi = 0.;
  Double_t the = 0.;
  Double_t sgn;
  TMatrixD fast(m_iFast + 1,6);
  TMatrixD work(11,6);
  Double_t tdi[] = {0.,0.,0.,0.};
  Double_t tds;
  Double_t tpr[] = {0.,0.,0.,0.};
  Double_t thp;
  Double_t thps;
  TMatrixD temp(3,5);
  Int_t np = 0;
  Int_t numElements = e->GetEntries();
  TObject* o;
  for(Int_t elem=0;elem<numElements;elem++) {
    o = e->At(elem);
    
    if (np >= m_maxpart) { 
      printf("Too many particles input to LCDEventShape");
      return;
    }

    TString nam(o->IsA()->GetName());
    if (nam.Contains("TVector3")) {
      mom(np,1) = ((TVector3 *) o)->X();
      mom(np,2) = ((TVector3 *) o)->Y();
      mom(np,3) = ((TVector3 *) o)->Z();
      mom(np,4) = TMath::Sqrt(mom(np,1)*mom(np,1) + mom(np,2)*mom(np,2)
			      + mom(np,3)*mom(np,3));
    } else if (nam.Contains("TLorentzVector")) {
      mom(np,1) = ((TLorentzVector *) o)->X();
      mom(np,2) = ((TLorentzVector *) o)->Y();
      mom(np,3) = ((TLorentzVector *) o)->Z();
      mom(np,4) = TMath::Sqrt(mom(np,1)*mom(np,1) + mom(np,2)*mom(np,2)
			      + mom(np,3)*mom(np,3));
    } else {
      printf("LCDEventShape::SetEvent input is not a TVector3 or a TLorentzVectorn");
      continue;
    }

    if ( TMath::Abs( m_dDeltaThPower ) <= 0.001 ) {
      mom(np,5) = 1.0;
    } else {
      mom(np,5) = TMath::Power(mom(np,4),m_dDeltaThPower);
    }
    tmax = tmax + mom(np,4)*mom(np,5);
    np++;
  }
  if ( np < 2 ) {
    m_dThrust[1] = -1.0;
    m_dOblateness = -1.0;
    return;
  }
  // for pass = 1: find thrust axis.
  // for pass = 2: find major axis.
  for ( Int_t pass=1; pass < 3; pass++) {
    if ( pass == 2 ) {
      phi = ulAngle(m_dAxes(1,1), m_dAxes(1,2));
      ludbrb( &mom, 0, -phi, 0., 0., 0. );
      for ( Int_t i = 0; i < 3; i++ ) {
	for ( Int_t j = 1; j < 4; j++ ) {
	  temp(i,j) = m_dAxes(i+1,j);
	}
	temp(i,4) = 0;
      }
      ludbrb(&temp,0.,-phi,0.,0.,0.);
      for ( Int_t ib = 0; ib < 3; ib++ ) {
	for ( Int_t j = 1; j < 4; j++ ) {
	  m_dAxes(ib+1,j) = temp(ib,j);
	}
      }
      the = ulAngle( m_dAxes(1,3), m_dAxes(1,1) );
      ludbrb( &mom, -the, 0., 0., 0., 0. );
      for ( Int_t ic = 0; ic < 3; ic++ ) {
	for ( Int_t j = 1; j < 4; j++ ) {
	  temp(ic,j) = m_dAxes(ic+1,j);
	}
	temp(ic,4) = 0;
      }
      ludbrb(&temp,-the,0.,0.,0.,0.);
      for ( Int_t id = 0; id < 3; id++ ) {	
	for ( Int_t j = 1; j < 4; j++ ) {
	  m_dAxes(id+1,j) = temp(id,j);
	}
      }
    }
    for ( Int_t ifas = 0; ifas < m_iFast + 1 ; ifas++ ) {
      fast(ifas,4) = 0.;
    }
    // Find the m_iFast highest momentum particles and
    // put the highest in fast[0], next in fast[1],....fast[m_iFast-1].
    // fast[m_iFast] is just a workspace.
    for ( Int_t i = 0; i < np; i++ ) {
      if ( pass == 2 ) {
	mom(i,4) = TMath::Sqrt( mom(i,1)*mom(i,1) 
				+ mom(i,2)*mom(i,2) ); 
      }
      for ( Int_t ifas = m_iFast - 1; ifas > -1; ifas-- ) {
	if ( mom(i,4) > fast(ifas,4) ) {
	  for ( Int_t j = 1; j < 6; j++ ) {
	    fast(ifas+1,j) = fast(ifas,j);
	    if ( ifas == 0 ) fast(ifas,j) = mom(i,j);	    
	  }
	} else {
	  for ( Int_t j = 1; j < 6; j++ ) {
	    fast(ifas+1,j) = mom(i,j);
	  }
	  break;
	}
      }
    }
    // Find axis with highest thrust (case 1)/ highest major (case 2).
    for ( Int_t ie = 0; ie < work.GetNrows(); ie++ ) {
      work(ie,4) = 0.;
    }
    Int_t p = TMath::Min( m_iFast, np ) - 1;
    // Don't trust Math.pow to give right answer always.
    // Want nc = 2**p.
    Int_t nc = iPow(2,p); 
    for ( Int_t n = 0; n < nc; n++ ) {
      for ( Int_t j = 1; j < 4; j++ ) {
	tdi[j] = 0.;
      }
      for ( Int_t i = 0; i < TMath::Min(m_iFast,n); i++ ) {
	sgn = fast(i,5);
	if (iPow(2,(i+1))*((n+iPow(2,i))/iPow(2,(i+1))) >= i+1){
	  sgn = -sgn;
	}
	for ( Int_t j = 1; j < 5-pass; j++ ) {
	  tdi[j] = tdi[j] + sgn*fast(i,j);
	}
      }
      tds = tdi[1]*tdi[1] + tdi[2]*tdi[2] + tdi[3]*tdi[3];
      for ( Int_t iw = TMath::Min(n,9); iw > -1; iw--) {
	if( tds > work(iw,4) ) {
	  for ( Int_t j = 1; j < 5; j++ ) {
	    work(iw+1,j) = work(iw,j);
	    if ( iw == 0 ) {
	      if ( j < 4 ) {
		work(iw,j) = tdi[j];
	      } else {
		work(iw,j) = tds;
	      }
	    }
	  }
	} else {
	  for ( Int_t j = 1; j < 4; j++ ) {
	    work(iw+1,j) = tdi[j];
	  }
	  work(iw+1,4) = tds;
	}
      }
    }
    // Iterate direction of axis until stable maximum.
    m_dThrust[pass] = 0;
    thp = -99999.;
    Int_t nagree = 0;
    for ( Int_t iw = 0; 
	  iw < TMath::Min(nc,10) && nagree < m_iGood; iw++ ){
      thp = 0.;
      thps = -99999.;
      while ( thp > thps + m_dConv ) {
	thps = thp;
	for ( Int_t j = 1; j < 4; j++ ) {
	  if ( thp <= 1E-10 ) {
	    tdi[j] = work(iw,j);
	  } else {
	    tdi[j] = tpr[j];
	    tpr[j] = 0;
	  }
	}
	for ( Int_t i = 0; i < np; i++ ) {
	  sgn = sign(mom(i,5), 
		     tdi[1]*mom(i,1) + 
		     tdi[2]*mom(i,2) + 
		     tdi[3]*mom(i,3));
	  for ( Int_t j = 1; j < 5 - pass; j++ ){
	    tpr[j] = tpr[j] + sgn*mom(i,j);
	  }
	}
	thp = TMath::Sqrt(tpr[1]*tpr[1] 
			  + tpr[2]*tpr[2] 
			  + tpr[3]*tpr[3])/tmax;
      }
      // Save good axis. Try new initial axis until enough
      // tries agree.
      if ( thp < m_dThrust[pass] - m_dConv ) {
	break;
      }
      if ( thp > m_dThrust[pass] + m_dConv ) {
	nagree = 0;
	sgn = iPow( -1, (Int_t)TMath::Nint(m_random.Rndm()) );
	for ( Int_t j = 1; j < 4; j++ ) {
	  m_dAxes(pass,j) = sgn*tpr[j]/(tmax*thp);
	}
	m_dThrust[pass] = thp;
      }
      nagree = nagree + 1;
    }
  }
  // Find minor axis and value by orthogonality.
  sgn = iPow( -1, (Int_t)TMath::Nint(m_random.Rndm()));
  m_dAxes(3,1) = -sgn*m_dAxes(2,2);
  m_dAxes(3,2) = sgn*m_dAxes(2,1);
  m_dAxes(3,3) = 0.;
  thp = 0.;
  for ( Int_t i = 0; i < np; i++ ) {
    thp += mom(i,5)*TMath::Abs(m_dAxes(3,1)*mom(i,1) + 
			       m_dAxes(3,2)*mom(i,2));
  }
  m_dThrust[3] = thp/tmax;
  // Rotate back to original coordinate system.
  for ( Int_t i6 = 0; i6 < 3; i6++ ) {
    for ( Int_t j = 1; j < 4; j++ ) {
      temp(i6,j) = m_dAxes(i6+1,j);
    }
    temp(i6,4) = 0;
  }
  ludbrb(&temp,the,phi,0.,0.,0.);
  for ( Int_t i7 = 0; i7 < 3; i7++ ) {
    for ( Int_t j = 1; j < 4; j++ ) {
      m_dAxes(i7+1,j) = temp(i7,j);
    }
  }
  m_dOblateness = m_dThrust[2] - m_dThrust[3];
  
}
//______________________________________________________________

// Setting and getting parameters.

 void LCDEventShape::SetThMomPower(Double_t tp)
{
  // Error if sp not positive.
  if ( tp > 0. ) m_dDeltaThPower = tp - 1.0;
  return;
}
//______________________________________________________________

 Double_t LCDEventShape::GetThMomPower()
{
  return 1.0 + m_dDeltaThPower;
}
//______________________________________________________________

 void LCDEventShape::SetFast(Int_t nf)
{
  // Error if sp not positive.
  if ( nf > 3 ) m_iFast = nf;
  return;
}
//______________________________________________________________

 Int_t LCDEventShape::GetFast()
{
  return m_iFast;
}
//______________________________________________________________

// Returning results

 TVector3 LCDEventShape::GetThrustAxis() {
  return TVector3(m_dAxes(1,1),m_dAxes(1,2),m_dAxes(1,3));
}
//______________________________________________________________

 TVector3 LCDEventShape::GetMajorAxis() {
  return TVector3(m_dAxes(2,1),m_dAxes(2,2),m_dAxes(2,3));
}
//______________________________________________________________

 TVector3 LCDEventShape::GetMinorAxis() {
  return TVector3(m_dAxes(3,1),m_dAxes(3,2),m_dAxes(3,3));
}
//______________________________________________________________

 Double_t LCDEventShape::oblateness() {
  return m_dOblateness;
}
//______________________________________________________________

// utilities(from Jetset):
 Double_t LCDEventShape::ulAngle(Double_t x, Double_t y)
{
  Double_t ulangl = 0;
  Double_t r = TMath::Sqrt(x*x + y*y);
  if ( r < 1.0E-20 ) {
    return ulangl; 
  }
  if ( TMath::Abs(x)/r < 0.8 ) {
    ulangl = sign(TMath::ACos(x/r),y);
  } else {
    ulangl = TMath::ASin(y/r);
    if ( x < 0. && ulangl >= 0. ) {
      ulangl = TMath::Pi() - ulangl;
    } else if ( x < 0. ) {
      ulangl = -TMath::Pi() - ulangl;
    }
  }
  return ulangl;
}
//______________________________________________________________

 Double_t LCDEventShape::sign(Double_t a, Double_t b) {
  if ( b < 0 ) {
    return -TMath::Abs(a);
  } else {
    return TMath::Abs(a);
  }
}
//______________________________________________________________

 void LCDEventShape::ludbrb(TMatrixD* mom, 
			Double_t the, 
			Double_t phi, 
			Double_t bx, 
			Double_t by,
			Double_t bz)
{
  // Ignore "zeroth" elements in rot,pr,dp.
  // Trying to use physics-like notation.
  TMatrixD rot(4,4);
  Double_t pr[4];
  Double_t dp[5];
  Int_t np = mom->GetNrows();
  if ( the*the + phi*phi > 1.0E-20 )
    {
      rot(1,1) = TMath::Cos(the)*TMath::Cos(phi);
      rot(1,2) = -TMath::Sin(phi);
      rot(1,3) = TMath::Sin(the)*TMath::Cos(phi);
      rot(2,1) = TMath::Cos(the)*TMath::Sin(phi);
      rot(2,2) = TMath::Cos(phi);
      rot(2,3) = TMath::Sin(the)*TMath::Sin(phi);
      rot(3,1) = -TMath::Sin(the);
      rot(3,2) = 0.0;
      rot(3,3) = TMath::Cos(the);
      for ( Int_t i = 0; i < np; i++ ) {
	for ( Int_t j = 1; j < 4; j++ ) {
	  pr[j] = (*mom)(i,j);
	  (*mom)(i,j) = 0;
	}
	for ( Int_t jb = 1; jb < 4; jb++) {
	  for ( Int_t k = 1; k < 4; k++) {
	    (*mom)(i,jb) = (*mom)(i,jb) + rot(jb,k)*pr[k];
	  }
	}
      }
      Double_t beta = TMath::Sqrt( bx*bx + by*by + bz*bz );
      if ( beta*beta > 1.0E-20 ) {
	if ( beta >  0.99999999 ) {
	  //send message: boost too large, resetting to <~1.0.
	  bx = bx*(0.99999999/beta);
	  by = by*(0.99999999/beta);
	  bz = bz*(0.99999999/beta);
	  beta =   0.99999999;
	}
	Double_t gamma = 1.0/TMath::Sqrt(1.0 - beta*beta);
	for ( Int_t i = 0; i < np; i++ ) {
	  for ( Int_t j = 1; j < 5; j++ ) {
	    dp[j] = (*mom)(i,j);
	  }
	  Double_t bp = bx*dp[1] + by*dp[2] + bz*dp[3];
	  Double_t gbp = gamma*(gamma*bp/(1.0 + gamma) + dp[4]);
	  (*mom)(i,1) = dp[1] + gbp*bx;
	  (*mom)(i,2) = dp[2] + gbp*by;
	  (*mom)(i,3) = dp[3] + gbp*bz;
	  (*mom)(i,4) = gamma*(dp[4] + bp);
	}
      }
    }
  return;
}
//______________________________________________________________

 Int_t LCDEventShape::iPow(Int_t man, Int_t exp)
{
  Int_t ans = 1;
  for( Int_t k = 0; k < exp; k++) {
    ans = ans*man;
  }
  return ans;
}