Added path saving and final solution output

Note: I haven't tested yet, so this is probably horribly broken!