Added manhattan heuristic
Unit tests for manhattan heuristic and helper functions