#include <CLHEP/Matrix/Matrix.h>
#include <CLHEP/Matrix/Vector.h>
#include <iostream>
#include <vector>
using namespace std;
using namespace CLHEP;

void swim(double e, double p, double theta, double phi, vector<HepVector*> &traj) {
  double c = 30.0; // speed of light, cm/s
  double m=0.139, dt=0.1; // gev/c^2, ns
  HepVector r_0(3), r_1(3), r_2(3), k(3), B(3), v_0(3);
  HepMatrix A(3,3,0), I(3,3,1), M(3,3), onePlusA(3,3);
  int ierr;
  double E = sqrt(m*m + p*p);
  double gamma = E/m;
  double beta = sqrt(1.0 - 1.0/(gamma*gamma));
  v_0(1) = beta*c*sin(theta)*cos(phi); // cm/ns
  v_0(2) = beta*c*sin(theta)*sin(phi);
  v_0(3) = beta*c*cos(theta);
  double k_1 = (e*1.6e-19)*(dt*1.0e-9)/(gamma*(m*1.78e-27));
  cout << "gamma = " << gamma << " k_1 = " << k_1 << endl;
  B(1) = 0.0; // tesla
  B(2) = 0.0;
  B(3) = 4.0;
  k = k_1*B/2.0;
  r_0(1) = 0.0;
  r_0(2) = 0.0;
  r_0(3) = 0.0;
  r_1 = r_0 + v_0*dt;
  A(1,2) = -k(3);
  A(1,3) = k(2);
  A(2,1) = k(3);
  A(2,3) = -k(1);
  A(3,1) = -k(2);
  A(3,2) = k(1);
  onePlusA = I + A;
  M = onePlusA.inverse(ierr)*(I - A); // can we take advantage of anti-sym?
  HepVector *thisVector;
  for (int istep = 0; istep < 1000; istep++) {
    r_2 = r_1 + M*(r_1 - r_0);
    cout << istep << ' ' << r_2(1) << ' ' << r_2(2) << ' ' << r_2(3) << endl;
    thisVector = new HepVector(3);
    traj.push_back(thisVector);
    *thisVector = r_2;
    r_0 = r_1; r_1 = r_2;
  }
  return;
}