cleaned
Rewrote with C++ and vectors