// HitXYPlane2Generator.cpp

#include "HitXYPlane2Generator.h"
#include <cassert>
#include "HitXYPlane2.h"
#include "trfbase/VTrack.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"

using namespace trf;

enum {
 IV = ClusXYPlane2::IV,
 IZ = ClusXYPlane2::IZ
};

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

// output stream

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

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

// Constructor.

HitXYPlane2Generator::
HitXYPlane2Generator(double dist, double phi, const HitError& dhm)
: HitGenerator(), _sxyp(dist,phi), _dhm(dhm){
  assert( _dhm(IV,IV) >= 0.0 && _dhm(IZ,IZ) >=0. );

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

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

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

// Constructor with seed.

HitXYPlane2Generator::HitXYPlane2Generator(
                 double dist, double phi, const HitError& dhm, 
                 long seed)
: HitGenerator(seed), _sxyp(dist,phi), _dhm(dhm) {
  assert( _dhm(IV,IV) >= 0.0 && _dhm(IZ,IZ) >=0. ); 

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

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

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

// Copy constructor.

HitXYPlane2Generator::HitXYPlane2Generator(const HitXYPlane2Generator& rhs)
: HitGenerator(rhs), _sxyp(rhs._sxyp), _dhm(rhs._dhm) {
  assert( _dhm(IV,IV) >= 0.0 && _dhm(IZ,IZ) >=0. ); 

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

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

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

// Destructor.

HitXYPlane2Generator::~HitXYPlane2Generator() { }

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

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

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

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

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

  // calculate axy.
  double v_track = trv.get_vector()(SurfXYPlane::IV);
  double z_track = trv.get_vector()(SurfXYPlane::IZ);
  double a1  = _dhm(IV,IV);
  double a2  = _dhm(IZ,IZ);
  double a12 = _dhm(IV,IZ);
  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 vv = cphi*cphi*a1 + sphi*sphi*a2 + 2*cphi*sphi*a12;
  double zz = 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(vv*zz - (a1*a2 - a12*a12)) < 1.e-10 );

  // Very important test

  assert ( vv > 0.);
  assert ( zz > 0.);

  double dvv = gauss()/sqrt(vv);
  double dzz = gauss()/sqrt(zz);
  double b1 = cphi*dvv - sphi*dzz;
  double b2 = cphi*dzz + sphi*dvv;

  double av  = v_track + (a1*b1+a12*b2);
  double az  = z_track + (a2*b2+a12*b1);  

  // construct cluster
  double dist   = _sxyp.get_parameter(SurfXYPlane::DISTNORM);
  double phinor = _sxyp.get_parameter(SurfXYPlane::NORMPHI);
  pclu = new ClusXYPlane2(dist,phinor,av,az,_dhm );
  return pclu;
}

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