C++ attempt at algorithms PA1