Separated file management code and state representation
Added trajectory optimisation, but not really working