Code works. Invariants are conserved. Must be optimized and tested now.