Add library for commandline parsing
Add support for commandline options

Use simple input file to get a trajectory for the simple P-controller