/* $Id: CbmRichLightSpotMC.cxx,v 1.3 2005/12/08 15:13:04 turany Exp $ */

/* History of cvs commits:
 *
 * $Log: CbmRichLightSpotMC.cxx,v $
 * Revision 1.3  2005/12/08 15:13:04  turany
 * change GetRegistered, ActivateBranch and CheckActivatedBranch
 * to GetObject, also add a flag to the arguments of Register which control saving
 * to file(kTRUE) or only in memory(kFALSE)
 *
 * Revision 1.2  2005/07/18 09:54:37  kharlov
 * Extend MCpoint array
 *
 */

// -------------------------------------------------------------------------
//        CbmRichLightSpotMC source file
//        Created 09/02/05 by Boris.Polichtchouk@cern.ch
// MC light spot is a collection of MC points produced by one track.
// -------------------------------------------------------------------------


#include "TArrayI.h"
#include "TVector3.h"

#include "CbmRichLightSpotMC.h"
#include "CbmMCPoint.h"
#include "CbmRootManager.h"

// -----   Default constructor   -------------------------------------------
CbmRichLightSpotMC::CbmRichLightSpotMC() : CbmRichRing()
{
  const Int_t kMaxMCPoints = 500;
  fMCPointIndex = new TArrayI(kMaxMCPoints);
  fNpoints = 0;
}
// -------------------------------------------------------------------------

void CbmRichLightSpotMC::AddMCPointIndex (Int_t &mcpoint)
{
  // Add hit index to the array of indices
  fMCPointIndex->AddAt(mcpoint,fNpoints);
  fNpoints++;
}

// -------------------------------------------------------------------------

void CbmRichLightSpotMC::Print(const Option_t *opt) const
{
  // Print light spot data members
  printf("RICH MC light spot: ");
  printf("track id =%6d, Num. MC Points =%3d, center=(%.2f,%.2f) cm, R=%.2f cm\n",
	 fTrackID,fNpoints,fCenterX,fCenterY,fRadius);
}

// -------------------------------------------------------------------------

void CbmRichLightSpotMC::RingFit()
{
  // Fit the light spot by equation of circle
  // to find the circle radius and center
  //
  // Translated from F77 subroutine of S.Sadovsky

  if (fNpoints < 3) {
    fRadius  = 0;
    fCenterX = 0;
    fCenterY = 0;
    return;
  }

  CbmRootManager *manager= CbmRootManager::Instance();
  TClonesArray *pointList = (TClonesArray*)manager->GetObject("RichPoint");

  CbmMCPoint *pt = 0;

  Float_t c[3], a[3][3];

  Float_t b1 = 0;
  Float_t b2 = 0;
  Float_t b3 = 0;

  Float_t b12 = 0;
  Float_t b22 = 0;
  Float_t b32 = fNpoints;

  Float_t a11 = 0;
  Float_t a12 = 0;
  Float_t a13;
  Float_t a21;
  Float_t a22 = 0;
  Float_t a23;
  Float_t a31;
  Float_t a32;
  Float_t a33;

  TVector3 pos;
  for (Int_t i=0; i<fNpoints; i++) {
    pt = (CbmMCPoint*)pointList->At(fMCPointIndex->At(i));
    pt->Position(pos);

    b1 += (pos.X()*pos.X() + pos.Y()*pos.Y()) * pos.X();
    b2 += (pos.X()*pos.X() + pos.Y()*pos.Y()) * pos.Y();
    b3 += (pos.X()*pos.X() + pos.Y()*pos.Y());

    b12 += pos.X();
    b22 += pos.Y();

    a11 += 2*pos.X()*pos.X();
    a12 += 2*pos.X()*pos.Y();
    a22 += 2*pos.Y()*pos.Y();
  }

  a21 = a12;

  a13 = b12;
  a23 = b22;

  a31 = 2*b12;
  a32 = 2*b22;
  a33 = b32;

  c[0] = b1*b22 - b2*b12;
  c[1] = b1*b32 - b3*b12;
  c[2] = b2*b32 - b3*b22;

  a[0][0] = a11*b22 - a21*b12;
  a[1][0] = a12*b22 - a22*b12;
  a[2][0] = a13*b22 - a23*b12;

  a[0][1] = a11*b32 - a31*b12;
  a[1][1] = a12*b32 - a32*b12;
  a[2][1] = a13*b32 - a33*b12;

  a[0][2] = a21*b32-a31*b22;
  a[1][2] = a22*b32-a32*b22;
  a[2][2] = a23*b32-a33*b22;

  Float_t det1 = a[0][0]*a[1][1] - a[0][1]*a[1][0];

  Float_t x11 = (c[0]*a[1][1] - c[1]*a[1][0])/det1;
  Float_t x21 = (a[0][0]*c[1] - a[0][1]*c[0])/det1;

//   Float_t det2 = a[0][1]*a[1][2] - a[0][2]*a[1][1];
//   Float_t det3 = a[0][0]*a[1][2] - a[0][2]*a[1][0];
//   Float_t x12 = (c[1]*a[1][2] - c[2]*a[1][1])/det2;
//   Float_t x22 = (a[0][1]*c[2] - a[0][2]*c[1])/det2;

  fRadius  = TMath::Sqrt((b3 + b32*(x11*x11 + x21*x21) -
			  a31*x11 - a32*x21)/a33);
  fCenterX = x11;
  fCenterY = x21;
}

ClassImp(CbmRichLightSpotMC)