Cost function added time for some debugging
Gradient and cost working
Some minor corrections