Adding everything completed so far. Algorithm does not converge nor work, but
the first and second order heuristics have been written.