ArVizWidgetController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * \package    RobotAPI::gui-plugins::ArVizWidgetController
 * \author     Fabian Paus ( fabian dot paus at kit dot edu )
 * \date       2019
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArVizWidgetController.h"
 
#include <RobotAPI/gui-plugins/ArViz/ui_ArVizWidget.h>
 
#include <QFileDialog>
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/observers/variant/Variant.h>
 
namespace armarx
{
    struct ArVizWidgetBatchCallback : IceUtil::Shared
    {
        struct ArVizWidgetController* this_;
 
        void
        onSuccess(viz::data::RecordingBatch const& batch)
        {
            this_->onGetBatchAsync(batch);
        }
 
        void
        onFailure(Ice::Exception const& ex)
        {
            ARMARX_WARNING << "Failed to get batch async.\nReason:" << ex;
        }
    };
 
    ArVizWidgetController::ArVizWidgetController()
    {
        using This = ArVizWidgetController;
 
        widget = std::make_unique<Ui::ArVizWidget>();
        widget->setupUi(getWidget());
 
        updateTimer = new QTimer(this);
        connect(updateTimer, &QTimer::timeout, this, QOverload<>::of(&This::onUpdate));
 
        replayTimer = new QTimer(this);
        connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&This::onReplayTimerTick));
 
        connect(widget->layerTree, &QTreeWidget::itemChanged, this, &This::layerTreeChanged);
        connect(widget->expandButton, &QPushButton::clicked, this, &This::onExpandAll);
        connect(widget->collapseButton, &QPushButton::clicked, this, &This::onCollapseAll);
        connect(widget->filterEdit, &QLineEdit::textChanged, this, &This::onFilterTextChanged);
        connect(widget->hideAllButton, &QPushButton::clicked, this, &This::onHideAll);
        connect(widget->showAllButton, &QPushButton::clicked, this, &This::onShowAll);
        connect(widget->hideFilteredButton, &QPushButton::clicked, this, &This::onHideFiltered);
        connect(widget->showFilteredButton, &QPushButton::clicked, this, &This::onShowFiltered);
 
        connect(widget->recordingStartButton, &QPushButton::clicked, this, &This::onStartRecording);
        connect(widget->recordingStopButton, &QPushButton::clicked, this, &This::onStopRecording);
 
        connect(widget->refreshRecordingsButton,
                &QPushButton::clicked,
                this,
                &This::onRefreshRecordings);
        connect(widget->recordingList,
                &QListWidget::currentItemChanged,
                this,
                &This::onRecordingSelectionChanged);
 
        connect(widget->replayRevisionSpinBox,
                QOverload<int>::of(&QSpinBox::valueChanged),
                this,
                &This::onReplaySpinChanged);
        connect(widget->replayRevisionSlider,
                QOverload<int>::of(&QSlider::valueChanged),
                this,
                &This::onReplaySliderChanged);
 
        connect(widget->replayStartButton, &QPushButton::clicked, this, &This::onReplayStart);
        connect(widget->replayStopButton, &QPushButton::clicked, this, &This::onReplayStop);
        connect(widget->replayTimedButton, &QPushButton::toggled, this, &This::onReplayTimedStart);
 
        connect(widget->exportToVRMLButton, &QPushButton::clicked, this, &This::exportToVRML);
 
        connect(widget->deselectButton, &QPushButton::clicked, this, &This::onDeselectElement);
        connect(widget->listInteractiveElements,
                &QListWidget::itemDoubleClicked,
                this,
                &This::onInteractiveElementSelected);
 
        connect(this, &This::connectGui, this, &This::onConnectGui, Qt::QueuedConnection);
        connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
 
        // Layer info tree.
        connect(
            widget->layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
 
        connect(widget->layerInfoTreeGroupBox,
                &QGroupBox::toggled,
                &layerInfoTree,
                &LayerInfoTree::setEnabled);
        connect(widget->defaultShowLimitSpinBox,
                qOverload<int>(&QSpinBox::valueChanged),
                &layerInfoTree,
                &LayerInfoTree::setMaxElementCountDefault);
        layerInfoTree.setMaxElementCountDefault(widget->defaultShowLimitSpinBox->value());
 
        layerInfoTree.setWidget(widget->layerInfoTree);
        layerInfoTree.setEnabled(widget->layerInfoTreeGroupBox->isChecked());
        layerInfoTree.registerVisualizerCallbacks(visualizer);
 
        // We need a callback from the visualizer, when the layers have changed
        // So we can update the tree accordingly
        visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const& layers)
        { layersChanged(layers); };
 
        // Arviz Profile tree.
        arvizeProfileLayerWidget = new ArvizProfileManagerWidget();
        widget->verticalLayout->addWidget(arvizeProfileLayerWidget);
        connect(arvizeProfileLayerWidget,
                &ArvizProfileManagerWidget::publishUpdate,
                this,
                &This::updateLayersFromProfile);
        connect(arvizeProfileLayerWidget,
                &ArvizProfileManagerWidget::fetchUpdate,
                this,
                &This::updateLayers);
 
        replayTimer->start(33);
    }
 
    ArVizWidgetController::~ArVizWidgetController()
    {
    }
 
    void
    ArVizWidgetController::onInitComponent()
    {
        if (storageName.size() > 0)
        {
            usingProxy(storageName);
        }
        if (debugObserverName.size() > 0)
        {
            usingProxy(debugObserverName);
        }
 
        callbackData = new ArVizWidgetBatchCallback();
        callbackData->this_ = this;
        callback = viz::newCallback_StorageInterface_getRecordingBatch(
            callbackData,
            &ArVizWidgetBatchCallback::onSuccess,
            &ArVizWidgetBatchCallback::onFailure);
    }
 
    void
    ArVizWidgetController::onExitComponent()
    {
        visualizer.clearCache();
    }
 
    void
    ArVizWidgetController::onConnectComponent()
    {
        getProxy(storage, storageName);
        // DebugObserver is optional (check for null on every call)
        if (!debugObserverName.empty())
        {
            getProxy(debugObserver, debugObserverName, false, "", false);
        }
 
        lastTiming = visualizer.getTiming();
        visualizer.startAsync(storage);
 
        // Changes to UI elements are only allowed in the GUI thread
        emit connectGui();
    }
 
    void
    ArVizWidgetController::onDisconnectComponent()
    {
        // Changes to UI elements are only allowed in the GUI thread
        emit disconnectGui();
    }
 
    void
    ArVizWidgetController::updateLayers()
    {
        // Update shown/hidden layers
        layerTreeChanged(nullptr, 0);
    }
 
    void
    ArVizWidgetController::updateLayersFromProfile()
    {
        {
            QSignalBlocker blocker(widget->layerTree);
 
            // Iterate over all items and activate/deactivate layers accordingly
            for (int compIndex = 0; compIndex < widget->layerTree->topLevelItemCount(); ++compIndex)
            {
                QTreeWidgetItem* componentItem = widget->layerTree->topLevelItem(compIndex);
                bool parentChecked = true;
                const std::string component = componentItem->text(0).toStdString();
                for (int layerIndex = 0; layerIndex < componentItem->childCount(); ++layerIndex)
                {
                    QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
                    const std::string layer = layerItem->text(0).toStdString();
                    const viz::CoinLayerID layerID(component, layer);
                    const bool visible = not arvizeProfileLayerWidget->isHidden(layerID);
                    layerItem->setCheckState(
                        0, visible ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
                    parentChecked &= visible;
                    visualizer.showLayer(layerID, visible);
                }
                componentItem->setCheckState(
                    0, parentChecked ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
            }
        }
    }
 
    void
    ArVizWidgetController::onConnectGui()
    {
        onRefreshRecordings();
        currentRecordingSelected = false;
        changeMode(ArVizWidgetMode::Live);
 
        updateTimer->start(33);
    }
 
    void
    ArVizWidgetController::onDisconnectGui()
    {
        visualizer.stop();
        updateTimer->stop();
        changeMode(ArVizWidgetMode::NotConnected);
    }
 
    void
    ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
    {
        // Iterate over all items and activate/deactivate layers accordingly
        int componentCount = widget->layerTree->topLevelItemCount();
        for (int compIndex = 0; compIndex < componentCount; ++compIndex)
        {
            QTreeWidgetItem* componentItem = widget->layerTree->topLevelItem(compIndex);
            std::string component = componentItem->text(0).toStdString();
            Qt::CheckState componentCheck = componentItem->checkState(0);
            int layerCount = componentItem->childCount();
 
            arvizeProfileLayerWidget->update(component, componentCheck == Qt::Checked);
 
            if (componentItem == item)
            {
                // The parent was selected or deselected, so all children should be set accordingly
                ARMARX_VERBOSE << "Setting all children of " << component << " to "
                               << (componentCheck == Qt::Checked);
                for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
                {
                    QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
                    if (layerItem->checkState(0) != componentCheck)
                    {
                        layerItem->setCheckState(0, componentCheck);
                    }
                }
                return;
            }
            for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
            {
                QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
                std::string layer = layerItem->text(0).toStdString();
                bool layerVisible = (layerItem->checkState(0) == Qt::Checked);
                ARMARX_VERBOSE << "Layer: " << layer << ", Visible: " << layerVisible;
 
                viz::CoinLayerID layerID(component, layer);
 
                arvizeProfileLayerWidget->update(layerID, layerVisible);
 
                visualizer.showLayer(layerID, layerVisible);
            }
        }
    }
 
    void
    ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
    {
        QTreeWidget* tree = widget->layerTree;
 
        std::map<std::string, QTreeWidgetItem*> currentComponents;
        std::map<viz::CoinLayerID, QTreeWidgetItem*> currentLayers;
        int componentCount = widget->layerTree->topLevelItemCount();
        for (int compIndex = 0; compIndex < componentCount; ++compIndex)
        {
            QTreeWidgetItem* componentItem = widget->layerTree->topLevelItem(compIndex);
            std::string component = componentItem->text(0).toStdString();
            currentComponents.emplace(component, componentItem);
 
            int layerCount = componentItem->childCount();
            for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
            {
                QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
                std::string layer = layerItem->text(0).toStdString();
 
                viz::CoinLayerID layerID(component, layer);
                currentLayers.emplace(layerID, layerItem);
            }
        }
 
        const QList<QTreeWidgetItem*> selectedItems = widget->layerTree->selectedItems();
 
        // We need to determine which layers are new and where to append them
        QTreeWidgetItem* currentItem = nullptr;
        bool componentWasNew = false;
        std::string currentComponent;
        for (auto& entry : layers)
        {
            std::string const& component = entry.first;
            if (component != currentComponent)
            {
                auto iter = currentComponents.find(component);
                if (iter == currentComponents.end())
                {
                    // Create a new item
                    QSignalBlocker blocker(
                        tree); // otherwise added as unchecked and directly updated
                    currentItem = new QTreeWidgetItem(tree);
                    currentItem->setText(0, QString::fromStdString(component));
                    currentItem->setCheckState(0,
                                               arvizeProfileLayerWidget->isHidden(component)
                                                   ? Qt::Unchecked
                                                   : Qt::Checked);
 
                    componentWasNew = true;
                }
                else
                {
                    // Item exists already
                    currentItem = iter->second;
 
                    componentWasNew = false;
                }
 
                currentComponent = component;
            }
 
            auto iter = currentLayers.find(entry);
            if (iter == currentLayers.end())
            {
                // Create a new item
                std::string const& layer = entry.second;
                QTreeWidgetItem* layerItem = new QTreeWidgetItem;
                layerItem->setText(0, QString::fromStdString(layer));
                layerItem->setCheckState(
                    0, arvizeProfileLayerWidget->isHidden(entry) ? Qt::Unchecked : Qt::Checked);
 
                if (currentItem)
                {
                    currentItem->addChild(layerItem);
 
                    if (componentWasNew)
                    {
                        // Initially expand new items
                        tree->expandItem(currentItem);
                    }
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(10, entry.first + entry.second)
                                   << "Invalid component/layer ID: " << entry.first << " / "
                                   << entry.second;
                }
            }
            else
            {
                // Item exists already ==> make sure that it is not shown if necessary
                if (arvizeProfileLayerWidget->isHidden(iter->first) or
                    iter->second->checkState(0) == Qt::Unchecked)
                {
                    visualizer.showLayer(iter->first, false);
                }
            }
        }
    }
 
    void
    ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
    {
        (void)previous;
 
        if (!current->parent())
        {
            // A component was selected.
            return;
        }
 
        // A layer was selected.
        viz::CoinLayerID id(current->parent()->text(0).toStdString(),
                            current->text(0).toStdString());
 
        viz::CoinLayer* layer = visualizer.layers.findLayer(id);
        if (layer == nullptr)
        {
            ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second
                           << ") in Visualizer.";
        }
        else
        {
            layerInfoTree.setSelectedLayer(id, &visualizer.layers);
        }
    }
 
    void
    ArVizWidgetController::onCollapseAll(bool)
    {
        widget->layerTree->collapseAll();
    }
 
    void
    ArVizWidgetController::onExpandAll(bool)
    {
        widget->layerTree->expandAll();
    }
 
    void
    ArVizWidgetController::onHideAll(bool)
    {
        showAllLayers(false);
    }
 
    void
    ArVizWidgetController::onShowAll(bool)
    {
        showAllLayers(true);
    }
 
    void
    ArVizWidgetController::onHideFiltered(bool)
    {
        showFilteredLayers(false);
    }
 
    void
    ArVizWidgetController::onShowFiltered(bool)
    {
        showFilteredLayers(true);
    }
 
    void
    ArVizWidgetController::onFilterTextChanged(const QString& filter)
    {
        // Now we need to show these matches and hide the other items
        // Is there a better way? Probably, via QSortFilterProxyModel...
        QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
        for (QTreeWidgetItemIterator iter(widget->layerTree); *iter; ++iter)
        {
            QTreeWidgetItem* item = *iter;
            QString itemText = item->text(0);
            bool matches = filter.size() == 0 || itemText.contains(rx);
            item->setHidden(!matches);
            if (matches && item->parent())
            {
                // Make parent visible if a child is visible
                item->parent()->setHidden(false);
            }
        }
    }
 
    void
    ArVizWidgetController::showAllLayers(bool visible)
    {
        widget->layerTree->blockSignals(true);
 
        for (QTreeWidgetItemIterator iter(widget->layerTree); *iter; ++iter)
        {
            QTreeWidgetItem* item = *iter;
            item->setCheckState(0, visible ? Qt::Checked : Qt::Unchecked);
        }
 
        widget->layerTree->blockSignals(false);
 
        updateLayers();
    }
 
    void
    ArVizWidgetController::showFilteredLayers(bool visible)
    {
        widget->layerTree->blockSignals(true);
 
        QString filter = widget->filterEdit->text();
        QRegExp rx(filter, Qt::CaseInsensitive, QRegExp::Wildcard);
 
        for (QTreeWidgetItemIterator iter(widget->layerTree); *iter; ++iter)
        {
            QTreeWidgetItem* item = *iter;
            QString itemText = item->text(0);
            bool matches = filter.size() == 0 || itemText.contains(rx);
            if (matches)
            {
                item->setCheckState(0, visible ? Qt::Checked : Qt::Unchecked);
            }
        }
 
        widget->layerTree->blockSignals(false);
 
        updateLayers();
    }
 
    void
    ArVizWidgetController::onDeselectElement()
    {
        // We just deselect all elements.
        // Maybe we need to be more specific for strange use cases (?)
        visualizer.selection->deselectAll();
    }
 
    void
    ArVizWidgetController::onContextMenuClicked()
    {
        viz::ElementInteractionData* selected = visualizer.selectedElement;
        if (selected == nullptr)
        {
            ARMARX_WARNING << "Selected element is null, but a context menu option was clicked!";
            return;
        }
 
        QPushButton* clickedButton = static_cast<QPushButton*>(sender());
 
        QLayout* layout = widget->groupBoxContextMenu->layout();
        int count = layout->count();
        for (int i = 0; i < count; ++i)
        {
            QPushButton* button = static_cast<QPushButton*>(layout->itemAt(i)->widget());
            if (button == clickedButton)
            {
                viz::data::InteractionFeedback& interaction =
                    visualizer.interactionFeedbackBuffer.emplace_back();
                interaction.component = selected->layer.first;
                interaction.layer = selected->layer.second;
                interaction.element = selected->element;
                interaction.type = viz::data::InteractionFeedbackType::CONTEXT_MENU_CHOSEN;
                interaction.chosenContextMenuEntry = i;
                return;
            }
        }
    }
 
    void
    ArVizWidgetController::onInteractiveElementSelected(QListWidgetItem* item)
    {
        int index = widget->listInteractiveElements->row(item);
        if (index < 0)
        {
            return;
        }
 
        visualizer.selectElement(index);
    }
 
    static QString
    toQString(viz::ElementInteractionData const& inter)
    {
        std::string id = inter.layer.first + "/" + inter.layer.second + "/" + inter.element;
        return QString::fromStdString(id);
    }
 
    void
    ArVizWidgetController::onUpdate()
    {
        visualizer.update();
 
        // Show the currently selected element
        QString selectedElementName("<None>");
        QLayout* contextMenuLayout = widget->groupBoxContextMenu->layout();
        if (visualizer.selectedElement)
        {
            selectedElementName = toQString(*visualizer.selectedElement);
 
            // Show context menu options if enabled
            int currentCount = contextMenuLayout->count();
            viz::data::InteractionDescription const& desc = visualizer.selectedElement->interaction;
            std::vector<std::string> const& options = desc.contextMenuOptions;
            int newCount = 0;
            if (desc.enableFlags & viz::data::InteractionEnableFlags::CONTEXT_MENU)
            {
                newCount = (int)options.size();
            }
            if (newCount != currentCount)
            {
                // Remove all items
                QLayoutItem* item;
                while ((item = contextMenuLayout->takeAt(0)) != nullptr)
                {
                    delete item->widget();
                    delete item;
                }
 
                for (std::string const& option : options)
                {
                    QPushButton* button =
                        new QPushButton(QString::fromStdString(option), widget->groupBoxContextMenu);
                    connect(button,
                            &QPushButton::clicked,
                            this,
                            &ArVizWidgetController::onContextMenuClicked);
                    contextMenuLayout->addWidget(button);
                }
            }
            else
            {
                for (int i = 0; i < currentCount; ++i)
                {
                    QLayoutItem* item = contextMenuLayout->itemAt(i);
                    QPushButton* button = static_cast<QPushButton*>(item->widget());
                    button->setText(QString::fromStdString(options[i]));
                }
            }
        }
        else
        {
            QLayoutItem* item;
            while ((item = contextMenuLayout->takeAt(0)) != nullptr)
            {
                delete item->widget();
                delete item;
            }
        }
        widget->labelSelectedElement->setText(selectedElementName);
 
        // Show the interactive elements
        int currentCount = widget->listInteractiveElements->count();
        int newCount = (int)visualizer.elementInteractions.size();
        if (newCount != currentCount)
        {
            widget->listInteractiveElements->clear();
            for (auto& interaction : visualizer.elementInteractions)
            {
                QString elementID = toQString(*interaction);
                widget->listInteractiveElements->addItem(elementID);
            }
        }
 
        onTimingObserverUpdate();
    }
 
    void
    ArVizWidgetController::onTimingObserverUpdate()
    {
        viz::CoinVisualizer_UpdateTiming timing = visualizer.getTiming();
        //if (timing.counter > lastTiming.counter)
        {
            if (debugObserver)
            {
                timingMap["0. pull (ms)"] = new Variant(timing.pull.toMilliSecondsDouble());
                timingMap["1. apply (ms)"] =
                    new Variant(timing.applyTotal.total.toMilliSecondsDouble());
                timingMap["1.1 apply, addLayer (ms)"] =
                    new Variant(timing.applyTotal.addLayer.toMilliSecondsDouble());
                timingMap["1.2 apply, updateElements (ms)"] =
                    new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
                timingMap["1.3 apply, removeElements (ms)"] =
                    new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
                timingMap["2. layers (ms)"] =
                    new Variant(timing.layersChanged.toMilliSecondsDouble());
                timingMap["3. wait duration (ms)"] =
                    new Variant(timing.waitDuration.toMilliSecondsDouble());
                timingMap["4. update toggle"] = new Variant(timing.updateToggle);
                timingMap["total (ms)"] = new Variant(timing.total.toMilliSecondsDouble());
 
                timings.push_back(timing.total.toMilliSecondsDouble());
                int numTimings = 20;
                if ((int)timings.size() > numTimings)
                {
                    timings.erase(timings.begin());
                }
                double averageTime =
                    std::accumulate(timings.begin(), timings.end(), 0.0) / numTimings;
                timingMap["total avg (ms)"] = new Variant(averageTime);
 
                debugObserver->begin_setDebugChannel("ArViz_Timing", timingMap);
            }
        }
    }
 
    void
    ArVizWidgetController::onStartRecording()
    {
        std::string recordingID = widget->recordingIdTextBox->text().toStdString();
 
        std::string runningID = storage->startRecording(recordingID);
 
        changeMode(ArVizWidgetMode::Recording);
 
        if (runningID.size() > 0)
        {
            std::string message = "There is already a recording running with ID '" + runningID +
                                  "'. \n" + "You have to stop the running recording first";
            QMessageBox::information(
                widget->tabWidget, "Recording running", QString::fromStdString(message));
 
 
            return;
        }
    }
 
    void
    ArVizWidgetController::onStopRecording()
    {
        storage->stopRecording();
 
        onRefreshRecordings();
        changeMode(ArVizWidgetMode::Live);
    }
 
    void
    ArVizWidgetController::onRefreshRecordings()
    {
        allRecordings = storage->getAllRecordings().recordings;
        std::sort(allRecordings.begin(),
                  allRecordings.end(),
                  [](viz::data::Recording const& lhs, viz::data::Recording const& rhs)
                  { return lhs.id < rhs.id; });
 
        std::string currentText;
        QListWidgetItem* currentItem = widget->recordingList->currentItem();
        if (currentItem)
        {
            currentText = currentItem->text().toStdString();
        }
 
        widget->recordingList->clear();
        int currentTextIndex = -1;
        int index = 0;
        for (auto& recording : allRecordings)
        {
            if (recording.id == currentText)
            {
                currentTextIndex = index;
            }
            widget->recordingList->addItem(QString::fromStdString(recording.id));
            ++index;
        }
 
        if (currentTextIndex > 0)
        {
            widget->recordingList->setCurrentRow(currentTextIndex);
        }
    }
 
    void
    ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current,
                                                       QListWidgetItem* previous)
    {
        if (!current)
        {
            return;
        }
 
        std::string selectedID = current->text().toStdString();
        for (viz::data::Recording const& recording : allRecordings)
        {
            if (recording.id == selectedID)
            {
                selectRecording(recording);
                break;
            }
        }
    }
 
    static std::string
    timestampToString(long timestampInMicroSeconds, bool showMS = false)
    {
        IceUtil::Time time = IceUtil::Time::microSeconds(timestampInMicroSeconds);
        std::string timeString = time.toDateTime();
        if (!showMS)
        {
            timeString = timeString.substr(0, timeString.size() - 4);
        };
        return timeString;
    }
 
    void
    ArVizWidgetController::onReplaySpinChanged(int newValue)
    {
        widget->replayRevisionSlider->setValue(newValue);
    }
 
    void
    ArVizWidgetController::onReplaySliderChanged(int newValue)
    {
        if (currentRevision != newValue)
        {
            long timestamp = replayToRevision(newValue);
            if (timestamp > 0)
            {
                currentRevision = newValue;
                currentTimestamp = timestamp;
                widget->replayRevisionSpinBox->setValue(newValue);
 
 
                widget->replayTimestampLabel->setText(
                    QString::fromStdString(timestampToString(timestamp, true)));
            }
            else
            {
                widget->replayRevisionSlider->setValue(currentRevision);
            }
        }
    }
 
    void
    ArVizWidgetController::selectRecording(const viz::data::Recording& recording)
    {
        // Update recording description
        widget->recordingIdLabel->setText(QString::fromStdString(recording.id));
 
        widget->recordingRevisionLabel->setText(
            QString::fromStdString(std::to_string(recording.firstRevision) + " - " +
                                   std::to_string(recording.lastRevision)));
 
        std::string firstTimeString = timestampToString(recording.firstTimestampInMicroSeconds);
        std::string lastTimeString = timestampToString(recording.lastTimestampInMicroSeconds);
 
        widget->recordingTimestampLabel->setText(
            QString::fromStdString(firstTimeString + " - " + lastTimeString));
 
        IceUtil::Time duration = IceUtil::Time::microSeconds(
            recording.lastTimestampInMicroSeconds - recording.firstTimestampInMicroSeconds);
        int durationSeconds = duration.toSeconds();
        widget->recordingDurationLabel->setText(
            QString::fromStdString(std::to_string(durationSeconds) + " s"));
 
        widget->recordingBatchesLabel->setText(
            QString::fromStdString(std::to_string(recording.batchHeaders.size())));
 
        // Update replay control
        currentRecording = recording;
        currentRecordingSelected = true;
        enableWidgetAccordingToMode();
    }
 
    void
    ArVizWidgetController::onReplayStart(bool)
    {
        if (!currentRecordingSelected)
        {
            ARMARX_WARNING << "No recording selected, so no replay can be started";
            return;
        }
        {
            std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
            recordingBatchCache.clear();
        }
 
        visualizer.stop();
 
        changeMode(ArVizWidgetMode::ReplayingManual);
 
        widget->replayRevisionSpinBox->blockSignals(true);
        widget->replayRevisionSpinBox->setMinimum(currentRecording.firstRevision);
        widget->replayRevisionSpinBox->setMaximum(currentRecording.lastRevision);
        widget->replayRevisionSpinBox->setValue(currentRecording.firstRevision);
        widget->replayRevisionSpinBox->blockSignals(false);
 
        widget->replayRevisionSlider->blockSignals(true);
        widget->replayRevisionSlider->setMinimum(currentRecording.firstRevision);
        widget->replayRevisionSlider->setMaximum(currentRecording.lastRevision);
        widget->replayRevisionSlider->setValue(currentRecording.firstRevision);
        widget->replayRevisionSlider->blockSignals(false);
 
        currentRevision = -1;
        onReplaySliderChanged(widget->replayRevisionSlider->value());
    }
 
    void
    ArVizWidgetController::onReplayStop(bool)
    {
        visualizer.startAsync(storage);
 
        changeMode(ArVizWidgetMode::Live);
    }
 
    long
    ArVizWidgetController::replayToRevision(long revision)
    {
        if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
        {
            ARMARX_WARNING << "Cannot call replayToRevision, when not in replaying mode.\n"
                           << "Actual mode: " << int(mode);
            return -1;
        }
 
        viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
        for (auto& batchHeader : currentRecording.batchHeaders)
        {
            if (batchHeader.firstRevision <= revision && revision <= batchHeader.lastRevision)
            {
                matchingBatchHeader = &batchHeader;
                break;
            }
        }
        if (matchingBatchHeader == nullptr)
        {
            ARMARX_WARNING << "Could not find batch for revision: " << revision;
            return -1;
        }
 
        viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
 
        ARMARX_VERBOSE << "Replaying to revision : " << revision
                       << "\nGot batch: " << batch.header.firstRevision << " - "
                       << batch.header.lastRevision << "\nUpdates: " << batch.updates.size()
                       << "\nInitial state: " << batch.initialState.size();
 
 
        auto revisionLess = [](viz::data::TimestampedLayerUpdate const& lhs,
                               viz::data::TimestampedLayerUpdate const& rhs)
        { return lhs.revision < rhs.revision; };
        viz::data::TimestampedLayerUpdate pivot;
        pivot.revision = revision;
        auto updateBegin =
            std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
        auto updateEnd = std::upper_bound(updateBegin, batch.updates.end(), pivot, revisionLess);
 
        // TODO: Optimize: Only start from the last update position
        std::map<std::string, viz::data::LayerUpdate const*> updates;
 
        for (auto& update : batch.initialState)
        {
            updates[update.update.name] = &update.update;
        }
        for (auto updateIter = batch.updates.begin(); updateIter != updateEnd; ++updateIter)
        {
            updates[updateIter->update.name] = &updateIter->update;
        }
 
        auto layerIDsBefore = visualizer.getLayerIDs();
        for (auto& pair : updates)
        {
            visualizer.apply(*pair.second);
        }
        auto layerIDsAfter = visualizer.getLayerIDs();
        if (layerIDsAfter != layerIDsBefore)
        {
            visualizer.emitLayersChanged(layerIDsAfter);
        }
 
        return updateBegin->timestampInMicroseconds;
    }
 
    long
    ArVizWidgetController::getRevisionForTimestamp(long timestamp)
    {
        if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
        {
            ARMARX_WARNING << "Cannot call replayToTimestamp, when not in replaying mode.\n"
                           << "Actual mode: " << int(mode);
            return -1;
        }
 
        if (timestamp < currentRecording.firstTimestampInMicroSeconds)
        {
            ARMARX_INFO << "Requested timestamp is earlier than recording: "
                        << timestampToString(timestamp);
            return -1;
        }
 
        viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
        for (auto& batchHeader : currentRecording.batchHeaders)
        {
            matchingBatchHeader = &batchHeader;
            if (timestamp <= batchHeader.lastTimestampInMicroSeconds)
            {
                break;
            }
        }
 
        viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
 
        auto timestampLess = [](viz::data::TimestampedLayerUpdate const& lhs,
                                viz::data::TimestampedLayerUpdate const& rhs)
        { return lhs.timestampInMicroseconds < rhs.timestampInMicroseconds; };
        viz::data::TimestampedLayerUpdate pivot;
        pivot.timestampInMicroseconds = timestamp;
        auto updateEnd =
            std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, timestampLess);
        if (updateEnd == batch.updates.end())
        {
            return -2;
        }
        if (updateEnd != batch.updates.begin())
        {
            // lower_bound gives the first entry with a later timestamp then the goal
            // So we should only apply updates before this point
            updateEnd -= 1;
        }
 
        long revisionBeforeTimestamp = updateEnd->revision;
        return revisionBeforeTimestamp;
    }
 
    void
    ArVizWidgetController::onReplayTimedStart(bool checked)
    {
        if (checked)
        {
 
            changeMode(ArVizWidgetMode::ReplayingTimed);
            replayCurrentTimestamp = currentTimestamp;
        }
        else
        {
            changeMode(ArVizWidgetMode::ReplayingManual);
        }
    }
 
    void
    ArVizWidgetController::onReplayTimerTick()
    {
        if (mode == ArVizWidgetMode::ReplayingTimed)
        {
            double replaySpeed = widget->replaySpeedSpinBox->value();
 
            long currentRealTime = IceUtil::Time::now().toMicroSeconds();
            long elapsedRealTime = currentRealTime - lastReplayRealTime;
 
            replayCurrentTimestamp += elapsedRealTime * replaySpeed;
 
            long revision = getRevisionForTimestamp(replayCurrentTimestamp);
            if (revision == -2)
            {
                if (widget->replayLoopbackCheckBox->checkState() == Qt::Checked)
                {
                    replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
                }
                else
                {
                    revision = currentRecording.lastRevision;
                }
            }
            if (revision >= 0)
            {
                widget->replayRevisionSlider->setValue(revision);
            }
        }
 
        lastReplayRealTime = IceUtil::Time::now().toMicroSeconds();
    }
 
    void
    ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
    {
        mode = newMode;
 
        enableWidgetAccordingToMode();
    }
 
    void
    ArVizWidgetController::enableWidgetAccordingToMode()
    {
        switch (mode)
        {
            case ArVizWidgetMode::NotConnected:
            {
                widget->recordingStartButton->setDisabled(true);
                widget->recordingStopButton->setDisabled(true);
                widget->replayStartButton->setDisabled(true);
                widget->replayStopButton->setDisabled(true);
                widget->replayControlGroupBox->setDisabled(true);
            }
            break;
            case ArVizWidgetMode::Live:
            {
                widget->recordingStartButton->setDisabled(false);
                widget->recordingStopButton->setDisabled(true);
                widget->replayStartButton->setDisabled(false);
                widget->replayStopButton->setDisabled(true);
                widget->replayControlGroupBox->setDisabled(true);
                widget->recordingList->setDisabled(false);
            }
            break;
            case ArVizWidgetMode::Recording:
            {
                widget->recordingStartButton->setDisabled(true);
                widget->recordingStopButton->setDisabled(false);
                widget->replayStartButton->setDisabled(true);
                widget->replayStopButton->setDisabled(true);
                widget->replayControlGroupBox->setDisabled(true);
            }
            break;
            case ArVizWidgetMode::ReplayingManual:
            {
                widget->recordingStartButton->setDisabled(true);
                widget->recordingStopButton->setDisabled(true);
                widget->replayStartButton->setDisabled(true);
                widget->replayStopButton->setDisabled(false);
                widget->replayControlGroupBox->setDisabled(false);
                widget->replayRevisionSlider->setDisabled(false);
                widget->replayRevisionSpinBox->setDisabled(false);
                widget->recordingList->setDisabled(true);
            }
            break;
            case ArVizWidgetMode::ReplayingTimed:
            {
                widget->recordingStartButton->setDisabled(true);
                widget->recordingStopButton->setDisabled(true);
                widget->replayStartButton->setDisabled(true);
                widget->replayStopButton->setDisabled(false);
                widget->replayControlGroupBox->setDisabled(false);
                widget->replayRevisionSlider->setDisabled(true);
                widget->replayRevisionSpinBox->setDisabled(true);
                widget->recordingList->setDisabled(true);
            }
            break;
        }
 
        if (!currentRecordingSelected)
        {
            widget->replayStartButton->setDisabled(true);
            widget->replayStopButton->setDisabled(true);
        }
    }
 
    void
    ArVizWidgetController::onGetBatchAsync(const viz::data::RecordingBatch& batch)
    {
        // We received a batch asynchronously ==> Update the cache
        ARMARX_INFO << "Received async batch: " << batch.header.index;
        std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
 
        auto& entry = recordingBatchCache[batch.header.index];
        entry.data = batch;
        entry.lastUsed = IceUtil::Time::now();
 
        limitRecordingBatchCacheSize();
 
        recordingWaitingForBatchIndex = -1;
    }
 
    viz::data::RecordingBatch const&
    ArVizWidgetController::getRecordingBatch(long index)
    {
        ARMARX_TRACE;
 
        IceUtil::Time now = IceUtil::Time::now();
 
        std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
 
        auto iter = recordingBatchCache.find(index);
        if (iter != recordingBatchCache.end())
        {
            // Start prefetching neighbouring batches
            bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
            if (!asyncPrefetchIsRunning && recordingWaitingForBatchIndex == -1)
            {
                if (index + 1 < long(currentRecording.batchHeaders.size()) &&
                    recordingBatchCache.count(index + 1) == 0)
                {
                    //                    ARMARX_WARNING << "after begin_getRecordingBatch: " << (index + 1)
                    //                                   << " waiting for " << recordingWaitingForBatchIndex;
                    callbackResult =
                        storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
                    recordingWaitingForBatchIndex = index + 1;
                    ARMARX_INFO << "Now waiting for " << recordingWaitingForBatchIndex;
                }
                else if (index > 0 && recordingBatchCache.count(index - 1) == 0)
                {
                    //                    ARMARX_WARNING << "before begin_getRecordingBatch: " << (index - 1)
                    //                                   << " waiting for " << recordingWaitingForBatchIndex;
                    callbackResult =
                        storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
                    recordingWaitingForBatchIndex = index - 1;
                }
            }
 
            TimestampedRecordingBatch& entry = iter->second;
            entry.lastUsed = now;
            return entry.data;
        }
 
        // Maybe there has already been sent a asynchronous request to get
        if (index == recordingWaitingForBatchIndex)
        {
            lock.unlock();
            ARMARX_INFO << "Waiting to receive async batch: " << index;
            // Wait until request completes
            while (recordingWaitingForBatchIndex != -1)
            {
                QCoreApplication::processEvents();
            }
            return getRecordingBatch(index);
        }
 
        ARMARX_INFO << "Batch #" << index
                    << " is not in the cache. Getting synchronously, blocking GUI...";
 
        // Entry is not in the cache, we have to get it from ArVizStorage
        auto& newEntry = recordingBatchCache[index];
        newEntry.lastUsed = now;
        newEntry.data = storage->getRecordingBatch(currentRecording.id, index);
 
        limitRecordingBatchCacheSize();
 
        return newEntry.data;
    }
 
    void
    ArVizWidgetController::limitRecordingBatchCacheSize()
    {
        if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
        {
            // Remove the entry with the oldest last used timestamp
            auto oldestIter = recordingBatchCache.begin();
            for (auto iter = recordingBatchCache.begin(); iter != recordingBatchCache.end(); ++iter)
            {
                TimestampedRecordingBatch& entry = iter->second;
                if (entry.lastUsed < oldestIter->second.lastUsed)
                {
                    oldestIter = iter;
                }
            }
 
            recordingBatchCache.erase(oldestIter);
        }
    }
 
    SoNode*
    ArVizWidgetController::getScene()
    {
        return visualizer.root;
    }
 
    static const std::string CONFIG_KEY_STORAGE = "Storage";
    static const std::string CONFIG_KEY_DEBUG_OBSERVER = "DebugObserver";
 
    void
    ArVizWidgetController::loadSettings(QSettings* settings)
    {
        storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE), "ArVizStorage")
                          .toString()
                          .toStdString();
        debugObserverName =
            settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver")
                .toString()
                .toStdString();
    }
 
    void
    ArVizWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue(QString::fromStdString(CONFIG_KEY_STORAGE),
                           QString::fromStdString(storageName));
        settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
                           QString::fromStdString(debugObserverName));
    }
 
    QPointer<QDialog>
    ArVizWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!configDialog)
        {
            configDialog = new SimpleConfigDialog(parent);
            configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>(
                {CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
            configDialog->addProxyFinder<armarx::DebugObserverInterfacePrx>(
                {CONFIG_KEY_DEBUG_OBSERVER, "Debug observer", "DebugObserver"});
        }
        return qobject_cast<QDialog*>(configDialog);
    }
 
    void
    ArVizWidgetController::configured()
    {
        if (configDialog)
        {
            storageName = configDialog->getProxyName(CONFIG_KEY_STORAGE);
            debugObserverName = configDialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
        }
    }
 
    void
    ArVizWidgetController::exportToVRML()
    {
 
        QString fi = QFileDialog::getSaveFileName(
            Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
        std::string s = std::string(fi.toLatin1());
 
        if (s.empty())
        {
            return;
        }
        if (!simox::alg::ends_with(s, ".wrl"))
        {
            s += ".wrl";
        }
 
        visualizer.exportToVRML(s);
    }
} // namespace armarx