Getting there, need to test on real problem.