findPath now works, can be optimized further