void WorkerThreadBase::run()
{
    if(!spineSerialComm_.IsOpen() && !spineSerialComm_.Open(SerialPortNumber))
    {
        state_ = NO_SPINE;
        emit workerThreadStateChanged(state_);

        return;
    }

    state_ = SPINE_OK;
    emit workerThreadStateChanged(state_);

    algorithmRunning_   = false;
    startAlgorithm_     = false;

    bool prevStartButtonState = false;

    do
    {
        spineSerialComm_.Communicate();

        newSpineData_ = spineSerialComm_.HasNewData();

        if(newSpineData_)
        {
            spineSerialComm_.GetData(&spineData_);
            spineSerialComm_.ClearNewData();
        }

        //
        // Detects algorithm start command from WorkerThreadBase::StartAlgorithm()
        // or rising edge of spineData.GetStartButton()
        //
        if(!algorithmRunning_ && (startAlgorithm_ || (!prevStartButtonState && spineData_.GetStartButton())))
        {
            prevStartButtonState= spineData_.GetStartButton();
            algorithmRunning_   = true;
            startAlgorithm_     = false;

            if(imageProcessor_ != NULL)
            {
                connect(imageProcessor_, SIGNAL(NewVisionDataReady(VisionData*)), SLOT(newVisionDataReady(VisionData*)));
            }

            emit workerThreadStateChanged(ALGORITHM_RUNNING);

            algorithm_->Init();
            algorithm_->SetAttackGoal(spineData_.GetAttackGoal());
        }

        //
        // Detects algorithm stop from WorkerThreadBase::StopAlgorithm()
        // or falling edge of spineData.GetStartButton()
        //
        if(algorithmRunning_ && (prevStartButtonState && !spineData_.GetStartButton()))
        {
            prevStartButtonState    = spineData_.GetStartButton();
            algorithmRunning_       = false;
            spineCmd_.Stop();
        }


        if(algorithmRunning_)
        {
            //
            // Do Algorithm::Execute() only if new data
            //
            if(newSpineData_ || newVisionData_)
            {
                bool stateChanged   = algorithm_->Execute(0, newVisionData_, newSpineData_, &visionData_, &spineData_);

                if(stateChanged)
                {
                    emit algorithmStateChanged(algorithm_->GetState(), (char*)algorithm_->GetStateName());
                }
            }

            newVisionData_  = false;
            newSpineData_   = false;
        }

        //
        // emit requested speed (to UI?)
        //
        if(spineCmd_.NewCommands())
        {
            emit speedCmdChanged(spineCmd_.GetAbsSpeed(), spineCmd_.GetRadAngle(), spineCmd_.GetRadTurnRate());
        }

        //
        // Send requested speed to motor controller
        //
        if(spineCmd_.NewCommands() && !spineSerialComm_.SendControllerCommands(&spineCmd_))
        {
            break;
        }

        spineCmd_.ClearCommands();
    }
    while(spineSerialComm_.IsOpen() && !threadStopCmd_);

    algorithmRunning_   = false;
    state_              = NO_SPINE;

    if(spineSerialComm_.IsOpen())
    {
        spineCmd_.Stop();
        spineSerialComm_.SendControllerCommands(&spineCmd_);
    }

    emit workerThreadStateChanged(state_);

    if(imageProcessor_ != NULL)
    {
        disconnect(imageProcessor_, SIGNAL(NewVisionDataReady(VisionData*)), this, SLOT(newVisionDataReady(VisionData*)));
    }
}