Задача с семафорами

#include <QMutex>
#include <QSemaphore>
#include <QVector>
#include <QTime>
#include <QThread>
 
QMutex mutex;
 
int numGoing = 0;           // число едущих поездов
int numAwake = 0;           // число проснувшихся поездов
int travelTime = 100;       // время поезда в пути
float waitFactor = 1.0;     // коэффициент ожидания (имеет эффект если > 2)
QTime waitStart[2];         // время когда поезд ушел спать
 
typedef QVector <QSemaphore *> TSemVec;
TSemVec waitSem[2];
 
bool ShouldTrainGo( int direction, bool awaken )
{
    if (numGoing * direction < 0) return false; // едут в др напр-и, спать
    if (numAwake * direction < 0) return false; // не все еще пошли в др напр-и, спать 
    if (numAwake) {   // пропускаем проснувшихся (и тех кто успел)
        if (awaken) 
            numAwake -= direction;
        return true;
    }
 
    if (!numGoing) return true;  // никого нет, едем
    
    int invIndex = (direction > 0) ? 1 : 0;
    if (waitSem[invIndex].size()) {        // есть ждущие на др стороне
#if 1
        // если сейчас пустим из А - когда дождемся из B  
        QTime arriveTime = QTime::currentTime();
        arriveTime.addMSecs(travelTime * 2);
 
        // вычисляем время от прибытия в B до прибытия в A
        int totalWeight = waitStart[invIndex].msecsTo(arriveTime);
 
        // сравниваем с пределом
        if (totalWeight > travelTime * waitFactor) return false;
#else
        return false;
#endif
    }
    return true;
}
 
void PrepareWait( int direction, QSemaphore * sem )
{
    int index = (direction > 0) ? 0 : 1;
    waitSem[index].push_back(sem);
    if (waitSem[index].size() == 1)
        waitStart[index].start();
}
 
void WakeUpTrains( int direction )
{
    int invIndex = (direction > 0) ? 1 : 0;
    for (int i = 0; i < waitSem[invIndex].size(); ++i)
        waitSem[invIndex][i]->release();
 
    numAwake = waitSem[invIndex].size() * direction; // сохраняем число разбуженных
    waitSem[invIndex].clear();
}
 
void MoveTrain( void )
{
    QThread::msleep(travelTime);
}
 
void Process1Train( int direction )
{
    QSemaphore sem;       // на этом семафоре поезд будет ждать
    bool awaken = false;  // truе если поезд спал
 
    while (true) {
        mutex.lock();
        bool go = ShouldTrainGo(direction, awaken);   // решаем можно ли ехать под защитой мутекса
        if (go) 
            numGoing += direction;   // обновляем счетик едущих
        else {
            awaken = true;
            PrepareWait(direction, &sem);
        }
        mutex.unlock();
 
        if (go) 
            break;
        else 
            sem.acquire();   // нельзя ехать, спим
    } 
 
    MoveTrain();    // едем
 
    mutex.lock();
    numGoing -= direction;
    if (!numGoing)
        WakeUpTrains(direction);   // будим спящие поезда под защитой мутекса
    mutex.unlock();
}