if you're just doing 1 undo, then you dont even need anything crazy like all that array stuff. just have a few alterable variables that save the x and y position and direction of the block before you move one step. then if you undo, read the variables and position the blocks back to their previous position.
Edit: Here you go, I made a quick example.