bug fix. Diagonal part seems correct, but there is one counterintuitive step that I need to verify.