Fix matrix generation, quite slow for now
Implement the final stuff, still not working 100%