Fixed out of bounds, deleted diagonal code, fixed if statements. Almost completely working