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

using namespace std;

#define PI 3.1415926
#include "HDGEOMETRY/DMagneticFieldMap.h"
#include "MyTrajectory.h"
#include "MyTrajectoryBfield.h"

int main() 
{
  DMagneticFieldMap *bfield;
  double p = 1.0 , theta = PI/6.0, phi = PI/4.0;
  HepVector B(3);
  B(1) = 0.0;
  B(2) = 0.0;
  B(3) = 4.0;
  double xp0 = 0.0;
  double z0 = 0.0;
  double pinv = 1.0/p;
  MyTrajectoryBfield trajectory(bfield);
  trajectory.swim(xp0, z0, theta, phi, pinv);
  trajectory.print();
  return 0;
}