ClusterShapes.cc

    // std::cout << "here we are " << std::endl;
    for (int i(0); i < _nHits; ++i) {
      if (i != i1 && i != i3) {
	i2 = i;
	if (_zHit[i2] < z1) {
	  int itemp = i1;
	  i1 = i2;
	  i2 = itemp;
	}
	else if (_zHit[i2] > z3) {
	  int itemp = i3;
	  i3 = i2;
	  i2 = itemp;
	}        
	break;
      }
    }      
    // std::cout << i1 << " " << i2 << " " << i3 << std::endl;
  }


  double x0  = 0.5*(_xHit[i2]+_xHit[i1]);
  double y0  = 0.5*(_yHit[i2]+_yHit[i1]);
  double x0p = 0.5*(_xHit[i3]+_xHit[i2]);
  double y0p = 0.5*(_yHit[i3]+_yHit[i2]);
  double ax  = _yHit[i2] - _yHit[i1];
  double ay  = _xHit[i1] - _xHit[i2];
  double axp = _yHit[i3] - _yHit[i2];
  double ayp = _xHit[i2] - _xHit[i3];
  double det = ax * ayp - axp * ay;
  double time;

  if (det == 0.) {
    time = 500.;
  }
  else {
    gsl_matrix* A = gsl_matrix_alloc(2,2);
    gsl_vector* B = gsl_vector_alloc(2);
    gsl_vector* T = gsl_vector_alloc(2);     
    gsl_matrix_set(A,0,0,ax);
    gsl_matrix_set(A,0,1,-axp);
    gsl_matrix_set(A,1,0,ay);
    gsl_matrix_set(A,1,1,-ayp);
    gsl_vector_set(B,0,x0p-x0);
    gsl_vector_set(B,1,y0p-y0);
    gsl_linalg_HH_solve(A,B,T);
    time = gsl_vector_get(T,0); 
    gsl_matrix_free(A);
    gsl_vector_free(B);
    gsl_vector_free(T);
  }

  double X0 = x0 + ax*time;
  double Y0 = y0 + ay*time;

  double dX = _xHit[i1] - X0;
  double dY = _yHit[i1] - Y0;

  double R0 = sqrt(dX*dX + dY*dY);

  /*
    if (problematic == 1) {
    std::cout << i1 << " " << i2 << " " << i3 << std::endl;
    std::cout << _xHit[i1] << " " << _yHit[i1] << " " << _zHit[i1] << std::endl;
    std::cout << _xHit[i2] << " " << _yHit[i2] << " " << _zHit[i2] << std::endl;
    std::cout << _xHit[i3] << " " << _yHit[i3] << " " << _zHit[i3] << std::endl;
    std::cout << "R0 = " << R0 << std::endl;
    }
  */

  double phi1 = (double)atan2(_yHit[i1]-Y0,_xHit[i1]-X0);
  double phi2 = (double)atan2(_yHit[i2]-Y0,_xHit[i2]-X0);
  double phi3 = (double)atan2(_yHit[i3]-Y0,_xHit[i3]-X0);

  // testing bz > 0 hypothesis

  if ( phi1 > phi2 ) 
    phi2 = phi2 + 2.0*M_PI;
  if ( phi1 > phi3 )
    phi3 = phi3 + 2.0*M_PI;
  if ( phi2 > phi3 )
    phi3 = phi3 + 2.0*M_PI;

  double bz_plus = (phi3 - phi1) / (_zHit[i3]-_zHit[i1]);
  double phi0_plus = phi1 - bz_plus * _zHit[i1];
  double dphi_plus = fabs( bz_plus * _zHit[i2] + phi0_plus - phi2 );

  // testing bz < 0 hypothesis

  phi1 = (double)atan2(_yHit[i1]-Y0,_xHit[i1]-X0);
  phi2 = (double)atan2(_yHit[i2]-Y0,_xHit[i2]-X0);
  phi3 = (double)atan2(_yHit[i3]-Y0,_xHit[i3]-X0);

  if ( phi1 < phi2 ) 
    phi2 = phi2 - 2.0*M_PI;
  if ( phi1 < phi3 )
    phi3 = phi3 - 2.0*M_PI;
  if ( phi2 < phi3 )
    phi3 = phi3 - 2.0*M_PI;

  double bz_minus = (phi3 - phi1) / (_zHit[i3]-_zHit[i1]);
  double phi0_minus = phi1 - bz_minus * _zHit[i1];
  double dphi_minus = fabs( bz_minus * _zHit[i2] + phi0_minus - phi2 );

  double bz;
  double phi0;

  if (dphi_plus < dphi_minus) {
    bz = bz_plus;
    phi0 = phi0_plus;
  }
  else {
    bz = bz_minus;
    phi0 = phi0_minus;

  }

  double par_init[5];

  if (parametrisation == 1) {
    par_init[0] = (double)X0;
    par_init[1] = (double)Y0;
    par_init[2] = (double)R0;
    par_init[3] = (double)bz;
    par_init[4] = (double)phi0;
  }
  else if (parametrisation == 2) {
    par_init[0] = (double)X0;
    par_init[1] = (double)Y0;
    par_init[2] = (double)(-phi0/bz);
    par_init[3] = (double)R0;
    par_init[4] = (double)(1/bz);
  }

  else if (parametrisation == 3) {  // parameter vector: (z0,Phi0,omega,d0,tanL)

    // debug
    // std::cout << std::setprecision(6) << "InitFit (X0,Y0,R0,bz,phi0) = " << "(" << X0 << "," << Y0 << "," << R0 << "," << bz << "," << phi0 << ")" << std::endl;

    
    // debug
    /*    
	  X0 = -1205.28;
	  Y0 = 175.317;
	  R0 = 1217.97;
	  bz = 0.00326074;
	  phi0 = -0.144444;
    */ 


    // debug
    // std::cout << std::setprecision(6) << "InitUsed (X0,Y0,R0,bz,phi0) = " << "(" << X0 << "," << Y0 << "," << R0 << "," << bz << "," << phi0 << ")" << std::endl;
        
    double omega = 1/R0*direction;
    double tanL  = (-1.0)*omega/bz;

    double Phi0  = (-1.0)*atan2(X0,Y0);
    
    if (direction == 1) {

      if (tanL >= 0.0) Phi0 += M_PI; // add pi (see LC-DET-2006-004) //  >= or > ?
      else Phi0 -= M_PI; // < or <= ?
      
    }

    //double d0 = R0 - X0/sin(Phi0);
    //double d0 = R0 + Y0/cos(Phi0);

    double d0 = 0.0;
    if (true /*direction != 1*/) d0 = R0 - sqrt(X0*X0 + Y0*Y0);
    // else d0 = R0 + sqrt(X0*X0 + Y0*Y0);



    // double d0 = R0 - ( (X0-Y0)/(sqrt(2.0)*cos(pi/4 - Phi0)) );    
    // double d0 = R0 - ((X0-Y0)/(sin(Phi0)+cos(Phi0)));

    // double Phi0 = asin(X0/(R0-d0));

    
    double z0 = (1/bz)*((-1.0)*phi0+Phi0+(omega*M_PI)/(2.0*fabs(omega)));


    // debug
    /*
      std::cout << std::setprecision(6) << "InitFitCalculated (d0,z0,phi0,omega,tanL) = " << "(" << d0 << "," << z0 << "," << Phi0 << "," << omega << "," << tanL << ")" 
      << "  " << "sign(omega) = " << direction << std::endl;
    */

    // debug        
    /*
      d0    = 0.00016512;
      z0    = 0.000853511;
      Phi0  = 1.11974;
      omega = -4.22171e-05;
      tanL  = -0.33436;
    */


    

    // debug
    // std::cout << std::setprecision(6) << "InitFitUsed (d0,z0,phi0,omega,tanL) = " << "(" << d0 << "," << z0 << "," << Phi0 << "," << omega << "," << tanL << ")" << std::endl;

    par_init[0] = z0;
    par_init[1] = Phi0;
    par_init[2] = omega;
    par_init[3] = d0;
    par_init[4] = tanL;
  }
  else return 1;


  // local variables
  int status = 0;
  int iter = 0;

  int npar = 5; // five parameters to fit
  int ndim = 0;
  if (parametrisation == 1) ndim = 2; // two dependent dimensions 
  else if (parametrisation == 2) ndim = 3; // three dependent dimensions
  else if (parametrisation == 3) ndim = 3; // three dependent dimensions

  else return 1;



  double chi2_nofit = 0.0;
  int iFirst = 1;
  for (int ipoint(0); ipoint < _nHits; ipoint++) {
    double distRPZ[2];
    double Dist = DistanceHelix(_xHit[ipoint],_yHit[ipoint],_zHit[ipoint],
				X0,Y0,R0,bz,phi0,distRPZ);
    double chi2rphi = distRPZ[0]/_exHit[ipoint];
    chi2rphi = chi2rphi*chi2rphi;
    double chi2z = distRPZ[1]/_ezHit[ipoint];
    chi2z = chi2z*chi2z;
    chi2_nofit = chi2_nofit + chi2rphi + chi2z;
    if (Dist > distmax || iFirst == 1) {
      distmax = Dist;
      iFirst = 0;
    }
  }      
  chi2_nofit = chi2_nofit/double(_nHits);

  if ( status_out == 1 ) {
    for (int i(0); i < 5; ++i) {
      parameter[i] = (double)par_init[i];
      dparameter[i] = 0.0;      
    }
    chi2 = chi2_nofit;
    return 0;
  }

  // converging criteria
  const double abs_error = 1e-4;
  const double rel_error = 1e-4;

  gsl_multifit_function_fdf fitfunct;

  const gsl_multifit_fdfsolver_type* T = gsl_multifit_fdfsolver_lmsder;

  gsl_multifit_fdfsolver* s = gsl_multifit_fdfsolver_alloc(T,ndim*_nHits,npar);

  gsl_matrix* covar = gsl_matrix_alloc(npar,npar);   // covariance matrix

  data d;
  d.n = _nHits;
  d.x = &_xHit[0];
  d.y = &_yHit[0];
  d.z = &_zHit[0];
  d.ex = &_exHit[0];
  d.ey = &_eyHit[0];
  d.ez = &_ezHit[0];


  if (parametrisation == 1) {
    fitfunct.f = &functParametrisation1;
    fitfunct.df = &dfunctParametrisation1;
    fitfunct.fdf = &fdfParametrisation1;
    fitfunct.n = ndim*_nHits;
    fitfunct.p = npar;
    fitfunct.params = &d;
  }
  else if (parametrisation == 2) {
    fitfunct.f = &functParametrisation2;
    fitfunct.df = &dfunctParametrisation2;
    fitfunct.fdf = &fdfParametrisation2;
    fitfunct.n = ndim*_nHits;
    fitfunct.p = npar;
    fitfunct.params = &d;
  }
  else if (parametrisation == 3) {
    fitfunct.f = &functParametrisation3;
    fitfunct.df = &dfunctParametrisation3;
    fitfunct.fdf = &fdfParametrisation3;
    fitfunct.n = ndim*_nHits;
    fitfunct.p = npar;
    fitfunct.params = &d;
  }
  else return 1;

  gsl_vector_view pinit = gsl_vector_view_array(par_init,npar);
  gsl_multifit_fdfsolver_set(s,&fitfunct,&pinit.vector);

  // perform fit
  do {
    iter++;
    status = gsl_multifit_fdfsolver_iterate(s);

    if (status) break;
    status = gsl_multifit_test_delta (s->dx, s->x,abs_error,rel_error);

  } while ( status==GSL_CONTINUE && iter < max_iter);

  //fg: jacobian has been dropped from gsl_multifit_fdfsolver in gsl 2:
  gsl_matrix * J = gsl_matrix_alloc(s->fdf->n, s->fdf->p);
  gsl_multifit_fdfsolver_jac( s, J);
  gsl_multifit_covar( J, rel_error, covar );
  //  gsl_multifit_covar (s->J, rel_error, covar);



  chi2 = 0.0;

  if (parametrisation == 1) {
    X0   = (double)gsl_vector_get(s->x,0);
    Y0   = (double)gsl_vector_get(s->x,1);
    R0   = (double)gsl_vector_get(s->x,2);
    bz   = (double)gsl_vector_get(s->x,3);
    phi0 = (double)gsl_vector_get(s->x,4);
  }
  else if (parametrisation == 2) {
    X0   = (double)gsl_vector_get(s->x,0);
    Y0   = (double)gsl_vector_get(s->x,1);
    R0   = (double)gsl_vector_get(s->x,3);
    bz   = (double)(1/gsl_vector_get(s->x,4));
    phi0 = (double)(-gsl_vector_get(s->x,2)/gsl_vector_get(s->x,4));
  }
  else if (parametrisation == 3) { // (parameter vector: (z0,phi0,omega,d0,tanL)

    double z0    = gsl_vector_get(s->x,0);
    double Phi0  = gsl_vector_get(s->x,1);
    double omega = gsl_vector_get(s->x,2);
    double d0    = gsl_vector_get(s->x,3);
    double tanL  = gsl_vector_get(s->x,4);

    X0   = (double)( ( (1/omega) - d0 )*sin(Phi0) );
    Y0   = (double)( (-1)*( (1/omega) - d0 )*cos(Phi0) );
    R0   = (double)( 1/fabs(omega) );
    bz   = (double)( (-1)*(omega/tanL) );
    phi0 = (double)( ( (z0*omega)/tanL ) + Phi0 + ( (omega*M_PI)/(2*fabs(omega)) ) );
  }
  else return 1;

  iFirst = 1;
  double ddmax = 0.0;
  for (int ipoint(0); ipoint < _nHits; ipoint++) {
    double distRPZ[2];
    double Dist = DistanceHelix(_xHit[ipoint],_yHit[ipoint],_zHit[ipoint],
				X0,Y0,R0,bz,phi0,distRPZ);
    double chi2rphi = distRPZ[0]/_exHit[ipoint];
    chi2rphi = chi2rphi*chi2rphi;
    double chi2z = distRPZ[1]/_ezHit[ipoint];
    chi2z = chi2z*chi2z;
    chi2 = chi2 + chi2rphi + chi2z;
    if (Dist > ddmax || iFirst == 1) {
      iFirst = 0;
      ddmax = Dist;
    }
  }
      

  chi2 = chi2/double(_nHits);
  if (chi2 < chi2_nofit) {
    for (int i = 0; i < npar; i++) {
      parameter[i]  = gsl_vector_get(s->x,i);
      dparameter[i] = sqrt(gsl_matrix_get(covar,i,i));
    }    
    distmax = ddmax;
  }
  else {
    chi2 = chi2_nofit;
    for (int i = 0; i < npar; i++) {
      parameter[i] = (double)par_init[i];
      dparameter[i] = 0.0;
    }
  }

  //  if (problematic == 1)
  //    std::cout << "chi2 = " << chi2 << std::endl;

  gsl_multifit_fdfsolver_free(s);
  gsl_matrix_free(covar);
  return 0; 

}



// ##########################################
// #####                                #####
// #####        private methods         #####
// #####                                #####
// ##########################################

//=============================================================================

void ClusterShapes::findElipsoid() {

  /**   Elipsoid parameter calculations see cluster_proper.f  */
  float cx,cy,cz ;
  float dx,dy,dz ;
  float r_hit_max, d_begn, d_last, r_max, proj;
  if (_ifNotInertia == 1) findInertia() ;
  //   Normalize the eigen values of inertia tensor
  float wr1 = sqrt(_ValAnalogInertia[0]/_totAmpl);
  float wr2 = sqrt(_ValAnalogInertia[1]/_totAmpl);
  float wr3 = sqrt(_ValAnalogInertia[2]/_totAmpl);
  _r1 = sqrt(wr2*wr3);                // spatial axis length -- the largest
  _r2 = sqrt(wr1*wr3);                // spatial axis length -- less
  _r3 = sqrt(wr1*wr2);                // spatial axis length -- even more less
  _vol = 4.*M_PI*_r1*_r2*_r3/3.;      // ellipsoid volume
  _r_ave = pow(_vol,1/3);             // average radius  (quibc root)
  _density = _totAmpl/_vol;           // density
  //    _eccentricity = _r_ave/_r1;   // Cluster Eccentricity
  _eccentricity =_analogWidth/_r1;   // Cluster Eccentricity

  // Find Minumal and Maximal Lenght for Principal axis
  r_hit_max = -100000.;
  d_begn    =  100000.;
  d_last    = -100000.;
  cx = _VecAnalogInertia[0] ;
  cy = _VecAnalogInertia[1] ;
  cz = _VecAnalogInertia[2] ;
  for (int i(0); i < _nHits; ++i) {
    dx = _xHit[i] - _xgr;
    dy = _yHit[i] - _ygr;
    dz = _zHit[i] - _zgr;
    r_max = sqrt(dx*dx + dy*dy + dz*dz);;
    if(r_max > r_hit_max) r_hit_max = r_max;
    proj = dx*cx + dy*cy + dz*cz;
    if(proj < d_begn)
      d_begn = proj;
    //            lad_begn = ladc(L)
    if(proj > d_last)
      d_last = proj;
    //            lad_last = ladc(L)
  }
  //        if (r_hit_max > 0.0)
  //	  _r1 = 1.05*r_hit_max; // + 5% of length
  _r1_forw = fabs(d_last);
  _r1_back = fabs(d_begn);
}

//=============================================================================

void ClusterShapes::findGravity() {

  _totAmpl = 0. ;
  for (int i(0); i < 3; ++i) {
    _analogGravity[i] = 0.0 ;
  }
  for (int i(0); i < _nHits; ++i) {
    _totAmpl+=_aHit[i] ;
    _analogGravity[0]+=_aHit[i]*_xHit[i] ;
    _analogGravity[1]+=_aHit[i]*_yHit[i] ;
    _analogGravity[2]+=_aHit[i]*_zHit[i] ;
  }
  for (int i(0); i < 3; ++i) {
    _analogGravity[i]/=_totAmpl ;
  }
  _xgr = _analogGravity[0];
  _ygr = _analogGravity[1];
  _zgr = _analogGravity[2];
  _ifNotGravity = 0;
}

//=============================================================================

void ClusterShapes::findInertia() {

  double aIne[3][3];
  //  float radius1;
  float radius2 = 0.0;

  findGravity();

  for (int i(0); i < 3; ++i) {
    for (int j(0); j < 3; ++j) {
      aIne[i][j] = 0.0;
    }
  }

  for (int i(0); i < _nHits; ++i) {
    float dX = _xHit[i] - _analogGravity[0];
    float dY = _yHit[i] - _analogGravity[1];
    float dZ = _zHit[i] - _analogGravity[2];
    aIne[0][0] += _aHit[i]*(dY*dY+dZ*dZ);
    aIne[1][1] += _aHit[i]*(dX*dX+dZ*dZ);
    aIne[2][2] += _aHit[i]*(dX*dX+dY*dY);
    aIne[0][1] -= _aHit[i]*dX*dY;
    aIne[0][2] -= _aHit[i]*dX*dZ;
    aIne[1][2] -= _aHit[i]*dY*dZ;
  }

  for (int i(0); i < 2; ++i) {
    for (int j = i+1; j < 3; ++j) {
      aIne[j][i] = aIne[i][j];
    }
  }
  //****************************************
  // analog Inertia
  //****************************************

  gsl_matrix_view aMatrix = gsl_matrix_view_array((double*)aIne,3,3);
  gsl_vector* aVector = gsl_vector_alloc(3);
  gsl_matrix* aEigenVec = gsl_matrix_alloc(3,3);
  gsl_eigen_symmv_workspace* wa = gsl_eigen_symmv_alloc(3);
  gsl_eigen_symmv(&aMatrix.matrix,aVector,aEigenVec,wa);
  gsl_eigen_symmv_free(wa);
  gsl_eigen_symmv_sort(aVector,aEigenVec,GSL_EIGEN_SORT_ABS_ASC);

  for (int i(0); i < 3; i++) {
    _ValAnalogInertia[i] = gsl_vector_get(aVector,i);
    for (int j(0); j < 3; j++) {
      _VecAnalogInertia[i+3*j] = gsl_matrix_get(aEigenVec,i,j);
    }
  }

  // Main principal points away from IP

  _radius = 0.;
  radius2 = 0.;

  for (int i(0); i < 3; ++i) {
    _radius += _analogGravity[i]*_analogGravity[i];
    radius2 += (_analogGravity[i]+_VecAnalogInertia[i])*(_analogGravity[i]+_VecAnalogInertia[i]);
  }

  if ( radius2 < _radius) {
    for (int i(0); i < 3; ++i)
      _VecAnalogInertia[i] = - _VecAnalogInertia[i];
  }

  _radius = sqrt(_radius);
  _ifNotInertia = 0;

  // The final job
  findWidth();
  findElipsoid();

  gsl_vector_free(aVector);
  gsl_matrix_free(aEigenVec);

}

//=============================================================================

void ClusterShapes::findWidth() {

  float dist = 0.0;
  if (_ifNotInertia == 1)  findInertia() ;
  _analogWidth  = 0.0 ;
  for (int i(0); i < _nHits; ++i) {
    dist = findDistance(i) ;
    _analogWidth+=_aHit[i]*dist*dist ;
  }
  _analogWidth  = sqrt(_analogWidth / _totAmpl) ;
  _ifNotWidth = 0 ;
}

//=============================================================================

float ClusterShapes::findDistance(int i) {

  float cx = 0.0;
  float cy = 0.0;
  float cz = 0.0;
  float dx = 0.0;
  float dy = 0.0;
  float dz = 0.0;
  cx = _VecAnalogInertia[0] ;
  cy = _VecAnalogInertia[1] ;
  cz = _VecAnalogInertia[2] ;
  dx = _analogGravity[0] - _xHit[i] ;
  dy = _analogGravity[1] - _yHit[i] ;
  dz = _analogGravity[2] - _zHit[i] ;
  float tx = cy*dz - cz*dy ;
  float ty = cz*dx - cx*dz ;
  float tz = cx*dy - cy*dx ;
  float tt = sqrt(tx*tx+ty*ty+tz*tz) ;
  float ti = sqrt(cx*cx+cy*cy+cz*cz) ;
  float f = tt / ti ;
  return f ;
}
/**
   Function sdist(xp,yp,zp,cx,cy,cz,xv,yv,zv)
   c----------------------------------------------------------------------
   c        Distance from line to point
   c       xp, yp, zp -- point is at the line
   c       xv, yv, zv -- point is out of line
   ********************************************************************
   *     Last update     V.L.Morgunov     08-Apr-2002                 *
   ********************************************************************
   real xp,yp,zp,cx,cy,cz,xv,yv,zv,t1,t2,t3,tt,sdist

   t1 = cy*(zp-zv)-cz*(yp-yv)
   t2 = cz*(xp-xv)-cx*(zp-zv)
   t3 = cx*(yp-yv)-cy*(xp-xv)
   tt = sqrt(cx**2+cy**2+cz**2)
   sdist = sqrt(t1**2+t2**2+t3**2)/tt

   return
   end
*/

//=============================================================================

float ClusterShapes::vecProduct(float * x1, float * x2) {

  float x1abs(0.);
  float x2abs(0.);
  float prod(0.);

  for (int i(0); i < 3; ++i) {
    x1abs += x1[i]*x1[i];
    x2abs += x2[i]*x2[i];
    prod  += x1[i]*x2[i];
  }


  x1abs = sqrt(x1abs);
  x2abs = sqrt(x2abs);

  if (x1abs > 0.0 && x2abs > 0.0) {
    prod = prod/(x1abs*x2abs);
  }
  else {
    prod = 0.;
  }

  return prod;

}

//=============================================================================

float ClusterShapes::vecProject(float * x, float * axis) {
  float axisabs(0.);
  float prod(0.);
  for (int i(0); i < 3; ++i) {
    axisabs += axis[i]*axis[i];
    prod  += x[i]*axis[i];
  }
  axisabs = sqrt(axisabs);
  if (axisabs > 0.0 ) {
    prod = prod/axisabs;
  }
  else {
    prod = 0.;
  }
  return prod;
}

//=============================================================================

double ClusterShapes::DistanceHelix(double x, double y, double z, double X0, double Y0,
				    double R0, double bz, double phi0, double * distRPZ) {

  double phi  = atan2(y-Y0,x-X0);
  double R    = sqrt( (y-Y0)*(y-Y0) + (x-X0)*(x-X0) );
  double dXY2 = (R-R0)*(R-R0);
  double _const_2pi = 2.0*M_PI;
  double xN = (bz*z + phi0 - phi)/_const_2pi;

  int n1 = 0;
  int n2 = 0;
  int nSpirals = 0;

  if (xN > 0) {
    n1 = (int)xN;
    n2 = n1 + 1;
  }
  else {
    n1 = (int)xN - 1;
    n2 = n1 + 1;
  }

  if (fabs(n1-xN) < fabs(n2-xN)) {
    nSpirals = n1;
  }
  else {
    nSpirals = n2;
  }

  double dZ = (phi + _const_2pi*nSpirals - phi0)/bz - z;
  double dZ2 = dZ*dZ;

  distRPZ[0] = sqrt(dXY2);
  distRPZ[1] = sqrt(dZ2);

  return sqrt(dXY2 + dZ2);

}

//=============================================================================

int ClusterShapes::transformToEigensystem(float* xStart, int& index_xStart, float* X0, float* Rm) {
  
  if (_ifNotInertia == 1) findInertia();

  float MainAxis[3];
  float MainCentre[3];


  MainAxis[0] = _VecAnalogInertia[0];
  MainAxis[1] = _VecAnalogInertia[1];
  MainAxis[2] = _VecAnalogInertia[2];
  MainCentre[0] = _analogGravity[0];
  MainCentre[1] = _analogGravity[1];
  MainCentre[2] = _analogGravity[2];

  //analoginertia looks something wrong!
  //change it to the direction of CoG
  //float ll=sqrt(MainCentre[0]*MainCentre[0]+MainCentre[1]*MainCentre[1]+MainCentre[2]*MainCentre[2]);
  //MainAxis[0]=MainCentre[0]/ll;
  //MainAxis[1]=MainCentre[1]/ll;
  //MainAxis[2]=MainCentre[2]/ll;

  int ifirst = 0;
  float xx[3];
  float prodmin = 1.0e+100;
  int index = 0;

  for (int i(0); i < _nHits; ++i) {
    xx[0] = _xHit[i] - MainCentre[0];
    xx[1] = _yHit[i] - MainCentre[1];
    xx[2] = _zHit[i] - MainCentre[2];
    float prod = vecProject(xx,MainAxis);
    if (ifirst == 0 || prod < prodmin) {
      ifirst = 1;
      prodmin = prod;
      index = i;
    }
  }
  xStart[0] = MainCentre[0] + prodmin*MainAxis[0];
  xStart[1] = MainCentre[1] + prodmin*MainAxis[1];
  xStart[2] = MainCentre[2] + prodmin*MainAxis[2];
  index_xStart = index;
  
  float l=sqrt(MainAxis[0]*MainAxis[0]+MainAxis[1]*MainAxis[1]+MainAxis[2]*MainAxis[2]);
  //float ll=sqrt(xStart[0]*xStart[0]+xStart[1]*xStart[1]+xStart[2]*xStart[2]);
  //std::cout << "xstart: " << index_xStart << " " << prodmin << " " << xStart[0] << std::endl;

  //calculate the surface of hcal
  float ecalrad=2.058000000e+03;   //in mm 
  float plugz=2.650000000e+03;   //in mm 
  
  float tmpcos=MainAxis[2]/l;
  float tmpsin=sqrt(MainAxis[0]*MainAxis[0]+MainAxis[1]*MainAxis[1])/l;
  float detend=0.0;
  if(fabs(xStart[2])<2.450000000e+03){   //if in the barrel
    detend=(ecalrad-sqrt(xStart[0]*xStart[0]+xStart[1]*xStart[1]))/tmpsin;
  }else{  //if in plug
    detend=(plugz-fabs(xStart[2]))/fabs(tmpcos);
  }
  if(detend<0.0) detend=0.0;

  // std::cout << "check length: " << detend << " "
  //           << xStart[0]/ll << " " << xStart[1]/ll << " " << xStart[2]/ll << " " 
  //           << MainAxis[0]/l << " " << MainAxis[1]/l << " " << MainAxis[1]/l << std::endl;
  
  for (int i(0); i < _nHits; ++i) {
    xx[0] = _xHit[i] - xStart[0];
    xx[1] = _yHit[i] - xStart[1];
    xx[2] = _zHit[i] - xStart[2];
    float xx2(0.);
    for (int j(0); j < 3; ++j) xx2 += xx[j]*xx[j];
    
    _xl[i] = 0.001 + vecProject(xx,MainAxis);
    _xt[i] = sqrt(std::max(0.0,xx2 + 0.01 - _xl[i]*_xl[i]));
    //    std::cout << i << " " << _xl[i] << " " << _xt[i] << " " << _aHit[i] << " "
    //              << std::endl;
  }
  
  //first, check ecal and solve wrong behaviour
  //std::cout << "param: " << X0[0] << " " << X0[1] << " " << Rm[0] << " " << Rm[1] << std::endl;
  for (int i = 0; i < _nHits; ++i) { 
    //if(_types[i]==0 || _types[i]==3){   //if the hit is in Ecal
    _t[i] = _xl[i]/X0[0];
    _s[i] = _xt[i]/Rm[0];
    if(detend<_xl[i]){
      //std::cout << "something wrong!! " << detend << " " << _xl[i] << " " << _t[i] << std::endl;
      //detend = _xl[i]; //to avoid wrong behaviour
    }
    //}
  }
  
  //second, check hcal
  /*for (int i = 0; i < _nHits; ++i) { 
    if(_types[i]==1 || _types[i]==4){   //if the hit is in Hcal
    if(_xl[i]>detend){
    _t[i] = detend/X0[0]+(_xl[i]-detend)/X0[1];
    _s[i] = _xt[i]/Rm[1];
    }else{
    _t[i] = _xl[i]/X0[0];
    _s[i] = _xt[i]/Rm[0];
    }   
    }
    // std::cout << _types[i] << " " << _xl[i] << " "
    //           << _xl[i]+sqrt(xStart[0]*xStart[0]+xStart[1]*xStart[1]+xStart[2]*xStart[2])  << " "
    //           << _t[i] << " " << _s[i] << std::endl;
    }*/
  
  _ifNotEigensystem = 0;
  
  return 0; // no error messages at the moment

}

//=============================================================================

float ClusterShapes::calculateChi2Fit3DProfileSimple(float a, float b, float c,
						     float d) {

  // ClusterShapes::transformToEigensystem needs to be executed before

  float chi2 = 0.0;
  float Ampl = 0.0; 

  for (int i(0); i < _nHits; ++i) {
    // old definition of Ampl and chi2
    //Ampl = a*(float)pow(_xl[i],b)*exp(-c*_xl[i]-d*_xt[i]); 
    //chi2 += ((Ampl - _aHit[i])*(Ampl - _aHit[i]))/(_aHit[i]*_aHit[i]);

    Ampl = a*(float)pow(_xl[i],b)*exp(-c*_xl[i]-d*_xt[i]);
    chi2 += (log(_aHit[i]) - log(Ampl))*(log(_aHit[i]) - log(Ampl));
  }

  chi2 = chi2/std::max((float)1.0,(float)(_nHits - 4));

  return chi2;

}

//=============================================================================

float ClusterShapes::calculateChi2Fit3DProfileAdvanced(float E0, float a, float b,
						       float d, float t0) {

  // ClusterShapes::transformToEigensystem needs to be executed before

  float chi2  = 0.0;
  float Ampl  = 0.0;
  float shift = 0.0;

  for (int i(0); i < _nHits; ++i) {

    shift = _t[i]-t0;

    if (shift <= 0) Ampl = 0.0;
    else {
      Ampl = E0 * b * invG(a) * pow(b*(shift),a-1) 
	* exp(-b*(shift)) * exp(-d*_s[i]);
    }

    // debug 
    /*
      std::cout << "OUT : " << Ampl << "  " << E0  << "  " << a  << "  " << b  << "  " 
      << d  << "  " << t0  << "  " << _t[i] << "  " << invG(a)  << "  "
      << pow(b*(_t[i]-t0),a-1)  << "  " << exp(-b*(_t[i]-t0))  << "  " 
      << exp(-d*_s[i]) 
      << std::endl;
    */

    chi2 += ((Ampl - _aHit[i])*(Ampl - _aHit[i]))/(_aHit[i]*_aHit[i]);
    //chi2 += (log(_aHit[i]) - log(Ampl))*(log(_aHit[i]) - log(Ampl));
    //chi2 += log((Ampl - _aHit[i])*(Ampl - _aHit[i]))/log(_aHit[i]*_aHit[i]);

  }
  chi2 = chi2/std::max((float)1.0,(float)(_nHits - 4));

  return chi2;

}

//=============================================================================

int ClusterShapes::fit3DProfileSimple(float& chi2, float& a, float& b, float& c,
				      float& d) {

  // Fit function: _aHit[i] = a * pow(_xl[i],b) * exp(-c*_xl[i]) * exp(-d*_xt[i])

  float Slnxl(0.);
  float Sxl(0.);
  float Sxt(0.);
  float Sln2xl(0.);
  float Sxllnxl(0.);
  float Sxtlnxl(0.);
  float Sxlxl(0.);
  float Sxlxt(0.);
  float Sxtxt(0.);
  float SlnA(0.);
  float SlnAlnxl(0.);
  float SlnAxl(0.);
  float SlnAxt(0.);

  // for a quadratic matrix
  for (int i = 0; i < _nHits; i++) {
    Slnxl += log(_xl[i]);
    Sxl += _xl[i];
    Sxt += _xt[i];
    Sln2xl += log(_xl[i])*log(_xl[i]);
    Sxllnxl += _xl[i]*log(_xl[i]);
    Sxtlnxl += _xt[i]*log(_xl[i]);
    Sxlxl += _xl[i]*_xl[i];
    Sxlxt += _xl[i]*_xt[i];
    Sxtxt += _xt[i]*_xt[i];
    SlnA += log(_aHit[i]);
    SlnAlnxl += log(_aHit[i])*log(_xl[i]);
    SlnAxl += log(_aHit[i])*_xl[i];
    SlnAxt += log(_aHit[i])*_xt[i]; 
  }
  // create system of linear equations, written as Ae = z

  gsl_matrix* A = gsl_matrix_alloc(4,4);
  gsl_vector* z = gsl_vector_alloc(4);
  gsl_vector* e = gsl_vector_alloc(4);
  
  // initialise matrix and vectors
  
  gsl_matrix_set(A,0,0,_nHits);
  gsl_matrix_set(A,0,1,Slnxl);
  gsl_matrix_set(A,0,2,-Sxl);
  gsl_matrix_set(A,0,3,-Sxt);
  
  gsl_matrix_set(A,1,0,Slnxl);
  gsl_matrix_set(A,1,1,Sln2xl);
  gsl_matrix_set(A,1,2,-Sxllnxl);
  gsl_matrix_set(A,1,3,-Sxtlnxl);
  
  gsl_matrix_set(A,2,0,-Sxl);
  gsl_matrix_set(A,2,1,-Sxllnxl);
  gsl_matrix_set(A,2,2,Sxlxl);
  gsl_matrix_set(A,2,3,Sxlxt);

  gsl_matrix_set(A,3,0,-Sxt);
  gsl_matrix_set(A,3,1,-Sxtlnxl);
  gsl_matrix_set(A,3,2,Sxlxt);
  gsl_matrix_set(A,3,3,Sxtxt);

  gsl_vector_set(z,0,SlnA);
  gsl_vector_set(z,1,SlnAlnxl);
  gsl_vector_set(z,2,-SlnAxl);
  gsl_vector_set(z,3,-SlnAxt);

  gsl_linalg_HH_solve(A,z,e);

  a = exp(gsl_vector_get(e,0));
  b = gsl_vector_get(e,1);
  c = gsl_vector_get(e,2);
  d = gsl_vector_get(e,3);

  chi2 = calculateChi2Fit3DProfileSimple(a,b,c,d);
  
  gsl_matrix_free(A);
  gsl_vector_free(z);
  gsl_vector_free(e);

  int result = 0;  // no error handling at the moment
  return result;

}

//=============================================================================

int ClusterShapes::fit3DProfileAdvanced(float& chi2, double* par_init, double* par,
					int npar, float* t, float* s, float* E, 
					float E0) {

  // local variables
  int status = 0;
  int iter = 0;
  int max_iter = 1000;


  // converging criteria
  const double abs_error = 0.0;
  const double rel_error = 1e-1;



  gsl_multifit_function_fdf fitfunct;

  const gsl_multifit_fdfsolver_type* T = gsl_multifit_fdfsolver_lmsder;

  //std::cout << T << " " << _nHits << " " << npar << std::endl;

  gsl_multifit_fdfsolver* Solver = gsl_multifit_fdfsolver_alloc(T,_nHits,npar);