Fixed most bugs with my code, should be working and cleaned up.