Everything working

Decided not to store the results in a matrix and just write to the output. The algorithm is working correctly. Need to clean up the code
Code cleaned and ready to go