Reorganized structures and added defines to make code cleaner. CODE STILL NOT WORKING ATM