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 <ArmarXCore/observers/variant/Variant.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
#include <QFileDialog>
 
 
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.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);
        };
 
        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::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();
 
            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);
                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
                    currentItem = new QTreeWidgetItem(tree);
                    currentItem->setText(0, QString::fromStdString(component));
                    // A new item is visible by default
                    currentItem->setCheckState(0, 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));
                // A new item is visible by default
                layerItem->setCheckState(0, 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 ==> nothing to be done
            }
        }
    }
 
    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);
 
        // Update shown/hidden layers
        layerTreeChanged(nullptr, 0);
    }
 
    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);
 
        // Update shown/hidden layers
        layerTreeChanged(nullptr, 0);
    }
 
    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);
    }
}