updated function to check for problems
Updated function call once it was fixed
Corrected problem with indexing and fully implemented in c++