#include "DLine.h"
using namespace std;

#define DELTA_R = 10000.0

// default constructor line along z-axis
DLine::DLine() : r0(0.0, 0.0, 0.0), r1(0.0, 0.0, 1.0), debug_level(0) {}

DLine::DLine(Hep3Vector r0_in, Hep3Vector r1_in): r0(r0_in), r1(r1_in), debug_level(0) {};

DLine::DLine(double x, double y, double z, double theta, double phi) : debug_level(0) {
  r0(0) = x;
  r0(1) = y;
  r0(2) = z;
  double sinth = sin(theta);
  r1(0) = x + sinth*cos(phi);
  r1(1) = y + sinth*sin(phi);
  r1(2) = z + cos(theta);
  if (debug_level > 2) cout << "DLine::DLine: line constructed: " << r0 << r1 << endl;
  return;
}

double DLine::doca(HepVector point) {
  Hep3Vector point3(point(1), point(2), point(3));
  return doca(point3);
}

double DLine::doca(Hep3Vector point) {
  Hep3Vector num, diff;
  diff = r1 - r0;
  num = diff.cross(r0 - point);
  double doca = num.mag()/diff.mag();
  if (debug_level > 2) cout << "DLine:doca: doca = " << doca << endl;
  return doca;
};