Implementation of the dist function. Add of several tests.

Fix of a major issue in the simulation: sometimes the number of states
wasn't consistent. Add of test to check the consistency of the
Trajectory structure (unit/check_trajectory_consistency).