First step of merging pathfinder to rest of project

compilable but not testet!!