Block matrix code simplified, but algo still needs works