added diagonal cost
switched parameters