Fix gradient function and add test