Fixed init of cost in forage method.
Added tests for all three algos.
Removed unneccessary includes and defines.