Fixed compilation errors in matrix code outline
Porting old matrix code to new code; compiles and starting to work