Cost path looks to be building correctly