revised c++ alg. still need memory optimization and testing.