All matrices working properly, need to fix current calculation