Work on last Step TODO: add col and diag checking to algth 1 & 2