#include <iostream>
#include <vector>
#include <CLHEP/Matrix/Vector.h>

using namespace std;

#include "MyTrajectory.h"

MyTrajectory::MyTrajectory() : nparams(4) {
  // explicit default constructor, set number of params for straight line
  //  cout << "MyTrajectory constructor called\n";
  //  cout << "nparams = " << nparams << endl;
  return;
}

MyTrajectory::~MyTrajectory() {
  clear();
  return;
}

unsigned int MyTrajectory::getNumberOfParams() {
  return nparams;
}

void MyTrajectory::clear() {
  if (traj.size() != 0) {
    for (vector<HepVector*>::iterator iVector = traj.begin();
	 iVector != traj.end();
	 iVector++) {
      // cout << "traj x = " << (**iVector)(1) << ", traj y = " << (**iVector)(2) << ", traj z = " << (**iVector)(3) << endl;
      delete *iVector;
    }
  }
  traj.clear();
  return;
}

void MyTrajectory::swim(HepVector startingPoint, double theta, double phi)
// default swim, straight line
{
  //  cout << "straight line swim with start point = "<< startingPoint << "theta = " << theta << " phi = " << phi << endl;
  checkClear();
  double stepLength = 1.0;
  HepVector step(3);
  step(1) = stepLength*sin(theta)*cos(phi);
  step(2) = stepLength*sin(theta)*sin(phi);
  step(3) = stepLength*cos(theta);
  HepVector* thisVector;
  thisVector = new HepVector(3);
  *thisVector = startingPoint;
  traj.push_back(thisVector);
  HepVector lastVector(3);
  lastVector = startingPoint;
  for (int i = 0; i < 600; i++) {
    thisVector = new HepVector(3);
    *thisVector = lastVector + step;
    //    if (i == 100) cout << "point on traj " << i << " " << *thisVector;
    traj.push_back(thisVector);
    lastVector = *thisVector;
  }
  return;
}

void MyTrajectory::swim(const HepVector& startingVector) {
  HepVector startingPoint(3);
  startingPoint(1) = startingVector(1);
  startingPoint(2) = startingVector(2);
  startingPoint(3) = 0.0; // start at z = 0 every time
  double theta = startingVector(3);
  double phi = startingVector(4);
  swim(startingPoint, theta, phi);
  return;
}

void MyTrajectory::print()
{
  //  cout << "### MyTrajectory print out begin ###" << endl;
  for (vector<HepVector*>::iterator iVector = traj.begin();
       iVector != traj.end();
       iVector++) {
    //    cout << (**iVector)(1) << " " << (**iVector)(2) << " " << (**iVector)(3)
    //	 << endl;
  }
  //  cout << "### MyTrajectory print out end ###" << endl;
}

vector<HepVector*>* MyTrajectory::getTrajectory() {
  return &traj;
}

double MyTrajectory::doca(HepVector& point) {
  unsigned int ilo, ihi, imid;
  ilo = 0;
  ihi = traj.size() - 1;
  double distlo, disthi, distmid;
  HepVector delta(3);
  delta = point - *(traj[ilo]);
  distlo = delta.norm();
  delta = point - *(traj[ihi]);
  disthi = delta.norm();
  if (distlo < disthi) {
    imid = ilo + 1;
  } else {
    imid = ihi - 1;
  }
  delta = point - *(traj[imid]);
  distmid = delta.norm();
  // cout << "initialize: " << ilo << " " << imid << " " << ihi << " " << distlo << " " << distmid << " " << disthi << endl;
  if (distmid >= distlo || distmid >= disthi) {
    //    cout << "bad initialization of doca search: " << ilo << " " << imid << " " << ihi << " " << distlo << " " << distmid << " " << disthi << endl;
    int ierror = 1;
    throw ierror;
  }
  double x1, x2, y1, y2, xnew, distnew = 0;
  unsigned int inew;
  while (ilo != imid && imid != ihi) {
    x1 = (double)(imid - ilo);
    x2 = -(double)(ihi - imid); // unsigned int's cannot be negative
    y1 = distmid - distlo;
    y2 = distmid - disthi;
    xnew = (double)imid
      - 0.5*(
	     (x1*x1*y2 - x2*x2*y1)
	     /
	     (x1*y2 - x2*y1)
	     );
    inew = (unsigned int)(xnew + 0.5);
    delta = point - *(traj[inew]);
    distnew = delta.norm();
    //    cout << "xnew = " << xnew << " inew = " << inew << " distnew = " << distnew << endl;
    if (distnew <= distmid) { // new point is the new lowest point
      if (inew < imid) { // if new index less than old mid point index
	ihi = imid; // new high point is the old mid point
	disthi = distmid;
      } else { // new index is greater than the old mid point index
	ilo = imid; // new low point is the old mid point
	distlo = distmid;
      }
      imid = inew; // new point is the new mid point
      distmid = distnew;
    } else { // old mid-point is still the lowest point
      if (inew < imid) { // if new index less than old mid point index
	ilo = inew; // new low point is the new point
	distlo = distnew;
      } else { // new index is greater than the old mid point index
	ihi = inew; // new high point is the new point
	disthi = distnew;
      }
    }
    //    cout << "after housekeeping: " << ilo << " " << imid << " " << ihi << " " << distlo << " " << distmid << " " << disthi << endl;
  }
  ihi = imid + 1;
  ilo = imid - 1;
  HepVector dlo(3), dmid(3), dhi(3);
  double dlon, dmidn, dhin, dinterp;
  dlo = point - *(traj[ilo]);
  dmid = point - *(traj[imid]);
  dhi = point - *(traj[ihi]);
  dlon = dlo.norm();
  dmidn = dmid.norm();
  dhin = dhi.norm();
  dinterp = sqrt(para_min(dlon*dlon, dmidn*dmidn, dhin*dhin));
  //cout << "doca final calc: " << dlon << " " << dmidn << " " << dhin << " " << dinterp << endl;
  return dinterp;

}