Corrected the main to prevent constant semaphore acquiring/releasing that was freezing the message queue. Refactoring, adding some comments, minor improvements.