Fixed enough bugs such that it gets the sample problem correctly