Cleaned up restore code in FenwickNode, not perfect but better (2 repetitions instead of 4). Closes #12.