implemented gCost calculation, refactored tests and implementation, updated readme