Updated main.c. Should be working.

Still needs to be tested