More optimizations and added path finder with pos