Data Structure and Algorithm code in c++