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

using namespace std;

#include "MyTrajectory.h"

MyTrajectory::MyTrajectory(int level) : nparams(4), delta(4, 0.001), debug_level(level) {
  // 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::dist(HepVector& point, int trajIndex) {
  HepVector delta(3);
  delta = point - *traj[trajIndex];
  return delta.norm();
}

double MyTrajectory::dist(DLine& line, int trajIndex) {
  return line.doca(*traj[trajIndex]);
}

int MyTrajectory::get_xy(double z, double &x, double &y) {
  int iBefore = 0, iAfter = traj.size() - 1, iTry;
  double zBefore = (*traj[iBefore])(3);
  double zAfter = (*traj[iAfter])(3);
  double zTry;
  if (z < zBefore || z > zAfter) {
    return 1;
  }
  while (iAfter - iBefore > 1) {
    iTry = iBefore + (int)((double)(iAfter - iBefore)*(z - zBefore)/(zAfter - zBefore) + 0.5);
    // cout << iBefore << ' ' << iTry << ' ' << iAfter << endl;
    if (iBefore == iTry) iTry++;
    if (iAfter == iTry) iTry--;
    // cout << iBefore << ' ' << iTry << ' ' << iAfter << endl;
    zTry = (*traj[iTry])(3);
    // cout << "zTry = " << zTry << endl;
    if (z < zTry) {
      iAfter = iTry;
      zAfter = (*traj[iAfter])(3);
    } else if (z > zTry) {
      iBefore = iTry;
      zBefore = (*traj[iBefore])(3);
    } else {
      x = (*traj[iTry])(1);
      y = (*traj[iTry])(2);
    }
    // cout << z << ' ' << zBefore << ' ' << zTry << ' ' << zAfter << endl;
  }
  double frac, otherfrac;
  frac = (z - zBefore)/(zAfter - zBefore);
  otherfrac = 1.0 - frac;
  // cout << frac << ' ' << otherfrac << endl;
  // cout << "x before, after " << (*traj[iBefore])(1) << ' '<< (*traj[iAfter])(1) << endl;
  // cout << "y before, after " << (*traj[iBefore])(2) << ' '<< (*traj[iAfter])(2) << endl;
  x = frac*(*traj[iAfter])(1) + otherfrac*(*traj[iBefore])(1);
  y = frac*(*traj[iAfter])(2) + otherfrac*(*traj[iBefore])(2);
  return 0;
}

double MyTrajectory::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;
  double ymin = -b*b/(4.0*a) + c;
  if (ymin < 0.0) ymin = 0.0;
  return ymin;
}