WORKING CODE - Not perfect but working

Changed the init_uvp function, added write_matrix for debugging but now
it is commented, and changed the special boundary because it was wrong.