<commit_before>//=============================================================================================================
/**
 * @file     brainflowboard.cpp
 * @author   Andrey Parfenov <a1994ndrey@gmail.com>
 * @version  dev
 * @date     February, 2020
 *
 * @section  LICENSE
 *
 * Copyright (C) 2020, Andrey Parfenov. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Contains the definition of the BrainFlowBoard class.
 *
 */

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <QMessageBox>

#include "data_filter.h"

#include "brainflowboard.h"
#include "FormFiles/brainflowsetupwidget.h"
#include "FormFiles/brainflowstreamingwidget.h"

#include <fiff/fiff_info.h>

#include <scMeas/realtimemultisamplearray.h>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace BRAINFLOWBOARDPLUGIN;
using namespace SCSHAREDLIB;
using namespace SCMEASLIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

BrainFlowBoard::BrainFlowBoard()
: m_pBoardShim(NULL)
, m_pChannels(NULL)
, m_iBoardId((int)BoardIds::SYNTHETIC_BOARD)
, m_bIsRunning(false)
, m_pOutput(NULL)
, m_iSamplingFreq(0)
, m_sStreamerParams("")
, m_iNumberChannels(0)
, m_pFiffInfo(QSharedPointer<FiffInfo>::create())
{
    m_pShowSettingsAction = new QAction(QIcon(":/images/options.png"), tr("Streaming Settings"),this);
    m_pShowSettingsAction->setStatusTip(tr("Streaming Settings"));
    connect(m_pShowSettingsAction, &QAction::triggered, this, &BrainFlowBoard::showSettings);
    addPluginAction(m_pShowSettingsAction);

    m_pOutput = PluginOutputData<RealTimeMultiSampleArray>::create(this, "BrainFlowBoard", "BrainFlow Board Output");
    m_outputConnectors.append(m_pOutput);
}

//*************************************************************************************************************

BrainFlowBoard::~BrainFlowBoard()
{
    releaseSession(false);
}

//*************************************************************************************************************

void BrainFlowBoard::showSettings()
{
    BrainFlowStreamingWidget *widget = new BrainFlowStreamingWidget(this);
    widget->show();
}

//*************************************************************************************************************

QSharedPointer<IPlugin> BrainFlowBoard::clone() const
{
    QSharedPointer<BrainFlowBoard> pClone(new BrainFlowBoard());
    return pClone;
}

//*************************************************************************************************************

void BrainFlowBoard::init()
{
    BoardShim::set_log_file((char *)"brainflow_log.txt");
    BoardShim::enable_dev_board_logger();
}

//*************************************************************************************************************

void BrainFlowBoard::unload()
{
}

//*************************************************************************************************************

void BrainFlowBoard::setUpFiffInfo()
{
    //
    //Clear old fiff info data
    //
    m_pFiffInfo->clear();

    //
    //Set number of channels, sampling frequency and high/-lowpass
    //
    m_pFiffInfo->nchan = m_iNumberChannels;
    m_pFiffInfo->sfreq = m_iSamplingFreq;
    m_pFiffInfo->highpass = 0.001f;
    m_pFiffInfo->lowpass = m_iSamplingFreq/2;

    //
    //Set up the channel info
    //
    QStringList QSLChNames;
    m_pFiffInfo->chs.clear();

    for(int i = 0; i < m_pFiffInfo->nchan; ++i)
    {
        //Create information for each channel
        QString sChType;
        FiffChInfo fChInfo;

        //Set channel name
        sChType = QString("Ch ");
        if(i<10) {
            sChType.append("00");
        }

        if(i>=10 && i<100) {
            sChType.append("0");
        }

        fChInfo.ch_name = sChType.append(sChType.number(i));

        //Set channel type
        fChInfo.kind = FIFFV_EEG_CH;

        //Set logno
        fChInfo.logNo = i;

        //Set coord frame
        fChInfo.coord_frame = FIFFV_COORD_HEAD;

        //Set unit
        fChInfo.unit = FIFF_UNIT_V;

        //Set EEG electrode location - Convert from mm to m
        fChInfo.eeg_loc(0,0) = 0;
        fChInfo.eeg_loc(1,0) = 0;
        fChInfo.eeg_loc(2,0) = 0;

        //Set EEG electrode direction - Convert from mm to m
        fChInfo.eeg_loc(0,1) = 0;
        fChInfo.eeg_loc(1,1) = 0;
        fChInfo.eeg_loc(2,1) = 0;

        //Also write the eeg electrode locations into the meg loc variable (mne_ex_read_raw() matlab function wants this)
        fChInfo.chpos.r0(0) = 0;
        fChInfo.chpos.r0(1) = 0;
        fChInfo.chpos.r0(2) = 0;

        fChInfo.chpos.ex(0) = 1;
        fChInfo.chpos.ex(1) = 0;
        fChInfo.chpos.ex(2) = 0;

        fChInfo.chpos.ey(0) = 0;
        fChInfo.chpos.ey(1) = 1;
        fChInfo.chpos.ey(2) = 0;

        fChInfo.chpos.ez(0) = 0;
        fChInfo.chpos.ez(1) = 0;
        fChInfo.chpos.ez(2) = 1;

//        //Digital input channel
//        if(i == m_pFiffInfo->nchan-1)
//        {
//            //Set channel type
//            fChInfo.kind = FIFFV_STIM_CH;

//            sChType = QString("STIM");
//            fChInfo.ch_name = sChType;
//        }

        QSLChNames << sChType;

        m_pFiffInfo->chs.append(fChInfo);
    }

    //Set channel names in fiff_info_base
    m_pFiffInfo->ch_names = QSLChNames;

    //
    //Set head projection
    //
    m_pFiffInfo->dev_head_t.from = FIFFV_COORD_DEVICE;
    m_pFiffInfo->dev_head_t.to = FIFFV_COORD_HEAD;
    m_pFiffInfo->ctf_head_t.from = FIFFV_COORD_DEVICE;
    m_pFiffInfo->ctf_head_t.to = FIFFV_COORD_HEAD;
}

//*************************************************************************************************************

bool BrainFlowBoard::start()
{
    if (!m_pBoardShim)
    {
        QMessageBox msgBox;
        msgBox.setText("Configure streaming session before run!");
        msgBox.exec();
        return false;
    }
    try {
        m_pBoardShim->start_stream(m_iSamplingFreq * 100, (char*)m_sStreamerParams.c_str());
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        QMessageBox msgBox;
        msgBox.setText("Failed to start streaming.");
        msgBox.exec();
        return false;
    }
    m_bIsRunning = true;
    QThread::start();
    return true;
}

//*************************************************************************************************************

bool BrainFlowBoard::stop()
{
    try {
        m_pBoardShim->stop_stream();
        m_bIsRunning = false;
        m_pOutput->data()->clear();
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        return false;
    }
    return true;
}

//*************************************************************************************************************

IPlugin::PluginType BrainFlowBoard::getType() const
{
    return _ISensor;
}

//*************************************************************************************************************

QString BrainFlowBoard::getName() const
{
    return "BrainFlow Board Plugin";
}

//*************************************************************************************************************

QWidget* BrainFlowBoard::setupWidget()
{
    BrainFlowSetupWidget* widget = new BrainFlowSetupWidget(this);
    return widget;
}

//*************************************************************************************************************

void BrainFlowBoard::prepareSession(BrainFlowInputParams params,
                                    std::string streamerParams,
                                    int boardId,
                                    int dataType,
                                    int vertScale)
{
    if (m_pBoardShim)
    {
        QMessageBox msgBox;
        msgBox.setText("Streaming session already prepared.");
        msgBox.exec();
        return;
    }

    BoardShim::log_message((int)LogLevels::LEVEL_ERROR, "Vert Scale is %d %d", vertScale);
    QMessageBox msgBox;
    m_iBoardId = boardId;
    m_sStreamerParams = streamerParams;

    try {
        switch (dataType)
        {
            case 0:
                m_pChannels = BoardShim::get_eeg_channels(boardId, &m_iNumberChannels);
                break;
            case 1:
                m_pChannels = BoardShim::get_emg_channels(boardId, &m_iNumberChannels);
                break;
            case 2:
                m_pChannels = BoardShim::get_ecg_channels(boardId, &m_iNumberChannels);
                break;
            case 3:
                m_pChannels = BoardShim::get_eog_channels(boardId, &m_iNumberChannels);
                break;
            case 4:
                m_pChannels = BoardShim::get_eda_channels(boardId, &m_iNumberChannels);
                break;
            default:
                throw BrainFlowException ("unsupported data type", UNSUPPORTED_BOARD_ERROR);
                break;
        }
        m_iSamplingFreq = BoardShim::get_sampling_rate(boardId);
        m_pBoardShim = new BoardShim(m_iBoardId, params);
        m_pBoardShim->prepare_session();

        setUpFiffInfo();

        m_pOutput->data()->initFromFiffInfo(m_pFiffInfo);
        m_pOutput->data()->setMultiArraySize(1);

        msgBox.setText("Streaming session is ready");
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        msgBox.setText("Invalid parameters. Check logs for details.");

        delete[] m_pChannels;
        delete m_pBoardShim;
        m_pChannels = NULL;
        m_pBoardShim = NULL;
        m_iNumberChannels = 0;
    }

    msgBox.exec();
}

//*************************************************************************************************************

void BrainFlowBoard::configureBoard(std::string config)
{
    QMessageBox msgBox;
    if (!m_pBoardShim)
    {
        msgBox.setText("Prepare Session first.");
        return;
    }
    else
    {
        try {
            m_pBoardShim->config_board((char *)config.c_str());
            msgBox.setText("Configured.");
        } catch (const BrainFlowException &err) {
            BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
            msgBox.setText("Failed to Configure.");
        }
    }
    msgBox.exec();
}

//*************************************************************************************************************

void BrainFlowBoard::run()
{
    int iMinSamples = 10;
    unsigned long lSamplingPeriod = (unsigned long)(1000000.0 / m_iSamplingFreq) * iMinSamples;
    int numRows = BoardShim::get_num_rows(m_iBoardId);

    double **data = NULL;
    int dataCount = 0;
    while(m_bIsRunning)
    {
        usleep(lSamplingPeriod);
        data = m_pBoardShim->get_board_data (&dataCount);
        if (dataCount == 0)
        {
            continue;
        }

        Eigen::MatrixXd matrix (m_iNumberChannels, dataCount);
        for (int j = 0; j < m_iNumberChannels; j++)
        {
            for (int i = 0; i < dataCount; i++)
            {
                matrix(j, i) = data[m_pChannels[j]][i];
            }
        }

        m_pOutput->data()->setValue(matrix);

        for (int i = 0; i < numRows; i++)
        {
            delete[] data[i];
        }
        delete[] data;
    }
}

//*************************************************************************************************************

void BrainFlowBoard::releaseSession(bool useQmessage)
{
    if (m_pBoardShim)
    {
        try {
            m_pBoardShim->release_session();
        } catch (...) {
        }
    }
    delete m_pBoardShim;
    delete[] m_pChannels;

    for (int i = 0; i < m_iNumberChannels; i++)
    {
        m_outputConnectors.pop_back();
    }
    m_pBoardShim = NULL;
    m_pChannels = NULL;
    m_iBoardId = (int)BoardIds::SYNTHETIC_BOARD;
    m_bIsRunning = false;
    m_pOutput = NULL;
    m_iSamplingFreq = 0;
    m_sStreamerParams = "";
    m_iNumberChannels = 0;

    if (useQmessage)
    {
        QMessageBox msgBox;
        msgBox.setText("Released.");
        msgBox.exec();
    }
}
<commit_msg>Add using namespace<commit_after>//=============================================================================================================
/**
 * @file     brainflowboard.cpp
 * @author   Andrey Parfenov <a1994ndrey@gmail.com>
 * @version  dev
 * @date     February, 2020
 *
 * @section  LICENSE
 *
 * Copyright (C) 2020, Andrey Parfenov. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Contains the definition of the BrainFlowBoard class.
 *
 */

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <QMessageBox>

#include "data_filter.h"

#include "brainflowboard.h"
#include "FormFiles/brainflowsetupwidget.h"
#include "FormFiles/brainflowstreamingwidget.h"

#include <fiff/fiff_info.h>

#include <scMeas/realtimemultisamplearray.h>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace BRAINFLOWBOARDPLUGIN;
using namespace SCSHAREDLIB;
using namespace SCMEASLIB;
using namespace FIFFLIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

BrainFlowBoard::BrainFlowBoard()
: m_pBoardShim(NULL)
, m_pChannels(NULL)
, m_iBoardId((int)BoardIds::SYNTHETIC_BOARD)
, m_bIsRunning(false)
, m_pOutput(NULL)
, m_iSamplingFreq(0)
, m_sStreamerParams("")
, m_iNumberChannels(0)
, m_pFiffInfo(QSharedPointer<FiffInfo>::create())
{
    m_pShowSettingsAction = new QAction(QIcon(":/images/options.png"), tr("Streaming Settings"),this);
    m_pShowSettingsAction->setStatusTip(tr("Streaming Settings"));
    connect(m_pShowSettingsAction, &QAction::triggered, this, &BrainFlowBoard::showSettings);
    addPluginAction(m_pShowSettingsAction);

    m_pOutput = PluginOutputData<RealTimeMultiSampleArray>::create(this, "BrainFlowBoard", "BrainFlow Board Output");
    m_outputConnectors.append(m_pOutput);
}

//*************************************************************************************************************

BrainFlowBoard::~BrainFlowBoard()
{
    releaseSession(false);
}

//*************************************************************************************************************

void BrainFlowBoard::showSettings()
{
    BrainFlowStreamingWidget *widget = new BrainFlowStreamingWidget(this);
    widget->show();
}

//*************************************************************************************************************

QSharedPointer<IPlugin> BrainFlowBoard::clone() const
{
    QSharedPointer<BrainFlowBoard> pClone(new BrainFlowBoard());
    return pClone;
}

//*************************************************************************************************************

void BrainFlowBoard::init()
{
    BoardShim::set_log_file((char *)"brainflow_log.txt");
    BoardShim::enable_dev_board_logger();
}

//*************************************************************************************************************

void BrainFlowBoard::unload()
{
}

//*************************************************************************************************************

void BrainFlowBoard::setUpFiffInfo()
{
    //
    //Clear old fiff info data
    //
    m_pFiffInfo->clear();

    //
    //Set number of channels, sampling frequency and high/-lowpass
    //
    m_pFiffInfo->nchan = m_iNumberChannels;
    m_pFiffInfo->sfreq = m_iSamplingFreq;
    m_pFiffInfo->highpass = 0.001f;
    m_pFiffInfo->lowpass = m_iSamplingFreq/2;

    //
    //Set up the channel info
    //
    QStringList QSLChNames;
    m_pFiffInfo->chs.clear();

    for(int i = 0; i < m_pFiffInfo->nchan; ++i)
    {
        //Create information for each channel
        QString sChType;
        FiffChInfo fChInfo;

        //Set channel name
        sChType = QString("Ch ");
        if(i<10) {
            sChType.append("00");
        }

        if(i>=10 && i<100) {
            sChType.append("0");
        }

        fChInfo.ch_name = sChType.append(sChType.number(i));

        //Set channel type
        fChInfo.kind = FIFFV_EEG_CH;

        //Set logno
        fChInfo.logNo = i;

        //Set coord frame
        fChInfo.coord_frame = FIFFV_COORD_HEAD;

        //Set unit
        fChInfo.unit = FIFF_UNIT_V;

        //Set EEG electrode location - Convert from mm to m
        fChInfo.eeg_loc(0,0) = 0;
        fChInfo.eeg_loc(1,0) = 0;
        fChInfo.eeg_loc(2,0) = 0;

        //Set EEG electrode direction - Convert from mm to m
        fChInfo.eeg_loc(0,1) = 0;
        fChInfo.eeg_loc(1,1) = 0;
        fChInfo.eeg_loc(2,1) = 0;

        //Also write the eeg electrode locations into the meg loc variable (mne_ex_read_raw() matlab function wants this)
        fChInfo.chpos.r0(0) = 0;
        fChInfo.chpos.r0(1) = 0;
        fChInfo.chpos.r0(2) = 0;

        fChInfo.chpos.ex(0) = 1;
        fChInfo.chpos.ex(1) = 0;
        fChInfo.chpos.ex(2) = 0;

        fChInfo.chpos.ey(0) = 0;
        fChInfo.chpos.ey(1) = 1;
        fChInfo.chpos.ey(2) = 0;

        fChInfo.chpos.ez(0) = 0;
        fChInfo.chpos.ez(1) = 0;
        fChInfo.chpos.ez(2) = 1;

//        //Digital input channel
//        if(i == m_pFiffInfo->nchan-1)
//        {
//            //Set channel type
//            fChInfo.kind = FIFFV_STIM_CH;

//            sChType = QString("STIM");
//            fChInfo.ch_name = sChType;
//        }

        QSLChNames << sChType;

        m_pFiffInfo->chs.append(fChInfo);
    }

    //Set channel names in fiff_info_base
    m_pFiffInfo->ch_names = QSLChNames;

    //
    //Set head projection
    //
    m_pFiffInfo->dev_head_t.from = FIFFV_COORD_DEVICE;
    m_pFiffInfo->dev_head_t.to = FIFFV_COORD_HEAD;
    m_pFiffInfo->ctf_head_t.from = FIFFV_COORD_DEVICE;
    m_pFiffInfo->ctf_head_t.to = FIFFV_COORD_HEAD;
}

//*************************************************************************************************************

bool BrainFlowBoard::start()
{
    if (!m_pBoardShim)
    {
        QMessageBox msgBox;
        msgBox.setText("Configure streaming session before run!");
        msgBox.exec();
        return false;
    }
    try {
        m_pBoardShim->start_stream(m_iSamplingFreq * 100, (char*)m_sStreamerParams.c_str());
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        QMessageBox msgBox;
        msgBox.setText("Failed to start streaming.");
        msgBox.exec();
        return false;
    }
    m_bIsRunning = true;
    QThread::start();
    return true;
}

//*************************************************************************************************************

bool BrainFlowBoard::stop()
{
    try {
        m_pBoardShim->stop_stream();
        m_bIsRunning = false;
        m_pOutput->data()->clear();
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        return false;
    }
    return true;
}

//*************************************************************************************************************

IPlugin::PluginType BrainFlowBoard::getType() const
{
    return _ISensor;
}

//*************************************************************************************************************

QString BrainFlowBoard::getName() const
{
    return "BrainFlow Board Plugin";
}

//*************************************************************************************************************

QWidget* BrainFlowBoard::setupWidget()
{
    BrainFlowSetupWidget* widget = new BrainFlowSetupWidget(this);
    return widget;
}

//*************************************************************************************************************

void BrainFlowBoard::prepareSession(BrainFlowInputParams params,
                                    std::string streamerParams,
                                    int boardId,
                                    int dataType,
                                    int vertScale)
{
    if (m_pBoardShim)
    {
        QMessageBox msgBox;
        msgBox.setText("Streaming session already prepared.");
        msgBox.exec();
        return;
    }

    BoardShim::log_message((int)LogLevels::LEVEL_ERROR, "Vert Scale is %d %d", vertScale);
    QMessageBox msgBox;
    m_iBoardId = boardId;
    m_sStreamerParams = streamerParams;

    try {
        switch (dataType)
        {
            case 0:
                m_pChannels = BoardShim::get_eeg_channels(boardId, &m_iNumberChannels);
                break;
            case 1:
                m_pChannels = BoardShim::get_emg_channels(boardId, &m_iNumberChannels);
                break;
            case 2:
                m_pChannels = BoardShim::get_ecg_channels(boardId, &m_iNumberChannels);
                break;
            case 3:
                m_pChannels = BoardShim::get_eog_channels(boardId, &m_iNumberChannels);
                break;
            case 4:
                m_pChannels = BoardShim::get_eda_channels(boardId, &m_iNumberChannels);
                break;
            default:
                throw BrainFlowException ("unsupported data type", UNSUPPORTED_BOARD_ERROR);
                break;
        }
        m_iSamplingFreq = BoardShim::get_sampling_rate(boardId);
        m_pBoardShim = new BoardShim(m_iBoardId, params);
        m_pBoardShim->prepare_session();

        setUpFiffInfo();

        m_pOutput->data()->initFromFiffInfo(m_pFiffInfo);
        m_pOutput->data()->setMultiArraySize(1);

        msgBox.setText("Streaming session is ready");
    } catch (const BrainFlowException &err) {
        BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
        msgBox.setText("Invalid parameters. Check logs for details.");

        delete[] m_pChannels;
        delete m_pBoardShim;
        m_pChannels = NULL;
        m_pBoardShim = NULL;
        m_iNumberChannels = 0;
    }

    msgBox.exec();
}

//*************************************************************************************************************

void BrainFlowBoard::configureBoard(std::string config)
{
    QMessageBox msgBox;
    if (!m_pBoardShim)
    {
        msgBox.setText("Prepare Session first.");
        return;
    }
    else
    {
        try {
            m_pBoardShim->config_board((char *)config.c_str());
            msgBox.setText("Configured.");
        } catch (const BrainFlowException &err) {
            BoardShim::log_message((int)LogLevels::LEVEL_ERROR, err.what());
            msgBox.setText("Failed to Configure.");
        }
    }
    msgBox.exec();
}

//*************************************************************************************************************

void BrainFlowBoard::run()
{
    int iMinSamples = 10;
    unsigned long lSamplingPeriod = (unsigned long)(1000000.0 / m_iSamplingFreq) * iMinSamples;
    int numRows = BoardShim::get_num_rows(m_iBoardId);

    double **data = NULL;
    int dataCount = 0;
    while(m_bIsRunning)
    {
        usleep(lSamplingPeriod);
        data = m_pBoardShim->get_board_data (&dataCount);
        if (dataCount == 0)
        {
            continue;
        }

        Eigen::MatrixXd matrix (m_iNumberChannels, dataCount);
        for (int j = 0; j < m_iNumberChannels; j++)
        {
            for (int i = 0; i < dataCount; i++)
            {
                matrix(j, i) = data[m_pChannels[j]][i];
            }
        }

        m_pOutput->data()->setValue(matrix);

        for (int i = 0; i < numRows; i++)
        {
            delete[] data[i];
        }
        delete[] data;
    }
}

//*************************************************************************************************************

void BrainFlowBoard::releaseSession(bool useQmessage)
{
    if (m_pBoardShim)
    {
        try {
            m_pBoardShim->release_session();
        } catch (...) {
        }
    }
    delete m_pBoardShim;
    delete[] m_pChannels;

    for (int i = 0; i < m_iNumberChannels; i++)
    {
        m_outputConnectors.pop_back();
    }
    m_pBoardShim = NULL;
    m_pChannels = NULL;
    m_iBoardId = (int)BoardIds::SYNTHETIC_BOARD;
    m_bIsRunning = false;
    m_pOutput = NULL;
    m_iSamplingFreq = 0;
    m_sStreamerParams = "";
    m_iNumberChannels = 0;

    if (useQmessage)
    {
        QMessageBox msgBox;
        msgBox.setText("Released.");
        msgBox.exec();
    }
}
<|endoftext|>


<commit_before>#pragma once

#include <type_traits>

#include <boost/mpl/size.hpp>
#include <boost/mpl/unique.hpp>
#include <boost/mpl/vector.hpp>

namespace blackhole {

/// Metafunction, that detects whether a parameter pack has duplicate items.
template<typename... Args>
struct unique :
    public std::conditional<
        sizeof...(Args) == boost::mpl::size<
            typename boost::mpl::unique<
                boost::mpl::vector<Args...>,
                std::is_same<boost::mpl::_1, boost::mpl::_2>
            >::type
        >::value,
        std::true_type,
        std::false_type
    >::type
{};

} // namespace blackhole
<commit_msg>[GCC 4.4] Fuck you. Workaround variadic pack expansion.<commit_after>#pragma once

#include <type_traits>

#include <boost/mpl/size.hpp>
#include <boost/mpl/unique.hpp>
#include <boost/mpl/vector.hpp>

namespace blackhole {

/// General definition of the helper class
template<typename... Args> struct from_variadic;

/// This one handles the case when no types passed.
template<>
struct from_variadic<> {
    typedef boost::mpl::vector<> type;
};

/// This one is a specialization for the case when only one type is passed.
template<typename T>
struct from_variadic<T> {
    typedef boost::mpl::vector<T> type;
};

/*!
 * This specialization does the actual job.
 * It splits the whole pack into 2 parts: one with single type T and the rest
 * of types Args.
 * As soon as it is done T is added to an mpl::vector.
 */
template<typename T, typename... Args>
struct from_variadic<T, Args...> {
    typedef typename boost::mpl::push_front<
        typename from_variadic<Args...>::type,
        T
    >::type type;
};

/// Metafunction, that detects whether a parameter pack has duplicate items.
template<typename... Args>
struct unique :
    public std::conditional<
        sizeof...(Args) == boost::mpl::size<
            typename boost::mpl::unique<
                typename from_variadic<Args...>::type,
                std::is_same<boost::mpl::_1, boost::mpl::_2>
            >::type
        >::value,
        std::true_type,
        std::false_type
    >::type
{};

} // namespace blackhole
<|endoftext|>


<commit_before>#pragma once

#include <type_traits>

#include <boost/mpl/size.hpp>
#include <boost/mpl/unique.hpp>
#include <boost/mpl/vector.hpp>

namespace blackhole {

/// Metafunction, that determines whether a parameter pack has duplicate items.
template<typename... Args>
struct unique :
    public std::conditional<
        sizeof...(Args) == boost::mpl::size<
            typename boost::mpl::unique<
                boost::mpl::vector<Args...>,
                std::is_same<boost::mpl::_1, boost::mpl::_2>
            >::type
        >::value,
        std::true_type,
        std::false_type
    >::type
{};

} // namespace blackhole
<commit_msg>[Code Style] Wording.<commit_after>#pragma once

#include <type_traits>

#include <boost/mpl/size.hpp>
#include <boost/mpl/unique.hpp>
#include <boost/mpl/vector.hpp>

namespace blackhole {

/// Metafunction, that detects whether a parameter pack has duplicate items.
template<typename... Args>
struct unique :
    public std::conditional<
        sizeof...(Args) == boost::mpl::size<
            typename boost::mpl::unique<
                boost::mpl::vector<Args...>,
                std::is_same<boost::mpl::_1, boost::mpl::_2>
            >::type
        >::value,
        std::true_type,
        std::false_type
    >::type
{};

} // namespace blackhole
<|endoftext|>


<commit_before>/*
 * THE NEW CHRONOTEXT TOOLKIT: https://github.com/arielm/new-chronotext-toolkit
 * COPYRIGHT (C) 2012-2014, ARIEL MALKA ALL RIGHTS RESERVED.
 *
 * THE FOLLOWING SOURCE-CODE IS DISTRIBUTED UNDER THE MODIFIED BSD LICENSE:
 * https://github.com/arielm/new-chronotext-toolkit/blob/master/LICENSE.md
 */

#include "chronotext/path/FollowablePath.h"
#include "chronotext/utils/MathUtils.h"
#include "chronotext/utils/Utils.h"

using namespace ci;
using namespace std;

namespace chronotext
{
    FollowablePath::FollowablePath(Mode mode, int capacity)
    :
    mode(mode)
    {
        if (capacity > 0)
        {
            points.reserve(capacity);
            len.reserve(capacity);
        }
    }

    FollowablePath::FollowablePath(const vector<Vec2f> &points, Mode mode)
    :
    mode(mode)
    {
        add(points);
    }

    FollowablePath::FollowablePath(DataSourceRef source, Mode mode)
    :
    mode(mode)
    {
        read(source);
    }

    void FollowablePath::read(DataSourceRef source)
    {
        auto stream = source->createStream();

        int newPointsSize;
        stream->readLittle(&newPointsSize);

        extendCapacity(newPointsSize);

        // ---

        Vec2f point;

        for (int i = 0; i < newPointsSize; i++)
        {
            stream->readLittle(&point.x);
            stream->readLittle(&point.y);
            add(point);
        }
    }

    void FollowablePath::write(DataTargetRef target)
    {
        auto stream = target->getStream();

        stream->writeLittle(size());

        for (auto &point : points)
        {
            stream->writeLittle(point.x);
            stream->writeLittle(point.y);
        }
    }

    void FollowablePath::add(const vector<Vec2f> &newPoints)
    {
        extendCapacity(newPoints.size());

        for (auto &point : newPoints)
        {
            add(point);
        }
    }

    void FollowablePath::add(const ci::Vec2f &point)
    {
        if (!points.empty())
        {
            Vec2f delta = point - points.back();

            if (delta != Vec2f::zero())
            {
                len.push_back(len.back() + delta.length());
            }
            else
            {
                return;
            }
        }
        else
        {
            len.push_back(0);
        }

        points.push_back(point);
    }

    void FollowablePath::clear()
    {
        points.clear();
        len.clear();
    }

    int FollowablePath::size() const
    {
        return points.size();
    }

    bool FollowablePath::empty() const
    {
        return points.empty();
    }

    float FollowablePath::getLength() const
    {
        if (!points.empty())
        {
            return len.back();
        }
        else
        {
            return 0;
        }
    }

    Rectf FollowablePath::getBounds() const
    {
        float minX = numeric_limits<float>::max();
        float minY = numeric_limits<float>::max();
        float maxX = numeric_limits<float>::min();
        float maxY = numeric_limits<float>::min();

        for (auto &point : points)
        {
            if (point.x < minX) minX = point.x;
            if (point.y < minY) minY = point.y;

            if (point.x > maxX) maxX = point.x;
            if (point.y > maxY) maxY = point.y;
        }

        return Rectf(minX, minY, maxX, maxY);
    }

    void FollowablePath::close()
    {
        if (size() > 2)
        {
            if (points.front() != points.back())
            {
                add(points.front());
            }
        }
    }

    FollowablePath::Value FollowablePath::pos2Value(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = 0;
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = length;
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        float ratio = (pos - len[index]) / (len[index + 1] - len[index]);

        FollowablePath::Value value;
        value.point = p0 + (p1 - p0) * ratio;
        value.angle = math<float>::atan2(p1.y - p0.y, p1.x - p0.x);
        value.position = pos;

        return value;
    }

    Vec2f FollowablePath::pos2Point(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    return points.front();
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    return points.back();
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        float ratio = (pos - len[index]) / (len[index + 1] - len[index]);
        return p0 + (p1 - p0) * ratio;
    }

    float FollowablePath::pos2Angle(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = 0;
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = length;
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        return math<float>::atan2(p1.y - p0.y, p1.x - p0.x);
    }

    float FollowablePath::pos2SampledAngle(float pos, float sampleSize) const
    {
        Vec2f gradient = pos2Gradient(pos, sampleSize);

        /*
         * WE USE AN EPSILON VALUE TO AVOID
         * DEGENERATED RESULTS IN SOME EXTREME CASES
         * (E.G. CLOSE TO 180 DEGREE DIFF. BETWEEN TWO SEGMENTS)
         */
        if (gradient.lengthSquared() > 1.0)
        {
            return math<float>::atan2(gradient.y, gradient.x);
        }
        else
        {
            return pos2Angle(pos);
        }
    }

    Vec2f FollowablePath::pos2Gradient(float pos, float sampleSize) const
    {
        Vec2f pm = pos2Point(pos - sampleSize * 0.5f);
        Vec2f pp = pos2Point(pos + sampleSize * 0.5f);

        return (pp - pm) * 0.5f;
    }

    /*
     * RETURNS false IF CLOSEST POINT IS FARTHER THAN threshold DISTANCE
     *
     * REFERENCE: "Minimum Distance between a Point and a Line" BY Paul Bourke
     * http://paulbourke.net/geometry/pointlineplane/
     */
    bool FollowablePath::findClosestPoint(const Vec2f &point, float threshold, FollowablePath::ClosePoint &res) const
    {
        float min = threshold * threshold; // BECAUSE IT IS MORE EFFICIENT TO WORK WITH MAGNIFIED DISTANCES

        int index = -1;
        int _size = size();
        Vec2f _point;
        float _len;

        for (int i = 0; i < _size; i++)
        {
            int i0, i1;

            if (i == _size - 1)
            {
                i0 = i - 1;
                i1 = i;
            }
            else
            {
                i0 = i;
                i1 = i + 1;
            }

            auto p0 = points[i0];
            auto p1 = points[i1];

            Vec2f delta = p1 - p0;
            float l = len[i1] - len[i0];
            float u = delta.dot(point - p0) / (l * l);

            if (u >= 0 && u <= 1)
            {
                Vec2f p = p0 + u * delta;
                float mag = (p - point).lengthSquared();

                if (mag < min)
                {
                    min = mag;
                    index = i0;

                    _point = p;
                    _len = len[index] + u * l;
                }
            }
            else
            {
                float mag0 = (p0 - point).lengthSquared();
                float mag1 = (p1 - point).lengthSquared();

                if ((mag0 < min) && (mag0 < mag1))
                {
                    min = mag0;
                    index = i0;

                    _point = points[i0];
                    _len = len[index];
                }
                else if ((mag1 < min) && (mag1 < mag0))
                {
                    min = mag1;
                    index = i1;

                    _point = points[i1];
                    _len = len[index];
                }
            }
        }

        if (index != -1)
        {
            res.point = _point;
            res.position = _len;
            res.distance = math<float>::sqrt(min);

            return true;
        }

        return false;
    }

    /*
     * segmentIndex MUST BE < size
     *
     * REFERENCE: "Minimum Distance between a Point and a Line" BY Paul Bourke
     * http://paulbourke.net/geometry/pointlineplane/
     */
    FollowablePath::ClosePoint FollowablePath::closestPointFromSegment(const Vec2f &point, int segmentIndex) const
    {
        FollowablePath::ClosePoint res;

        int i0 = segmentIndex;
        int i1 = segmentIndex + 1;

        auto p0 = points[i0];
        auto p1 = points[i1];

        Vec2f delta = p1 - p0;
        float l = len[i1] - len[i0];
        float u = delta.dot(point - p0) / (l * l);

        if (u >= 0 && u <= 1)
        {
            Vec2f p = p0 + u * delta;
            float mag = (p - point).lengthSquared();

            res.point = p;
            res.position = len[i0] + u * l;
            res.distance = math<float>::sqrt(mag);
        }
        else
        {
            float mag0 = (p0 - point).lengthSquared();
            float mag1 = (p1 - point).lengthSquared();

            if (mag0 < mag1)
            {
                res.point = p0;
                res.position = len[i0];
                res.distance = math<float>::sqrt(mag0);
            }
            else
            {
                res.point = p1;
                res.position = len[i1];
                res.distance = math<float>::sqrt(mag1);
            }
        }

        return res;
    }

    void FollowablePath::extendCapacity(int amount)
    {
        int newCapacity = size() + amount;
        points.reserve(newCapacity);
        len.reserve(newCapacity);
    }
}
<commit_msg>COSMETICS<commit_after>/*
 * THE NEW CHRONOTEXT TOOLKIT: https://github.com/arielm/new-chronotext-toolkit
 * COPYRIGHT (C) 2012-2014, ARIEL MALKA ALL RIGHTS RESERVED.
 *
 * THE FOLLOWING SOURCE-CODE IS DISTRIBUTED UNDER THE MODIFIED BSD LICENSE:
 * https://github.com/arielm/new-chronotext-toolkit/blob/master/LICENSE.md
 */

#include "chronotext/path/FollowablePath.h"
#include "chronotext/utils/MathUtils.h"
#include "chronotext/utils/Utils.h"

using namespace ci;
using namespace std;

namespace chronotext
{
    FollowablePath::FollowablePath(Mode mode, int capacity)
    :
    mode(mode)
    {
        if (capacity > 0)
        {
            extendCapacity(capacity);
        }
    }

    FollowablePath::FollowablePath(const vector<Vec2f> &points, Mode mode)
    :
    mode(mode)
    {
        add(points);
    }

    FollowablePath::FollowablePath(DataSourceRef source, Mode mode)
    :
    mode(mode)
    {
        read(source);
    }

    void FollowablePath::read(DataSourceRef source)
    {
        auto stream = source->createStream();

        int newPointsSize;
        stream->readLittle(&newPointsSize);

        extendCapacity(newPointsSize);

        // ---

        Vec2f point;

        for (int i = 0; i < newPointsSize; i++)
        {
            stream->readLittle(&point.x);
            stream->readLittle(&point.y);
            add(point);
        }
    }

    void FollowablePath::write(DataTargetRef target)
    {
        auto stream = target->getStream();

        stream->writeLittle(size());

        for (auto &point : points)
        {
            stream->writeLittle(point.x);
            stream->writeLittle(point.y);
        }
    }

    void FollowablePath::add(const vector<Vec2f> &newPoints)
    {
        extendCapacity(newPoints.size());

        for (auto &point : newPoints)
        {
            add(point);
        }
    }

    void FollowablePath::add(const ci::Vec2f &point)
    {
        if (!points.empty())
        {
            Vec2f delta = point - points.back();

            if (delta != Vec2f::zero())
            {
                len.push_back(len.back() + delta.length());
            }
            else
            {
                return;
            }
        }
        else
        {
            len.push_back(0);
        }

        points.push_back(point);
    }

    void FollowablePath::clear()
    {
        points.clear();
        len.clear();
    }

    int FollowablePath::size() const
    {
        return points.size();
    }

    bool FollowablePath::empty() const
    {
        return points.empty();
    }

    float FollowablePath::getLength() const
    {
        if (!points.empty())
        {
            return len.back();
        }
        else
        {
            return 0;
        }
    }

    Rectf FollowablePath::getBounds() const
    {
        float minX = numeric_limits<float>::max();
        float minY = numeric_limits<float>::max();
        float maxX = numeric_limits<float>::min();
        float maxY = numeric_limits<float>::min();

        for (auto &point : points)
        {
            if (point.x < minX) minX = point.x;
            if (point.y < minY) minY = point.y;

            if (point.x > maxX) maxX = point.x;
            if (point.y > maxY) maxY = point.y;
        }

        return Rectf(minX, minY, maxX, maxY);
    }

    void FollowablePath::close()
    {
        if (size() > 2)
        {
            if (points.front() != points.back())
            {
                add(points.front());
            }
        }
    }

    FollowablePath::Value FollowablePath::pos2Value(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = 0;
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = length;
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        float ratio = (pos - len[index]) / (len[index + 1] - len[index]);

        FollowablePath::Value value;
        value.point = p0 + (p1 - p0) * ratio;
        value.angle = math<float>::atan2(p1.y - p0.y, p1.x - p0.x);
        value.position = pos;

        return value;
    }

    Vec2f FollowablePath::pos2Point(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    return points.front();
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    return points.back();
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        float ratio = (pos - len[index]) / (len[index + 1] - len[index]);
        return p0 + (p1 - p0) * ratio;
    }

    float FollowablePath::pos2Angle(float pos) const
    {
        float length = len.back();

        if (mode == MODE_LOOP || mode == MODE_MODULO)
        {
            pos = boundf(pos, length);
        }
        else
        {
            if (pos <= 0)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = 0;
                }
            }
            else if (pos >= length)
            {
                if (mode == MODE_BOUNDED)
                {
                    pos = length;
                }
            }
        }

        int index = search(len, pos, 1, size());
        auto p0 = points[index];
        auto p1 = points[index + 1];

        return math<float>::atan2(p1.y - p0.y, p1.x - p0.x);
    }

    float FollowablePath::pos2SampledAngle(float pos, float sampleSize) const
    {
        Vec2f gradient = pos2Gradient(pos, sampleSize);

        /*
         * WE USE AN EPSILON VALUE TO AVOID
         * DEGENERATED RESULTS IN SOME EXTREME CASES
         * (E.G. CLOSE TO 180 DEGREE DIFF. BETWEEN TWO SEGMENTS)
         */
        if (gradient.lengthSquared() > 1.0)
        {
            return math<float>::atan2(gradient.y, gradient.x);
        }
        else
        {
            return pos2Angle(pos);
        }
    }

    Vec2f FollowablePath::pos2Gradient(float pos, float sampleSize) const
    {
        Vec2f pm = pos2Point(pos - sampleSize * 0.5f);
        Vec2f pp = pos2Point(pos + sampleSize * 0.5f);

        return (pp - pm) * 0.5f;
    }

    /*
     * RETURNS false IF CLOSEST POINT IS FARTHER THAN threshold DISTANCE
     *
     * REFERENCE: "Minimum Distance between a Point and a Line" BY Paul Bourke
     * http://paulbourke.net/geometry/pointlineplane/
     */
    bool FollowablePath::findClosestPoint(const Vec2f &point, float threshold, FollowablePath::ClosePoint &res) const
    {
        float min = threshold * threshold; // BECAUSE IT IS MORE EFFICIENT TO WORK WITH MAGNIFIED DISTANCES

        int index = -1;
        int _size = size();
        Vec2f _point;
        float _len;

        for (int i = 0; i < _size; i++)
        {
            int i0, i1;

            if (i == _size - 1)
            {
                i0 = i - 1;
                i1 = i;
            }
            else
            {
                i0 = i;
                i1 = i + 1;
            }

            auto p0 = points[i0];
            auto p1 = points[i1];

            Vec2f delta = p1 - p0;
            float l = len[i1] - len[i0];
            float u = delta.dot(point - p0) / (l * l);

            if (u >= 0 && u <= 1)
            {
                Vec2f p = p0 + u * delta;
                float mag = (p - point).lengthSquared();

                if (mag < min)
                {
                    min = mag;
                    index = i0;

                    _point = p;
                    _len = len[index] + u * l;
                }
            }
            else
            {
                float mag0 = (p0 - point).lengthSquared();
                float mag1 = (p1 - point).lengthSquared();

                if ((mag0 < min) && (mag0 < mag1))
                {
                    min = mag0;
                    index = i0;

                    _point = points[i0];
                    _len = len[index];
                }
                else if ((mag1 < min) && (mag1 < mag0))
                {
                    min = mag1;
                    index = i1;

                    _point = points[i1];
                    _len = len[index];
                }
            }
        }

        if (index != -1)
        {
            res.point = _point;
            res.position = _len;
            res.distance = math<float>::sqrt(min);

            return true;
        }

        return false;
    }

    /*
     * segmentIndex MUST BE < size
     *
     * REFERENCE: "Minimum Distance between a Point and a Line" BY Paul Bourke
     * http://paulbourke.net/geometry/pointlineplane/
     */
    FollowablePath::ClosePoint FollowablePath::closestPointFromSegment(const Vec2f &point, int segmentIndex) const
    {
        FollowablePath::ClosePoint res;

        int i0 = segmentIndex;
        int i1 = segmentIndex + 1;

        auto p0 = points[i0];
        auto p1 = points[i1];

        Vec2f delta = p1 - p0;
        float l = len[i1] - len[i0];
        float u = delta.dot(point - p0) / (l * l);

        if (u >= 0 && u <= 1)
        {
            Vec2f p = p0 + u * delta;
            float mag = (p - point).lengthSquared();

            res.point = p;
            res.position = len[i0] + u * l;
            res.distance = math<float>::sqrt(mag);
        }
        else
        {
            float mag0 = (p0 - point).lengthSquared();
            float mag1 = (p1 - point).lengthSquared();

            if (mag0 < mag1)
            {
                res.point = p0;
                res.position = len[i0];
                res.distance = math<float>::sqrt(mag0);
            }
            else
            {
                res.point = p1;
                res.position = len[i1];
                res.distance = math<float>::sqrt(mag1);
            }
        }

        return res;
    }

    void FollowablePath::extendCapacity(int amount)
    {
        int newCapacity = size() + amount;
        points.reserve(newCapacity);
        len.reserve(newCapacity);
    }
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "MockLinkMissionItemHandler.h"
#include "MockLink.h"

#include <QDebug>

QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")

MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol)
    : _mockLink(mockLink)
    , _missionItemResponseTimer(NULL)
    , _failureMode(FailNone)
    , _sendHomePositionOnEmptyList(false)
    , _mavlinkProtocol(mavlinkProtocol)
    , _failReadRequestListFirstResponse(true)
    , _failReadRequest1FirstResponse(true)
    , _failWriteMissionCountFirstResponse(true)
{
    Q_ASSERT(mockLink);
}

MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
{

}

void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
{
    if (!_missionItemResponseTimer) {
        _missionItemResponseTimer = new QTimer();
        connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout);
    }
    _missionItemResponseTimer->start(500);
}

bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        _handleMissionRequestList(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST:
        _handleMissionRequest(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM:
        _handleMissionItem(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_COUNT:
        _handleMissionCount(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ACK:
        // Acks are received back for each MISSION_ITEM message
        break;

    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        // Sets the currently active mission item
        break;

    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
        // Delete all mission items
        _missionItems.clear();
        break;

    default:
        return false;
    }

    return true;
}

void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";

    _failReadRequest1FirstResponse = true;

    if (_failureMode == FailReadRequestListNoResponse) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse";
    } else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) {
        _failReadRequestListFirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse";
    } else {
        mavlink_mission_request_list_t request;

        _failReadRequestListFirstResponse = true;
        mavlink_msg_mission_request_list_decode(&msg, &request);

        Q_ASSERT(request.target_system == _mockLink->vehicleId());

        _requestType = (MAV_MISSION_TYPE)request.mission_type;

        int itemCount;
        switch (_requestType) {
        case MAV_MISSION_TYPE_MISSION:
            itemCount = _missionItems.count();
            if (itemCount == 0 && _sendHomePositionOnEmptyList) {
                itemCount = 1;
            }
            break;
        case MAV_MISSION_TYPE_FENCE:
            itemCount = _fenceItems.count();
            break;
        case MAV_MISSION_TYPE_RALLY:
            itemCount = _rallyItems.count();
            break;
        default:
            Q_ASSERT(false);
        }

        mavlink_message_t   responseMsg;

        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
                                            MAV_COMP_ID_MISSIONPLANNER,
                                            _mockLink->mavlinkChannel(),
                                            &responseMsg,               // Outgoing message
                                            msg.sysid,                  // Target is original sender
                                            msg.compid,                 // Target is original sender
                                            itemCount,                  // Number of mission items
                                            _requestType);
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
}

void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";

    mavlink_mission_request_t request;

    mavlink_msg_mission_request_decode(&msg, &request);

    Q_ASSERT(request.target_system == _mockLink->vehicleId());

    if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
    } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse";
    } else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) {
        _failReadRequest1FirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse";
    } else {
        // FIXME: Track whether all items are requested, or requested in sequence

        if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) ||
                (_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) {
            // Send back the incorrect sequence number
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number";
            request.seq++;
        }

        if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
                (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_mission_item_t  item;
            mavlink_message_t       responseMsg;

            switch (request.mission_type) {
            case MAV_MISSION_TYPE_MISSION:
                if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
                    item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
                    item.command = MAV_CMD_NAV_WAYPOINT;
                    item.current = false;
                    item.autocontinue = true;
                    item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
                } else {
                    item = _missionItems[request.seq];
                }
                break;
            case MAV_MISSION_TYPE_FENCE:
                item = _fenceItems[request.seq];
                break;
            case MAV_MISSION_TYPE_RALLY:
                item = _rallyItems[request.seq];
                break;
            default:
                Q_ASSERT(false);
            }

            mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
                                               MAV_COMP_ID_MISSIONPLANNER,
                                               _mockLink->mavlinkChannel(),
                                               &responseMsg,            // Outgoing message
                                               msg.sysid,               // Target is original sender
                                               msg.compid,              // Target is original sender
                                               request.seq,             // Index of mission item being sent
                                               item.frame,
                                               item.command,
                                               item.current,
                                               item.autocontinue,
                                               item.param1, item.param2, item.param3, item.param4,
                                               item.x, item.y, item.z,
                                               _requestType);
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
}

void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
{
    mavlink_mission_count_t missionCount;

    mavlink_msg_mission_count_decode(&msg, &missionCount);
    Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());

    _requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
    _writeSequenceCount = missionCount.count;
    Q_ASSERT(_writeSequenceCount >= 0);

    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;

    switch (missionCount.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems.clear();
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems.clear();
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems.clear();
        break;
    }

    if (_writeSequenceCount == 0) {
        _sendAck(MAV_MISSION_ACCEPTED);
    } else {
        if (_failureMode == FailWriteMissionCountNoResponse) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) {
            _failWriteMissionCountFirstResponse = false;
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        _failWriteMissionCountFirstResponse = true;
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
    }
}

void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;

    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
    } else {
        if (sequenceNumber >= _writeSequenceCount) {
            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
            return;
        }

        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
                (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
            sequenceNumber ++;
        }

        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
                (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_message_t message;

            mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
                                                  MAV_COMP_ID_MISSIONPLANNER,
                                                  _mockLink->mavlinkChannel(),
                                                  &message,
                                                  _mavlinkProtocol->getSystemId(),
                                                  _mavlinkProtocol->getComponentId(),
                                                  sequenceNumber,
                                                  _requestType);
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
    }
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;

    mavlink_message_t message;

    mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
                                      MAV_COMP_ID_MISSIONPLANNER,
                                      _mockLink->mavlinkChannel(),
                                      &message,
                                      _mavlinkProtocol->getSystemId(),
                                      _mavlinkProtocol->getComponentId(),
                                      ackType,
                                      _requestType);
    _mockLink->respondWithMavlinkMessage(message);
}

void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";

    _missionItemResponseTimer->stop();

    mavlink_mission_item_t missionItem;

    mavlink_msg_mission_item_decode(&msg, &missionItem);

    Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());

    switch (missionItem.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems[missionItem.seq] = missionItem;
        break;
    }

    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
    } else {
        if (_failureMode != FailWriteFinalAckNoResponse) {
            MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;

            if (_failureMode ==  FailWriteFinalAckErrorAck) {
                ack = MAV_MISSION_ERROR;
            }
            _sendAck(ack);
        }
    }
}

void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
{
    qWarning() << "Timeout waiting for next MISSION_ITEM";
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType)
{
    _sendAck(ackType);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void)
{
    // FIXME: NYI
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
{
    // FIXME: NYI
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
{
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
    }
}
<commit_msg>Fix CLEAR_ALL<commit_after>/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "MockLinkMissionItemHandler.h"
#include "MockLink.h"

#include <QDebug>

QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")

MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol)
    : _mockLink(mockLink)
    , _missionItemResponseTimer(NULL)
    , _failureMode(FailNone)
    , _sendHomePositionOnEmptyList(false)
    , _mavlinkProtocol(mavlinkProtocol)
    , _failReadRequestListFirstResponse(true)
    , _failReadRequest1FirstResponse(true)
    , _failWriteMissionCountFirstResponse(true)
{
    Q_ASSERT(mockLink);
}

MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
{

}

void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
{
    if (!_missionItemResponseTimer) {
        _missionItemResponseTimer = new QTimer();
        connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout);
    }
    _missionItemResponseTimer->start(500);
}

bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        _handleMissionRequestList(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST:
        _handleMissionRequest(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM:
        _handleMissionItem(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_COUNT:
        _handleMissionCount(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ACK:
        // Acks are received back for each MISSION_ITEM message
        break;

    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        // Sets the currently active mission item
        break;

    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
        // Delete all plan items
        _missionItems.clear();
        _fenceItems.clear();
        _rallyItems.clear();
        _sendAck(MAV_MISSION_ACCEPTED);
        break;

    default:
        return false;
    }

    return true;
}

void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";

    _failReadRequest1FirstResponse = true;

    if (_failureMode == FailReadRequestListNoResponse) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse";
    } else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) {
        _failReadRequestListFirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse";
    } else {
        mavlink_mission_request_list_t request;

        _failReadRequestListFirstResponse = true;
        mavlink_msg_mission_request_list_decode(&msg, &request);

        Q_ASSERT(request.target_system == _mockLink->vehicleId());

        _requestType = (MAV_MISSION_TYPE)request.mission_type;

        int itemCount;
        switch (_requestType) {
        case MAV_MISSION_TYPE_MISSION:
            itemCount = _missionItems.count();
            if (itemCount == 0 && _sendHomePositionOnEmptyList) {
                itemCount = 1;
            }
            break;
        case MAV_MISSION_TYPE_FENCE:
            itemCount = _fenceItems.count();
            break;
        case MAV_MISSION_TYPE_RALLY:
            itemCount = _rallyItems.count();
            break;
        default:
            Q_ASSERT(false);
        }

        mavlink_message_t   responseMsg;

        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
                                            MAV_COMP_ID_MISSIONPLANNER,
                                            _mockLink->mavlinkChannel(),
                                            &responseMsg,               // Outgoing message
                                            msg.sysid,                  // Target is original sender
                                            msg.compid,                 // Target is original sender
                                            itemCount,                  // Number of mission items
                                            _requestType);
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
}

void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";

    mavlink_mission_request_t request;

    mavlink_msg_mission_request_decode(&msg, &request);

    Q_ASSERT(request.target_system == _mockLink->vehicleId());

    if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
    } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse";
    } else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) {
        _failReadRequest1FirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse";
    } else {
        // FIXME: Track whether all items are requested, or requested in sequence

        if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) ||
                (_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) {
            // Send back the incorrect sequence number
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number";
            request.seq++;
        }

        if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
                (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_mission_item_t  item;
            mavlink_message_t       responseMsg;

            switch (request.mission_type) {
            case MAV_MISSION_TYPE_MISSION:
                if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
                    item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
                    item.command = MAV_CMD_NAV_WAYPOINT;
                    item.current = false;
                    item.autocontinue = true;
                    item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
                } else {
                    item = _missionItems[request.seq];
                }
                break;
            case MAV_MISSION_TYPE_FENCE:
                item = _fenceItems[request.seq];
                break;
            case MAV_MISSION_TYPE_RALLY:
                item = _rallyItems[request.seq];
                break;
            default:
                Q_ASSERT(false);
            }

            mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
                                               MAV_COMP_ID_MISSIONPLANNER,
                                               _mockLink->mavlinkChannel(),
                                               &responseMsg,            // Outgoing message
                                               msg.sysid,               // Target is original sender
                                               msg.compid,              // Target is original sender
                                               request.seq,             // Index of mission item being sent
                                               item.frame,
                                               item.command,
                                               item.current,
                                               item.autocontinue,
                                               item.param1, item.param2, item.param3, item.param4,
                                               item.x, item.y, item.z,
                                               _requestType);
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
}

void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
{
    mavlink_mission_count_t missionCount;

    mavlink_msg_mission_count_decode(&msg, &missionCount);
    Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());

    _requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
    _writeSequenceCount = missionCount.count;
    Q_ASSERT(_writeSequenceCount >= 0);

    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;

    switch (missionCount.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems.clear();
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems.clear();
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems.clear();
        break;
    }

    if (_writeSequenceCount == 0) {
        _sendAck(MAV_MISSION_ACCEPTED);
    } else {
        if (_failureMode == FailWriteMissionCountNoResponse) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) {
            _failWriteMissionCountFirstResponse = false;
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        _failWriteMissionCountFirstResponse = true;
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
    }
}

void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;

    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
    } else {
        if (sequenceNumber >= _writeSequenceCount) {
            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
            return;
        }

        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
                (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
            sequenceNumber ++;
        }

        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
                (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_message_t message;

            mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
                                                  MAV_COMP_ID_MISSIONPLANNER,
                                                  _mockLink->mavlinkChannel(),
                                                  &message,
                                                  _mavlinkProtocol->getSystemId(),
                                                  _mavlinkProtocol->getComponentId(),
                                                  sequenceNumber,
                                                  _requestType);
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
    }
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;

    mavlink_message_t message;

    mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
                                      MAV_COMP_ID_MISSIONPLANNER,
                                      _mockLink->mavlinkChannel(),
                                      &message,
                                      _mavlinkProtocol->getSystemId(),
                                      _mavlinkProtocol->getComponentId(),
                                      ackType,
                                      _requestType);
    _mockLink->respondWithMavlinkMessage(message);
}

void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";

    _missionItemResponseTimer->stop();

    mavlink_mission_item_t missionItem;

    mavlink_msg_mission_item_decode(&msg, &missionItem);

    Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());

    switch (missionItem.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems[missionItem.seq] = missionItem;
        break;
    }

    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
    } else {
        if (_failureMode != FailWriteFinalAckNoResponse) {
            MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;

            if (_failureMode ==  FailWriteFinalAckErrorAck) {
                ack = MAV_MISSION_ERROR;
            }
            _sendAck(ack);
        }
    }
}

void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
{
    qWarning() << "Timeout waiting for next MISSION_ITEM";
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType)
{
    _sendAck(ackType);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void)
{
    // FIXME: NYI
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
{
    // FIXME: NYI
    Q_ASSERT(false);
}

void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
{
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
    }
}
<|endoftext|>