Finished the legalizer code

It gives consistent results, comparable to previous prototypes.