<commit_before>// Copyright 2010 Anders Bakken
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <QtTest/QtTest>

#include <textcursor.h>
#include <textdocument.h>
QT_FORWARD_DECLARE_CLASS(TextCursor)

//TESTED_CLASS=
//TESTED_FILES=

class tst_TextCursor : public QObject
{
    Q_OBJECT

public:
    tst_TextCursor();
    virtual ~tst_TextCursor();

private slots:
    void operatorEquals_data();
    void operatorEquals();
    void setPosition();
    void setPosition_data();
    void movePosition_data();
    void movePosition();
private:
    TextDocument document;
};

tst_TextCursor::tst_TextCursor()
{
    document.append("This is some text");
}

tst_TextCursor::~tst_TextCursor()
{
}

void tst_TextCursor::setPosition_data()
{
    QTest::addColumn<int>("one");
    QTest::addColumn<int>("two");
    QTest::addColumn<TextCursor::MoveMode>("moveMode");
    QTest::addColumn<int>("expectedPos");
    QTest::addColumn<int>("expectedAnchor");
    QTest::addColumn<QString>("selectedText");

    QTest::newRow("1") << 0 << 1 << TextCursor::MoveAnchor << 1 << 1 << QString();
    QTest::newRow("2") << 0 << 2 << TextCursor::KeepAnchor << 2 << 0 << QString("Fi");
    QTest::newRow("3") << 5 << 0 << TextCursor::KeepAnchor << 0 << 5 << QString("First");
}

void tst_TextCursor::setPosition()
{
    static QString text("First1\n"
                        "Second2\n"
                        "Third333\n"
                        "Fourth4444\n"
                        "Fifth55555\n");
    TextDocument document;
    document.setText(text);

    TextCursor cursor(&document);
    QFETCH(int, one);
    QFETCH(int, two);
    QFETCH(TextCursor::MoveMode, moveMode);
    QFETCH(int, expectedPos);
    QFETCH(int, expectedAnchor);
    QFETCH(QString, selectedText);

    cursor.setPosition(one);
    cursor.setPosition(two, moveMode);
    QCOMPARE(expectedPos, cursor.position());
    QCOMPARE(expectedAnchor, cursor.anchor());
    QCOMPARE(selectedText, cursor.selectedText());
}

void tst_TextCursor::movePosition_data()
{
    QTest::addColumn<int>("initialPosition");
    QTest::addColumn<int>("initialAnchor");
    QTest::addColumn<TextCursor::MoveOperation>("moveOperation");
    QTest::addColumn<TextCursor::MoveMode>("moveMode");
    QTest::addColumn<int>("expectedPosition");
    QTest::addColumn<int>("expectedAnchor");
    QTest::addColumn<QString>("selectedText");

    QTest::newRow("Start") << 20 << 20 << TextCursor::Start << TextCursor::MoveAnchor
                           << 0 << 0 << QString();
    QTest::newRow("Up") << 25 << 25 << TextCursor::Up
                        << TextCursor::MoveAnchor << 1 << 1 << QString();
    QTest::newRow("StartOfLine") << 24 << 25 << TextCursor::StartOfLine
                                 << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfLine2") << 35 << 35 << TextCursor::StartOfLine
                                  << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfBlock") << 85 << 85 << TextCursor::StartOfBlock
                                  << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfWord") << 28 << 28 << TextCursor::StartOfWord
                                 << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("PreviousBlock") << 89 << 89 << TextCursor::PreviousBlock
                                   << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("PreviousCharacter") << 0 << 0 << TextCursor::PreviousCharacter
                                       << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("PreviousWord") << 0 << 0 << TextCursor::PreviousWord
                                  << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("Left") << 0 << 0 << TextCursor::Left
                          << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("WordLeft") << 0 << 0 << TextCursor::WordLeft
                              << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("FirstCharacter") << 0 << 0 << TextCursor::NextCharacter
                                    << TextCursor::KeepAnchor << 1 << 0 << "I";

    QTest::newRow("FirstCharacterReverse") << 1 << 1 << TextCursor::PreviousCharacter
                                           << TextCursor::KeepAnchor << 0 << 1 << "I";
    QTest::newRow("LastCharacter") << 179 << 179 << TextCursor::NextCharacter
                                   << TextCursor::KeepAnchor << 180 << 179 << "\n";
    QTest::newRow("LastCharacterReverse") << 180 << 180 << TextCursor::PreviousCharacter
                                          << TextCursor::KeepAnchor << 179 << 180 << "\n";

    QTest::newRow("EmptyBlock") << 180 << 180 << TextCursor::NextBlock
                                << TextCursor::KeepAnchor << 181 << 180 << "\n";

    QTest::newRow("EmptyBlockReverse") << 181 << 181 << TextCursor::PreviousBlock
                                       << TextCursor::KeepAnchor << 180 << 181 << "\n";

//     QTest::newRow("End") << 0 << 0 << TextCursor::End
//                          << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("Down") << 0 << 0 << TextCursor::Down
//                           << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("EndOfLine") << 0 << 0 << TextCursor::EndOfLine
//                                << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("EndOfWord") << 0 << 0 << TextCursor::EndOfWord << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("EndOfBlock") << 0 << 0 << TextCursor::EndOfBlock << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextBlock") << 0 << 0 << TextCursor::NextBlock << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextCharacter") << 0 << 0 << TextCursor::NextCharacter << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextWord") << 0 << 0 << TextCursor::NextWord << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("Right") << 0 << 0 << TextCursor::Right << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("WordRight") << 0 << 0 << TextCursor::WordRight << TextCursor::MoveAnchor << 0 << 0;
}

void tst_TextCursor::movePosition()
{
    static const char *trueToLife = "I can only sing it loud\n" // 0 - 23
                                    "always try to sing it clear what the hell are we all doing here\n" // 24 - 87
                                    "making too much of nothing\n" // 88 - 114
                                    "or creating one unholy mess an unfair study in survival, I guess\n" // 115 - 179
                                    "\nSomething\n";  // 180 - 190
    TextDocument document;
    document.setText(QString::fromLatin1(trueToLife));

    TextCursor cursor(&document);
    QFETCH(int, initialPosition);
    QFETCH(int, initialAnchor);
    QFETCH(TextCursor::MoveOperation, moveOperation);
    QFETCH(TextCursor::MoveMode, moveMode);
    QFETCH(int, expectedPosition);
    QFETCH(int, expectedAnchor);
    QFETCH(QString, selectedText);

    cursor.setPosition(initialAnchor);
    if (initialPosition != initialAnchor) {
        cursor.setPosition(initialPosition, TextCursor::KeepAnchor);
    }
    Q_ASSERT(cursor.position() == initialPosition);
    Q_ASSERT(cursor.anchor() == initialAnchor);
    cursor.movePosition(moveOperation, moveMode);
    QCOMPARE(cursor.position(), expectedPosition);
    QCOMPARE(cursor.anchor(), expectedAnchor);
    QCOMPARE(cursor.selectedText(), selectedText);
}

void tst_TextCursor::operatorEquals_data()
{
    QTest::addColumn<TextCursor*>("left");
    QTest::addColumn<TextCursor*>("right");
    QTest::addColumn<bool>("expected");

    QTest::newRow("null vs null") << new TextCursor << new TextCursor << true;
    QTest::newRow("non null vs null") << new TextCursor(&document) << new TextCursor << false;

    QTest::newRow("normal vs normal equal") << new TextCursor(&document) << new TextCursor(&document) << true;
    TextCursor *cursor = new TextCursor(&document);
    cursor->setPosition(1);
    QTest::newRow("normal vs normal different") << new TextCursor(&document) << cursor << false;

    cursor = new TextCursor(&document);
    cursor->setPosition(1, TextCursor::KeepAnchor);

    TextCursor *cursor2 = new TextCursor(&document);
    cursor2->setPosition(1, TextCursor::KeepAnchor);
    QTest::newRow("selection vs selection") << cursor << cursor2 << true;

    cursor = new TextCursor(&document);
    cursor->setPosition(1, TextCursor::KeepAnchor);

    cursor2 = new TextCursor(&document);
    cursor2->setPosition(1, TextCursor::MoveAnchor);
    QTest::newRow("selection vs selection different") << cursor << cursor2 << false;

}

void tst_TextCursor::operatorEquals()
{
    QFETCH(TextCursor*, left);
    QFETCH(TextCursor*, right);
    QFETCH(bool, expected);

    const bool equal = (*left == *right);
    const bool notEqual = (*left != *right);
    delete left;
    delete right;
    QVERIFY(equal != notEqual);
    QCOMPARE(equal, expected);
}

Q_DECLARE_METATYPE(TextCursor *);
Q_DECLARE_METATYPE(TextCursor::MoveOperation);
Q_DECLARE_METATYPE(TextCursor::MoveMode);

QTEST_MAIN(tst_TextCursor)
#include "tst_textcursor.moc"
<commit_msg>Make test compile on AIX<commit_after>// Copyright 2010 Anders Bakken
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <QtTest/QtTest>

#include <textcursor.h>
#include <textdocument.h>
QT_FORWARD_DECLARE_CLASS(TextCursor)

//TESTED_CLASS=
//TESTED_FILES=

class tst_TextCursor : public QObject
{
    Q_OBJECT

public:
    tst_TextCursor();
    virtual ~tst_TextCursor();

private slots:
    void operatorEquals_data();
    void operatorEquals();
    void setPosition();
    void setPosition_data();
    void movePosition_data();
    void movePosition();
private:
    TextDocument document;
};

Q_DECLARE_METATYPE(TextCursor *);
Q_DECLARE_METATYPE(TextCursor::MoveOperation);
Q_DECLARE_METATYPE(TextCursor::MoveMode);

tst_TextCursor::tst_TextCursor()
{
    document.append("This is some text");
}

tst_TextCursor::~tst_TextCursor()
{
}

void tst_TextCursor::setPosition_data()
{
    QTest::addColumn<int>("one");
    QTest::addColumn<int>("two");
    QTest::addColumn<TextCursor::MoveMode>("moveMode");
    QTest::addColumn<int>("expectedPos");
    QTest::addColumn<int>("expectedAnchor");
    QTest::addColumn<QString>("selectedText");

    QTest::newRow("1") << 0 << 1 << TextCursor::MoveAnchor << 1 << 1 << QString();
    QTest::newRow("2") << 0 << 2 << TextCursor::KeepAnchor << 2 << 0 << QString("Fi");
    QTest::newRow("3") << 5 << 0 << TextCursor::KeepAnchor << 0 << 5 << QString("First");
}

void tst_TextCursor::setPosition()
{
    static QString text("First1\n"
                        "Second2\n"
                        "Third333\n"
                        "Fourth4444\n"
                        "Fifth55555\n");
    TextDocument document;
    document.setText(text);

    TextCursor cursor(&document);
    QFETCH(int, one);
    QFETCH(int, two);
    QFETCH(TextCursor::MoveMode, moveMode);
    QFETCH(int, expectedPos);
    QFETCH(int, expectedAnchor);
    QFETCH(QString, selectedText);

    cursor.setPosition(one);
    cursor.setPosition(two, moveMode);
    QCOMPARE(expectedPos, cursor.position());
    QCOMPARE(expectedAnchor, cursor.anchor());
    QCOMPARE(selectedText, cursor.selectedText());
}

void tst_TextCursor::movePosition_data()
{
    QTest::addColumn<int>("initialPosition");
    QTest::addColumn<int>("initialAnchor");
    QTest::addColumn<TextCursor::MoveOperation>("moveOperation");
    QTest::addColumn<TextCursor::MoveMode>("moveMode");
    QTest::addColumn<int>("expectedPosition");
    QTest::addColumn<int>("expectedAnchor");
    QTest::addColumn<QString>("selectedText");

    QTest::newRow("Start") << 20 << 20 << TextCursor::Start << TextCursor::MoveAnchor
                           << 0 << 0 << QString();
    QTest::newRow("Up") << 25 << 25 << TextCursor::Up
                        << TextCursor::MoveAnchor << 1 << 1 << QString();
    QTest::newRow("StartOfLine") << 24 << 25 << TextCursor::StartOfLine
                                 << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfLine2") << 35 << 35 << TextCursor::StartOfLine
                                  << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfBlock") << 85 << 85 << TextCursor::StartOfBlock
                                  << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("StartOfWord") << 28 << 28 << TextCursor::StartOfWord
                                 << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("PreviousBlock") << 89 << 89 << TextCursor::PreviousBlock
                                   << TextCursor::MoveAnchor << 24 << 24 << QString();
    QTest::newRow("PreviousCharacter") << 0 << 0 << TextCursor::PreviousCharacter
                                       << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("PreviousWord") << 0 << 0 << TextCursor::PreviousWord
                                  << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("Left") << 0 << 0 << TextCursor::Left
                          << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("WordLeft") << 0 << 0 << TextCursor::WordLeft
                              << TextCursor::MoveAnchor << 0 << 0 << QString();
    QTest::newRow("FirstCharacter") << 0 << 0 << TextCursor::NextCharacter
                                    << TextCursor::KeepAnchor << 1 << 0 << "I";

    QTest::newRow("FirstCharacterReverse") << 1 << 1 << TextCursor::PreviousCharacter
                                           << TextCursor::KeepAnchor << 0 << 1 << "I";
    QTest::newRow("LastCharacter") << 179 << 179 << TextCursor::NextCharacter
                                   << TextCursor::KeepAnchor << 180 << 179 << "\n";
    QTest::newRow("LastCharacterReverse") << 180 << 180 << TextCursor::PreviousCharacter
                                          << TextCursor::KeepAnchor << 179 << 180 << "\n";

    QTest::newRow("EmptyBlock") << 180 << 180 << TextCursor::NextBlock
                                << TextCursor::KeepAnchor << 181 << 180 << "\n";

    QTest::newRow("EmptyBlockReverse") << 181 << 181 << TextCursor::PreviousBlock
                                       << TextCursor::KeepAnchor << 180 << 181 << "\n";

//     QTest::newRow("End") << 0 << 0 << TextCursor::End
//                          << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("Down") << 0 << 0 << TextCursor::Down
//                           << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("EndOfLine") << 0 << 0 << TextCursor::EndOfLine
//                                << TextCursor::MoveAnchor << 0 << 0 << QString();
//     QTest::newRow("EndOfWord") << 0 << 0 << TextCursor::EndOfWord << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("EndOfBlock") << 0 << 0 << TextCursor::EndOfBlock << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextBlock") << 0 << 0 << TextCursor::NextBlock << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextCharacter") << 0 << 0 << TextCursor::NextCharacter << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("NextWord") << 0 << 0 << TextCursor::NextWord << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("Right") << 0 << 0 << TextCursor::Right << TextCursor::MoveAnchor << 0 << 0;
//     QTest::newRow("WordRight") << 0 << 0 << TextCursor::WordRight << TextCursor::MoveAnchor << 0 << 0;
}

void tst_TextCursor::movePosition()
{
    static const char *trueToLife = "I can only sing it loud\n" // 0 - 23
                                    "always try to sing it clear what the hell are we all doing here\n" // 24 - 87
                                    "making too much of nothing\n" // 88 - 114
                                    "or creating one unholy mess an unfair study in survival, I guess\n" // 115 - 179
                                    "\nSomething\n";  // 180 - 190
    TextDocument document;
    document.setText(QString::fromLatin1(trueToLife));

    TextCursor cursor(&document);
    QFETCH(int, initialPosition);
    QFETCH(int, initialAnchor);
    QFETCH(TextCursor::MoveOperation, moveOperation);
    QFETCH(TextCursor::MoveMode, moveMode);
    QFETCH(int, expectedPosition);
    QFETCH(int, expectedAnchor);
    QFETCH(QString, selectedText);

    cursor.setPosition(initialAnchor);
    if (initialPosition != initialAnchor) {
        cursor.setPosition(initialPosition, TextCursor::KeepAnchor);
    }
    Q_ASSERT(cursor.position() == initialPosition);
    Q_ASSERT(cursor.anchor() == initialAnchor);
    cursor.movePosition(moveOperation, moveMode);
    QCOMPARE(cursor.position(), expectedPosition);
    QCOMPARE(cursor.anchor(), expectedAnchor);
    QCOMPARE(cursor.selectedText(), selectedText);
}

void tst_TextCursor::operatorEquals_data()
{
    QTest::addColumn<TextCursor*>("left");
    QTest::addColumn<TextCursor*>("right");
    QTest::addColumn<bool>("expected");

    QTest::newRow("null vs null") << new TextCursor << new TextCursor << true;
    QTest::newRow("non null vs null") << new TextCursor(&document) << new TextCursor << false;

    QTest::newRow("normal vs normal equal") << new TextCursor(&document) << new TextCursor(&document) << true;
    TextCursor *cursor = new TextCursor(&document);
    cursor->setPosition(1);
    QTest::newRow("normal vs normal different") << new TextCursor(&document) << cursor << false;

    cursor = new TextCursor(&document);
    cursor->setPosition(1, TextCursor::KeepAnchor);

    TextCursor *cursor2 = new TextCursor(&document);
    cursor2->setPosition(1, TextCursor::KeepAnchor);
    QTest::newRow("selection vs selection") << cursor << cursor2 << true;

    cursor = new TextCursor(&document);
    cursor->setPosition(1, TextCursor::KeepAnchor);

    cursor2 = new TextCursor(&document);
    cursor2->setPosition(1, TextCursor::MoveAnchor);
    QTest::newRow("selection vs selection different") << cursor << cursor2 << false;

}

void tst_TextCursor::operatorEquals()
{
    QFETCH(TextCursor*, left);
    QFETCH(TextCursor*, right);
    QFETCH(bool, expected);

    const bool equal = (*left == *right);
    const bool notEqual = (*left != *right);
    delete left;
    delete right;
    QVERIFY(equal != notEqual);
    QCOMPARE(equal, expected);
}

QTEST_MAIN(tst_TextCursor)
#include "tst_textcursor.moc"
<|endoftext|>


<commit_before>#include "mavlink_commands.h"
#include "system_impl.h"
#include <future>
#include <memory>

namespace dronecode_sdk {

// TODO: Currently the mavlink command handling is made in a way to only
//       process one command at any time. Therefore, the work state is global
//       for this whole class.
//       The limitation is made because:
//       - We are not sure what exactly will happen if the commands are sent in parallel
//         and what kind of edge cases we might run into.
//       - The queue used does not support going through and checking each and every
//         item yet.

MAVLinkCommands::MAVLinkCommands(SystemImpl &parent) : _parent(parent)
{
    _parent.register_mavlink_message_handler(
        MAVLINK_MSG_ID_COMMAND_ACK,
        std::bind(&MAVLinkCommands::receive_command_ack, this, std::placeholders::_1),
        this);
}

MAVLinkCommands::~MAVLinkCommands()
{
    _parent.unregister_all_mavlink_message_handlers(this);
}

MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandInt &command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [&prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::IN_PROGRESS) {
            prom->set_value(result);
        }
    });

    // Block now to wait for result.
    return res.get();
}

MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandLong &command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [&prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::IN_PROGRESS) {
            prom->set_value(result);
        }
    });

    return res.get();
}

void MAVLinkCommands::queue_command_async(const CommandInt &command,
                                          command_result_callback_t callback)
{
    // LogDebug() << "Command " << (int)(command.command) << " to send to "
    //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

    Work new_work{};
    mavlink_msg_command_int_pack(GCSClient::system_id,
                                 GCSClient::component_id,
                                 &new_work.mavlink_message,
                                 command.target_system_id,
                                 command.target_component_id,
                                 command.frame,
                                 command.command,
                                 command.current,
                                 command.autocontinue,
                                 command.params.param1,
                                 command.params.param2,
                                 command.params.param3,
                                 command.params.param4,
                                 command.params.x,
                                 command.params.y,
                                 command.params.z);

    new_work.callback = callback;
    new_work.mavlink_command = command.command;
    _work_queue.push_back(new_work);
}

void MAVLinkCommands::queue_command_async(const CommandLong &command,
                                          command_result_callback_t callback)
{
    // LogDebug() << "Command " << (int)(command.command) << " to send to "
    //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

    Work new_work{};
    mavlink_msg_command_long_pack(GCSClient::system_id,
                                  GCSClient::component_id,
                                  &new_work.mavlink_message,
                                  command.target_system_id,
                                  command.target_component_id,
                                  command.command,
                                  command.confirmation,
                                  command.params.param1,
                                  command.params.param2,
                                  command.params.param3,
                                  command.params.param4,
                                  command.params.param5,
                                  command.params.param6,
                                  command.params.param7);

    new_work.callback = callback;
    new_work.mavlink_command = command.command;
    _work_queue.push_back(new_work);
}

void MAVLinkCommands::receive_command_ack(mavlink_message_t message)
{
    mavlink_command_ack_t command_ack;
    mavlink_msg_command_ack_decode(&message, &command_ack);

    // LogDebug() << "We got an ack: " << command_ack.command;

    _state_mutex.lock();

    auto work = _work_queue.borrow_front();
    if (!work) {
        return;
    }

    if (work->mavlink_command != command_ack.command) {
        // If the command does not match with our current command, ignore it.
        LogWarn() << "Command ack " << int(command_ack.command)
                  << " not matching our current command: " << work->mavlink_command;
        _work_queue.return_front();
        return;
    }

    switch (command_ack.result) {
        case MAV_RESULT_ACCEPTED:
            _state = State::NONE;
            _state_mutex.unlock();
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            call_callback(work->callback, Result::SUCCESS, 1.0f);
            break;

        case MAV_RESULT_DENIED:
            LogWarn() << "command denied (" << work->mavlink_command << ").";
            _state = State::NONE;
            _state_mutex.unlock();
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_UNSUPPORTED:
            LogWarn() << "command unsupported (" << work->mavlink_command << ").";
            _state = State::NONE;
            _state_mutex.unlock();
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_TEMPORARILY_REJECTED:
            LogWarn() << "command temporarily rejected (" << work->mavlink_command << ").";
            _state = State::NONE;
            _state_mutex.unlock();
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_FAILED:
            _state = State::NONE;
            _state_mutex.unlock();
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_IN_PROGRESS:
            if (static_cast<int>(command_ack.progress) != 255) {
                LogInfo() << "progress: " << static_cast<int>(command_ack.progress) << " % ("
                          << work->mavlink_command << ").";
            }
            _state = State::IN_PROGRESS;
            _state_mutex.unlock();
            // If we get a progress update, we can raise the timeout
            // to something higher because we know the initial command
            // has arrived. A possible timeout for this case is the initial
            // timeout * the possible retries because this should match the
            // case where there is no progress update and we keep trying.
            _parent.unregister_timeout_handler(_timeout_cookie);
            _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                             work->retries_to_do * work->timeout_s,
                                             &_timeout_cookie);
            _work_queue.return_front();
            // FIXME: We can only call callbacks with promises once, so let's not do it
            //        on IN_PROGRESS.
            // call_callback(work->callback, Result::IN_PROGRESS, command_ack.progress /
            //               100.0f);
            break;

        default:
            LogWarn() << "Received unknown ack.";
            _state_mutex.unlock();
            _work_queue.return_front();
            break;
    }
}

void MAVLinkCommands::receive_timeout()
{
    std::lock_guard<std::mutex> lock(_state_mutex);

    if (_state != State::WAITING) {
        LogWarn() << "Received command timeout but not waiting.";
        return;
    }

    // If we're not waiting, we ignore this.
    auto work = _work_queue.borrow_front();

    if (!work) {
        // FIXME:
        // There is nothing to do.
        // However, we probably shouldn't be in State::WAITING if nothing is in the queue.
        // Otherwise, we might wait forever because we don't register a new timeout.
        // What's odd is that from my code review, it should not be possible to ever end up
        // in State::WAITING with an empty queue, especially since the state is protected by
        // a mutex.
        _state = State::NONE;
        LogErr() << "Received timeout without item in queue.";
        return;
    }

    if (work->retries_to_do > 0) {
        // We're not sure the command arrived, let's retransmit.
        LogWarn() << "sending again, retries to do: " << work->retries_to_do << "  ("
                  << work->mavlink_command << ").";
        if (!_parent.send_message(work->mavlink_message)) {
            LogErr() << "connection send error in retransmit (" << work->mavlink_command << ").";
            _state = State::NONE;
            _work_queue.pop_front();
            call_callback(work->callback, Result::CONNECTION_ERROR, NAN);

        } else {
            --work->retries_to_do;
            _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                             work->timeout_s,
                                             &_timeout_cookie);
            _work_queue.return_front();
        }

    } else {
        // We have tried retransmitting, giving up now.
        LogErr() << "Retrying failed (" << work->mavlink_command << ")";

        const bool had_been_waiting = (_state == State::WAITING);

        _state = State::NONE;
        _work_queue.pop_front();

        if (had_been_waiting) {
            call_callback(work->callback, Result::TIMEOUT, NAN);
        }
    }
}

void MAVLinkCommands::do_work()
{
    std::lock_guard<std::mutex> lock(_state_mutex);

    auto work = _work_queue.borrow_front();
    if (!work) {
        // Nothing to do.
        return;
    }

    // If the work state is none, we can start the next command.
    switch (_state) {
        case State::NONE:
            // LogDebug() << "sending it the first time (" << work->mavlink_command << ")";
            if (!_parent.send_message(work->mavlink_message)) {
                LogErr() << "connection send error (" << work->mavlink_command << ")";
                _work_queue.pop_front();
                _state = State::NONE;
                call_callback(work->callback, Result::CONNECTION_ERROR, NAN);
            } else {
                _state = State::WAITING;
                _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                                 work->timeout_s,
                                                 &_timeout_cookie);
                _work_queue.return_front();
            }
            break;
        case State::WAITING:
        // FALLTHROUGH
        case State::IN_PROGRESS:
            _work_queue.return_front();
            break;
    }
}

void MAVLinkCommands::call_callback(const command_result_callback_t &callback,
                                    Result result,
                                    float progress)
{
    if (!callback) {
        return;
    }

    // It seems that we need to queue the callback on the thread pool otherwise
    // we lock ourselves out when we send a command in the callback receiving a command result.
    _parent.call_user_callback([callback, result, progress]() { callback(result, progress); });
}

} // namespace dronecode_sdk
<commit_msg>mavlink_commands: Use unique_lock which can be unlocked early<commit_after>#include "mavlink_commands.h"
#include "system_impl.h"
#include <future>
#include <memory>

namespace dronecode_sdk {

// TODO: Currently the mavlink command handling is made in a way to only
//       process one command at any time. Therefore, the work state is global
//       for this whole class.
//       The limitation is made because:
//       - We are not sure what exactly will happen if the commands are sent in parallel
//         and what kind of edge cases we might run into.
//       - The queue used does not support going through and checking each and every
//         item yet.

MAVLinkCommands::MAVLinkCommands(SystemImpl &parent) : _parent(parent)
{
    _parent.register_mavlink_message_handler(
        MAVLINK_MSG_ID_COMMAND_ACK,
        std::bind(&MAVLinkCommands::receive_command_ack, this, std::placeholders::_1),
        this);
}

MAVLinkCommands::~MAVLinkCommands()
{
    _parent.unregister_all_mavlink_message_handlers(this);
}

MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandInt &command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [&prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::IN_PROGRESS) {
            prom->set_value(result);
        }
    });

    // Block now to wait for result.
    return res.get();
}

MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandLong &command)
{
    // We wrap the async call with a promise and future.
    auto prom = std::make_shared<std::promise<Result>>();
    auto res = prom->get_future();

    queue_command_async(command, [&prom](Result result, float progress) {
        UNUSED(progress);
        // We can only fulfill the promise once in C++11.
        // Therefore we have to ignore the IN_PROGRESS state and wait
        // for the final result.
        if (result != Result::IN_PROGRESS) {
            prom->set_value(result);
        }
    });

    return res.get();
}

void MAVLinkCommands::queue_command_async(const CommandInt &command,
                                          command_result_callback_t callback)
{
    // LogDebug() << "Command " << (int)(command.command) << " to send to "
    //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

    Work new_work{};
    mavlink_msg_command_int_pack(GCSClient::system_id,
                                 GCSClient::component_id,
                                 &new_work.mavlink_message,
                                 command.target_system_id,
                                 command.target_component_id,
                                 command.frame,
                                 command.command,
                                 command.current,
                                 command.autocontinue,
                                 command.params.param1,
                                 command.params.param2,
                                 command.params.param3,
                                 command.params.param4,
                                 command.params.x,
                                 command.params.y,
                                 command.params.z);

    new_work.callback = callback;
    new_work.mavlink_command = command.command;
    _work_queue.push_back(new_work);
}

void MAVLinkCommands::queue_command_async(const CommandLong &command,
                                          command_result_callback_t callback)
{
    // LogDebug() << "Command " << (int)(command.command) << " to send to "
    //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

    Work new_work{};
    mavlink_msg_command_long_pack(GCSClient::system_id,
                                  GCSClient::component_id,
                                  &new_work.mavlink_message,
                                  command.target_system_id,
                                  command.target_component_id,
                                  command.command,
                                  command.confirmation,
                                  command.params.param1,
                                  command.params.param2,
                                  command.params.param3,
                                  command.params.param4,
                                  command.params.param5,
                                  command.params.param6,
                                  command.params.param7);

    new_work.callback = callback;
    new_work.mavlink_command = command.command;
    _work_queue.push_back(new_work);
}

void MAVLinkCommands::receive_command_ack(mavlink_message_t message)
{
    mavlink_command_ack_t command_ack;
    mavlink_msg_command_ack_decode(&message, &command_ack);

    // LogDebug() << "We got an ack: " << command_ack.command;

    std::unique_lock<std::mutex> lock(_state_mutex);

    auto work = _work_queue.borrow_front();
    if (!work) {
        return;
    }

    if (work->mavlink_command != command_ack.command) {
        // If the command does not match with our current command, ignore it.
        LogWarn() << "Command ack " << int(command_ack.command)
                  << " not matching our current command: " << work->mavlink_command;
        _work_queue.return_front();
        return;
    }

    switch (command_ack.result) {
        case MAV_RESULT_ACCEPTED:
            _state = State::NONE;
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            lock.unlock();
            call_callback(work->callback, Result::SUCCESS, 1.0f);
            break;

        case MAV_RESULT_DENIED:
            LogWarn() << "command denied (" << work->mavlink_command << ").";
            _state = State::NONE;
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            lock.unlock();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_UNSUPPORTED:
            LogWarn() << "command unsupported (" << work->mavlink_command << ").";
            _state = State::NONE;
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            lock.unlock();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_TEMPORARILY_REJECTED:
            LogWarn() << "command temporarily rejected (" << work->mavlink_command << ").";
            _state = State::NONE;
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            lock.unlock();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_FAILED:
            _state = State::NONE;
            _parent.unregister_timeout_handler(_timeout_cookie);
            _work_queue.pop_front();
            lock.unlock();
            call_callback(work->callback, Result::COMMAND_DENIED, NAN);
            break;

        case MAV_RESULT_IN_PROGRESS:
            if (static_cast<int>(command_ack.progress) != 255) {
                LogInfo() << "progress: " << static_cast<int>(command_ack.progress) << " % ("
                          << work->mavlink_command << ").";
            }
            _state = State::IN_PROGRESS;
            // If we get a progress update, we can raise the timeout
            // to something higher because we know the initial command
            // has arrived. A possible timeout for this case is the initial
            // timeout * the possible retries because this should match the
            // case where there is no progress update and we keep trying.
            _parent.unregister_timeout_handler(_timeout_cookie);
            _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                             work->retries_to_do * work->timeout_s,
                                             &_timeout_cookie);
            _work_queue.return_front();
            // FIXME: We can only call callbacks with promises once, so let's not do it
            //        on IN_PROGRESS.
            // call_callback(work->callback, Result::IN_PROGRESS, command_ack.progress /
            //               100.0f);
            break;

        default:
            LogWarn() << "Received unknown ack.";
            _work_queue.return_front();
            break;
    }
}

void MAVLinkCommands::receive_timeout()
{
    std::lock_guard<std::mutex> lock(_state_mutex);

    if (_state != State::WAITING) {
        LogWarn() << "Received command timeout but not waiting.";
        return;
    }

    // If we're not waiting, we ignore this.
    auto work = _work_queue.borrow_front();

    if (!work) {
        // FIXME:
        // There is nothing to do.
        // However, we probably shouldn't be in State::WAITING if nothing is in the queue.
        // Otherwise, we might wait forever because we don't register a new timeout.
        // What's odd is that from my code review, it should not be possible to ever end up
        // in State::WAITING with an empty queue, especially since the state is protected by
        // a mutex.
        _state = State::NONE;
        LogErr() << "Received timeout without item in queue.";
        return;
    }

    if (work->retries_to_do > 0) {
        // We're not sure the command arrived, let's retransmit.
        LogWarn() << "sending again, retries to do: " << work->retries_to_do << "  ("
                  << work->mavlink_command << ").";
        if (!_parent.send_message(work->mavlink_message)) {
            LogErr() << "connection send error in retransmit (" << work->mavlink_command << ").";
            _state = State::NONE;
            _work_queue.pop_front();
            call_callback(work->callback, Result::CONNECTION_ERROR, NAN);

        } else {
            --work->retries_to_do;
            _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                             work->timeout_s,
                                             &_timeout_cookie);
            _work_queue.return_front();
        }

    } else {
        // We have tried retransmitting, giving up now.
        LogErr() << "Retrying failed (" << work->mavlink_command << ")";

        const bool had_been_waiting = (_state == State::WAITING);

        _state = State::NONE;
        _work_queue.pop_front();

        if (had_been_waiting) {
            call_callback(work->callback, Result::TIMEOUT, NAN);
        }
    }
}

void MAVLinkCommands::do_work()
{
    std::lock_guard<std::mutex> lock(_state_mutex);

    auto work = _work_queue.borrow_front();
    if (!work) {
        // Nothing to do.
        return;
    }

    // If the work state is none, we can start the next command.
    switch (_state) {
        case State::NONE:
            // LogDebug() << "sending it the first time (" << work->mavlink_command << ")";
            if (!_parent.send_message(work->mavlink_message)) {
                LogErr() << "connection send error (" << work->mavlink_command << ")";
                _work_queue.pop_front();
                _state = State::NONE;
                call_callback(work->callback, Result::CONNECTION_ERROR, NAN);
            } else {
                _state = State::WAITING;
                _parent.register_timeout_handler(std::bind(&MAVLinkCommands::receive_timeout, this),
                                                 work->timeout_s,
                                                 &_timeout_cookie);
                _work_queue.return_front();
            }
            break;
        case State::WAITING:
        // FALLTHROUGH
        case State::IN_PROGRESS:
            _work_queue.return_front();
            break;
    }
}

void MAVLinkCommands::call_callback(const command_result_callback_t &callback,
                                    Result result,
                                    float progress)
{
    if (!callback) {
        return;
    }

    // It seems that we need to queue the callback on the thread pool otherwise
    // we lock ourselves out when we send a command in the callback receiving a command result.
    _parent.call_user_callback([callback, result, progress]() { callback(result, progress); });
}

} // namespace dronecode_sdk
<|endoftext|>


<commit_before>/*
 *
 * Event
 * ledger-core
 *
 * Created by Pierre Pollastri on 27/04/2017.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Ledger
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#include <collections/DynamicObject.hpp>
#include "Event.hpp"

ledger::core::api::EventCode ledger::core::Event::getCode() {
    return _code;
}

std::shared_ptr<ledger::core::api::DynamicObject> ledger::core::Event::getPayload() {
    return _payload;
}

bool ledger::core::Event::isSticky() {
    return _sticky;
}

int32_t ledger::core::Event::getStickyTag() {
    return _tag;
}

ledger::core::Event::Event(ledger::core::api::EventCode code,
                           const std::shared_ptr<ledger::core::api::DynamicObject> &payload) {
    _code = code;
    _payload = payload;
    _sticky = false;
    if (payload) {
        std::static_pointer_cast<DynamicObject>(payload)->setReadOnly(true);
    }
}

void ledger::core::Event::setSticky(int32_t tag) {
    _tag = tag;
    _sticky = true;
}

std::shared_ptr<ledger::core::api::Event> ledger::core::make_event(ledger::core::api::EventCode code, const std::shared_ptr<ledger::core::api::DynamicObject>& payload) {
    return std::make_shared<ledger::core::Event>(code, payload);
}

std::share_ptr<ledger::core::api::Event> ledger::core::api::Event::newInstance(EventCode code,
                                                                               const std::shared_ptr<DynamicObject> &payload) {
    return std::make_shared<ledger::core::Event>(code, payload);
}<commit_msg>Fix typo<commit_after>/*
 *
 * Event
 * ledger-core
 *
 * Created by Pierre Pollastri on 27/04/2017.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Ledger
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#include <collections/DynamicObject.hpp>
#include "Event.hpp"

ledger::core::api::EventCode ledger::core::Event::getCode() {
    return _code;
}

std::shared_ptr<ledger::core::api::DynamicObject> ledger::core::Event::getPayload() {
    return _payload;
}

bool ledger::core::Event::isSticky() {
    return _sticky;
}

int32_t ledger::core::Event::getStickyTag() {
    return _tag;
}

ledger::core::Event::Event(ledger::core::api::EventCode code,
                           const std::shared_ptr<ledger::core::api::DynamicObject> &payload) {
    _code = code;
    _payload = payload;
    _sticky = false;
    if (payload) {
        std::static_pointer_cast<DynamicObject>(payload)->setReadOnly(true);
    }
}

void ledger::core::Event::setSticky(int32_t tag) {
    _tag = tag;
    _sticky = true;
}

std::shared_ptr<ledger::core::api::Event> ledger::core::make_event(ledger::core::api::EventCode code, const std::shared_ptr<ledger::core::api::DynamicObject>& payload) {
    return std::make_shared<ledger::core::Event>(code, payload);
}

std::shared_ptr<ledger::core::api::Event> ledger::core::api::Event::newInstance(EventCode code,
                                                                               const std::shared_ptr<DynamicObject> &payload) {
    return std::make_shared<ledger::core::Event>(code, payload);
}<|endoftext|>


<commit_before>#include "label.h"

#include "util/geom.h"
#include "labels/labelMesh.h"
#include "glm/gtx/rotate_vector.hpp"

namespace Tangram {

Label::Label(Label::Transform _transform, glm::vec2 _size, Type _type, LabelMesh& _mesh, Range _vertexRange, Options _options) :
    m_type(_type),
    m_options(_options),
    m_transform(_transform),
    m_dim(_size),
    m_mesh(_mesh),
    m_vertexRange(_vertexRange) {

    m_transform.state.alpha = m_type == Type::debug ? 1.0 : 0.0;
    m_currentState = m_type == Type::debug ? State::visible : State::wait_occ;
    m_occludedLastFrame = false;
    m_occlusionSolved = false;
    m_updateMeshVisibility = true;
    m_dirty = true;
}

Label::~Label() {}

bool Label::updateScreenTransform(const glm::mat4& _mvp, const glm::vec2& _screenSize, bool _testVisibility) {

    glm::vec2 screenPosition;
    float rot = 0;

    switch (m_type) {
        case Type::debug:
        case Type::point:
        {
            glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));

            if (_testVisibility && (v1.w <= 0)) {
                return false;
            }

            screenPosition = clipToScreenSpace(v1, _screenSize);

            // center on half the width
            screenPosition.x -= m_dim.x * 0.5f;

            break;
        }
        case Type::line:
        {
            // project label position from mercator world space to clip coordinates
            glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));
            glm::vec4 v2 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition2, 0.0, 1.0));

            // check whether the label is behind the camera using the perspective division factor
            if (_testVisibility && (v1.w <= 0 || v2.w <= 0)) {
                return false;
            }

            // project to screen space
            glm::vec2 p1 = clipToScreenSpace(v1, _screenSize);
            glm::vec2 p2 = clipToScreenSpace(v2, _screenSize);

            rot = angleBetweenPoints(p1, p2) + M_PI_2;

            if (rot > M_PI_2 || rot < -M_PI_2) { // un-readable labels
                rot += M_PI;
            } else {
                std::swap(p1, p2);
            }

            glm::vec2 p1p2 = p2 - p1;
            glm::vec2 t = glm::normalize(-p1p2);

            float length = glm::length(p1p2);

            float exceedHeuristic = 30; // default heuristic : 30%

            if (_testVisibility && (m_dim.x > length)) {
                float exceed = (1 - (length / m_dim.x)) * 100;
                if (exceed > exceedHeuristic) {
                    return false;
                }
            }

            screenPosition = (p1 + p2) * 0.5f + t * m_dim.x * 0.5f;

            break;
        }
    }

    // update screen position
    glm::vec2 offset = m_options.offset;

    if (m_transform.state.rotation != 0.f) {
        offset = glm::rotate(offset, m_transform.state.rotation);
    }

    glm::vec2 newScreenPos = screenPosition + offset;
    if (newScreenPos != m_transform.state.screenPos) {
        m_transform.state.screenPos = newScreenPos;
        m_dirty = true;
    }

    // update screen rotation
    if (m_transform.state.rotation != rot) {
        m_transform.state.rotation = rot;
        m_dirty = true;
    }

    return true;
}

bool Label::update(const glm::mat4& _mvp, const glm::vec2& _screenSize, float _dt) {
    bool animate = false;

    animate = updateState(_mvp, _screenSize, _dt);
    m_occlusionSolved = false;

    return animate;
}

bool Label::offViewport(const glm::vec2& _screenSize) {
    const auto& quad = m_obb.getQuad();

    for (int i = 0; i < 4; ++i) {
        const auto& p = quad[i];
        if (p.x < _screenSize.x && p.y < _screenSize.y && p.x > 0 && p.y > 0) {
            return false;
        }
    }

    return true;
}

void Label::setOcclusion(bool _occlusion) {
    if (!canOcclude()) {
        return;
    }

    m_occludedLastFrame = _occlusion;
}

bool Label::canOcclude() {
    if (!m_options.collide) {
        return false;
    }

    int occludeFlags = (State::visible | State::wait_occ | State::fading_in | State::sleep);
    return (occludeFlags & m_currentState) && !(m_type == Type::debug);
}

bool Label::visibleState() const {
    int visibleFlags = (State::visible | State::fading_in | State::fading_out);
    return (visibleFlags & m_currentState);
}

void Label::occlusionSolved() {
    m_occlusionSolved = true;
}

void Label::enterState(const State& _state, float _alpha) {
    m_currentState = _state;
    setAlpha(_alpha);
}

void Label::setAlpha(float _alpha) {
    float alpha = CLAMP(_alpha, 0.0, 1.0);
    if (m_transform.state.alpha != alpha) {
        m_transform.state.alpha = alpha;
        m_dirty = true;
    }

    if (alpha == 0.f) {
        m_updateMeshVisibility = true;
    }
}

void Label::pushTransform() {

    // update the buffer on valid states
    if (m_dirty) {
        static size_t attribOffset = offsetof(Label::Vertex, state);
        static size_t alphaOffset = offsetof(Label::Vertex::State, alpha) + attribOffset;

        if (visibleState()) {
            // update the complete state on the mesh
            m_mesh.updateAttribute(m_vertexRange, m_transform.state, attribOffset);
        } else {

            // for any non-visible states, we don't need to overhead the gpu with updates on the
            // alpha attribute, but simply do it once until the label goes back in a visible state
            if (m_updateMeshVisibility) {
                m_mesh.updateAttribute(m_vertexRange, m_transform.state.alpha, alphaOffset);
                m_updateMeshVisibility = false;
            }
        }

        m_dirty = false;
    }
}

void Label::resetState() {
    m_currentState = State::wait_occ;
    m_occludedLastFrame = false;
    m_occlusionSolved = false;
    m_updateMeshVisibility = true;
    m_dirty = true;
}

bool Label::updateState(const glm::mat4& _mvp, const glm::vec2& _screenSize, float _dt) {
    if (m_currentState == State::dead) {
        return false;
    }

    bool occludedLastFrame = m_occludedLastFrame;
    m_occludedLastFrame = false;

    bool ruleSatisfied = updateScreenTransform(_mvp, _screenSize);

    // one of the label rules has not been satisfied
    if (!ruleSatisfied) {

        // go to dead state, this breaks determinism, but reduce potential label set since a lot
        // of discarded labels are discared for line exceed (lots of tiny small lines on a curve
        // for example), which won't have their rule satisfied
        enterState(State::dead, 0.0);
        return false;
    }

    // update the view-space bouding box
    updateBBoxes();

    // checks whether the label is out of the viewport
    if (offViewport(_screenSize)) {
        enterState(State::out_of_screen, 0.0);
    }

    bool animate = false;

    switch (m_currentState) {
        case State::visible:
            if (occludedLastFrame) {
                m_fade = FadeEffect(false, m_options.hideTransition.ease, m_options.hideTransition.time);
                enterState(State::fading_out, 1.0);
            }
            break;
        case State::fading_in:
            if (occludedLastFrame) {
                enterState(State::sleep, 0.0);
                break;
            }
            setAlpha(m_fade.update(_dt));
            animate = true;
            if (m_fade.isFinished()) {
                enterState(State::visible, 1.0);
            }
            break;
        case State::fading_out:
            setAlpha(m_fade.update(_dt));
            animate = true;
            if (m_fade.isFinished()) {
                enterState(State::sleep, 0.0);
            }
            break;
        case State::out_of_screen:
            if (!offViewport(_screenSize)) {
                enterState(State::wait_occ, 0.0);
            }
            break;
        case State::wait_occ:
            if (m_occlusionSolved) {
                if (occludedLastFrame) {
                    enterState(State::dead, 0.0); // dead
                }  else {
                    m_fade = FadeEffect(true, m_options.showTransition.ease, m_options.showTransition.time);
                    enterState(State::fading_in, 0.0);
                }
            }
            break;
        case State::sleep:
            if (!occludedLastFrame) {
                m_fade = FadeEffect(true, m_options.showTransition.ease, m_options.showTransition.time);
                enterState(State::fading_in, 0.0);
            }
            break;
        case State::dead:
            break;
    }

    return animate;
}

}
<commit_msg>Use enterState<commit_after>#include "label.h"

#include "util/geom.h"
#include "labels/labelMesh.h"
#include "glm/gtx/rotate_vector.hpp"

namespace Tangram {

Label::Label(Label::Transform _transform, glm::vec2 _size, Type _type, LabelMesh& _mesh, Range _vertexRange, Options _options) :
    m_type(_type),
    m_options(_options),
    m_transform(_transform),
    m_dim(_size),
    m_mesh(_mesh),
    m_vertexRange(_vertexRange) {

    m_transform.state.alpha = m_type == Type::debug ? 1.0 : 0.0;
    m_currentState = m_type == Type::debug ? State::visible : State::wait_occ;
    m_occludedLastFrame = false;
    m_occlusionSolved = false;
    m_updateMeshVisibility = true;
    m_dirty = true;
}

Label::~Label() {}

bool Label::updateScreenTransform(const glm::mat4& _mvp, const glm::vec2& _screenSize, bool _testVisibility) {

    glm::vec2 screenPosition;
    float rot = 0;

    switch (m_type) {
        case Type::debug:
        case Type::point:
        {
            glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));

            if (_testVisibility && (v1.w <= 0)) {
                return false;
            }

            screenPosition = clipToScreenSpace(v1, _screenSize);

            // center on half the width
            screenPosition.x -= m_dim.x * 0.5f;

            break;
        }
        case Type::line:
        {
            // project label position from mercator world space to clip coordinates
            glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));
            glm::vec4 v2 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition2, 0.0, 1.0));

            // check whether the label is behind the camera using the perspective division factor
            if (_testVisibility && (v1.w <= 0 || v2.w <= 0)) {
                return false;
            }

            // project to screen space
            glm::vec2 p1 = clipToScreenSpace(v1, _screenSize);
            glm::vec2 p2 = clipToScreenSpace(v2, _screenSize);

            rot = angleBetweenPoints(p1, p2) + M_PI_2;

            if (rot > M_PI_2 || rot < -M_PI_2) { // un-readable labels
                rot += M_PI;
            } else {
                std::swap(p1, p2);
            }

            glm::vec2 p1p2 = p2 - p1;
            glm::vec2 t = glm::normalize(-p1p2);

            float length = glm::length(p1p2);

            float exceedHeuristic = 30; // default heuristic : 30%

            if (_testVisibility && (m_dim.x > length)) {
                float exceed = (1 - (length / m_dim.x)) * 100;
                if (exceed > exceedHeuristic) {
                    return false;
                }
            }

            screenPosition = (p1 + p2) * 0.5f + t * m_dim.x * 0.5f;

            break;
        }
    }

    // update screen position
    glm::vec2 offset = m_options.offset;

    if (m_transform.state.rotation != 0.f) {
        offset = glm::rotate(offset, m_transform.state.rotation);
    }

    glm::vec2 newScreenPos = screenPosition + offset;
    if (newScreenPos != m_transform.state.screenPos) {
        m_transform.state.screenPos = newScreenPos;
        m_dirty = true;
    }

    // update screen rotation
    if (m_transform.state.rotation != rot) {
        m_transform.state.rotation = rot;
        m_dirty = true;
    }

    return true;
}

bool Label::update(const glm::mat4& _mvp, const glm::vec2& _screenSize, float _dt) {
    bool animate = false;

    animate = updateState(_mvp, _screenSize, _dt);
    m_occlusionSolved = false;

    return animate;
}

bool Label::offViewport(const glm::vec2& _screenSize) {
    const auto& quad = m_obb.getQuad();

    for (int i = 0; i < 4; ++i) {
        const auto& p = quad[i];
        if (p.x < _screenSize.x && p.y < _screenSize.y && p.x > 0 && p.y > 0) {
            return false;
        }
    }

    return true;
}

void Label::setOcclusion(bool _occlusion) {
    if (!canOcclude()) {
        return;
    }

    m_occludedLastFrame = _occlusion;
}

bool Label::canOcclude() {
    if (!m_options.collide) {
        return false;
    }

    int occludeFlags = (State::visible | State::wait_occ | State::fading_in | State::sleep);
    return (occludeFlags & m_currentState) && !(m_type == Type::debug);
}

bool Label::visibleState() const {
    int visibleFlags = (State::visible | State::fading_in | State::fading_out);
    return (visibleFlags & m_currentState);
}

void Label::occlusionSolved() {
    m_occlusionSolved = true;
}

void Label::enterState(const State& _state, float _alpha) {
    m_currentState = _state;
    setAlpha(_alpha);
}

void Label::setAlpha(float _alpha) {
    float alpha = CLAMP(_alpha, 0.0, 1.0);
    if (m_transform.state.alpha != alpha) {
        m_transform.state.alpha = alpha;
        m_dirty = true;
    }

    if (alpha == 0.f) {
        m_updateMeshVisibility = true;
    }
}

void Label::pushTransform() {

    // update the buffer on valid states
    if (m_dirty) {
        static size_t attribOffset = offsetof(Label::Vertex, state);
        static size_t alphaOffset = offsetof(Label::Vertex::State, alpha) + attribOffset;

        if (visibleState()) {
            // update the complete state on the mesh
            m_mesh.updateAttribute(m_vertexRange, m_transform.state, attribOffset);
        } else {

            // for any non-visible states, we don't need to overhead the gpu with updates on the
            // alpha attribute, but simply do it once until the label goes back in a visible state
            if (m_updateMeshVisibility) {
                m_mesh.updateAttribute(m_vertexRange, m_transform.state.alpha, alphaOffset);
                m_updateMeshVisibility = false;
            }
        }

        m_dirty = false;
    }
}

void Label::resetState() {
    m_occludedLastFrame = false;
    m_occlusionSolved = false;
    m_updateMeshVisibility = true;
    m_dirty = true;
    enterState(State::wait_occ, 0.0);
}

bool Label::updateState(const glm::mat4& _mvp, const glm::vec2& _screenSize, float _dt) {
    if (m_currentState == State::dead) {
        return false;
    }

    bool occludedLastFrame = m_occludedLastFrame;
    m_occludedLastFrame = false;

    bool ruleSatisfied = updateScreenTransform(_mvp, _screenSize);

    // one of the label rules has not been satisfied
    if (!ruleSatisfied) {

        // go to dead state, this breaks determinism, but reduce potential label set since a lot
        // of discarded labels are discared for line exceed (lots of tiny small lines on a curve
        // for example), which won't have their rule satisfied
        enterState(State::dead, 0.0);
        return false;
    }

    // update the view-space bouding box
    updateBBoxes();

    // checks whether the label is out of the viewport
    if (offViewport(_screenSize)) {
        enterState(State::out_of_screen, 0.0);
    }

    bool animate = false;

    switch (m_currentState) {
        case State::visible:
            if (occludedLastFrame) {
                m_fade = FadeEffect(false, m_options.hideTransition.ease, m_options.hideTransition.time);
                enterState(State::fading_out, 1.0);
            }
            break;
        case State::fading_in:
            if (occludedLastFrame) {
                enterState(State::sleep, 0.0);
                break;
            }
            setAlpha(m_fade.update(_dt));
            animate = true;
            if (m_fade.isFinished()) {
                enterState(State::visible, 1.0);
            }
            break;
        case State::fading_out:
            setAlpha(m_fade.update(_dt));
            animate = true;
            if (m_fade.isFinished()) {
                enterState(State::sleep, 0.0);
            }
            break;
        case State::out_of_screen:
            if (!offViewport(_screenSize)) {
                enterState(State::wait_occ, 0.0);
            }
            break;
        case State::wait_occ:
            if (m_occlusionSolved) {
                if (occludedLastFrame) {
                    enterState(State::dead, 0.0); // dead
                }  else {
                    m_fade = FadeEffect(true, m_options.showTransition.ease, m_options.showTransition.time);
                    enterState(State::fading_in, 0.0);
                }
            }
            break;
        case State::sleep:
            if (!occludedLastFrame) {
                m_fade = FadeEffect(true, m_options.showTransition.ease, m_options.showTransition.time);
                enterState(State::fading_in, 0.0);
            }
            break;
        case State::dead:
            break;
    }

    return animate;
}

}
<|endoftext|>


<commit_before>#include "vboMesh.h"
#include "shaderProgram.h"

int VboMesh::s_validGeneration = 0;

VboMesh::VboMesh() {
    m_glVertexBuffer = 0;
    m_glIndexBuffer = 0;
    m_nVertices = 0;
    m_nIndices = 0;
    m_dirtyOffset = 0;
    m_dirtySize = 0;

    m_dirty = false;
    m_isUploaded = false;
    m_isCompiled = false;

    m_generation = -1;
}

VboMesh::VboMesh(std::shared_ptr<VertexLayout> _vertexLayout, GLenum _drawMode, GLenum _hint) : VboMesh() {
    m_vertexLayout = _vertexLayout;
    m_hint = _hint;

    setDrawMode(_drawMode);
}

VboMesh::~VboMesh() {
    if (m_glVertexBuffer) glDeleteBuffers(1, &m_glVertexBuffer);
    if (m_glIndexBuffer) glDeleteBuffers(1, &m_glIndexBuffer);

    delete[] m_glVertexData;
    delete[] m_glIndexData;
}

void VboMesh::setVertexLayout(std::shared_ptr<VertexLayout> _vertexLayout) {
    m_vertexLayout = _vertexLayout;
}

void VboMesh::setDrawMode(GLenum _drawMode) {
    switch (_drawMode) {
        case GL_POINTS:
        case GL_LINE_STRIP:
        case GL_LINE_LOOP:
        case GL_LINES:
        case GL_TRIANGLE_STRIP:
        case GL_TRIANGLE_FAN:
        case GL_TRIANGLES:
            m_drawMode = _drawMode;
            break;
        default:
            logMsg("WARNING: Invalid draw mode for mesh! Defaulting to GL_TRIANGLES\n");
            m_drawMode = GL_TRIANGLES;
    }
}

void VboMesh::update(GLintptr _offset, GLsizei _size, unsigned char* _data) {
    if (m_hint == GL_STATIC_DRAW) {
        logMsg("WARNING: wrong usage hint provided to the Vbo\n");
    }

    std::memcpy(m_glVertexData + _offset, _data, _size);

    m_dirtyOffset = _offset;
    m_dirtySize = _size;

    m_dirty = true;
}

bool VboMesh::subDataUpload() {
    if (m_dirtySize == 0) {
        return false;
    }

    glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);

    long vertexBytes = m_nVertices * m_vertexLayout->getStride();

    // updating the entire buffer
    if (m_dirtySize - vertexBytes < m_vertexLayout->getStride()) {

        // invalidate the data store on the driver
        glBufferData(GL_ARRAY_BUFFER, vertexBytes, NULL, m_hint);

        // if this buffer is still used by gpu on current frame this call will not wait
        // for the frame to finish using the vbo but directly upload the data
        glBufferData(GL_ARRAY_BUFFER, vertexBytes, m_glVertexData, m_hint);
    } else {
        // perform simple sub data upload for part of the buffer
        glBufferSubData(GL_ARRAY_BUFFER, m_dirtyOffset, m_dirtySize, m_glVertexData + m_dirtyOffset);
    }

    m_dirtyOffset = 0;
    m_dirtySize = 0;

    m_dirty = false;

    return true;
}

bool VboMesh::upload() {
    if (m_isUploaded) {
        return false;
    }

    // Generate vertex buffer, if needed
    if (m_glVertexBuffer == 0) {
        glGenBuffers(1, &m_glVertexBuffer);
    }

    // TODO check if compiled?
    // Buffer vertex data
    int vertexBytes = m_nVertices * m_vertexLayout->getStride();

    glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);
    glBufferData(GL_ARRAY_BUFFER, vertexBytes, m_glVertexData, m_hint);

    if (m_glIndexData) {

        if (m_glIndexBuffer == 0) {
            glGenBuffers(1, &m_glIndexBuffer);
        }

        // Buffer element index data
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_glIndexBuffer);
        glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_nIndices * sizeof(GLushort), m_glIndexData, m_hint);
    }

    // m_glVertexData.resize(0);
    // m_glIndexData.resize(0);

    // TODO: For now, we retain copies of the vertex and index data in CPU memory to allow VBOs
    // to easily rebuild themselves after GL context loss. For optimizing memory usage (and for
    // other reasons) we'll want to change this in the future. This probably means going back to
    // data sources and styles to rebuild the vertex data.

    m_generation = s_validGeneration;

    m_isUploaded = true;

    return true;

}

void VboMesh::draw(const std::shared_ptr<ShaderProgram> _shader) {

    checkValidity();

    if (!m_isCompiled) return;

    if (m_nVertices == 0) return;

    bool bound = false;

    // Ensure that geometry is buffered into GPU
    if (!m_isUploaded) {
        bound = upload();
    } else if (m_dirty) {
        bound = subDataUpload();
    }

    if (!bound) {
        // Bind buffers for drawing
        glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);

        if (m_nIndices > 0) {
            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_glIndexBuffer);
        }
    }

    // Enable shader program
    _shader->use();

    size_t indiceOffset = 0;
    size_t vertexOffset = 0;

    for (auto& o : m_vertexOffsets) {
        uint32_t nIndices = o.first;
        uint32_t nVertices = o.second;

        size_t byteOffset = vertexOffset * m_vertexLayout->getStride();

        // Enable vertex attribs via vertex layout object
        m_vertexLayout->enable(*_shader, byteOffset);

        // Draw as elements or arrays
        if (nIndices > 0) {
            glDrawElements(m_drawMode, nIndices, GL_UNSIGNED_SHORT,
                           (void*)(indiceOffset * sizeof(GLushort)));
        } else if (nVertices > 0) {
            glDrawArrays(m_drawMode, 0, nVertices);
        }

        vertexOffset += nVertices;
        indiceOffset += nIndices;
    }
}

void VboMesh::checkValidity() {
    if (m_generation != s_validGeneration) {
        m_isUploaded = false;
        m_glVertexBuffer = 0;
        m_glIndexBuffer = 0;

        m_generation = s_validGeneration;
    }
}

void VboMesh::invalidateAllVBOs() {

    ++s_validGeneration;

}
<commit_msg>fix sub mesh update<commit_after>#include "vboMesh.h"
#include "shaderProgram.h"

int VboMesh::s_validGeneration = 0;

VboMesh::VboMesh() {
    m_glVertexBuffer = 0;
    m_glIndexBuffer = 0;
    m_nVertices = 0;
    m_nIndices = 0;
    m_dirtyOffset = 0;
    m_dirtySize = 0;

    m_dirty = false;
    m_isUploaded = false;
    m_isCompiled = false;

    m_generation = -1;
}

VboMesh::VboMesh(std::shared_ptr<VertexLayout> _vertexLayout, GLenum _drawMode, GLenum _hint) : VboMesh() {
    m_vertexLayout = _vertexLayout;
    m_hint = _hint;

    setDrawMode(_drawMode);
}

VboMesh::~VboMesh() {
    if (m_glVertexBuffer) glDeleteBuffers(1, &m_glVertexBuffer);
    if (m_glIndexBuffer) glDeleteBuffers(1, &m_glIndexBuffer);

    delete[] m_glVertexData;
    delete[] m_glIndexData;
}

void VboMesh::setVertexLayout(std::shared_ptr<VertexLayout> _vertexLayout) {
    m_vertexLayout = _vertexLayout;
}

void VboMesh::setDrawMode(GLenum _drawMode) {
    switch (_drawMode) {
        case GL_POINTS:
        case GL_LINE_STRIP:
        case GL_LINE_LOOP:
        case GL_LINES:
        case GL_TRIANGLE_STRIP:
        case GL_TRIANGLE_FAN:
        case GL_TRIANGLES:
            m_drawMode = _drawMode;
            break;
        default:
            logMsg("WARNING: Invalid draw mode for mesh! Defaulting to GL_TRIANGLES\n");
            m_drawMode = GL_TRIANGLES;
    }
}

void VboMesh::update(GLintptr _offset, GLsizei _size, unsigned char* _data) {
    if (m_hint == GL_STATIC_DRAW) {
        logMsg("WARNING: wrong usage hint provided to the Vbo\n");
    }

    std::memcpy(m_glVertexData + _offset, _data, _size);

    m_dirtyOffset = _offset;
    m_dirtySize = _size;

    m_dirty = true;
}

bool VboMesh::subDataUpload() {
    if (m_dirtySize == 0) {
        return false;
    }

    glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);

    long vertexBytes = m_nVertices * m_vertexLayout->getStride();

    // updating the entire buffer
    if (vertexBytes - m_dirtySize < m_vertexLayout->getStride()) {

        // invalidate the data store on the driver
        glBufferData(GL_ARRAY_BUFFER, vertexBytes, NULL, m_hint);

        // if this buffer is still used by gpu on current frame this call will not wait
        // for the frame to finish using the vbo but directly upload the data
        glBufferData(GL_ARRAY_BUFFER, vertexBytes, m_glVertexData, m_hint);
    } else {
        // perform simple sub data upload for part of the buffer
        glBufferSubData(GL_ARRAY_BUFFER, m_dirtyOffset, m_dirtySize, m_glVertexData + m_dirtyOffset);
    }

    m_dirtyOffset = 0;
    m_dirtySize = 0;

    m_dirty = false;

    return true;
}

bool VboMesh::upload() {
    if (m_isUploaded) {
        return false;
    }

    // Generate vertex buffer, if needed
    if (m_glVertexBuffer == 0) {
        glGenBuffers(1, &m_glVertexBuffer);
    }

    // TODO check if compiled?
    // Buffer vertex data
    int vertexBytes = m_nVertices * m_vertexLayout->getStride();

    glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);
    glBufferData(GL_ARRAY_BUFFER, vertexBytes, m_glVertexData, m_hint);

    if (m_glIndexData) {

        if (m_glIndexBuffer == 0) {
            glGenBuffers(1, &m_glIndexBuffer);
        }

        // Buffer element index data
        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_glIndexBuffer);
        glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_nIndices * sizeof(GLushort), m_glIndexData, m_hint);
    }

    // m_glVertexData.resize(0);
    // m_glIndexData.resize(0);

    // TODO: For now, we retain copies of the vertex and index data in CPU memory to allow VBOs
    // to easily rebuild themselves after GL context loss. For optimizing memory usage (and for
    // other reasons) we'll want to change this in the future. This probably means going back to
    // data sources and styles to rebuild the vertex data.

    m_generation = s_validGeneration;

    m_isUploaded = true;

    return true;

}

void VboMesh::draw(const std::shared_ptr<ShaderProgram> _shader) {

    checkValidity();

    if (!m_isCompiled) return;

    if (m_nVertices == 0) return;

    bool bound = false;

    // Ensure that geometry is buffered into GPU
    if (!m_isUploaded) {
        bound = upload();
    } else if (m_dirty) {
        bound = subDataUpload();
    }

    if (!bound) {
        // Bind buffers for drawing
        glBindBuffer(GL_ARRAY_BUFFER, m_glVertexBuffer);

        if (m_nIndices > 0) {
            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_glIndexBuffer);
        }
    }

    // Enable shader program
    _shader->use();

    size_t indiceOffset = 0;
    size_t vertexOffset = 0;

    for (auto& o : m_vertexOffsets) {
        uint32_t nIndices = o.first;
        uint32_t nVertices = o.second;

        size_t byteOffset = vertexOffset * m_vertexLayout->getStride();

        // Enable vertex attribs via vertex layout object
        m_vertexLayout->enable(*_shader, byteOffset);

        // Draw as elements or arrays
        if (nIndices > 0) {
            glDrawElements(m_drawMode, nIndices, GL_UNSIGNED_SHORT,
                           (void*)(indiceOffset * sizeof(GLushort)));
        } else if (nVertices > 0) {
            glDrawArrays(m_drawMode, 0, nVertices);
        }

        vertexOffset += nVertices;
        indiceOffset += nIndices;
    }
}

void VboMesh::checkValidity() {
    if (m_generation != s_validGeneration) {
        m_isUploaded = false;
        m_glVertexBuffer = 0;
        m_glIndexBuffer = 0;

        m_generation = s_validGeneration;
    }
}

void VboMesh::invalidateAllVBOs() {

    ++s_validGeneration;

}
<|endoftext|>


<commit_before>// Copyright (C) 2013-2014 Thalmic Labs Inc.
// Confidential and not for redistribution. See LICENSE.txt.
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <string>

// The only file that needs to be included to use the Myo C++ SDK is myo.hpp.
#include <myo.hpp>

// Classes that inherit from myo::DeviceListener can be used to receive events from Myo devices. DeviceListener
// provides several virtual functions for handling different kinds of events. If you do not override an event, the
// default behavior is to do nothing.
class DataCollector : public myo::DeviceListener {
public:
    DataCollector()
    : roll_w(0), pitch_w(0), yaw_w(0), currentPose()
    {
    }
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2f;
        using std::asinf;
        using std::sqrtf;

        // Calculate the normalized quaternion.
        float norm = sqrtf(quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z() + quat.w() * quat.w());
        myo::Quaternion<float> normalized(quat.x()/norm, quat.y()/norm, quat.z()/norm, quat.w()/norm);

        // Calculate Euler angles (roll, pitch, and yaw) from the normalized quaternion.
        float roll = atan2f(2.0f * (normalized.w() * normalized.x() + normalized.y() * normalized.z()),
                           1.0f - 2.0f * (normalized.x() * normalized.x() + normalized.y() * normalized.y()));
        float pitch = asinf(2.0f * (normalized.w() * normalized.y() - normalized.z() * normalized.x()));
        float yaw = atan2f(2.0f * (normalized.w() * normalized.z() + normalized.x() * normalized.y()),
                           1.0f - 2.0f * (normalized.y() * normalized.y() + normalized.z() * normalized.z()));

        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    }

    // onPose() is called whenever the Myo detects that the person wearing it has changed their pose, for example,
    // making a fist, or not making a fist anymore.
    void onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
    {
        currentPose = pose;

		if (currentPose == myo::Pose::fist) {
			if (pitch_w < 5) {
				// Vibrate the Myo whenever we've detected that the user has made a fist pump.
				myo->vibrate(myo::Myo::VibrationMedium);
			}
		}
	}

    // There are other virtual functions in DeviceListener that we could override here, like onAccelerometerData().
    // For this example, the functions overridden above are sufficient.

    // We define this function to print the current values that were updated by the on...() functions above.
    void print()
    {
        // Clear the current line
        std::cout << '\r';

        // Pose::toString() provides the human-readable name of a pose. We can also output a Pose directly to an
        // output stream (e.g. std::cout << currentPose;). In this case we want to get the pose name's length so
        // that we can fill the rest of the field with spaces below, so we obtain it as a string using toString().
        std::string poseString = currentPose.toString();

        // Output the current values
        std::cout << '[' << std::string(roll_w, '*') << std::string(18 - roll_w, ' ') << ']'
                  << '[' << std::string(pitch_w, '*') << std::string(18 - pitch_w, ' ') << ']'
                  << '[' << std::string(yaw_w, '*') << std::string(18 - yaw_w, ' ') << ']'
                  << '[' << poseString << std::string(16 - poseString.size(), ' ') << ']'
                  << std::flush;
    }

    // These values are set by onOrientationData() and onPose() above.
    int roll_w, pitch_w, yaw_w;
    myo::Pose currentPose;
};

int main(int argc, char** argv)
{
    // We catch any exceptions that might occur below -- see the catch statement for more details.
    try {

    // First, we create a Hub. The Hub provides access to one or more Myos.
    myo::Hub hub;

	std::cout << "Bike signs." << std::endl;

    std::cout << "Attempting to find a Myo..." << std::endl;

    // Next, we try to find a Myo (any Myo) that's nearby and connect to it. waitForAnyMyo() takes a timeout
    // value in milliseconds. In this case we will try to find a Myo for 10 seconds, and if that fails, the function
    // will return a null pointer.
    myo::Myo* myo = hub.waitForAnyMyo(10000);

    // If waitForAnyMyo() returned a null pointer, we failed to find a Myo, so exit with an error message.
    if (!myo) {
        throw std::runtime_error("Unable to find a Myo!");
    }

    // We've found a Myo, let's output its MAC address.
    std::cout << "Connected to " << std::hex << std::setfill('0') << std::setw(12) << myo->macAddress() << std::dec
              << "." << std::endl << std::endl;

    // Next we construct an instance of our DeviceListener, so that we can register it with the Hub.
    DataCollector collector;

    // Hub::addListener() takes the address of any object whose class inherits from DeviceListener, and will cause
    // Hub::run() to send events to all registered device listeners.
    hub.addListener(&collector);

    // Finally we enter our main loop.
    while (1) {
        // In each iteration of our main loop, we run the Myo event loop for a set number of milliseconds.
        // In this case, we wish to update our display 20 times a second, so we run for 1000/20 milliseconds.
        hub.run(1000/20);
        // After processing events, we call the print() member function we defined above to print out the values we've
        // obtained from any events that have occurred.
        collector.print();
    }

    // If a standard exception occurred, we print out its message and exit.
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cerr << "Press enter to continue.";
        std::cin.ignore();
        return 1;
    }
}
<commit_msg>Left and right turn signal detection.<commit_after>// Copyright (C) 2013-2014 Thalmic Labs Inc.
// Confidential and not for redistribution. See LICENSE.txt.
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <string>

// The only file that needs to be included to use the Myo C++ SDK is myo.hpp.
#include <myo.hpp>

// Classes that inherit from myo::DeviceListener can be used to receive events from Myo devices. DeviceListener
// provides several virtual functions for handling different kinds of events. If you do not override an event, the
// default behavior is to do nothing.
class DataCollector : public myo::DeviceListener {
public:
    DataCollector()
    : roll_w(0), pitch_w(0), yaw_w(0), currentPose()
    {
    }
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2f;
        using std::asinf;
        using std::sqrtf;

        // Calculate the normalized quaternion.
        float norm = sqrtf(quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z() + quat.w() * quat.w());
        myo::Quaternion<float> normalized(quat.x()/norm, quat.y()/norm, quat.z()/norm, quat.w()/norm);

        // Calculate Euler angles (roll, pitch, and yaw) from the normalized quaternion.
        float roll = atan2f(2.0f * (normalized.w() * normalized.x() + normalized.y() * normalized.z()),
                           1.0f - 2.0f * (normalized.x() * normalized.x() + normalized.y() * normalized.y()));
        float pitch = asinf(2.0f * (normalized.w() * normalized.y() - normalized.z() * normalized.x()));
        float yaw = atan2f(2.0f * (normalized.w() * normalized.z() + normalized.x() * normalized.y()),
                           1.0f - 2.0f * (normalized.y() * normalized.y() + normalized.z() * normalized.z()));

        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    }

    // onPose() is called whenever the Myo detects that the person wearing it has changed their pose, for example,
    // making a fist, or not making a fist anymore.
    void onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
    {
        currentPose = pose;

		if (currentPose == myo::Pose::fist) {
			if (pitch_w < 5) {
				// Vibrate the Myo whenever we've detected a right turn signal.
				myo->vibrate(myo::Myo::VibrationMedium);
			}
			else if (roll_w > 3 && roll_w < 7) {
				// Vibrate the Myo whenever we've detected a left turn signal.
				myo->vibrate(myo::Myo::VibrationShort);
			}
		}
	}

    // There are other virtual functions in DeviceListener that we could override here, like onAccelerometerData().
    // For this example, the functions overridden above are sufficient.

    // We define this function to print the current values that were updated by the on...() functions above.
    void print()
    {
        // Clear the current line
        std::cout << '\r';

        // Pose::toString() provides the human-readable name of a pose. We can also output a Pose directly to an
        // output stream (e.g. std::cout << currentPose;). In this case we want to get the pose name's length so
        // that we can fill the rest of the field with spaces below, so we obtain it as a string using toString().
        std::string poseString = currentPose.toString();

        // Output the current values
        std::cout << '[' << std::string(roll_w, '*') << std::string(18 - roll_w, ' ') << ']'
                  << '[' << std::string(pitch_w, '*') << std::string(18 - pitch_w, ' ') << ']'
                  << '[' << std::string(yaw_w, '*') << std::string(18 - yaw_w, ' ') << ']'
                  << '[' << poseString << std::string(16 - poseString.size(), ' ') << ']'
                  << std::flush;
    }

    // These values are set by onOrientationData() and onPose() above.
    int roll_w, pitch_w, yaw_w;
    myo::Pose currentPose;
};

int main(int argc, char** argv)
{
    // We catch any exceptions that might occur below -- see the catch statement for more details.
    try {

    // First, we create a Hub. The Hub provides access to one or more Myos.
    myo::Hub hub;

	std::cout << "Bike signs." << std::endl;

    std::cout << "Attempting to find a Myo..." << std::endl;

    // Next, we try to find a Myo (any Myo) that's nearby and connect to it. waitForAnyMyo() takes a timeout
    // value in milliseconds. In this case we will try to find a Myo for 10 seconds, and if that fails, the function
    // will return a null pointer.
    myo::Myo* myo = hub.waitForAnyMyo(10000);

    // If waitForAnyMyo() returned a null pointer, we failed to find a Myo, so exit with an error message.
    if (!myo) {
        throw std::runtime_error("Unable to find a Myo!");
    }

    // We've found a Myo, let's output its MAC address.
    std::cout << "Connected to " << std::hex << std::setfill('0') << std::setw(12) << myo->macAddress() << std::dec
              << "." << std::endl << std::endl;

    // Next we construct an instance of our DeviceListener, so that we can register it with the Hub.
    DataCollector collector;

    // Hub::addListener() takes the address of any object whose class inherits from DeviceListener, and will cause
    // Hub::run() to send events to all registered device listeners.
    hub.addListener(&collector);

    // Finally we enter our main loop.
    while (1) {
        // In each iteration of our main loop, we run the Myo event loop for a set number of milliseconds.
        // In this case, we wish to update our display 20 times a second, so we run for 1000/20 milliseconds.
        hub.run(1000/20);
        // After processing events, we call the print() member function we defined above to print out the values we've
        // obtained from any events that have occurred.
        collector.print();
    }

    // If a standard exception occurred, we print out its message and exit.
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cerr << "Press enter to continue.";
        std::cin.ignore();
        return 1;
    }
}
<|endoftext|>