path optimization improvement. still slightly buggy