#ifndef _MYTRAJECTORY_H_
#define _MYTRAJECTORY_H_

#include "DLine.h"

class MyTrajectory {
 public:
  MyTrajectory(int level = 1);
  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, HepVector &poca);
  void checkClear();
  virtual unsigned int getNumberOfParams();
  double dist(HepVector& point, int trajIndex);
  double dist(DLine& line, int trajIndex);
  void para_min(double yMinus, double yZero, double yPlus, double &xMinFrac,
		double &yMin);
  virtual vector<double> getDelta() {
    return delta;
  }
  int get_xy(double z, double &x, double &y); //
  void dump_ascii(ostream *trajFile, int tag);

 protected:
  vector<HepVector*> traj;

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

template<class C> double MyTrajectory::doca(C& spaceObject, HepVector &poca) {
  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);
  if (debug_level >= 4) cout << "MyTrajectory::doca, initialize: " << ilo << " " << imid << " " << ihi << " " << distlo << " " << distmid << " " << disthi << endl;
  if (distmid >= distlo || distmid >= disthi) {
    cout << "MyTrajectory:doca, 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);
    if (debug_level >= 4) cout << "MyTrajectory::doca: 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;
      }
    }
    if (debug_level >= 4) cout << "MyTrajectory::doca, 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);
  double xMinFrac, yMin;
  para_min(dlon*dlon, dmidn*dmidn, dhin*dhin, xMinFrac, yMin);
  dinterp = sqrt(yMin);
  if (xMinFrac > 0.0) {
    poca = xMinFrac*(*traj[ihi]) + (1.0 - xMinFrac)*(*traj[imid]);
  } else {
    poca = -xMinFrac*(*traj[ilo]) + (1.0 + xMinFrac)*(*traj[imid]);
  }
  if (debug_level >= 4) cout << "doca final calc: " << dlon << " " << dmidn << " " << dhin << " " << dinterp << endl;
  return dinterp;

}

#endif //  _MYTRAJECTORY_H_