Finally made vision functions separate and cleaned up the code. Also added main.h