Fixed some bugs but still not done yet
Everything is working

Just have to clean up, optimize and test
Forgot to save the registers on the stack