Further progress on overall algorithm, some implementation