Cleaned up reference solver, not yet on par with previous version...