[WIP] Refactor MSM

You could just add 3 and 4 to the start and end of this trajectory, for example.