Update to newest version

End to end working, demo in gridworld. Needs refactoring and cleaning up.