Made functions work with an arbitrary number of groups
Made some optimisations by using linear algebra