Add correctness check for algorithm