Completing velocity verlet and debugging