<commit_before>/*
    Copyright (C) 2011  Martin Klapetek <martin.klapetek@gmail.com>
    Copyright (C) 2011  Dario Freddi <dario.freddi@collabora.com>

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "contact-request-handler.h"

#include <TelepathyQt4/Connection>
#include <TelepathyQt4/ContactManager>
#include <TelepathyQt4/PendingOperation>
#include <TelepathyQt4/Account>

#include <KTelepathy/error-dictionary.h>

#include <QtCore/QFutureWatcher>

#include <KDebug>
#include <KGlobal>
#include <KAboutData>
#include <KMenu>
#include <KAction>
#include <KStatusNotifierItem>

Q_DECLARE_METATYPE(Tp::ContactPtr)

bool kde_tp_filter_contacts_by_publication_status(const Tp::ContactPtr &contact)
{
    return contact->publishState() == Tp::Contact::PresenceStateAsk;
}

ContactRequestHandler::ContactRequestHandler(const Tp::AccountManagerPtr& am, QObject *parent)
    : QObject(parent)
{
    m_accountManager = am;
    connect(m_accountManager.data(), SIGNAL(newAccount(Tp::AccountPtr)),
            this, SLOT(onNewAccountAdded(Tp::AccountPtr)));

    QList<Tp::AccountPtr> accounts = m_accountManager->allAccounts();

    Q_FOREACH(const Tp::AccountPtr &account, accounts) {
        onNewAccountAdded(account);
    }

    m_noContactsAction = new KAction(i18n("No pending contact requests at the moment"), this);
    m_noContactsAction->setEnabled(false);
}

ContactRequestHandler::~ContactRequestHandler()
{

}

void ContactRequestHandler::onNewAccountAdded(const Tp::AccountPtr& account)
{
    kWarning();
    Q_ASSERT(account->isReady(Tp::Account::FeatureCore));

    if (account->connection()) {
        monitorPresence(account->connection());
    }

    connect(account.data(),
            SIGNAL(connectionChanged(Tp::ConnectionPtr)),
            this, SLOT(onConnectionChanged(Tp::ConnectionPtr)));
}

void ContactRequestHandler::onConnectionChanged(const Tp::ConnectionPtr& connection)
{
    if (!connection.isNull()) {
        monitorPresence(connection);
    }
}

void ContactRequestHandler::monitorPresence(const Tp::ConnectionPtr &connection)
{
    kDebug();
    connect(connection->contactManager().data(), SIGNAL(presencePublicationRequested(Tp::Contacts)),
            this, SLOT(onPresencePublicationRequested(Tp::Contacts)));

    connect(connection->contactManager().data(),
            SIGNAL(stateChanged(Tp::ContactListState)),
            this, SLOT(onContactManagerStateChanged(Tp::ContactListState)));

    onContactManagerStateChanged(connection->contactManager(),
                                 connection->contactManager()->state());
}

void ContactRequestHandler::onContactManagerStateChanged(Tp::ContactListState state)
{
    onContactManagerStateChanged(Tp::ContactManagerPtr(qobject_cast< Tp::ContactManager* >(sender())), state);
}

void ContactRequestHandler::onContactManagerStateChanged(const Tp::ContactManagerPtr &contactManager,
                                                         Tp::ContactListState state)
{
    if (state == Tp::ContactListStateSuccess) {
        QFutureWatcher< Tp::ContactPtr > *watcher = new QFutureWatcher< Tp::ContactPtr >(this);
        connect(watcher, SIGNAL(finished()), this, SLOT(onAccountsPresenceStatusFiltered()));
        watcher->setFuture(QtConcurrent::filtered(contactManager->allKnownContacts(),
                                                  kde_tp_filter_contacts_by_publication_status));

        kDebug() << "Watcher is on";
    } else {
        kDebug() << "Watcher still off, state is" << state << "contactManager is" << contactManager.isNull();
    }
}

void ContactRequestHandler::onAccountsPresenceStatusFiltered()
{
    kDebug() << "Watcher is here";
    QFutureWatcher< Tp::ContactPtr > *watcher = dynamic_cast< QFutureWatcher< Tp::ContactPtr > * >(sender());
    kDebug() << "Watcher is casted";
    Tp::Contacts contacts = watcher->future().results().toSet();
    kDebug() << "Watcher is used";
    if (!contacts.isEmpty()) {
        onPresencePublicationRequested(contacts);
    }
    watcher->deleteLater();
}

void ContactRequestHandler::onPresencePublicationRequested(const Tp::Contacts& contacts)
{
    kDebug() << "New contact requested";

    Q_FOREACH (const Tp::ContactPtr &contact, contacts) {
        Tp::ContactManagerPtr manager = contact->manager();

        if (contact->subscriptionState() == Tp::Contact::PresenceStateYes) {
            Tp::PendingOperation *op = manager->authorizePresencePublication(QList< Tp::ContactPtr >() << contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect(op, SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onFinalizeSubscriptionFinished(Tp::PendingOperation*)));
        } else {
            m_pendingContacts.insert(contact->id(), contact);

            updateMenus();

            m_notifierItem.data()->showMessage(i18n("New contact request"),
                                               i18n("The contact %1 added you to its contact list",
                                                    contact->id()),
                                               QLatin1String("list-add-user"));
        }
    }
}

void ContactRequestHandler::onFinalizeSubscriptionFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error adding contact"),
                                           i18n("%1 has been added successfully to your contact list, "
                                                "but might be unable to see your presence. Error details: %2",
                                                contact->alias(), KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                                QLatin1String("dialog-error"));
    } else {
        // Yeah. All fine, so don't notify
    }
}

void ContactRequestHandler::updateNotifierItemTooltip()
{
    if (!m_menuItems.size()) {
        // Set passive
        m_notifierItem.data()->setStatus(KStatusNotifierItem::Passive);
        // Add the usual "nothing" action, if needed
        if (!m_notifierMenu->actions().contains(m_noContactsAction)) {
            m_notifierMenu->addAction(m_noContactsAction);
        }

        m_notifierItem.data()->setToolTip(QLatin1String("list-add-user"),
                                          i18n("No incoming contact requests"),
                                          QString());
    } else {
        // Set active
        m_notifierItem.data()->setStatus(KStatusNotifierItem::Active);
        // Remove the "nothing" action, if needed
        if (m_notifierMenu->actions().contains(m_noContactsAction)) {
            m_notifierMenu->removeAction(m_noContactsAction);
        }

        m_notifierItem.data()->setToolTip(QLatin1String("list-add-user"),
                                          i18np("You have 1 incoming contact request",
                                                "You have %1 incoming contact requests",
                                                m_menuItems.size()),
                                          QString());
    }
}

void ContactRequestHandler::onContactRequestApproved()
{
    QString contactId = qobject_cast<KAction*>(sender())->data().toString();

    if (!contactId.isEmpty()) {
        Tp::ContactPtr contact = m_pendingContacts.value(contactId);
        if (!contact.isNull()) {
            Tp::PendingOperation *op = contact->manager()->authorizePresencePublication(QList< Tp::ContactPtr >() << contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect (op, SIGNAL(finished(Tp::PendingOperation*)),
                     this, SLOT(onAuthorizePresencePublicationFinished(Tp::PendingOperation*)));
        }
    }
}

void ContactRequestHandler::onAuthorizePresencePublicationFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error accepting contact request"),
                                           i18n("There was an error while accepting the request: %1",
                                                KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                           QLatin1String("dialog-error"));

        // Re-enable the action
        m_menuItems.value(contact->id())->setEnabled(true);
    } else {
        // Yeah
        m_notifierItem.data()->showMessage(i18n("Contact request accepted"),
                                           i18n("%1 will now be able to see your presence",
                                                contact->alias()), QLatin1String("dialog-ok-apply"));

        // If needed, reiterate the request on the other end
        if (contact->manager()->canRequestPresenceSubscription() &&
            contact->subscriptionState() == Tp::Contact::PresenceStateNo) {
            connect(contact->manager()->requestPresenceSubscription(QList< Tp::ContactPtr >() << contact),
                    SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onFinalizeSubscriptionFinished(Tp::PendingOperation*)));
        }

        // Update the menu
        m_pendingContacts.remove(contact->id());
        updateMenus();
    }
}

void ContactRequestHandler::onContactRequestDenied()
{
    QString contactId = qobject_cast<KAction*>(sender())->data().toString();

    // Disable the action in the meanwhile
    m_menuItems.value(contactId)->setEnabled(false);

    if (!contactId.isEmpty()) {
        Tp::ContactPtr contact = m_pendingContacts.value(contactId);
        if (!contact.isNull()) {
            Tp::PendingOperation *op = contact->manager()->removePresencePublication(QList< Tp::ContactPtr >() << contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect(op, SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onRemovePresencePublicationFinished(Tp::PendingOperation*)));
        }
    }
}

void ContactRequestHandler::onRemovePresencePublicationFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error denying contact request"),
                                           i18n("There was an error while denying the request: %1",
                                                KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                                QLatin1String("dialog-error"));

        // Re-enable the action
        m_menuItems.value(contact->id())->setEnabled(true);
    } else {
        // Yeah
        m_notifierItem.data()->showMessage(i18n("Contact request denied"),
                                           i18n("%1 will not be able to see your presence",
                                                contact->alias()), QLatin1String("dialog-ok-apply"));

        // Update the menu
        m_pendingContacts.remove(contact->id());
        updateMenus();
    }
}

void ContactRequestHandler::updateMenus()
{
    if (m_notifierItem.isNull()) {
        m_notifierItem = new KStatusNotifierItem(QLatin1String("telepathy_kde_contact_requests"), this);
        m_notifierItem.data()->setCategory(KStatusNotifierItem::Communications);
        m_notifierItem.data()->setStatus(KStatusNotifierItem::NeedsAttention);
        m_notifierItem.data()->setIconByName(QLatin1String("user-identity"));
        m_notifierItem.data()->setAttentionIconByName(QLatin1String("list-add-user"));
        m_notifierItem.data()->setStandardActionsEnabled(false);
        m_notifierItem.data()->setTitle(i18nc("Menu title", "Pending contact requests"));

        m_notifierMenu = new KMenu(0);
        m_notifierMenu->addTitle(i18nc("Context menu title", "Received contact requests"));

        m_notifierItem.data()->setContextMenu(m_notifierMenu);
    }

    kDebug() << m_pendingContacts.keys();

    QHash<QString, Tp::ContactPtr>::const_iterator i;
    for (i = m_pendingContacts.constBegin(); i != m_pendingContacts.constEnd(); ++i) {
        if (m_menuItems.contains(i.key())) {
            // Skip
            continue;
        }

        kDebug();
        Tp::ContactPtr contact = i.value();

        KMenu *contactMenu = new KMenu(m_notifierMenu);
        contactMenu->setTitle(i18n("Request from %1", contact->alias()));
        contactMenu->setObjectName(contact->id());

        KAction *menuAction;
        if (!contact->publishStateMessage().isEmpty()) {
            contactMenu->addTitle(contact->publishStateMessage());
        } else {
            contactMenu->addTitle(contact->alias());
        }
        menuAction = new KAction(KIcon(QLatin1String("dialog-ok-apply")), i18n("Approve"), contactMenu);
        menuAction->setData(i.key());
        connect(menuAction, SIGNAL(triggered()),
                this, SLOT(onContactRequestApproved()));
        contactMenu->addAction(menuAction);

        menuAction = new KAction(KIcon(QLatin1String("dialog-close")), i18n("Deny"), contactMenu);
        menuAction->setData(i.key());
        connect(menuAction, SIGNAL(triggered()),
                this, SLOT(onContactRequestDenied()));
        contactMenu->addAction(menuAction);

        m_notifierMenu->addMenu(contactMenu);
        m_menuItems.insert(i.key(), contactMenu);
    }

    QHash<QString, KMenu*>::iterator j = m_menuItems.begin();
    while (j != m_menuItems.end()) {
        if (m_pendingContacts.contains(j.key())) {
            // Skip
            ++j;
            continue;
        }

        // Remove
        m_notifierMenu->removeAction(j.value()->menuAction());
        j = m_menuItems.erase(j);
    }

    updateNotifierItemTooltip();
}

#include "contact-request-handler.moc"
<commit_msg>Handle multi-account requests with the same contact Id as if they were one, and authorize them altogether<commit_after>/*
    Copyright (C) 2011  Martin Klapetek <martin.klapetek@gmail.com>
    Copyright (C) 2011  Dario Freddi <dario.freddi@collabora.com>

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "contact-request-handler.h"

#include <TelepathyQt4/Connection>
#include <TelepathyQt4/ContactManager>
#include <TelepathyQt4/PendingOperation>
#include <TelepathyQt4/PendingComposite>
#include <TelepathyQt4/Account>

#include <KTelepathy/error-dictionary.h>

#include <QtCore/QFutureWatcher>

#include <KDebug>
#include <KGlobal>
#include <KAboutData>
#include <KMenu>
#include <KAction>
#include <KStatusNotifierItem>

Q_DECLARE_METATYPE(Tp::ContactPtr)

bool kde_tp_filter_contacts_by_publication_status(const Tp::ContactPtr &contact)
{
    return contact->publishState() == Tp::Contact::PresenceStateAsk;
}

ContactRequestHandler::ContactRequestHandler(const Tp::AccountManagerPtr& am, QObject *parent)
    : QObject(parent)
{
    m_accountManager = am;
    connect(m_accountManager.data(), SIGNAL(newAccount(Tp::AccountPtr)),
            this, SLOT(onNewAccountAdded(Tp::AccountPtr)));

    QList<Tp::AccountPtr> accounts = m_accountManager->allAccounts();

    Q_FOREACH(const Tp::AccountPtr &account, accounts) {
        onNewAccountAdded(account);
    }

    m_noContactsAction = new KAction(i18n("No pending contact requests at the moment"), this);
    m_noContactsAction->setEnabled(false);
}

ContactRequestHandler::~ContactRequestHandler()
{

}

void ContactRequestHandler::onNewAccountAdded(const Tp::AccountPtr& account)
{
    kWarning();
    Q_ASSERT(account->isReady(Tp::Account::FeatureCore));

    if (account->connection()) {
        monitorPresence(account->connection());
    }

    connect(account.data(),
            SIGNAL(connectionChanged(Tp::ConnectionPtr)),
            this, SLOT(onConnectionChanged(Tp::ConnectionPtr)));
}

void ContactRequestHandler::onConnectionChanged(const Tp::ConnectionPtr& connection)
{
    if (!connection.isNull()) {
        monitorPresence(connection);
    }
}

void ContactRequestHandler::monitorPresence(const Tp::ConnectionPtr &connection)
{
    kDebug();
    connect(connection->contactManager().data(), SIGNAL(presencePublicationRequested(Tp::Contacts)),
            this, SLOT(onPresencePublicationRequested(Tp::Contacts)));

    connect(connection->contactManager().data(),
            SIGNAL(stateChanged(Tp::ContactListState)),
            this, SLOT(onContactManagerStateChanged(Tp::ContactListState)));

    onContactManagerStateChanged(connection->contactManager(),
                                 connection->contactManager()->state());
}

void ContactRequestHandler::onContactManagerStateChanged(Tp::ContactListState state)
{
    onContactManagerStateChanged(Tp::ContactManagerPtr(qobject_cast< Tp::ContactManager* >(sender())), state);
}

void ContactRequestHandler::onContactManagerStateChanged(const Tp::ContactManagerPtr &contactManager,
                                                         Tp::ContactListState state)
{
    if (state == Tp::ContactListStateSuccess) {
        QFutureWatcher< Tp::ContactPtr > *watcher = new QFutureWatcher< Tp::ContactPtr >(this);
        connect(watcher, SIGNAL(finished()), this, SLOT(onAccountsPresenceStatusFiltered()));
        watcher->setFuture(QtConcurrent::filtered(contactManager->allKnownContacts(),
                                                  kde_tp_filter_contacts_by_publication_status));

        kDebug() << "Watcher is on";
    } else {
        kDebug() << "Watcher still off, state is" << state << "contactManager is" << contactManager.isNull();
    }
}

void ContactRequestHandler::onAccountsPresenceStatusFiltered()
{
    kDebug() << "Watcher is here";
    QFutureWatcher< Tp::ContactPtr > *watcher = dynamic_cast< QFutureWatcher< Tp::ContactPtr > * >(sender());
    kDebug() << "Watcher is casted";
    Tp::Contacts contacts = watcher->future().results().toSet();
    kDebug() << "Watcher is used";
    if (!contacts.isEmpty()) {
        onPresencePublicationRequested(contacts);
    }
    watcher->deleteLater();
}

void ContactRequestHandler::onPresencePublicationRequested(const Tp::Contacts& contacts)
{
    kDebug() << "New contact requested";

    Q_FOREACH (const Tp::ContactPtr &contact, contacts) {
        Tp::ContactManagerPtr manager = contact->manager();

        if (contact->subscriptionState() == Tp::Contact::PresenceStateYes) {
            Tp::PendingOperation *op = manager->authorizePresencePublication(QList< Tp::ContactPtr >() << contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect(op, SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onFinalizeSubscriptionFinished(Tp::PendingOperation*)));
        } else {
            // Handle multiaccount requests properly
            if (m_pendingContacts.contains(contact->id())) {
                // It's likely we have a simultaneous request
                bool newReq = true;
                QHash<QString, Tp::ContactPtr>::const_iterator i = m_pendingContacts.find(contact->id());
                while (i != m_pendingContacts.constEnd() && i.key() == contact->id()) {
                    if (i.value().data() == contact.data()) {
                        newReq = false;
                        break;
                    }
                    ++i;
                }

                if (newReq) {
                    // Insert multi
                    m_pendingContacts.insertMulti(contact->id(), contact);
                }
            } else {
                // Simple insertion
                m_pendingContacts.insert(contact->id(), contact);
            }

            updateMenus();

            m_notifierItem.data()->showMessage(i18n("New contact request"),
                                               i18n("The contact %1 added you to its contact list",
                                                    contact->id()),
                                               QLatin1String("list-add-user"));
        }
    }
}

void ContactRequestHandler::onFinalizeSubscriptionFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error adding contact"),
                                           i18n("%1 has been added successfully to your contact list, "
                                                "but might be unable to see your presence. Error details: %2",
                                                contact->alias(), KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                                QLatin1String("dialog-error"));
    } else {
        // Yeah. All fine, so don't notify
    }
}

void ContactRequestHandler::updateNotifierItemTooltip()
{
    if (!m_menuItems.size()) {
        // Set passive
        m_notifierItem.data()->setStatus(KStatusNotifierItem::Passive);
        // Add the usual "nothing" action, if needed
        if (!m_notifierMenu->actions().contains(m_noContactsAction)) {
            m_notifierMenu->addAction(m_noContactsAction);
        }

        m_notifierItem.data()->setToolTip(QLatin1String("list-add-user"),
                                          i18n("No incoming contact requests"),
                                          QString());
    } else {
        // Set active
        m_notifierItem.data()->setStatus(KStatusNotifierItem::Active);
        // Remove the "nothing" action, if needed
        if (m_notifierMenu->actions().contains(m_noContactsAction)) {
            m_notifierMenu->removeAction(m_noContactsAction);
        }

        m_notifierItem.data()->setToolTip(QLatin1String("list-add-user"),
                                          i18np("You have 1 incoming contact request",
                                                "You have %1 incoming contact requests",
                                                m_menuItems.size()),
                                          QString());
    }
}

void ContactRequestHandler::onContactRequestApproved()
{
    QString contactId = qobject_cast<KAction*>(sender())->data().toString();

    // Disable the action in the meanwhile
    m_menuItems.value(contactId)->setEnabled(false);

    if (!contactId.isEmpty()) {
        QList<Tp::PendingOperation*> operations;
        QHash<QString, Tp::ContactPtr>::const_iterator i = m_pendingContacts.find(contactId);
        while (i != m_pendingContacts.constEnd() && i.key() == contactId) {
            Tp::PendingOperation *op = i.value()->manager()->authorizePresencePublication(QList< Tp::ContactPtr >() << i.value());
            op->setProperty("__contact", QVariant::fromValue(i.value()));
            operations.append(op);
            ++i;
        }

        // Take the first value, if any
        if (!operations.isEmpty()) {
            Tp::ContactPtr contact = m_pendingContacts.find(contactId).value();

            Tp::PendingComposite *op = new Tp::PendingComposite(operations, true, contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect(op, SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onAuthorizePresencePublicationFinished(Tp::PendingOperation*)));
        }
    }

}

void ContactRequestHandler::onAuthorizePresencePublicationFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error accepting contact request"),
                                           i18n("There was an error while accepting the request: %1",
                                                KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                           QLatin1String("dialog-error"));

        // Re-enable the action
        m_menuItems.value(contact->id())->setEnabled(true);
    } else {
        // Yeah
        m_notifierItem.data()->showMessage(i18n("Contact request accepted"),
                                           i18n("%1 will now be able to see your presence",
                                                contact->alias()), QLatin1String("dialog-ok-apply"));

        // If needed, reiterate the request on the other end
        if (contact->manager()->canRequestPresenceSubscription() &&
            contact->subscriptionState() == Tp::Contact::PresenceStateNo) {
            connect(contact->manager()->requestPresenceSubscription(QList< Tp::ContactPtr >() << contact),
                    SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onFinalizeSubscriptionFinished(Tp::PendingOperation*)));
        }

        // Update the menu
        m_pendingContacts.remove(contact->id());
        updateMenus();
    }
}

void ContactRequestHandler::onContactRequestDenied()
{
    QString contactId = qobject_cast<KAction*>(sender())->data().toString();

    // Disable the action in the meanwhile
    m_menuItems.value(contactId)->setEnabled(false);

    if (!contactId.isEmpty()) {
        QList<Tp::PendingOperation*> operations;
        QHash<QString, Tp::ContactPtr>::const_iterator i = m_pendingContacts.find(contactId);
        while (i != m_pendingContacts.constEnd() && i.key() == contactId) {
            Tp::PendingOperation *op = i.value()->manager()->removePresencePublication(QList< Tp::ContactPtr >() << i.value());
            op->setProperty("__contact", QVariant::fromValue(i.value()));
            operations.append(op);
            ++i;
        }

        // Take the first value, if any
        if (!operations.isEmpty()) {
            Tp::ContactPtr contact = m_pendingContacts.find(contactId).value();

            Tp::PendingComposite *op = new Tp::PendingComposite(operations, true, contact);
            op->setProperty("__contact", QVariant::fromValue(contact));

            connect(op, SIGNAL(finished(Tp::PendingOperation*)),
                    this, SLOT(onRemovePresencePublicationFinished(Tp::PendingOperation*)));
        }
    }
}

void ContactRequestHandler::onRemovePresencePublicationFinished(Tp::PendingOperation *op)
{
    Tp::ContactPtr contact = op->property("__contact").value< Tp::ContactPtr >();

    if (op->isError()) {
        // ARGH
        m_notifierItem.data()->showMessage(i18n("Error denying contact request"),
                                           i18n("There was an error while denying the request: %1",
                                                KTp::ErrorDictionary::displayVerboseErrorMessage(op->errorName())),
                                                QLatin1String("dialog-error"));

        // Re-enable the action
        m_menuItems.value(contact->id())->setEnabled(true);
    } else {
        // Yeah
        m_notifierItem.data()->showMessage(i18n("Contact request denied"),
                                           i18n("%1 will not be able to see your presence",
                                                contact->alias()), QLatin1String("dialog-ok-apply"));

        // Update the menu
        m_pendingContacts.remove(contact->id());
        updateMenus();
    }
}

void ContactRequestHandler::updateMenus()
{
    if (m_notifierItem.isNull()) {
        m_notifierItem = new KStatusNotifierItem(QLatin1String("telepathy_kde_contact_requests"), this);
        m_notifierItem.data()->setCategory(KStatusNotifierItem::Communications);
        m_notifierItem.data()->setStatus(KStatusNotifierItem::NeedsAttention);
        m_notifierItem.data()->setIconByName(QLatin1String("user-identity"));
        m_notifierItem.data()->setAttentionIconByName(QLatin1String("list-add-user"));
        m_notifierItem.data()->setStandardActionsEnabled(false);
        m_notifierItem.data()->setTitle(i18nc("Menu title", "Pending contact requests"));

        m_notifierMenu = new KMenu(0);
        m_notifierMenu->addTitle(i18nc("Context menu title", "Received contact requests"));

        m_notifierItem.data()->setContextMenu(m_notifierMenu);
    }

    kDebug() << m_pendingContacts.keys();

    QHash<QString, Tp::ContactPtr>::const_iterator i;
    for (i = m_pendingContacts.constBegin(); i != m_pendingContacts.constEnd(); ++i) {
        if (m_menuItems.contains(i.key())) {
            // Skip
            continue;
        }

        kDebug();
        Tp::ContactPtr contact = i.value();

        KMenu *contactMenu = new KMenu(m_notifierMenu);
        contactMenu->setTitle(i18n("Request from %1", contact->alias()));
        contactMenu->setObjectName(contact->id());

        KAction *menuAction;
        if (!contact->publishStateMessage().isEmpty()) {
            contactMenu->addTitle(contact->publishStateMessage());
        } else {
            contactMenu->addTitle(contact->alias());
        }
        menuAction = new KAction(KIcon(QLatin1String("dialog-ok-apply")), i18n("Approve"), contactMenu);
        menuAction->setData(i.key());
        connect(menuAction, SIGNAL(triggered()),
                this, SLOT(onContactRequestApproved()));
        contactMenu->addAction(menuAction);

        menuAction = new KAction(KIcon(QLatin1String("dialog-close")), i18n("Deny"), contactMenu);
        menuAction->setData(i.key());
        connect(menuAction, SIGNAL(triggered()),
                this, SLOT(onContactRequestDenied()));
        contactMenu->addAction(menuAction);

        m_notifierMenu->addMenu(contactMenu);
        m_menuItems.insert(i.key(), contactMenu);
    }

    QHash<QString, KMenu*>::iterator j = m_menuItems.begin();
    while (j != m_menuItems.end()) {
        if (m_pendingContacts.contains(j.key())) {
            // Skip
            ++j;
            continue;
        }

        // Remove
        m_notifierMenu->removeAction(j.value()->menuAction());
        j = m_menuItems.erase(j);
    }

    updateNotifierItemTooltip();
}

#include "contact-request-handler.moc"
<|endoftext|>


<commit_before>// Copyright (c) 2011 Zeex
//
// 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 <algorithm>
#include <cstdio>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <numeric>
#include <map>

#include <malloc.h> // _alloca
#include <stdlib.h>

#include "amxprofiler.h"
#include "logprintf.h"
#include "plugin.h"
#include "version.h"

extern void *pAMXFunctions;

static std::map<AMX*, AMX_DBG> amxDebugInfo;
static std::map<AMX*, AMXProfiler*> amxProfilers;

static bool ByExecutionTime(const AMXProfilerStat &op1, const AMXProfilerStat &op2) {
    return op1.executionTime > op2.executionTime;
}

static bool ByNumberOfCalls(const AMXProfilerStat &op1, const AMXProfilerStat &op2) {
    return op1.numberOfCalls > op2.numberOfCalls;
}

static const char *FindNativeByIndex(AMX *amx, cell index) {
    AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

    AMX_FUNCSTUBNT *natives = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->natives);
    int numNatives = (hdr->libraries - hdr->natives) / hdr->defsize;

    for (int i = 0; i < numNatives; i++) {
        if (i == index) {
            return reinterpret_cast<char*>(amx->base + natives[i].nameofs);
        }
    }

    return 0;
}

namespace natives {

    // native Profiler_Start(const path_to_amx[]);
    cell AMX_NATIVE_CALL Profiler_Start(AMX *amx, cell *params) {
        char *path_to_amx;
        amx_StrParam(amx, params[1], path_to_amx);

        FILE *fp = fopen(path_to_amx, "rb");
        if (fp == 0) {
            return 0;
        }

        AMX_DBG amxdbg;
        if (dbg_LoadInfo(&amxdbg, fp) != AMX_ERR_NONE) {
            fclose(fp);
            return 0;
        }

        amxDebugInfo[amx] = amxdbg;
        fclose(fp);

        amxProfilers[amx]->Run();

        return 1;
    }

    // native Profiler_Stop();
    cell AMX_NATIVE_CALL Profiler_Stop(AMX *amx, cell *params) {
        AMXProfiler *prof = amxProfilers[amx];

        if (!prof->IsRunning()) {
            return 0;
        }

        prof->Terminate();
        return 1;
    }

    // native Profiler_PrintStats(const filename[]);
    cell AMX_NATIVE_CALL Profiler_PrintStats(AMX *amx, cell *params) {
        AMXProfiler *prof = amxProfilers[amx];

        if (prof->IsRunning()) {
            return 0;
        }

        char *filename;
        amx_StrParam(amx, params[1], filename);

        std::ofstream stream(filename);
        if (!stream.is_open()) {
            return 0;
        }

        AMX_DBG amxdbg = ::amxDebugInfo[amx];

        std::vector<AMXProfilerStat> v = prof->GetStats();
        std::sort(v.begin(), v.end(), ByExecutionTime);

        // Table header
        stream << "
\n"
               << "\t
\n"
               << "\t\tFunction	\n"
               << "\t\tNo. calls	\n"
               << "\t\tTime per call	\n"
               << "\t\tOverall time, %	\n"
               << "\t
\n";

        // Calculate overall execution time
        uint64_t totalTime = 0;
        for (std::vector<AMXProfilerStat>::iterator it = v.begin(); it != v.end(); ++it) {
            totalTime += it->executionTime;
        }

        for (std::vector<AMXProfilerStat>::iterator it = v.begin(); it != v.end(); ++it)
        {
            if (it->numberOfCalls <= 0) {
                continue;
            }

            stream << "\t
\n";

            if (it->native) {
                stream << "\t\t" << FindNativeByIndex(amx, it->address) << "	\n";
            } else {
                const char *name;
                if (dbg_LookupFunction(&amxdbg, it->address, &name) == AMX_ERR_NONE) {
                    stream << "\t\t" << name << "	\n";
                } else {
                    stream << "\t\t" << std::hex << it->address << std::dec << "	\n";
                }
            }
            stream << "\t\t" << it->numberOfCalls << "	\n"
                   << "\t\t" << std::fixed << std::setprecision(0)
                                 << it->executionTime * 1.0 / it->numberOfCalls << "	\n"
                   << "\t\t" << std::setprecision(4)
                                 << it->executionTime * 100.0 / totalTime << "	\n";
            stream << "\t
\n";
        }
        stream << "
\n";

        return 1;
    }

    const AMX_NATIVE_INFO all[] = {
        {"Profiler_Start", Profiler_Start},
        {"Profiler_Stop",  Profiler_Stop},
        {"Profiler_Print", Profiler_PrintStats},
        {0,                0}
    };
}

PLUGIN_EXPORT unsigned int PLUGIN_CALL Supports() {
    return SUPPORTS_VERSION | SUPPORTS_AMX_NATIVES;
}

PLUGIN_EXPORT bool PLUGIN_CALL Load(void **pluginData) {
    pAMXFunctions = pluginData[PLUGIN_DATA_AMX_EXPORTS];
    logprintf = (logprintf_t)pluginData[PLUGIN_DATA_LOGPRINTF];

    logprintf("  Profiler plugin " VERSION " (built on " __DATE__ ")");

    return true;
}

PLUGIN_EXPORT void PLUGIN_CALL Unload() {
    return;
}

PLUGIN_EXPORT int PLUGIN_CALL AmxLoad(AMX *amx) {
    amxProfilers[amx] = new AMXProfiler(amx);
    return amx_Register(amx, natives::all, -1);
}

PLUGIN_EXPORT int PLUGIN_CALL AmxUnload(AMX *amx) {
    delete amxProfilers[amx];
    return AMX_ERR_NONE;
}
<commit_msg>git is crazy<commit_after>// Copyright (c) 2011 Zeex
//
// 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 <algorithm>
#include <cstdio>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <numeric>
#include <map>

#include <malloc.h> // _alloca
#include <stdlib.h>

#include "amxprofiler.h"
#include "logprintf.h"
#include "plugin.h"
#include "version.h"

extern void *pAMXFunctions;

static std::map<AMX*, AMX_DBG> amxDebugInfo;
static std::map<AMX*, AMXProfiler*> amxProfilers;

static bool ByExecutionTime(const AMXProfilerStat &op1, const AMXProfilerStat &op2) {
    return op1.executionTime > op2.executionTime;
}

static bool ByNumberOfCalls(const AMXProfilerStat &op1, const AMXProfilerStat &op2) {
    return op1.numberOfCalls > op2.numberOfCalls;
}

static const char *FindNativeByIndex(AMX *amx, cell index) {
    AMX_HEADER *hdr = reinterpret_cast<AMX_HEADER*>(amx->base);

    AMX_FUNCSTUBNT *natives = reinterpret_cast<AMX_FUNCSTUBNT*>(amx->base + hdr->natives);
    int numNatives = (hdr->libraries - hdr->natives) / hdr->defsize;

    for (int i = 0; i < numNatives; i++) {
        if (i == index) {
            return reinterpret_cast<char*>(amx->base + natives[i].nameofs);
        }
    }

    return 0;
}

namespace natives {

    // native Profiler_Start(const path_to_amx[]);
    cell AMX_NATIVE_CALL Profiler_Start(AMX *amx, cell *params) {
        char *path_to_amx;
        amx_StrParam(amx, params[1], path_to_amx);

        FILE *fp = fopen(path_to_amx, "rb");
        if (fp == 0) {
            return 0;
        }

        AMX_DBG amxdbg;
        if (dbg_LoadInfo(&amxdbg, fp) != AMX_ERR_NONE) {
            fclose(fp);
            return 0;
        }

        amxDebugInfo[amx] = amxdbg;
        fclose(fp);

        amxProfilers[amx]->Run();

        return 1;
    }

    // native Profiler_Stop();
    cell AMX_NATIVE_CALL Profiler_Stop(AMX *amx, cell *params) {
        AMXProfiler *prof = amxProfilers[amx];

        if (!prof->IsRunning()) {
            return 0;
        }

        prof->Terminate();
        return 1;
    }

    // native Profiler_PrintStats(const filename[]);
    cell AMX_NATIVE_CALL Profiler_PrintStats(AMX *amx, cell *params) {
        AMXProfiler *prof = amxProfilers[amx];

        if (prof->IsRunning()) {
            return 0;
        }

        char *filename;
        amx_StrParam(amx, params[1], filename);

        std::ofstream stream(filename);
        if (!stream.is_open()) {
            return 0;
        }

        std::vector<AMXProfilerStat> v = prof->GetStats();
        std::sort(v.begin(), v.end(), ByExecutionTime);

        // Table header
        stream << "
\n"
               << "\t
\n"
               << "\t\tFunction	\n"
               << "\t\tNo. calls	\n"
               << "\t\tTime per call	\n"
               << "\t\tOverall time, %	\n"
               << "\t
\n";

        // Calculate overall execution time
        uint64_t totalTime = 0;
        for (std::vector<AMXProfilerStat>::iterator it = v.begin(); it != v.end(); ++it) {
            totalTime += it->executionTime;
        }

        for (std::vector<AMXProfilerStat>::iterator it = v.begin(); it != v.end(); ++it)
        {
            if (it->numberOfCalls <= 0) {
                continue;
            }

            stream << "\t
\n";

            if (it->native) {
                stream << "\t\t" << FindNativeByIndex(amx, it->address) << "	\n";
            } else {
                AMX_DBG amxdbg = ::amxDebugInfo[amx];
                const char *name;
                if (dbg_LookupFunction(&amxdbg, it->address, &name) == AMX_ERR_NONE) {
                    stream << "\t\t" << name << "	\n";
                } else {
                    stream << "\t\t" << std::hex << it->address << std::dec << "	\n";
                }
            }
            stream << "\t\t" << it->numberOfCalls << "	\n"
                   << "\t\t" << std::fixed << std::setprecision(0)
                                 << it->executionTime * 1.0 / it->numberOfCalls << "	\n"
                   << "\t\t" << std::setprecision(4)
                                 << it->executionTime * 100.0 / totalTime << "	\n";
            stream << "\t
\n";
        }
        stream << "
\n";

        return 1;
    }

    const AMX_NATIVE_INFO all[] = {
        {"Profiler_Start", Profiler_Start},
        {"Profiler_Stop",  Profiler_Stop},
        {"Profiler_Print", Profiler_PrintStats},
        {0,                0}
    };
}

PLUGIN_EXPORT unsigned int PLUGIN_CALL Supports() {
    return SUPPORTS_VERSION | SUPPORTS_AMX_NATIVES;
}

PLUGIN_EXPORT bool PLUGIN_CALL Load(void **pluginData) {
    pAMXFunctions = pluginData[PLUGIN_DATA_AMX_EXPORTS];
    logprintf = (logprintf_t)pluginData[PLUGIN_DATA_LOGPRINTF];

    logprintf("  Profiler plugin " VERSION " (built on " __DATE__ ")");

    return true;
}

PLUGIN_EXPORT void PLUGIN_CALL Unload() {
    return;
}

PLUGIN_EXPORT int PLUGIN_CALL AmxLoad(AMX *amx) {
    amxProfilers[amx] = new AMXProfiler(amx);
    return amx_Register(amx, natives::all, -1);
}

PLUGIN_EXPORT int PLUGIN_CALL AmxUnload(AMX *amx) {
    delete amxProfilers[amx];
    return AMX_ERR_NONE;
}
<|endoftext|>


<commit_before>/*===================================================
// Skeleton Project for CS-150: 3D Computer Graphics
//===================================================*/

#include <glad/glad.h> // must be included first
#include <GLFW/glfw3.h>
#include <iostream>
#include <array>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

using namespace std;

struct Vertex {
    glm::vec3 position;
    glm::vec3 color;
};

const int NUM_TRIANGLES = 1;

const array<Vertex, 3*NUM_TRIANGLES> triangles =
{{
    { glm::vec3(-0.5f, -0.289, 0.0f), glm::vec3(1.0f, 0.0f, 0.0f) },
    { glm::vec3(0.5f, -0.289f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f) },
    { glm::vec3(0.0f,  0.577f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }
}};

const GLchar* vertexShaderSource = R"glsl(
#version 330
uniform mat4 MVP;
in vec3 vCol;
in vec3 vPos;
out vec3 color;

void main()
{
    gl_Position = MVP * vec4(vPos, 1.0);
    color = vCol;
}
)glsl";

const GLchar* fragmentShaderSource = R"glsl(
#version 330
in vec3 color;
out vec4 fragColor;

void main()
{
    fragColor = vec4(color, 1.0);
}
)glsl";

void errorCallback(int error, const char* description)
{
    cerr << "GLFW Error: " << description << endl;
}

void keyCallback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
    if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
        glfwSetWindowShouldClose(window, GLFW_TRUE);
}

void compileShader(GLuint shader, const char *shaderText)
{
    glShaderSource(shader, 1, &shaderText, NULL);
    glCompileShader(shader);
    GLchar infoLog[8192];
    GLint success = 0;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
    if (!success)
    {
        glGetShaderInfoLog(shader, 8192, NULL, infoLog);
        cerr << "Shader failed to complile." << endl
             << "Error log: " << infoLog << endl;
    }
}

int main(void)
{
    GLFWwindow* window;

    glfwSetErrorCallback(errorCallback);

    if (!glfwInit())
        exit(EXIT_FAILURE);

    // Request OpenGL version 3.3.
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); // Don't use old OpenGL
    glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GLFW_TRUE); // OSX needs this

    window = glfwCreateWindow(640, 480, "CS 150 Template Project", NULL, NULL);
    if (!window)
    {
        glfwTerminate();
        exit(EXIT_FAILURE);
    }

    glfwSetKeyCallback(window, keyCallback);

    glfwMakeContextCurrent(window);
    gladLoadGLLoader((GLADloadproc) glfwGetProcAddress);
    cout << "GL version: " << glGetString(GL_VERSION) << endl
         << "GL vendor: " << glGetString(GL_VENDOR) << endl
         << "GL renderer: " << glGetString(GL_RENDERER) << endl
         << "GL shading language version: "
         << glGetString(GL_SHADING_LANGUAGE_VERSION) << endl;

    glfwSwapInterval(1); // Framerate matches monitor refresh rate

    // Compile shaders and check for errors
    GLuint vertexShader, fragmentShader, program;
    vertexShader = glCreateShader(GL_VERTEX_SHADER);
    fragmentShader = glCreateShader(GL_FRAGMENT_SHADER);
    compileShader(vertexShader, vertexShaderSource);
    compileShader(fragmentShader, fragmentShaderSource);
    program = glCreateProgram();
    glAttachShader(program, vertexShader);
    glAttachShader(program, fragmentShader);
    glLinkProgram(program);
    GLint success = 0;
    glGetProgramiv(program, GL_LINK_STATUS, &success);
    if (!success)
    {
        cerr << "ERROR: Shader linking failed." << endl;
        glDeleteProgram(program);
        program = 0u;
    }
    glDeleteShader(vertexShader);
    glDeleteShader(fragmentShader);

    GLint l_MVP = glGetUniformLocation(program, "MVP");
    GLint l_vPos = glGetAttribLocation(program, "vPos");
    GLint l_vCol = glGetAttribLocation(program, "vCol");

    // Send data to OpenGL context
    GLuint VAO, VBO; // vertex array object, vertex buffer object
    glGenVertexArrays(1, &VAO);
    glBindVertexArray(VAO);
    glGenBuffers(1, &VBO);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, triangles.size() * sizeof(Vertex),
                 triangles.data(), GL_STATIC_DRAW);
    glEnableVertexAttribArray(l_vPos);
    glVertexAttribPointer(l_vPos, 3, GL_FLOAT, GL_FALSE,
                          sizeof(Vertex), (GLvoid*) 0);
    glEnableVertexAttribArray(l_vCol);
    glVertexAttribPointer(l_vCol, 3, GL_FLOAT, GL_FALSE,
                          sizeof(Vertex), (GLvoid*) (sizeof(glm::vec3)));

    glUseProgram(program);
    glEnable(GL_DEPTH_TEST);
    glm::mat4 M, V, P, MVP;
    glm::vec3 eye = glm::vec3(0.0, 0.0, 5.0);
    glm::vec3 center = glm::vec3(0.0, 0.0, 0.0);
    glm::vec3 up = glm::vec3(0.0, 1.0, 0.0);
    V = glm::lookAt(eye, center, up);

    while (!glfwWindowShouldClose(window))
    {
        float ratio;
        int width, height;

        glfwGetFramebufferSize(window, &width, &height);
        glViewport(0, 0, width, height);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        ratio = width / (float) height;
        P = glm::perspective(0.50f, ratio, 1.0f, 100.0f);
        M = glm::rotate(glm::mat4(1.0f),
                        (float) glfwGetTime(), glm::vec3(0.0f, 0.0f, 1.0f));
        MVP = P * V * M;

        glUniformMatrix4fv(l_MVP, 1, GL_FALSE, glm::value_ptr(MVP));
        glDrawArrays(GL_TRIANGLES, 0, 3*NUM_TRIANGLES);

        // check for OpenGL errors
        GLenum error_code;
        while ((error_code = glGetError()) != GL_NO_ERROR)
            cerr << "OpenGL error HEX: " << hex << error_code << endl;

        glfwSwapBuffers(window);
        glfwPollEvents();
    }

    glfwDestroyWindow(window);
    glfwTerminate();
    exit(EXIT_SUCCESS);
}
<commit_msg>moved glUseProgram call<commit_after>/*===================================================
// Skeleton Project for CS-150: 3D Computer Graphics
//===================================================*/

#include <glad/glad.h> // must be included first
#include <GLFW/glfw3.h>
#include <iostream>
#include <array>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

using namespace std;

struct Vertex {
    glm::vec3 position;
    glm::vec3 color;
};

const int NUM_TRIANGLES = 1;

const array<Vertex, 3*NUM_TRIANGLES> triangles =
{{
    { glm::vec3(-0.5f, -0.289, 0.0f), glm::vec3(1.0f, 0.0f, 0.0f) },
    { glm::vec3(0.5f, -0.289f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f) },
    { glm::vec3(0.0f,  0.577f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }
}};

const GLchar* vertexShaderSource = R"glsl(
#version 330
uniform mat4 MVP;
in vec3 vCol;
in vec3 vPos;
out vec3 color;

void main()
{
    gl_Position = MVP * vec4(vPos, 1.0);
    color = vCol;
}
)glsl";

const GLchar* fragmentShaderSource = R"glsl(
#version 330
in vec3 color;
out vec4 fragColor;

void main()
{
    fragColor = vec4(color, 1.0);
}
)glsl";

void errorCallback(int error, const char* description)
{
    cerr << "GLFW Error: " << description << endl;
}

void keyCallback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
    if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
        glfwSetWindowShouldClose(window, GLFW_TRUE);
}

void compileShader(GLuint shader, const char *shaderText)
{
    glShaderSource(shader, 1, &shaderText, NULL);
    glCompileShader(shader);
    GLchar infoLog[8192];
    GLint success = 0;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
    if (!success)
    {
        glGetShaderInfoLog(shader, 8192, NULL, infoLog);
        cerr << "Shader failed to complile." << endl
             << "Error log: " << infoLog << endl;
    }
}

int main(void)
{
    GLFWwindow* window;

    glfwSetErrorCallback(errorCallback);

    if (!glfwInit())
        exit(EXIT_FAILURE);

    // Request OpenGL version 3.3.
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); // Don't use old OpenGL
    glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GLFW_TRUE); // OSX needs this

    window = glfwCreateWindow(640, 480, "CS 150 Template Project", NULL, NULL);
    if (!window)
    {
        glfwTerminate();
        exit(EXIT_FAILURE);
    }

    glfwSetKeyCallback(window, keyCallback);

    glfwMakeContextCurrent(window);
    gladLoadGLLoader((GLADloadproc) glfwGetProcAddress);
    cout << "GL version: " << glGetString(GL_VERSION) << endl
         << "GL vendor: " << glGetString(GL_VENDOR) << endl
         << "GL renderer: " << glGetString(GL_RENDERER) << endl
         << "GL shading language version: "
         << glGetString(GL_SHADING_LANGUAGE_VERSION) << endl;

    glfwSwapInterval(1); // Framerate matches monitor refresh rate

    // Compile shaders and check for errors
    GLuint vertexShader, fragmentShader, program;
    vertexShader = glCreateShader(GL_VERTEX_SHADER);
    fragmentShader = glCreateShader(GL_FRAGMENT_SHADER);
    compileShader(vertexShader, vertexShaderSource);
    compileShader(fragmentShader, fragmentShaderSource);
    program = glCreateProgram();
    glAttachShader(program, vertexShader);
    glAttachShader(program, fragmentShader);
    glLinkProgram(program);
    GLint success = 0;
    glGetProgramiv(program, GL_LINK_STATUS, &success);
    if (!success)
    {
        cerr << "ERROR: Shader linking failed." << endl;
        glDeleteProgram(program);
        program = 0u;
    }
    glDeleteShader(vertexShader);
    glDeleteShader(fragmentShader);
    glUseProgram(program);

    GLint l_MVP = glGetUniformLocation(program, "MVP");
    GLint l_vPos = glGetAttribLocation(program, "vPos");
    GLint l_vCol = glGetAttribLocation(program, "vCol");

    // Send data to OpenGL context
    GLuint VAO, VBO; // vertex array object, vertex buffer object
    glGenVertexArrays(1, &VAO);
    glBindVertexArray(VAO);
    glGenBuffers(1, &VBO);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, triangles.size() * sizeof(Vertex),
                 triangles.data(), GL_STATIC_DRAW);
    glEnableVertexAttribArray(l_vPos);
    glVertexAttribPointer(l_vPos, 3, GL_FLOAT, GL_FALSE,
                          sizeof(Vertex), (GLvoid*) 0);
    glEnableVertexAttribArray(l_vCol);
    glVertexAttribPointer(l_vCol, 3, GL_FLOAT, GL_FALSE,
                          sizeof(Vertex), (GLvoid*) (sizeof(glm::vec3)));

    glEnable(GL_DEPTH_TEST);
    glm::mat4 M, V, P, MVP;
    glm::vec3 eye = glm::vec3(0.0, 0.0, 5.0);
    glm::vec3 center = glm::vec3(0.0, 0.0, 0.0);
    glm::vec3 up = glm::vec3(0.0, 1.0, 0.0);
    V = glm::lookAt(eye, center, up);

    while (!glfwWindowShouldClose(window))
    {
        float ratio;
        int width, height;

        glfwGetFramebufferSize(window, &width, &height);
        glViewport(0, 0, width, height);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        ratio = width / (float) height;
        P = glm::perspective(0.50f, ratio, 1.0f, 100.0f);
        M = glm::rotate(glm::mat4(1.0f),
                        (float) glfwGetTime(), glm::vec3(0.0f, 0.0f, 1.0f));
        MVP = P * V * M;

        glUniformMatrix4fv(l_MVP, 1, GL_FALSE, glm::value_ptr(MVP));
        glDrawArrays(GL_TRIANGLES, 0, 3*NUM_TRIANGLES);

        // check for OpenGL errors
        GLenum error_code;
        while ((error_code = glGetError()) != GL_NO_ERROR)
            cerr << "OpenGL error HEX: " << hex << error_code << endl;

        glfwSwapBuffers(window);
        glfwPollEvents();
    }

    glfwDestroyWindow(window);
    glfwTerminate();
    exit(EXIT_SUCCESS);
}
<|endoftext|>


<commit_before>/*
 * Reference: http://plms.oxfordjournals.org/content/s2-42/1/230.full.pdf+html
 *
 * Instruction set
 * ===============
 * m-config   symbol   operations   final m-config
 * string     char     op1,op2,...  string
 *
 * op:
 *   L - move left one step
 *   R - move right one step
 *   P<symbol> - print symbol on tap
 *   E - erase symbol from tap
 * symbol:
 *   ~ - matches blank cell
 *   * - matches any symbol
 *   other char matches themselves
 *
 * Lines starting with # are ignored by the interpreter
 */

#include <iostream>
#include <string>
#include <regex>
#include <map>
#include <vector>
#include <queue>

using namespace std;

struct Configuration {
    string state;
    char symbol;
    bool operator<(const Configuration &o) const {
        return state < o.state || (state == o.state && symbol < o.symbol);
    }
};

struct Action {
    string ops;
    string state;
};

struct Machine {
    int tp = 0;
    string curr;
    map< Configuration, Action > program;
    map< int, char > tape;

    void load_program() {
        cin.sync_with_stdio(false);
        string line;
        while (getline(cin, line)) {
            // skip empty lines
            if (line.length() == 0) {
                continue;
            }

            // lines begining with # are comments
            if (line.at(0) == '#') {
                continue;
            }

            istringstream iss(line);
            string state, symbol, ops, fstate;
            iss >> state >> symbol >> ops >> fstate;

            if (curr.length() == 0) {
                curr = state;
            }

            Configuration c = {state, symbol.at(0)};
            Action a = {ops, fstate};
            program[c] = a;
        }
    }

    void print_program() {
        for (auto it = program.begin(); it != program.end(); it++) {
            cout << it->first.state << " ";
            cout << it->first.symbol << " ";
            cout << it->second.ops << " ";
            cout << it->second.state << endl;
        }
    }

    void print_tape() {
        cout << "tape: ";
        for (auto it = tape.begin(); it != tape.end(); it++) {
            cout << it->second;
        }
        cout << endl;
    }

    char read_tape() {
        if (tape.count(tp)) {
            return tape[tp];
        } else {
            return '~';
        }
    }

    int perform_ops(string ops) {
        for (int i = 0; i < ops.length(); i++) {
            char op = ops.at(i);
            switch (op) {
                case 'R':
                    tp++;
                    break;
                case 'L':
                    tp--;
                    break;
                case 'E':
                    tape[tp] = '~';
                    break;
                case 'P':
                    i++;
                    tape[tp] = ops.at(i);
                case ',':
                    break;
                default:
                    cout << "unknown op: " << op << endl;
                    exit(1);
            }
        }
        return tp;
    }

    void eval(int max) {
        int cnt = 0;
        while (cnt < max) {
            Configuration c = {curr, read_tape()};
            if (program.count(c) == 0) {
                c = {curr, '*'};
            }
            if (program.count(c) == 0) {
                break;
            }
            Action a = program[c];
            tp = perform_ops(a.ops);
            curr = a.state;

            cnt++;
            print_config(c);
            print_tape();
        }
    }

    void print_config(Configuration c) {
        cout << "conf: " << c.state << " " << c.symbol << endl;
    }
};

int main(int argc, char *argv[]) {
    Machine m;
    m.load_program();
    m.print_program();
    m.eval(1000);
    return 0;
}
<commit_msg>rename eval to run<commit_after>/*
 * Reference: http://plms.oxfordjournals.org/content/s2-42/1/230.full.pdf+html
 *
 * Instruction set
 * ===============
 * m-config   symbol   operations   final m-config
 * string     char     op1,op2,...  string
 *
 * op:
 *   L - move left one step
 *   R - move right one step
 *   P<symbol> - print symbol on tap
 *   E - erase symbol from tap
 * symbol:
 *   ~ - matches blank cell
 *   * - matches any symbol
 *   other char matches themselves
 *
 * Lines starting with # are ignored by the interpreter
 */

#include <iostream>
#include <string>
#include <regex>
#include <map>
#include <vector>
#include <queue>

using namespace std;

struct Configuration {
    string state;
    char symbol;
    bool operator<(const Configuration &o) const {
        return state < o.state || (state == o.state && symbol < o.symbol);
    }
};

struct Action {
    string ops;
    string state;
};

struct Machine {
    int tp = 0;
    string curr;
    map< Configuration, Action > program;
    map< int, char > tape;

    void load_program() {
        cin.sync_with_stdio(false);
        string line;
        while (getline(cin, line)) {
            // skip empty lines
            if (line.length() == 0) {
                continue;
            }

            // lines begining with # are comments
            if (line.at(0) == '#') {
                continue;
            }

            istringstream iss(line);
            string state, symbol, ops, fstate;
            iss >> state >> symbol >> ops >> fstate;

            if (curr.length() == 0) {
                curr = state;
            }

            Configuration c = {state, symbol.at(0)};
            Action a = {ops, fstate};
            program[c] = a;
        }
    }

    void print_program() {
        for (auto it = program.begin(); it != program.end(); it++) {
            cout << it->first.state << " ";
            cout << it->first.symbol << " ";
            cout << it->second.ops << " ";
            cout << it->second.state << endl;
        }
    }

    void print_tape() {
        cout << "tape: ";
        for (auto it = tape.begin(); it != tape.end(); it++) {
            cout << it->second;
        }
        cout << endl;
    }

    char read_tape() {
        if (tape.count(tp)) {
            return tape[tp];
        } else {
            return '~';
        }
    }

    int perform_ops(string ops) {
        for (int i = 0; i < ops.length(); i++) {
            char op = ops.at(i);
            switch (op) {
                case 'R':
                    tp++;
                    break;
                case 'L':
                    tp--;
                    break;
                case 'E':
                    tape[tp] = '~';
                    break;
                case 'P':
                    i++;
                    tape[tp] = ops.at(i);
                case ',':
                    break;
                default:
                    cout << "unknown op: " << op << endl;
                    exit(1);
            }
        }
        return tp;
    }

    void run(int max) {
        int cnt = 0;
        while (cnt < max) {
            Configuration c = {curr, read_tape()};
            if (program.count(c) == 0) {
                c = {curr, '*'};
            }
            if (program.count(c) == 0) {
                break;
            }
            Action a = program[c];
            tp = perform_ops(a.ops);
            curr = a.state;

            cnt++;
            print_config(c);
            print_tape();
        }
    }

    void print_config(Configuration c) {
        cout << "conf: " << c.state << " " << c.symbol << endl;
    }
};

int main(int argc, char *argv[]) {
    Machine m;
    m.load_program();
    m.print_program();
    m.run(1000);
    return 0;
}
<|endoftext|>


<commit_before>#include "netdrawer.h"

using namespace NEAT;
using namespace std;

/*
//void setPosition(double &x, double &y, double Min, double Max){
void setPosition(double &x, double &y){
	//randomly generate position for a node;
	double Min = 0.0, Max = 1000.0;
	double p = (double)rand() / RAND_MAX;
	x = Min + p * (Max - Min);
	x = (floor(x * 10))/10; //only keep one digit after the decimal point

	double q = (double)rand() / RAND_MAX;
	y = Min + q * (Max - Min);
	y = (floor(y * 10))/10;

	return;
}
*/

void setPositionX(double &x, int side){
	//side can only be 0, 1, 2 (representing left, right, middle)
	switch(side){
		case 0:
		x = 100.0;
		break;

		case 1:
		x = 500.0;
		break;

		case 2:  //middle
		x = 300.0;
	}

	return;
}

void printNode(NNode* node, double x, double y, std::ofstream &myfile){

	int type = node->type;
	//nodetype is 1, which is transition
	if(type){
		myfile << "<transition id=\"" << node->node_id << "\">\n";
		myfile << "<graphics>\n";
		myfile << "<position x=\"" << x << "\" y=\"" << y << "\"/>\n";
		myfile << "</graphics>\n";
		myfile << "<name>\n";
		myfile << "<value>" << node->node_id << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</name>\n";
		myfile << "<orientation>\n";
		myfile << "<value>0</value>\n";
		myfile << "</orientation>\n";
		myfile << "<rate>\n";
		myfile << "<value>1.0</value>\n";
		myfile << "</rate>\n";
		myfile << "<timed>\n";
		myfile << "<value>false</value>\n";
		myfile << "</timed>\n";
		myfile << "</transition>\n";
	}
	else{ ////nodetype is 0, which is place
		myfile << "<place id=\"" << node->node_id << "\">\n";
		myfile << "<graphics>\n";
		myfile << "<position x=\"" << x << "\" y=\"" << y << "\"/>\n";
		myfile << "</graphics>\n";
		myfile << "<name>\n";
		myfile << "<value>" << node->node_id << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</name>\n";
		myfile << "<initialMarking>\n";
		myfile << "<value>" << node->tok_count << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</initialMarking>\n";
		myfile << "</place>\n";
	}

}

void printLink(Link* nlink, std::map<NNode*, int> npos, std::vector<double> xpos, std::vector<double> ypos, std::ofstream &myfile){
	NNode* in = nlink->in_node;
	NNode* out = nlink->out_node;
	int in_ptr = npos[in];
	int out_ptr = npos[out];

	myfile << "<arc id=\"" << in->node_id << " to " << out->node_id << "\" ";
	myfile << "source=\"" << in->node_id << "\" target=\"" << out->node_id <<"\">\n";
	myfile << "<graphics/>\n";
	myfile << "<inscription>\n";
	myfile << "<value>" << (int)nlink->weight << "</value>\n";
	myfile << "<graphics/>\n";
	myfile << "</inscription>\n";
	myfile << "<arcpath id=\"000\" x=\"" << ((int)xpos[in_ptr]) + 11;
	myfile << "\" y=\"" << ((int)ypos[in_ptr]) + 5 << "\" curvePoint=\"false\"/>\n";
	myfile << "<arcpath id=\"001\" x=\"" << ((int)xpos[out_ptr]) + 11;
	myfile << "\" y=\"" << ((int)ypos[out_ptr]) + 5 << "\" curvePoint=\"false\"/>\n";
	myfile << "</arc>\n";
}

int netdrawer(const Network* network){

	//build an xml file for the network
	//initialize the xml file
	std::ofstream myfile;
	char buff[100];
 	sprintf(buff, "PetriNet_%d.xml", network->net_id);
	myfile.open(buff, std::ofstream::out);
	myfile <<"<?xml version=\"1.0\" encoding=\"iso-8859-1\"?>\n<pnml>\n<net id=\"Net-One\" type=\"P/T net\">";

	//a hashmap to record posiitons of nodes using node pointer (NNode*) and position number(int)
	std::map<NNode*, int> nodepos;
	//two vectors to record positions of the nodes
	std::vector<double> xpos;
	std::vector<double> ypos;

	//Write all the places
	std::vector<NNode*>::const_iterator curnode;
	int count = 0;
	double x, y = 50.0;
	setPositionX(x, 2); //setting positions for transitions

	//Write all the transitions
	for(curnode = network->transitions.begin(); curnode != network->transitions.end(); ++curnode){
		//generate an position
		nodepos[*curnode] = count;
		xpos.push_back(x);
		ypos.push_back(y);
		printNode(*curnode, x, y, myfile);

		y += 50;
		count++;
	}

	y = 50.0;
	//Write all the places
	for(curnode = network->places.begin(); curnode != network->places.end(); ++curnode) {
		//generate an random position for curnode;
		int side = randint(0, 1);
		setPositionX(x, side);
		nodepos[*curnode] = count;
		xpos.push_back(x);
		ypos.push_back(y);
		y += 50;

		printNode(*curnode, x, y, myfile);

		count++;
	}

	//Write all the links
	std::vector<Link*>::const_iterator curlink;
	for(curnode = network->places.begin(); curnode != network->places.end(); ++curnode) {
		for(curlink = (*curnode)->incoming.begin(); curlink != (*curnode)->incoming.end(); ++curlink){
			printLink(*curlink, nodepos, xpos, ypos, myfile);
		}
	}

	for(curnode = network->transitions.begin(); curnode != network->transitions.end(); ++curnode){
		for(curlink = (*curnode)->incoming.begin(); curlink != (*curnode)->incoming.end(); ++curlink){
			printLink(*curlink, nodepos, xpos, ypos, myfile);
		}
	}

	//closing xml file
	myfile <<"</net>\n</pnml>";
	myfile.close();
	return 0;
}

<commit_msg>add action_type in name of transition<commit_after>#include "netdrawer.h"

using namespace NEAT;
using namespace std;

/*
//void setPosition(double &x, double &y, double Min, double Max){
void setPosition(double &x, double &y){
	//randomly generate position for a node;
	double Min = 0.0, Max = 1000.0;
	double p = (double)rand() / RAND_MAX;
	x = Min + p * (Max - Min);
	x = (floor(x * 10))/10; //only keep one digit after the decimal point

	double q = (double)rand() / RAND_MAX;
	y = Min + q * (Max - Min);
	y = (floor(y * 10))/10;

	return;
}
*/

void setPositionX(double &x, int side){
	//side can only be 0, 1, 2 (representing left, right, middle)
	switch(side){
		case 0:
		x = 100.0;
		break;

		case 1:
		x = 500.0;
		break;

		case 2:  //middle
		x = 300.0;
	}

	return;
}

void printNode(NNode* node, double x, double y, std::ofstream &myfile){

	int type = node->type;
	//nodetype is 1, which is transition
	if(type){
		myfile << "<transition id=\"" << node->node_id << "\">\n";
		myfile << "<graphics>\n";
		myfile << "<position x=\"" << x << "\" y=\"" << y << "\"/>\n";
		myfile << "</graphics>\n";
		myfile << "<name>\n";
		myfile << "<value>" << node->node_id << "_";

		switch(node->action_ID){
			case 0:
			myfile << "move right";
			break;

			case 1:
			myfile << "move up";
			break;

			case 2:
			myfile << "move left";
			break;

			case 3:
			myfile << "move down";
			break;
		}

		myfile << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</name>\n";
		myfile << "<orientation>\n";
		myfile << "<value>0</value>\n";
		myfile << "</orientation>\n";
		myfile << "<rate>\n";
		myfile << "<value>1.0</value>\n";
		myfile << "</rate>\n";
		myfile << "<timed>\n";
		myfile << "<value>false</value>\n";
		myfile << "</timed>\n";
		myfile << "</transition>\n";
	}
	else{ ////nodetype is 0, which is place
		myfile << "<place id=\"" << node->node_id << "\">\n";
		myfile << "<graphics>\n";
		myfile << "<position x=\"" << x << "\" y=\"" << y << "\"/>\n";
		myfile << "</graphics>\n";
		myfile << "<name>\n";
		myfile << "<value>" << node->node_id << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</name>\n";
		myfile << "<initialMarking>\n";
		myfile << "<value>" << node->tok_count << "</value>\n";
		myfile << "<graphics/>\n";
		myfile << "</initialMarking>\n";
		myfile << "</place>\n";
	}

}

void printLink(Link* nlink, std::map<NNode*, int> npos, std::vector<double> xpos, std::vector<double> ypos, std::ofstream &myfile){
	NNode* in = nlink->in_node;
	NNode* out = nlink->out_node;
	int in_ptr = npos[in];
	int out_ptr = npos[out];

	myfile << "<arc id=\"" << in->node_id << " to " << out->node_id << "\" ";
	myfile << "source=\"" << in->node_id << "\" target=\"" << out->node_id <<"\">\n";
	myfile << "<graphics/>\n";
	myfile << "<inscription>\n";
	myfile << "<value>" << (int)nlink->weight << "</value>\n";
	myfile << "<graphics/>\n";
	myfile << "</inscription>\n";
	myfile << "<arcpath id=\"000\" x=\"" << ((int)xpos[in_ptr]) + 11;
	myfile << "\" y=\"" << ((int)ypos[in_ptr]) + 5 << "\" curvePoint=\"false\"/>\n";
	myfile << "<arcpath id=\"001\" x=\"" << ((int)xpos[out_ptr]) + 11;
	myfile << "\" y=\"" << ((int)ypos[out_ptr]) + 5 << "\" curvePoint=\"false\"/>\n";
	myfile << "</arc>\n";
}

int netdrawer(const Network* network){

	//build an xml file for the network
	//initialize the xml file
	std::ofstream myfile;
	char buff[100];
 	sprintf(buff, "PetriNet_%d.xml", network->net_id);
	myfile.open(buff, std::ofstream::out);
	myfile <<"<?xml version=\"1.0\" encoding=\"iso-8859-1\"?>\n<pnml>\n<net id=\"Net-One\" type=\"P/T net\">";

	//a hashmap to record posiitons of nodes using node pointer (NNode*) and position number(int)
	std::map<NNode*, int> nodepos;
	//two vectors to record positions of the nodes
	std::vector<double> xpos;
	std::vector<double> ypos;

	//Write all the places
	std::vector<NNode*>::const_iterator curnode;
	int count = 0;
	double x, y = 50.0;
	setPositionX(x, 2); //setting positions for transitions

	//Write all the transitions
	for(curnode = network->transitions.begin(); curnode != network->transitions.end(); ++curnode){
		//generate an position
		nodepos[*curnode] = count;
		xpos.push_back(x);
		ypos.push_back(y);
		printNode(*curnode, x, y, myfile);

		y += 50;
		count++;
	}

	y = 50.0;
	//Write all the places
	for(curnode = network->places.begin(); curnode != network->places.end(); ++curnode) {
		//generate an random position for curnode;
		int side = randint(0, 1);
		setPositionX(x, side);
		nodepos[*curnode] = count;
		xpos.push_back(x);
		ypos.push_back(y);
		y += 50;

		printNode(*curnode, x, y, myfile);

		count++;
	}

	//Write all the links
	std::vector<Link*>::const_iterator curlink;
	for(curnode = network->places.begin(); curnode != network->places.end(); ++curnode) {
		for(curlink = (*curnode)->incoming.begin(); curlink != (*curnode)->incoming.end(); ++curlink){
			printLink(*curlink, nodepos, xpos, ypos, myfile);
		}
	}

	for(curnode = network->transitions.begin(); curnode != network->transitions.end(); ++curnode){
		for(curlink = (*curnode)->incoming.begin(); curlink != (*curnode)->incoming.end(); ++curlink){
			printLink(*curlink, nodepos, xpos, ypos, myfile);
		}
	}

	//closing xml file
	myfile <<"</net>\n</pnml>";
	myfile.close();
	return 0;
}

<|endoftext|>


<commit_before>#include "platform_node.h"

PlatformNode::PlatformNode(void) {
    for (uint8_t i = 0; i < PLATFORM_TOTAL_CHANNEL_COUNT; i++) {
        _channels[i] = PLATFORM_DEFAULT_CHANNEL_VALUE;
    }
}

/**
 * Return true if new bind key was generated
 */
void PlatformNode::seed(void) {
    uint8_t val;

    val = EEPROM.read(EEPROM_ADDRESS_BIND_KEY_SEEDED);

    if (val != 0xf1) {
        EEPROM.write(EEPROM_ADDRESS_BIND_0, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_1, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_2, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_3, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_KEY_SEEDED, 0xf1);
    }
}

void PlatformNode::loadBindKey(uint8_t key[]) {
    key[0] = EEPROM.read(EEPROM_ADDRESS_BIND_0);
    key[1] = EEPROM.read(EEPROM_ADDRESS_BIND_1);
    key[2] = EEPROM.read(EEPROM_ADDRESS_BIND_2);
    key[3] = EEPROM.read(EEPROM_ADDRESS_BIND_3);
}

void PlatformNode::saveBindKey(uint8_t key[]) {
    EEPROM.write(EEPROM_ADDRESS_BIND_0, key[0]);
    EEPROM.write(EEPROM_ADDRESS_BIND_1, key[1]);
    EEPROM.write(EEPROM_ADDRESS_BIND_2, key[2]);
    EEPROM.write(EEPROM_ADDRESS_BIND_3, key[3]);
    EEPROM.write(EEPROM_ADDRESS_BIND_KEY_SEEDED, 0xf1);
}

int PlatformNode::getRcChannel(uint8_t channel) {
    if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
        return _channels[channel];
    } else {
        return PLATFORM_DEFAULT_CHANNEL_VALUE;
    }
}

void PlatformNode::setRcChannel(uint8_t channel, int value, int offset) {
    if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
        _channels[channel] = value + offset;
    }
}

void PlatformNode::enterBindMode(void) {
    isBindMode = true;

    // Set temporary bind mode
    bindKey[0] = 0xf1;
    bindKey[1] = 0x1e;
    bindKey[2] = 0x07;
    bindKey[3] = 0x42;

    radioNode.set(
        0, // Minimum power
        125000, // 125kHz bandwidth
        6, // low spreading factor, we do not need high RX sensitivity
        5, // same for coding rate
        868000000 //Fixed frequency while binding
    );
    bindModeExitMillis = millis() + 1000; //This happens only on RX
}

void PlatformNode::leaveBindMode(void) {
    isBindMode = false;
    loadBindKey(bindKey);
    radioNode.reset();
}<commit_msg>Update platform_node.cpp<commit_after>#include "platform_node.h"

PlatformNode::PlatformNode(void) {
    for (uint8_t i = 0; i < PLATFORM_TOTAL_CHANNEL_COUNT; i++) {
        _channels[i] = PLATFORM_DEFAULT_CHANNEL_VALUE;
    }
}

/**
 * Return true if new bind key was generated
 */
void PlatformNode::seed(void) {
    uint8_t val;

    val = EEPROM.read(EEPROM_ADDRESS_BIND_KEY_SEEDED);

    if (val != 0xf1) {
        EEPROM.write(EEPROM_ADDRESS_BIND_0, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_1, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_2, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_3, random(1, 255)); //Yes, from 1 to 254
        EEPROM.write(EEPROM_ADDRESS_BIND_KEY_SEEDED, 0xf1);
        #ifdef ARDUINO_SAMD_FEATHER_M0
        EEPROM.commit();
        #endif
    }
}

void PlatformNode::loadBindKey(uint8_t key[]) {
    key[0] = EEPROM.read(EEPROM_ADDRESS_BIND_0);
    key[1] = EEPROM.read(EEPROM_ADDRESS_BIND_1);
    key[2] = EEPROM.read(EEPROM_ADDRESS_BIND_2);
    key[3] = EEPROM.read(EEPROM_ADDRESS_BIND_3);
}

void PlatformNode::saveBindKey(uint8_t key[]) {
    EEPROM.write(EEPROM_ADDRESS_BIND_0, key[0]);
    EEPROM.write(EEPROM_ADDRESS_BIND_1, key[1]);
    EEPROM.write(EEPROM_ADDRESS_BIND_2, key[2]);
    EEPROM.write(EEPROM_ADDRESS_BIND_3, key[3]);
    EEPROM.write(EEPROM_ADDRESS_BIND_KEY_SEEDED, 0xf1);
    #ifdef ARDUINO_SAMD_FEATHER_M0
    EEPROM.commit();
    #endif
}

int PlatformNode::getRcChannel(uint8_t channel) {
    if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
        return _channels[channel];
    } else {
        return PLATFORM_DEFAULT_CHANNEL_VALUE;
    }
}

void PlatformNode::setRcChannel(uint8_t channel, int value, int offset) {
    if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
        _channels[channel] = value + offset;
    }
}

void PlatformNode::enterBindMode(void) {
    isBindMode = true;

    // Set temporary bind mode
    bindKey[0] = 0xf1;
    bindKey[1] = 0x1e;
    bindKey[2] = 0x07;
    bindKey[3] = 0x42;

    radioNode.set(
        0, // Minimum power
        125000, // 125kHz bandwidth
        6, // low spreading factor, we do not need high RX sensitivity
        5, // same for coding rate
        868000000 //Fixed frequency while binding
    );
    bindModeExitMillis = millis() + 1000; //This happens only on RX
}

void PlatformNode::leaveBindMode(void) {
    isBindMode = false;
    loadBindKey(bindKey);
    radioNode.reset();
}
<|endoftext|>


<commit_before>/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Dan Solomon
 *
 * 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 <console_bridge/console.h>

#include "descartes_moveit/moveit_state_adapter.h"
#include "descartes_core/pretty_print.hpp"
#include "descartes_moveit/seed_search.h"

#include <eigen_conversions/eigen_msg.h>
#include <random_numbers/random_numbers.h>
#include <ros/assert.h>
#include <sstream>

const static int SAMPLE_ITERATIONS = 10;

namespace
{
bool getJointVelocityLimits(const moveit::core::RobotState& state, const std::string& group_name,
                            std::vector<double>& output)
{
  std::vector<double> result;

  auto models = state.getJointModelGroup(group_name)->getActiveJointModels();
  for (const moveit::core::JointModel* model : models)
  {
    const auto& bounds = model->getVariableBounds();
    // Check to see if there is a single bounds constraint (more might indicate
    // not revolute joint)
    if (model->getType() != moveit::core::JointModel::REVOLUTE &&
        model->getType() != moveit::core::JointModel::PRISMATIC)
    {
      ROS_ERROR_STREAM(__FUNCTION__ << " Unexpected joint type. Currently works only"
                                       " with single axis prismatic or revolute joints.");
      return false;
    }
    else
    {
      result.push_back(bounds[0].max_velocity_);
    }
  }

  output = result;
  return true;
}

}  // end anon namespace

namespace descartes_moveit
{
MoveitStateAdapter::MoveitStateAdapter() : world_to_root_(Eigen::Affine3d::Identity())
{
}

bool MoveitStateAdapter::initialize(const std::string& robot_description, const std::string& group_name,
                                    const std::string& world_frame, const std::string& tcp_frame)
{
  // Initialize MoveIt state objects
  robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
  auto model = robot_model_loader_->getModel();
  if (!model)
  {
    logError("Failed to load robot model from robot description parameter: %s", robot_description.c_str());
    return false;
  }

  return initialize(model, group_name, world_frame, tcp_frame);
}

bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
                                    const std::string &world_frame, const std::string &tcp_frame)
{
  robot_model_ptr_ = robot_model;
  robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
  robot_state_->setToDefaultValues();
  planning_scene_.reset(new planning_scene::PlanningScene(robot_model));
  joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);

  // Assign robot frames
  group_name_ = group_name;
  tool_frame_ = tcp_frame;
  world_frame_ = world_frame;

  // Validate our model inputs w/ URDF
  if (!joint_group_)
  {
    logError("%s: Joint group '%s' does not exist in robot model", __FUNCTION__, group_name_.c_str());
    std::stringstream msg;
    msg << "Possible group names: " << robot_state_->getRobotModel()->getJointModelGroupNames();
    logError(msg.str().c_str());
    return false;
  }

  const auto& link_names = joint_group_->getLinkModelNames();
  if (tool_frame_ != link_names.back())
  {
    logError("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
             "will be implemented in the future",
             __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
    return false;
  }

  if (!::getJointVelocityLimits(*robot_state_, group_name, velocity_limits_))
  {
    logWarn("%s: Could not determine velocity limits of RobotModel from MoveIt", __FUNCTION__);
  }

  if (seed_states_.empty())
  {
    seed_states_ = seed::findRandomSeeds(*robot_state_, group_name_, SAMPLE_ITERATIONS);
    logDebug("Generated %lu random seeds", static_cast<unsigned long>(seed_states_.size()));
  }

  auto model_frame = robot_state_->getRobotModel()->getModelFrame();
  if (world_frame_ != model_frame)
  {
    logInform("%s: World frame '%s' does not match model root frame '%s', all poses will be"
              " transformed to world frame '%s'",
              __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());

    Eigen::Affine3d root_to_world = robot_state_->getFrameTransform(world_frame_);
    world_to_root_ = descartes_core::Frame(root_to_world.inverse());
  }

  return true;
}

bool MoveitStateAdapter::getIK(const Eigen::Affine3d& pose, const std::vector<double>& seed_state,
                               std::vector<double>& joint_pose) const
{
  robot_state_->setJointGroupPositions(group_name_, seed_state);
  return getIK(pose, joint_pose);
}

bool MoveitStateAdapter::getIK(const Eigen::Affine3d& pose, std::vector<double>& joint_pose) const
{
  bool rtn = false;

  // transform to group base
  Eigen::Affine3d tool_pose = world_to_root_.frame * pose;

  if (robot_state_->setFromIK(joint_group_, tool_pose, tool_frame_))
  {
    robot_state_->copyJointGroupPositions(group_name_, joint_pose);
    if (!isValid(joint_pose))
    {
      ROS_DEBUG_STREAM("Robot joint pose is invalid");
    }
    else
    {
      rtn = true;
    }
  }
  else
  {
    rtn = false;
  }

  return rtn;
}

bool MoveitStateAdapter::getAllIK(const Eigen::Affine3d& pose, std::vector<std::vector<double> >& joint_poses) const
{
  // The minimum difference between solutions should be greater than the search discretization
  // used by the IK solver.  This value is multiplied by 4 to remove any chance that a solution
  // in the middle of a discretization step could be double counted.  In reality, we'd like solutions
  // to be further apart than this.
  double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
  logDebug("Utilizing an min. difference of %f between IK solutions", epsilon);
  joint_poses.clear();
  for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
  {
    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
    std::vector<double> joint_pose;
    if (getIK(pose, joint_pose))
    {
      if (joint_poses.empty())
      {
        std::stringstream msg;
        msg << "Found *first* solution on " << sample_iter << " iteration, joint: " << joint_pose;
        logDebug(msg.str().c_str());
        joint_poses.push_back(joint_pose);
      }
      else
      {
        std::stringstream msg;
        msg << "Found *potential* solution on " << sample_iter << " iteration, joint: " << joint_pose;
        logDebug(msg.str().c_str());

        std::vector<std::vector<double> >::iterator joint_pose_it;
        bool match_found = false;
        for (joint_pose_it = joint_poses.begin(); joint_pose_it != joint_poses.end(); ++joint_pose_it)
        {
          if (descartes_core::utils::equal(joint_pose, (*joint_pose_it), epsilon))
          {
            logDebug("Found matching, potential solution is not new");
            match_found = true;
            break;
          }
        }
        if (!match_found)
        {
          std::stringstream msg;
          msg << "Found *new* solution on " << sample_iter << " iteration, joint: " << joint_pose;
          logDebug(msg.str().c_str());
          joint_poses.push_back(joint_pose);
        }
      }
    }
  }

  logDebug("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
           static_cast<unsigned long>(seed_states_.size()));

  if (joint_poses.empty())
  {
    logError("Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
    return false;
  }
  else
  {
    logInform("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
              static_cast<unsigned long>(seed_states_.size()));
    return true;
  }
}

bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) const
{
  bool in_collision = false;
  if (check_collisions_)
  {
    moveit::core::RobotState state (robot_model_ptr_);
    state.setToDefaultValues();
    robot_state_->setJointGroupPositions(joint_group_, joint_pose);
    in_collision = planning_scene_->isStateColliding(state, group_name_);
  }
  return in_collision;
}

bool MoveitStateAdapter::isInLimits(const std::vector<double> &joint_pose) const
{
  return joint_group_->satisfiesPositionBounds(joint_pose.data());
}

bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Affine3d& pose) const
{
  bool rtn = false;
  robot_state_->setJointGroupPositions(group_name_, joint_pose);
  if (isValid(joint_pose))
  {
    if (robot_state_->knowsFrameTransform(tool_frame_))
    {
      pose = world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_);
      rtn = true;
    }
    else
    {
      logError("Robot state does not recognize tool frame: %s", tool_frame_.c_str());
      rtn = false;
    }
  }
  else
  {
    logError("Invalid joint pose passed to get forward kinematics");
    rtn = false;
  }

  return rtn;
}

bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
{
  // Logical check on input sizes
  if (joint_group_->getActiveJointModels().size() != joint_pose.size())
  {
    logError("Size of joint pose: %lu doesn't match robot state variable size: %lu",
             static_cast<unsigned long>(joint_pose.size()),
             static_cast<unsigned long>(joint_group_->getActiveJointModels().size()));
    return false;
  }

  return isInLimits(joint_pose) && !isInCollision(joint_pose);
}

bool MoveitStateAdapter::isValid(const Eigen::Affine3d& pose) const
{
  // TODO: Could check robot extents first as a quick check
  std::vector<double> dummy;
  return getIK(pose, dummy);
}

int MoveitStateAdapter::getDOF() const
{
  return joint_group_->getVariableCount();
}

bool MoveitStateAdapter::isValidMove(const double* from_joint_pose,
                                     const double* to_joint_pose, double dt) const
{

  for (std::size_t i = 0; i < getDOF(); ++i)
  {
    double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
    double max_dtheta = dt * velocity_limits_[i];
    if (dtheta > max_dtheta)
      return false;
  }

  return true;
}

std::vector<double> MoveitStateAdapter::getJointVelocityLimits() const
{
  return velocity_limits_;
}

void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
{
  ROS_ASSERT_MSG(static_cast<bool>(robot_state_), "'robot_state_' member pointer is null. Have you called "
                                                  "initialize()?");
  *robot_state_ = state;
  planning_scene_->setCurrentState(state);
}

}  // descartes_moveit
<commit_msg>Fixed a screw up where the global robot state was being used for setting a groups position instead of the locally created one. The change was originally made for thread safety but this meant collision checking was just all kinds of wrong<commit_after>/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Dan Solomon
 *
 * 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 <console_bridge/console.h>

#include "descartes_moveit/moveit_state_adapter.h"
#include "descartes_core/pretty_print.hpp"
#include "descartes_moveit/seed_search.h"

#include <eigen_conversions/eigen_msg.h>
#include <random_numbers/random_numbers.h>
#include <ros/assert.h>
#include <sstream>

const static int SAMPLE_ITERATIONS = 10;

namespace
{
bool getJointVelocityLimits(const moveit::core::RobotState& state, const std::string& group_name,
                            std::vector<double>& output)
{
  std::vector<double> result;

  auto models = state.getJointModelGroup(group_name)->getActiveJointModels();
  for (const moveit::core::JointModel* model : models)
  {
    const auto& bounds = model->getVariableBounds();
    // Check to see if there is a single bounds constraint (more might indicate
    // not revolute joint)
    if (model->getType() != moveit::core::JointModel::REVOLUTE &&
        model->getType() != moveit::core::JointModel::PRISMATIC)
    {
      ROS_ERROR_STREAM(__FUNCTION__ << " Unexpected joint type. Currently works only"
                                       " with single axis prismatic or revolute joints.");
      return false;
    }
    else
    {
      result.push_back(bounds[0].max_velocity_);
    }
  }

  output = result;
  return true;
}

}  // end anon namespace

namespace descartes_moveit
{
MoveitStateAdapter::MoveitStateAdapter() : world_to_root_(Eigen::Affine3d::Identity())
{
}

bool MoveitStateAdapter::initialize(const std::string& robot_description, const std::string& group_name,
                                    const std::string& world_frame, const std::string& tcp_frame)
{
  // Initialize MoveIt state objects
  robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
  auto model = robot_model_loader_->getModel();
  if (!model)
  {
    logError("Failed to load robot model from robot description parameter: %s", robot_description.c_str());
    return false;
  }

  return initialize(model, group_name, world_frame, tcp_frame);
}

bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
                                    const std::string &world_frame, const std::string &tcp_frame)
{
  robot_model_ptr_ = robot_model;
  robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
  robot_state_->setToDefaultValues();
  planning_scene_.reset(new planning_scene::PlanningScene(robot_model));
  joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);

  // Assign robot frames
  group_name_ = group_name;
  tool_frame_ = tcp_frame;
  world_frame_ = world_frame;

  // Validate our model inputs w/ URDF
  if (!joint_group_)
  {
    logError("%s: Joint group '%s' does not exist in robot model", __FUNCTION__, group_name_.c_str());
    std::stringstream msg;
    msg << "Possible group names: " << robot_state_->getRobotModel()->getJointModelGroupNames();
    logError(msg.str().c_str());
    return false;
  }

  const auto& link_names = joint_group_->getLinkModelNames();
  if (tool_frame_ != link_names.back())
  {
    logError("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
             "will be implemented in the future",
             __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
    return false;
  }

  if (!::getJointVelocityLimits(*robot_state_, group_name, velocity_limits_))
  {
    logWarn("%s: Could not determine velocity limits of RobotModel from MoveIt", __FUNCTION__);
  }

  if (seed_states_.empty())
  {
    seed_states_ = seed::findRandomSeeds(*robot_state_, group_name_, SAMPLE_ITERATIONS);
    logDebug("Generated %lu random seeds", static_cast<unsigned long>(seed_states_.size()));
  }

  auto model_frame = robot_state_->getRobotModel()->getModelFrame();
  if (world_frame_ != model_frame)
  {
    logInform("%s: World frame '%s' does not match model root frame '%s', all poses will be"
              " transformed to world frame '%s'",
              __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());

    Eigen::Affine3d root_to_world = robot_state_->getFrameTransform(world_frame_);
    world_to_root_ = descartes_core::Frame(root_to_world.inverse());
  }

  return true;
}

bool MoveitStateAdapter::getIK(const Eigen::Affine3d& pose, const std::vector<double>& seed_state,
                               std::vector<double>& joint_pose) const
{
  robot_state_->setJointGroupPositions(group_name_, seed_state);
  return getIK(pose, joint_pose);
}

bool MoveitStateAdapter::getIK(const Eigen::Affine3d& pose, std::vector<double>& joint_pose) const
{
  bool rtn = false;

  // transform to group base
  Eigen::Affine3d tool_pose = world_to_root_.frame * pose;

  if (robot_state_->setFromIK(joint_group_, tool_pose, tool_frame_))
  {
    robot_state_->copyJointGroupPositions(group_name_, joint_pose);
    if (!isValid(joint_pose))
    {
      ROS_DEBUG_STREAM("Robot joint pose is invalid");
    }
    else
    {
      rtn = true;
    }
  }
  else
  {
    rtn = false;
  }

  return rtn;
}

bool MoveitStateAdapter::getAllIK(const Eigen::Affine3d& pose, std::vector<std::vector<double> >& joint_poses) const
{
  // The minimum difference between solutions should be greater than the search discretization
  // used by the IK solver.  This value is multiplied by 4 to remove any chance that a solution
  // in the middle of a discretization step could be double counted.  In reality, we'd like solutions
  // to be further apart than this.
  double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
  logDebug("Utilizing an min. difference of %f between IK solutions", epsilon);
  joint_poses.clear();
  for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
  {
    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
    std::vector<double> joint_pose;
    if (getIK(pose, joint_pose))
    {
      if (joint_poses.empty())
      {
        std::stringstream msg;
        msg << "Found *first* solution on " << sample_iter << " iteration, joint: " << joint_pose;
        logDebug(msg.str().c_str());
        joint_poses.push_back(joint_pose);
      }
      else
      {
        std::stringstream msg;
        msg << "Found *potential* solution on " << sample_iter << " iteration, joint: " << joint_pose;
        logDebug(msg.str().c_str());

        std::vector<std::vector<double> >::iterator joint_pose_it;
        bool match_found = false;
        for (joint_pose_it = joint_poses.begin(); joint_pose_it != joint_poses.end(); ++joint_pose_it)
        {
          if (descartes_core::utils::equal(joint_pose, (*joint_pose_it), epsilon))
          {
            logDebug("Found matching, potential solution is not new");
            match_found = true;
            break;
          }
        }
        if (!match_found)
        {
          std::stringstream msg;
          msg << "Found *new* solution on " << sample_iter << " iteration, joint: " << joint_pose;
          logDebug(msg.str().c_str());
          joint_poses.push_back(joint_pose);
        }
      }
    }
  }

  logDebug("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
           static_cast<unsigned long>(seed_states_.size()));

  if (joint_poses.empty())
  {
    logError("Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
    return false;
  }
  else
  {
    logInform("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
              static_cast<unsigned long>(seed_states_.size()));
    return true;
  }
}

bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) const
{
  bool in_collision = false;
  if (check_collisions_)
  {
    moveit::core::RobotState state (robot_model_ptr_);
    state.setToDefaultValues();
    state.setJointGroupPositions(joint_group_, joint_pose);
    in_collision = planning_scene_->isStateColliding(state, group_name_);
  }
  return in_collision;
}

bool MoveitStateAdapter::isInLimits(const std::vector<double> &joint_pose) const
{
  return joint_group_->satisfiesPositionBounds(joint_pose.data());
}

bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Affine3d& pose) const
{
  bool rtn = false;
  robot_state_->setJointGroupPositions(group_name_, joint_pose);
  if (isValid(joint_pose))
  {
    if (robot_state_->knowsFrameTransform(tool_frame_))
    {
      pose = world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_);
      rtn = true;
    }
    else
    {
      logError("Robot state does not recognize tool frame: %s", tool_frame_.c_str());
      rtn = false;
    }
  }
  else
  {
    logError("Invalid joint pose passed to get forward kinematics");
    rtn = false;
  }

  return rtn;
}

bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
{
  // Logical check on input sizes
  if (joint_group_->getActiveJointModels().size() != joint_pose.size())
  {
    logError("Size of joint pose: %lu doesn't match robot state variable size: %lu",
             static_cast<unsigned long>(joint_pose.size()),
             static_cast<unsigned long>(joint_group_->getActiveJointModels().size()));
    return false;
  }

  return isInLimits(joint_pose) && !isInCollision(joint_pose);
}

bool MoveitStateAdapter::isValid(const Eigen::Affine3d& pose) const
{
  // TODO: Could check robot extents first as a quick check
  std::vector<double> dummy;
  return getIK(pose, dummy);
}

int MoveitStateAdapter::getDOF() const
{
  return joint_group_->getVariableCount();
}

bool MoveitStateAdapter::isValidMove(const double* from_joint_pose,
                                     const double* to_joint_pose, double dt) const
{

  for (std::size_t i = 0; i < getDOF(); ++i)
  {
    double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
    double max_dtheta = dt * velocity_limits_[i];
    if (dtheta > max_dtheta)
      return false;
  }

  return true;
}

std::vector<double> MoveitStateAdapter::getJointVelocityLimits() const
{
  return velocity_limits_;
}

void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
{
  ROS_ASSERT_MSG(static_cast<bool>(robot_state_), "'robot_state_' member pointer is null. Have you called "
                                                  "initialize()?");
  *robot_state_ = state;
  planning_scene_->setCurrentState(state);
}

}  // descartes_moveit
<|endoftext|>