updated code to working version,

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