Basic algorithm, needs D(p) and F(p) added either statically or by some equation.