updated code to working version,
added some comments in detailing usage using the matrix i used to check
the code
hope it helps