Initial stab at lie integrator. Needs debugging.