// SurfDCA.cpp

#include "SurfDCA.h"
#include <cmath>
#include <assert.h>
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "spacegeom/SpacePath.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"
#include "trfbase/CrossStat.h"
#include "trfbase/VTrack.h"

#ifndef DEFECT_CMATH_NOT_STD
using std::atan;
#endif

using namespace trf;

//**********************************************************************
// Free functions.
//**********************************************************************

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "SurfDCA" );
  double x=0.,y=0.;
  if( data.has("x") && data.has("y") ) {
    x=data.get_double("x");
    y=data.get_double("y");
  }
  return ObjPtr( new SurfDCA(x,y) );
}

}

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

// Constructor.

SurfDCA::SurfDCA(double x, double y) : _x(x), _y(y) { }

SurfDCA::SurfDCA(const SurfDCA& dca) : _x(dca._x), _y(dca._y) { }

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

// Destructor.

SurfDCA::~SurfDCA() { }

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

// Virtual constructor.

SurfDCA* SurfDCA::new_pure_surface() const {
  return new SurfDCA(_x,_y);
}

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

// Return the creator.

ObjCreator SurfDCA::get_creator() {
  return create;
}

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

// Write the object data.

ObjData SurfDCA::write_data() const {
  ObjData data( get_type_name() );
  if( is_zero(_x) && is_zero(_y) )
    return data;
  data.add_double("x",get_x());
  data.add_double("y",get_y());
  return data;
}

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

// Return the surface parameter.

double SurfDCA::get_parameter(int ipar) const {
  assert( ipar == IX || ipar == IY );
  if( ipar == IX ) return _x;
  else return _y;
}

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

// Output stream operator.

void SurfDCA::ostr( ostream& stream ) const {
  stream << begin_object;
  stream << "DCA surface";
  stream << " to ("<<_x<<','<<_y<<")";
  stream << end_object;
}

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

// Equality.

bool SurfDCA::safe_pure_equal(const Surface& srf) const {
  return is_equal(_x,srf.get_parameter(IX)) && is_equal(_y,srf.get_parameter(IY));
}

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

// Ordering.

bool SurfDCA::safe_pure_less_than(const Surface& srf) const {

  if( safe_pure_equal(srf) ) return false;

  if( is_equal(_x,srf.get_parameter(IX)) )
    return _y < srf.get_parameter(IY);

  return _x < srf.get_parameter(IX);
}

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

// Return the crossing status for a track vector without error.

CrossStat SurfDCA::pure_status(const VTrack& trv) const {
  // If the track surface is the same as this, return at.
  SurfacePtr psrf = trv.get_surface();
  if ( psrf==this || pure_equal(*psrf) ) return CrossStat(CrossStat::AT);
  //  if ( psrf==this ) return CrossStat(CrossStat::AT);
  // Otherwise extract the space vector and set flags using alpha.
  const SpacePath& sp = trv.space_vector();
  CartesianPath point(sp.x()-_x,sp.y()-_y,sp.z(),sp.dx(),sp.dy(),sp.dz());
  double   vrtrk = point.v_rxy();
  double vphitrk = point.v_phi();
  double tan_a = vphitrk/vrtrk;
  double atrk = atan(vphitrk/vrtrk);
  if ( (tan_a > 0.0) && (vrtrk < 0.0) ) {
    atrk = atan(vphitrk/vrtrk) - PI;
  }
  if ( (tan_a < 0.0) && (vrtrk < 0.0) ) {
    atrk = atan(vphitrk/vrtrk) + PI;
  }
  double asrf = PI2;
  double prec = CrossStat::get_static_precision();
  if ( abs(atrk-asrf) < prec ) return CrossStat(CrossStat::ON);
  if ( abs(atrk) < asrf ) return CrossStat(CrossStat::OUTSIDE);
  return CrossStat(CrossStat::INSIDE);
}

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

// Return the difference between two track vectors.

TrackVector SurfDCA::vec_diff( const TrackVector& vec1, 
                               const TrackVector& vec2 ) const {
 TrackVector diff = vec1;
 diff -= vec2;
 diff(IPHID) = fmod2( diff(IPHID), TWOPI );
 return diff;
} 

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

// Return the space point for a track.

SpacePoint SurfDCA::space_point( const TrackVector& vec ) const {
  double r    = abs(vec(0));
  double z    = vec(1);
  double sign = 0.0;
  double phi  = 0.0;
  if ( vec(0) != 0.0 ) {
    sign = vec(0)/abs(vec(0));
    phi  = vec(2)-(sign*PI2);
    phi  = fmod2( phi, TWOPI );
  }
  return CartesianPoint( r*cos(phi)+_x, r*sin(phi) +_y, z );
}

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

// Return the space vector for a track.
//     dr/ds = cos(lambda)*cos(alpha)
// r*dphi/ds = cos(lambda)*sin(alpha)
//     dz/ds = sin(lambda)

SpacePath SurfDCA::space_vector( const TrackVector& vec,
                                 TrackSurfaceDirection dir ) const {
  double r    = abs(vec(0));
  double z    = vec(1);
  double sign = 1.0;
  double phi  = 0.0;
  if ( vec(0) != 0.0 ) {
    sign = vec(0)/abs(vec(0));
    phi  = vec(2)-(sign*PI2);
    phi  = fmod2( phi, TWOPI );
  }
  double tlam = vec(3);
  double salf = sign;
  double calf = 0.0;
  double slam = tlam/sqrt(1.0+tlam*tlam);
  double clam = 1.0/sqrt(1.0+tlam*tlam);
  double dr_ds     = clam*calf;
  double r_dphi_ds = clam*salf;
  double dz_ds     = slam;
  CylindricalPath sp(r , phi ,z , dr_ds, r_dphi_ds, dz_ds);
  return CartesianPath(sp.x()+_x,sp.y()+_y,sp.z(),sp.dx(),sp.dy(),sp.dz());
}

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

// Return q/p.

double SurfDCA::qoverp(const TrackVector& vec) const {
  double tlam = vec(ITLM);
  double clam = 1.0/sqrt(1.0+tlam*tlam);
  assert( clam != 0.0 );
  return vec(IQPT)*clam;
}

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

// Return the direction - always Forward.

TrackSurfaceDirection
SurfDCA::get_direction(const TrackVector& vec) const {
  return TSD_FORWARD;
}

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