Add doctests and improve code to fit doctests