// SpacePoint.cpp

#include "SpacePoint.h"
#include <cmath>
#include <limits>

#ifndef DEFECT_NO_STDLIB_NAMESPACES
#ifndef DEFECT_CMATH_NOT_STD
using std::fabs;
using std::cos;
using std::sin;
using std::sqrt;
using std::atan2;
#endif
using std::ostream;
using std::endl;
#endif


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

// Local definitions.

namespace {

double equalfac = 4.0;

}

//**********************************************************************
// Statics
//**********************************************************************

// Return if two doubles are close enough to be considered equal.

bool SpacePoint::equal(double x1, double x2) {
  // Handle x1 = x2 = 0.0.
  if ( x1 == x2 ) return true;
  double maxdif = equalfac*std::numeric_limits<double>::epsilon();
  double num = 2.0*(x2-x1);
  double den = x2 + x1;
  double dif = fabs(num/den);
  return dif <= maxdif;
}

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

// Return the tolerance used in the above.

double SpacePoint::equal_tolerance() {
  return equalfac*std::numeric_limits<double>::epsilon();
}

//**********************************************************************
// Member functions.
//**********************************************************************

// Constructor.
// Initial point is the origin with phi = theta = 0.

SpacePoint::SpacePoint( ) {
  _x = _y = _z = 0.0;
  _xy = _xyz = 0.0;
  _phi = _tht = 0.0;
}

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

// Destructor.

SpacePoint::~SpacePoint( ) { }

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

// return cos(phi)

double SpacePoint::cos_phi( ) const {
  if ( _xy ) return _x/_xy;
  return cos(_phi);
}

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

// return sin(phi)

double SpacePoint::sin_phi( ) const {
  if ( _xy ) return _y/_xy;
  return sin(_phi);
}

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

// return cos(theta)

double SpacePoint::cos_theta( ) const {
  if ( _xyz ) return _z/_xyz;
  return cos(_tht);
}

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

// return sin(theta)

double SpacePoint::sin_theta( ) const {
  if ( _xyz ) return _xy/_xyz;
  return sin(_tht);
}

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

// Construct a Cartesian point.

CartesianPoint::CartesianPoint(double x, double y, double z) {
  _x = x;
  _y = y;
  _z = z;
  _xy = sqrt(_x*_x+_y*_y);
  _xyz = sqrt(_xy*_xy+_z*_z);
  _phi = atan2(_y,_x);
  _tht = atan2(_xy,_z);
}

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

// Construct a cylindrical point.

CylindricalPoint::CylindricalPoint(double r, double phi, double z) {
  _xy = r;
  _phi = phi;
  _z = z;
  _x = r*cos(phi);
  _y = r*sin(phi);
  _xyz = sqrt(_xy*_xy+_z*_z);
  _tht = atan2(_xy,_z);
}

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

// Construct a spherical point.

SphericalPoint::SphericalPoint(double r, double phi, double theta) {
  _xyz = r;
  _phi = phi;
  _tht = theta;
  _xy = r*sin(theta);
  _x = _xy*cos(phi);
  _y = _xy*sin(phi);
  _z = r*cos(theta);
}

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

// Output stream operator.

ostream& operator<<(ostream & stream, const SpacePoint& spt) {
  stream << "    x: " << spt.x() << endl;
  stream << "    y: " << spt.y() << endl;
  stream << "    z: " << spt.z() << endl;
  stream << "  rxy: " << spt.rxy() << endl;
  stream << " rxyz: " << spt.rxyz() << endl;
  stream << "  phi: " << spt.phi() << endl;
  stream << "theta: " << spt.theta();
  return stream;
}

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

// Equality.

bool operator==(const SpacePoint& lhs, const SpacePoint& rhs) {
  return SpacePoint::equal( lhs.x(), rhs.x() ) &&
         SpacePoint::equal( lhs.y(), rhs.y() ) &&
         SpacePoint::equal( lhs.z(), rhs.z() );
}

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

// Inequality.

bool operator!=(const SpacePoint& lhs, const SpacePoint& rhs) {
  return ! (lhs == rhs);
}

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

// Return the distance between two space points.

double distance(const SpacePoint spt1, const SpacePoint spt2) {
  double dx = spt2.x() - spt1.x();
  double dy = spt2.y() - spt1.y();
  double dz = spt2.z() - spt1.z();
  return sqrt( dx*dx + dy*dy + dz*dz );
}

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