Working version of applyconstraints MINUS diagonal terms
Working version except for diagonal