// SurfXYPlane.cpp

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

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::fabs;
using std::sqrt;
using std::cos;
using std::sin;
using std::atan2;
using std::asin;
#endif

using namespace trf;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "SurfXYPlane" );
  double dist = data.get_double("dist");
  double phi = data.get_double("phi");
  return ObjPtr( new SurfXYPlane(dist,phi) );
}

}

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

// Output stream operator.

void SurfXYPlane::ostr( ostream& stream ) const {
 stream << begin_object ;
 raw_ostr(stream);
 stream << end_object;
}

void SurfXYPlane::raw_ostr( ostream& stream ) const {
 stream << "XY plane ";
 stream << "at phi = " << _normphi;
 stream << " and distance = "<< _distnorm;
}

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

// Equality.

bool SurfXYPlane::safe_pure_equal( const Surface& srf ) const {
 double s_phi = ((SurfXYPlane *)&srf)->_normphi;
 double s_dist= ((SurfXYPlane *)&srf)->_distnorm;
 return ( fabs(s_phi - _normphi)< 1e-7 && fabs(s_dist-_distnorm) < 1e-7);
}

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

// Comparison

bool SurfXYPlane::safe_pure_less_than(const Surface& srf) const {
  double s_phi = ((const SurfXYPlane& )srf)._normphi;
  double s_dist= ((const SurfXYPlane& )srf)._distnorm;
  return   ( _distnorm < s_dist - 1e-7 ) || 
    ( fabs(_distnorm - s_dist) < 1e-7  && _normphi < s_phi -1e-7 );
}

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

// Constructor.

SurfXYPlane::SurfXYPlane(double distnorm, double normphi )
{
  // Check if phi is between 0 and 2pi
  assert( normphi<TWOPI && normphi>= -1.e-9 );
  assert( distnorm >= 0.);
 _normphi = normphi;
 _distnorm = distnorm;
}
//**********************************************************************

// Destructor.

SurfXYPlane::~SurfXYPlane() { }

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

// Virtual constructor.

SurfXYPlane* SurfXYPlane::new_pure_surface( ) const {
  return new SurfXYPlane(_distnorm,_normphi);
}

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

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

CrossStat SurfXYPlane::pure_status( const VTrack& trv ) const {

  // If the track surface is the same as this, return at
  const SurfacePtr& psrf = trv.get_surface();
  if ( psrf==this || pure_equal(*psrf) ) return CrossStat(CrossStat::AT);
  // Otherwise extract the space point and set flags using u.
  double xtrk = trv.space_point().x();
  double ytrk = trv.space_point().y();
  double cphi = cos(_normphi);
  double sphi = sin(_normphi);
  double utrk = xtrk*cphi + ytrk*sphi;
  double usrf = _distnorm;
  double prec = CrossStat::get_static_precision();
  if ( abs(utrk-usrf) < prec ) return CrossStat(CrossStat::ON);
  if ( utrk > usrf ) return CrossStat(CrossStat::OUTSIDE);
  return CrossStat(CrossStat::INSIDE);
}

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

// Return the surface parameter.
double SurfXYPlane::get_parameter(int ipar) const {
  if ( ipar == NORMPHI ) return _normphi;
  if ( ipar == DISTNORM) return _distnorm;
  return 0.0;
}

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

// Return the difference between two track vectors.

TrackVector SurfXYPlane::vec_diff( const TrackVector& vec1, 
                                     const TrackVector& vec2 ) const {
 TrackVector diff = vec1;
 diff -= vec2;
 return diff;
} 

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

// Return the space point for a track.

SpacePoint SurfXYPlane::space_point( const TrackVector& vec ) const {
  double u   = _distnorm;
  double cphi = cos(_normphi);
  double sphi = sin(_normphi);
  double x =  u*cphi - vec(IV)*sphi;
  double y =  u*sphi + vec(IV)*cphi;
  return CartesianPoint( x , y, vec(IZ) );
}

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

// Return the space vector for a track. (v,z,dv/du,dz/du,q/p)
// du/ds = 1/sqrt(1+dv/du**2+dz/du**2)
// dv/ds = dv/du*du/ds
// dz/ds = dz/du*du/ds
// phi is positive counterclockwise from x to u

SpacePath SurfXYPlane::space_vector(const TrackVector& vec,
                                    TrackSurfaceDirection dir) const {
  double u     = _distnorm;
  double v     = vec(IV);
  double z     = vec(IZ);
  double dv_du = vec(IDVDU);
  double dz_du = vec(IDZDU);

  double cphi =  cos(_normphi);
  double sphi = sin(_normphi);

  double x =  u*cphi - v*sphi;
  double y =  u*sphi + v*cphi;

  double du_ds = 1./sqrt(1.+dv_du*dv_du+dz_du*dz_du);
  if ( dir == TSD_BACKWARD ) du_ds *= -1.0;
  else assert( dir == TSD_FORWARD );
  double dv_ds = dv_du*du_ds;

  double dz_ds = dz_du*du_ds;
  double dx_ds =  du_ds*cphi - dv_ds*sphi;
  double dy_ds =  du_ds*sphi + dv_ds*cphi;

  return CartesianPath(x, y, z, dx_ds, dy_ds, dz_ds);
}
//**********************************************************************

// Return the creator.

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

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

// Write the object data.

ObjData SurfXYPlane::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "dist", get_parameter(DISTNORM) );
  data.add_double( "phi", get_parameter(NORMPHI) );
  return data;
}

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