Fixed Undo, should work

Fixed the problem with undo feature. Print statements for debugging are
still in code. Need to fix code because some of it was copy-paste a few
times so variable names are a bit confusing.