Finished the data struct & algrithm

Error found,I decide to rewrite the code
fixed bug & finished testing