#include <iostream>
#include <math.h>

#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(DVector3 r0_in, DVector3 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);
  return;
}

double DLine::doca(DVector3 point) {
  DVector3 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;
};

Hep3Vector DLine::poca(DVector3 point) {
  double t;
  DVector3 diff;
  diff = r1 - r0;
  t = (point - r0)*diff/diff.mag2();
  DVector3 poca = r0 + diff*t;
  return poca;
};