Optimize and test correctness