#include "DLine.h"

class MyTrajectory {
 public:
  MyTrajectory();
  virtual ~MyTrajectory();
  void clear();
  virtual void swim(HepVector startingPoint, double theta, double phi);
  virtual void swim(const HepVector &startingVector);
  void print();
  vector<HepVector*>* getTrajectory();
  template<class C> double doca(C& spaceObject);
  void inline checkClear() {
    if (traj.size() != 0) {
      int ierror = 2;
      throw ierror;
    }
    return;
  }
  virtual unsigned int getNumberOfParams();
  double dist(HepVector& point, int trajIndex);
  double dist(DLine& line, int trajIndex);
  double para_min(double y1, double y2, double y3) {
    double a, b, c;
    a = 0.5*(y1 - 2.0*y2 + y3);
    b = 0.5*(-3.0*y1 + 4.0*y2 - y3);
    c = y1;
    return (-b*b/(4.0f*a) + c);
  }
  virtual vector<double> getDelta() {
    return delta;
  }

 protected:
  vector<HepVector*> traj;

 private:
  unsigned int nparams; // number of parameters for trajectory
  vector<double> delta;
};

template<class C> double MyTrajectory::doca(C& spaceObject) {
  unsigned int ilo, ihi, imid;
  ilo = 0;
  ihi = traj.size() - 1;
  double distlo, disthi, distmid;
  distlo = dist(spaceObject, ilo);
  disthi = dist(spaceObject, ihi);
  if (distlo < disthi) {
    imid = ilo + 1;
  } else {
    imid = ihi - 1;
  }
  distmid = dist(spaceObject, imid);
  // 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);
    distnew = dist(spaceObject, inew);
    //    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;
  double dlon, dmidn, dhin, dinterp;
  dlon = dist(spaceObject, ilo);
  dmidn = dist(spaceObject, imid);
  dhin = dist(spaceObject, ihi);
  dinterp = sqrt(para_min(dlon*dlon, dmidn*dmidn, dhin*dhin));
  //cout << "doca final calc: " << dlon << " " << dmidn << " " << dhin << " " << dinterp << endl;
  return dinterp;

}