Added working algorithm with errors

This version works but with errors on line 121, undefined index T, offset errors...