added tests for calculating g, h and g cost - passing