Removed unused methods
Removed unneeded stuff
Also keep a vector of children, to speed things up a little.
Fixed cost function