// HitZPlane2Generator.cpp

#include "HitZPlane2Generator.h"
#include <cassert>
#include "HitZPlane2.h"
#include "trfbase/VTrack.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"

using namespace trf;

enum { 
  IX = ClusZPlane2::IX,
  IY = ClusZPlane2::IY
};

//**********************************************************************

// output stream

void HitZPlane2Generator::ostr(ostream& stream) const {
  stream << begin_object ;
  stream << "Surface: " << _szp << new_line;
  stream << "Measurement error (dhm): " << _dhm << new_line;
  RandomGenerator::raw_ostr(stream);
  stream << end_object;
}

//**********************************************************************

// Constructor.

HitZPlane2Generator::
HitZPlane2Generator(double zpos, const HitError& dhm)
: HitGenerator(), _szp(zpos), _dhm(dhm){
  assert( _dhm(IX,IX) >= 0.0 && _dhm(IY,IY) >= 0.0);

  // check that determinant of _dhm is positive
  assert( _dhm(IX,IX)*_dhm(IY,IY) - _dhm(IX,IY)*_dhm(IX,IY) >= 0.0);

  assert( _dhm.size() == 2);
}

//**********************************************************************

// Constructor with seed.

HitZPlane2Generator::HitZPlane2Generator(
                 double zpos,const HitError& dhm, 
                 long seed)
: HitGenerator(seed), _szp(zpos), _dhm(dhm) {
  assert( _dhm(IX,IX) >= 0.0 && _dhm(IY,IY) >= 0.0);

  // check that determinant of _dhm is positive
  assert( _dhm(IX,IX)*_dhm(IY,IY) - _dhm(IX,IY)*_dhm(IX,IY) >= 0.0);

  assert( _dhm.size() == 2);
}

//**********************************************************************

// Copy constructor.

HitZPlane2Generator::HitZPlane2Generator(const HitZPlane2Generator& rhs)
: HitGenerator(rhs), _szp(rhs._szp), _dhm(rhs._dhm) {
  assert( _dhm(IX,IX) >= 0.0 && _dhm(IY,IY) >= 0.0 );

  // check that determinant of _dhm is positive
  assert( _dhm(IX,IX)*_dhm(IY,IY) - _dhm(IX,IY)*_dhm(IX,IY) >= 0.0);

  assert( _dhm.size() == 2 );
}

//**********************************************************************

// Destructor.

HitZPlane2Generator::~HitZPlane2Generator() { }

//**********************************************************************

// Generate a new cluster.
// Caller must delete.
// Return 0 for failure.

Cluster* HitZPlane2Generator::new_cluster(const VTrack& trv) {

  // Create cluster pointer.
  Cluster* pclu = 0;

  // Check track has been propagated to the surface.
  assert( _szp.pure_equal( *trv.get_surface() ) );
  if ( ! _szp.pure_equal( *trv.get_surface() ) ) return pclu;

  // calculate axy.
  double x_track = trv.get_vector()(SurfZPlane::IX);
  double y_track = trv.get_vector()(SurfZPlane::IY);
  double a1  = _dhm(IX,IX);
  double a2  = _dhm(IY,IY);
  double a12 = _dhm(IX,IY);
  double phi;
  if(a12 == 0.)
  {
   phi = 0.;
  }
  else
  {
   if(a1 == a2 )
   { 
    phi = PI/4.;
   }
   else 
   {
    phi = 0.5*atan(2*a12/(a1-a2));
   }
  }
  double cphi= cos(phi);
  double sphi= sin(phi);
  double xx = cphi*cphi*a1 + sphi*sphi*a2 + 2*cphi*sphi*a12;
  double yy = cphi*cphi*a2 + sphi*sphi*a1 - 2*cphi*sphi*a12;

  // simple check. To be removed later

  assert( fabs(cphi*cphi*a12 - sphi*sphi*a12 + cphi*sphi*(a2-a1)) < 1.e-10);
  assert( fabs(xx*yy - (a1*a2 - a12*a12)) < 1.e-10 );

  // Very important test

  assert ( xx > 0.);
  assert ( yy > 0.);

  double dxx = gauss()/sqrt(xx);
  double dyy = gauss()/sqrt(yy);
  double b1 = cphi*dxx - sphi*dyy;
  double b2 = cphi*dyy + sphi*dxx;

  double ax  = x_track + (a1*b1+a12*b2);
  double ay  = y_track + (a2*b2+a12*b1);  

  // construct cluster
  double zpos = _szp.get_parameter(SurfZPlane::ZPOS);
  pclu = new ClusZPlane2(zpos,ax,ay,_dhm );
  return pclu;
}

//**********************************************************************