Fixed insert(), based on pseudocode from the book