Not tested. Plenty of errors but I think the main idea is there for the submatrices calculations. Still need to gather the matrices together.