Tridiagonal matrix algorithm debugged