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

using namespace std;

#include "MyTrajectory.h"

MyTrajectory::MyTrajectory() : nparams(4), delta(4, 0.001)  {
  // 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]);
}