Merge development

Fix several bugs, introduce improved path finding