Insertion into simulation history buffer

This changes the insertion code so it also inserts previous states in the simulation history buffer (which is passed to the planners as input).

Some notes:

1. Implementing this change required adding the simulation object as a parameter to the add agent function. I changed all uses of this in this branch, but you might have to change this for usages in other branches.
2. The way past states are populated are just to take the velocity when inserted and then just linearly project that back for previous timesteps. If you inject an agent with no starting velocity, this will translate to the agent just remaining still for the previous timesteps.