Diagonalize Hamiltonian