Working X Matrix, need to finish C