cspacepanel.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <algorithm>

#include <QTimer>
#include <QFileDialog>

#include <se2ez/core/log.h>
#include <se2ez/core/colormap.h>
#include <se2ez/core/io.h>
#include <se2ez/core/joint.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/cspace.h>
#include <se2ez/plan/space.h>

#include "se2ez/gui/jointpanel.h"
#include "se2ez/gui/planpanel.h"
#include "se2ez/gui/cspacepanel.h"

#include "ui_cspacepanel.h"

using namespace se2ez;

gui::CSpacePanel::CSpacePanel(RobotPtr robot, std::string name,
                              std::map<std::string, ompl::base::ConstraintPtr> constraints, QWidget *parent)
  : Panel(name, parent)
  , ui_(new Ui::CSpacePanel)
  , robot_(robot)
  , state_(std::make_shared<State>(robot))
  , tempState_(std::make_shared<State>(robot))
  , image_(xSize_, ySize_, QImage::Format_RGB32)
  , wheel_(wheelSize_, wheelSize_, QImage::Format_RGB32)
  , constraints_(constraints)
{
    ui_->setupUi(this);

    ui_->jointA_->clear();
    ui_->jointB_->clear();

    for (const auto &name : robot_->getFrameNames())
    {
        const auto &joint = robot_->getFrame(name)->getJoint();
        if (joint.joints.size() - 1 == 1)
        {
            ui_->jointA_->addItem(QString(name.c_str()));
            ui_->jointB_->addItem(QString(name.c_str()));
        }
        else
        {
            for (unsigned int i = 0; i < joint.joints.size() - 1; ++i)
            {
                const auto &name = joint.joints[i].getName();
                ui_->jointA_->addItem(QString(name.c_str()));
                ui_->jointB_->addItem(QString(name.c_str()));
            }
        }
    };

    if (robot_->getNumJoints() > 1)
        ui_->jointA_->setCurrentIndex(1);

    ui_->constraints_->addItem("<none>");
    for (const auto &constraint : constraints)
        ui_->constraints_->addItem(QString(constraint.first.c_str()));

    updatePanel();
    updateGrid();

    connect(ui_->jointA_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(updateGrid()));
    connect(ui_->jointB_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(updateGrid()));
    connect(ui_->states_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(updatePanel()));
    connect(ui_->constraints_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(updateCallback()));
    connect(ui_->constraintMax_, SIGNAL(valueChanged(double)), this, SLOT(updateCallback()));
    connect(ui_->constraintPow_, SIGNAL(valueChanged(int)), this, SLOT(updateCallback()));
    connect(ui_->jacobian_, SIGNAL(stateChanged(int)), this, SLOT(updateCallback()));
    connect(ui_->project_, SIGNAL(stateChanged(int)), this, SLOT(updateCallback()));
    connect(ui_->doProject_, SIGNAL(clicked()), this, SLOT(projectState()));
    connect(ui_->constraintCollisions_, SIGNAL(stateChanged(int)), this, SLOT(updateCallback()));
    connect(ui_->display_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->fullscreen_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->hires_, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
    connect(ui_->state_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->sd_, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
    connect(ui_->colors_, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
    connect(ui_->save_, SIGNAL(clicked()), this, SLOT(saveImage()));

    connect(ui_->plan_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->edges_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->graph_, SIGNAL(stateChanged(int)), this, SLOT(update()));
    connect(ui_->planpanels_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(updatePlanPanel()));
    connect(ui_->slider_, SIGNAL(valueChanged(int)), this, SLOT(updatePlanState()));
    connect(ui_->animate_, SIGNAL(stateChanged(int)), this, SLOT(updateAnimate()));

    computeWheel();
}

gui::CSpacePanel::~CSpacePanel()
{
}

void gui::CSpacePanel::update()
{
    // SE2EZ_DEBUG("CSpacePanel::update()");
    emit stateChanged();
}

CSpaceGrid::Mode gui::CSpacePanel::getMode() const
{
    if (ui_->sd_->isChecked() && ui_->colors_->isChecked())
        return CSpaceGrid::DISTANCE_COLOR;
    else if (ui_->sd_->isChecked())
        return CSpaceGrid::DISTANCE;
    else if (ui_->colors_->isChecked())
        return CSpaceGrid::COLOR;
    else
        return CSpaceGrid::COLLISION;
}

void gui::CSpacePanel::saveImage()
{
    // SE2EZ_DEBUG("CSpacePanel::saveImage()");

    if (save_)
    {
        save_->join();
        delete save_;
    }

    QString fileName =
        QFileDialog::getSaveFileName(this, tr("Save Image"), "./cspace.png", tr("Image (*.png)"));

    if (fileName.isNull())
        return;

    save_ = new std::thread([&, fileName]() {
        unsigned int ja = ui_->jointA_->currentIndex();
        unsigned int jb = ui_->jointB_->currentIndex();
        SE2EZ_INFORM("Saving grid with indices %1% and %2%...", ja, jb);

        unsigned int xSize = ui_->xSave->value();
        unsigned int ySize = ui_->ySave->value();

        QImage toSave(xSize, ySize, QImage::Format_RGB32);
        auto temp = std::make_shared<CSpaceGrid>(robot_, ja, jb, xSize, ySize, getMode());

        temp->compute(true, state_, callback_);

        auto cs = temp->getImage();

        for (unsigned int i = 0; i < xSize; ++i)
        {
            auto *line = (QRgb *)toSave.scanLine(i);
            for (unsigned int j = 0; j < ySize; ++j)
            {
                auto px = cs->getPixel(xSize - i - 1, j);
                QColor(px.red, px.green, px.blue);
                line[j] = QColor(px.red, px.green, px.blue).rgb();
            }
        }

        QPainter paint;
        paint.begin(&toSave);

        drawGraph(paint, temp, false);
        drawPlan(paint, temp, false, true);
        drawState(paint, temp, false, false);

        toSave.save(fileName, "PNG");
        SE2EZ_INFORM("Done saving image to %1%", fileName.toStdString());
    });
}

void gui::CSpacePanel::updateRobot()
{
    {
        std::unique_lock<std::mutex> slock(mutex_);

        // Tear down grid
        imageGen_ = false;
        grid_->cancel();
        grid_->waitToComplete();
        grid_.reset();

        // Reallocate states
        state_ = std::make_shared<State>(robot_);
        tempState_ = std::make_shared<State>(robot_);

        // Drop panel connections
        if (jp_)
            QObject::disconnect(jpc_);
        jp_ = nullptr;

        if (pp_)
            QObject::disconnect(ppc_);
        pp_ = nullptr;

        jnames_.clear();
        pnames_.clear();
        panelnames_.clear();

        // Clear other data
        path_.clear();
        callback_ = {};

        if (robot_->getNumJoints() > 0)
            ui_->jointB_->setCurrentIndex(0);

        if (robot_->getNumJoints() > 1)
            ui_->jointA_->setCurrentIndex(1);
    }

    updateGrid();
}

void gui::CSpacePanel::updatePlanPanel()
{
    // SE2EZ_DEBUG("CSpacePanel::updatePlanPanel()");
    {
        std::unique_lock<std::mutex> slock(mutex_);
        if (pp_)
        {
            QObject::disconnect(ppc_);
            QObject::disconnect(pdc_);
        }

        pp_ = nullptr;
        if (ui_->planpanels_->currentIndex() != 0)
        {
            const std::string &name = ui_->planpanels_->currentText().toStdString();
            if (panelnames_.find(name) != panelnames_.end())
            {
                auto entry = panels->find(name);
                pp_ = dynamic_cast<gui::PlanPanel *>(entry->second);
                ppc_ = connect(pp_, SIGNAL(newPlan()), this, SLOT(updatePlan()));
                ppc_ = connect(pp_, SIGNAL(newPlan()), this, SLOT(updatePlanData()));
            }
        }
    }

    update();
}

void gui::CSpacePanel::updatePlan()
{
    // SE2EZ_DEBUG("CSpacePanel::updatePlan()");

    {
        std::unique_lock<std::mutex> slock(mutex_);
        path_ = pp_->getPath();

        if (path_.size())
        {
            ui_->slider_->blockSignals(true);
            ui_->slider_->setMaximum(path_.size() - 1);
            ui_->slider_->setEnabled(true);
            ui_->slider_->setValue(0);
            ui_->slider_->blockSignals(false);
        }
    }

    update();
}

void gui::CSpacePanel::updatePlanData()
{
    {
        std::unique_lock<std::mutex> slock(mutex_);
        if (pp_)
        {
            pd_ = pp_->getPlannerData();
        }
    }
}

void gui::CSpacePanel::updateAnimate()
{
    if (path_.size() && ui_->animate_->isChecked())
    {
        ui_->slider_->setValue((ui_->slider_->value() + 1) % ui_->slider_->maximum());
        QTimer::singleShot(1. / ui_->fps_->value() * 1000, this, SLOT(updateAnimate()));
    }
}

void gui::CSpacePanel::updatePlanState()
{
    // SE2EZ_DEBUG("CSpacePanel::updatePlanState()");

    {
        // std::unique_lock<std::mutex> slock(mutex_);

        if (jp_ && path_.size() && ui_->slider_->value() < (int)path_.size())
            jp_->setState(path_[ui_->slider_->value()]);
    }

    // SE2EZ_DEBUG("exiting CSpacePanel::updatePlanState()");
}

void gui::CSpacePanel::updatePanel()
{
    // SE2EZ_DEBUG("CSpacePanel::updatePanel()");

    {
        std::unique_lock<std::mutex> slock(mutex_);
        if (jp_)
            QObject::disconnect(jpc_);

        jp_ = nullptr;
        if (ui_->states_->currentIndex() != 0)
        {
            const std::string &name = ui_->states_->currentText().toStdString();
            if (panelnames_.find(name) != panelnames_.end())
            {
                auto entry = panels->find(name);
                jp_ = dynamic_cast<gui::JointPanel *>(entry->second);
                jpc_ = connect(jp_, SIGNAL(stateChanged()), this, SLOT(updateImage()));
            }
        }
    }

    update();
    updateImage(true);
}

void gui::CSpacePanel::updateImage(bool force)
{
    // SE2EZ_DEBUG("CSpacePanel::updateImage()");

    std::unique_lock<std::mutex> slock(mutex_);

    if (jp_)
    {
        jp_->getState(tempState_);

        Eigen::VectorXd a = tempState_->data;
        Eigen::VectorXd b = state_->data;

        state_->copy(tempState_);

        if (not force)
        {
            // Efficiency check. Don't need to recompute if we only vary along axes we've already computed.
            // TODO: Maybe move this to CSpaceGrid itself
            unsigned int ja = ui_->jointA_->currentIndex();
            unsigned int jb = ui_->jointB_->currentIndex();
            a[ja] = b[ja] = a[jb] = b[jb] = 0;

            if ((a - b).norm() <= math::eps)
                return;
        }
    }

    if (grid_)
    {
        grid_->compute(false, state_, callback_);
        imageGen_ = true;
    }
}

void gui::CSpacePanel::updateGrid()
{
    // SE2EZ_DEBUG("CSpacePanel::updateGrid()");

    {
        std::unique_lock<std::mutex> slock(mutex_);
        unsigned int ja = ui_->jointA_->currentIndex();
        unsigned int jb = ui_->jointB_->currentIndex();
        SE2EZ_INFORM("Updating grid with indices %1% and %2%", ja, jb);

        xCurSize_ = xSize_ * ((ui_->hires_->isChecked()) ? HiRes_ : 1);
        yCurSize_ = ySize_ * ((ui_->hires_->isChecked()) ? HiRes_ : 1);
        grid_ = std::make_shared<CSpaceGrid>(robot_, ja, jb, xCurSize_, yCurSize_, getMode());
        image_ = QImage(xCurSize_, yCurSize_, QImage::Format_RGB32);

        update();
    }

    updateImage(true);
}

void gui::CSpacePanel::computeWheel()
{
    for (unsigned int i = 0; i < wheelSize_; ++i)
    {
        auto *line = (QRgb *)wheel_.scanLine(i);
        for (unsigned int j = 0; j < wheelSize_; ++j)
        {
            double r, g, b;
            double x = i - wheelSize_ / 2.;
            double y = (wheelSize_ - j) - wheelSize_ / 2.;
            double d = std::sqrt(std::pow(x, 2) + std::pow(y, 2)) / (wheelSize_ / 2.);
            if (d <= 1)
            {
                hsv2rgb(std::atan2(y, x), d, 1, r, g, b);
                line[j] = QColor(255. * r, 255. * g, 255. * b).rgb();
            }
            else
                line[j] = QColor(255, 255, 255).rgb();
        }
    }
}

void gui::CSpacePanel::projectState()
{
    if (lockedProjection() && robot_->inLimits(tempState_) && jp_)
        jp_->setState(tempState_);
}

bool gui::CSpacePanel::lockedProjection()
{
    unsigned int iter = 0;
    double norm = 0;

    Eigen::VectorXd f(constraint_->getCoDimension());
    Eigen::MatrixXd j(constraint_->getCoDimension(), constraint_->getAmbientDimension());

    unsigned int ja = ui_->jointA_->currentIndex();
    unsigned int jb = ui_->jointB_->currentIndex();

    constraint_->function(tempState_->data, f);
    while ((norm = f.norm()) > constraint_->getTolerance() && iter++ < 50)
    {
        constraint_->jacobian(tempState_->data, j);
        Eigen::VectorXd temp = j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
        tempState_->data[ja] -= 0.5 * temp[ja];
        tempState_->data[jb] -= 0.5 * temp[jb];
        constraint_->function(tempState_->data, f);
    }

    return norm < constraint_->getTolerance();
}

void gui::CSpacePanel::updateCallback()
{
    // SE2EZ_DEBUG("CSpacePanel::updateCallback()");

    {
        std::unique_lock<std::mutex> slock(mutex_);

        imageGen_ = false;
        grid_->cancel();
        grid_->waitToComplete();

        if (ui_->constraints_->currentIndex() != 0)
        {
            const auto constraint = constraint_ =
                constraints_.find(ui_->constraints_->currentText().toStdString())->second;
            unsigned int exp = ui_->constraintPow_->value();
            double max = ui_->constraintMax_->value();
            bool jac = ui_->jacobian_->isChecked();
            bool drawC = ui_->constraintCollisions_->isChecked();
            unsigned int ja = ui_->jointA_->currentIndex();
            unsigned int jb = ui_->jointB_->currentIndex();

            callback_ = [constraint, exp, max, jac, drawC, ja, jb](ompl::PPM::Color &pixel,
                                                                   const StatePtr &state, bool collision) {
                bool draw = true;
                double r, g, b;
                if (not jac)
                {
                    Eigen::VectorXd f(constraint->getCoDimension());
                    constraint->function(state->data, f);
                    double v = f.norm();
                    if (v < max)
                    {
                        // const double va = max;
                        // static const double vb = 1e-8;
                        double nv = math::remap(max, 0, v, 0, 1);
                        plasma(std::pow(nv, exp), r, g, b);
                    }
                    else
                        draw = false;
                }
                else
                {
                    const double max = 2.;
                    Eigen::MatrixXd j(constraint->getCoDimension(), constraint->getAmbientDimension());
                    constraint->jacobian(state->data, j);
                    double x = j(0, ja);
                    double y = j(0, jb);
                    double t = std::atan2(y, x);

                    double n = std::sqrt(std::pow(x, 2) + std::pow(y, 2));
                    n = math::clamp(n, 0., max);
                    double s = math::remap(0, max, n, 0, 1);

                    hsv2rgb(t, s, 1, r, g, b);
                }

                if (draw)
                {
                    if (collision and drawC)
                    {
                        toGrayscale(r, g, b);
                        r /= 2.;
                        g /= 2.;
                        b /= 2.;
                    }

                    r *= 255;
                    g *= 255;
                    b *= 255;

                    pixel = ompl::PPM::Color{(unsigned char)r, (unsigned char)g, (unsigned char)b};
                }
            };
        }
        else
        {
            constraint_ = nullptr;
            callback_ = {};
        }
    }

    updateImage(true);
}

void gui::CSpacePanel::draw(QPainter &painter, RenderArea * /*canvas*/)
{
    // SE2EZ_DEBUG("CSpacePanel::draw()");

    if (!ui_->display_->isChecked())
        return;

    std::unique_lock<std::mutex> slock(mutex_);

    painter.save();
    painter.resetTransform();

    if (ui_->fullscreen_->isChecked() && not ui_->hires_->isChecked())
        painter.scale(3., 3.);

    QPoint p(xOffset_, yOffset_);
    painter.fillRect(xOffset_ - xBorder_, yOffset_ - yBorder_,  //
                     xCurSize_ + xBorder_ * 2, yCurSize_ + yBorder_ * 2, QColor(255, 0, 0));

    painter.drawImage(p, image_);

    QPen pen(QColor(255, 0, 0));
    painter.setPen(pen);

    painter.drawText(xOffset_ + xCurSize_ + xBorder_ + 3, yOffset_ + yText_, ui_->jointA_->currentText());
    painter.drawText(xOffset_, yOffset_ + yCurSize_ + yBorder_ + yText_, ui_->jointB_->currentText());

    drawGraph(painter, grid_, true);
    drawPlan(painter, grid_, true, ui_->hires_->isChecked());
    drawState(painter, grid_, true);

    painter.restore();
}

void gui::CSpacePanel::drawGraph(QPainter &painter, CSpaceGridPtr &grid, bool offset)
{
    bool edges = ui_->edges_->isChecked();
    bool states = ui_->graph_->isChecked();

    if (pp_ && pd_ && (edges || states))
    {
        unsigned int xo = ((offset) ? xOffset_ : 0);
        unsigned int yo = ((offset) ? yOffset_ : 0);

        for (unsigned int i = 0; i < pd_->numVertices(); ++i)
        {
            const auto &vert = pd_->getVertex(i);
            const auto &state = vert.getState()->as<plan::StateSpace::StateType>()->state;

            QColor color(0, 255, 164);
            if (vert.getTag() == 2)
                color = QColor(0, 164, 255);

            QPen pen(color);
            painter.setPen(pen);

            if (states)
            {
                auto c = grid->getCoords(state);
                unsigned int x = xo + c.y - xPoint_ / 2.;
                unsigned int y = yo + (grid->getHeight() - c.x) - yPoint_ / 2.;

                painter.fillRect(x, y, xPoint_, yPoint_, color);
            }

            if (edges)
            {
                std::vector<unsigned int> edge;
                unsigned int m = pd_->getEdges(i, edge);

                for (unsigned int j = 0; j < m; ++j)
                {
                    const auto &next =
                        pd_->getVertex(edge[j]).getState()->as<plan::StateSpace::StateType>()->state;

                    auto lines = grid->getLines(state, next);

                    for (const auto &line : lines)
                    {
                        unsigned int cx = xo + line.a.y;
                        unsigned int cy = yo + (grid->getHeight() - line.a.x);

                        unsigned int dx = xo + line.b.y;
                        unsigned int dy = yo + (grid->getHeight() - line.b.x);

                        painter.drawLine(cx, cy, dx, dy);
                    }
                }
            }
        }
    }
}

void gui::CSpacePanel::drawPlan(QPainter &painter, CSpaceGridPtr &grid, bool offset, bool bold)
{
    bool edges = ui_->plan_->isChecked();
    bool states = ui_->waypoints_->isChecked();
    if (pp_ && (edges || states))
    {
        QColor color(255, 164, 0);
        QPen pen(color);
        if (bold)
            pen.setWidth(3);
        painter.setPen(pen);

        unsigned int xo = ((offset) ? xOffset_ : 0);
        unsigned int yo = ((offset) ? yOffset_ : 0);

        for (unsigned int i = 0; i < path_.size(); ++i)
        {
            const auto &state = path_[i];

            if (states)
            {
                auto c = grid->getCoords(state);
                unsigned int x = xo + c.y - xPoint_ / 2.;
                unsigned int y = yo + (grid->getHeight() - c.x) - yPoint_ / 2.;

                painter.fillRect(x, y, xPoint_, yPoint_, color);
            }

            if (edges)
            {
                if (i < path_.size() - 1)
                {
                    const auto &next = path_[i + 1];
                    auto lines = grid->getLines(state, next);

                    for (const auto &line : lines)
                    {
                        unsigned int cx = xo + line.a.y;
                        unsigned int cy = yo + (grid->getHeight() - line.a.x);

                        unsigned int dx = xo + line.b.y;
                        unsigned int dy = yo + (grid->getHeight() - line.b.x);

                        painter.drawLine(cx, cy, dx, dy);
                    }
                }
            }
        }
    }
}

void gui::CSpacePanel::drawState(QPainter &painter, CSpaceGridPtr &grid, bool offset, bool interface)
{
    if (jp_ && ui_->state_->isChecked())
    {
        QPen pen(QColor(255, 0, 0));
        painter.setPen(pen);

        unsigned int xo = ((offset) ? xOffset_ : 0);
        unsigned int yo = ((offset) ? yOffset_ : 0);

        auto c = grid->getCoords(state_);
        unsigned int x = xo + c.y - xPoint_ / 2.;
        unsigned int y = yo + (grid->getHeight() - c.x) - yPoint_ / 2.;

        painter.fillRect(x - xPoint_ / 2., y - yPoint_ / 2., xPoint_, yPoint_, QColor(255, 0, 0));

        if (constraint_)
        {
            if (interface)
            {
                double ws = wheelSize_ / 2.;
                int xw = xOffset_ + xCurSize_ + xBorder_ + 3;
                int yw = yOffset_ + yCurSize_ - wheelSize_ - yText_ - 3;
                QPoint p(xw, yw);
                painter.drawImage(p, wheel_);

                Eigen::MatrixXd j(constraint_->getCoDimension(), constraint_->getAmbientDimension());
                constraint_->jacobian(state_->data, j);

                double xj = j(0, ui_->jointA_->currentIndex());
                double yj = j(0, ui_->jointB_->currentIndex());
                double xn = math::remap(-2, 2, xj, -ws, ws);
                double yn = math::remap(-2, 2, yj, -ws, ws);
                xn = math::clamp(xn, -ws, ws);
                yn = math::clamp(yn, -ws, ws);

                double d = constraint_->distance(state_->data);
                QString s(log::format("||F(q)||: %1$0.5f", d).c_str());
                painter.drawText(xOffset_ + xCurSize_ + xBorder_ + 3, yOffset_ + yCurSize_, s);

                painter.save();
                QPen pen(QColor(0, 0, 0, 100));
                pen.setWidth(3);
                painter.setPen(pen);

                painter.drawLine(xw + ws, yw + ws,  //
                                 xw + ws - yn, yw + ws + xn);
                painter.restore();
            }

            if (ui_->project_->isChecked())
            {
                if (lockedProjection() and robot_->inLimits(tempState_))
                {
                    auto d = grid->getCoords(tempState_);
                    unsigned int xp = xo + d.y - xPoint_ / 2.;
                    unsigned int yp = yo + (grid->getHeight() - d.x) - yPoint_ / 2.;

                    painter.drawLine(x, y, xp, yp);
                    painter.fillRect(xp - xPoint_ / 2., yp - yPoint_ / 2., xPoint_, yPoint_,
                                     QColor(255, 0, 0));
                }
            }
        }
    }
}

bool gui::CSpacePanel::click(QMouseEvent * /*event*/, double /*x*/, double /*y*/, int sx, int sy)
{
    // SE2EZ_DEBUG("CSpacePanel::click()");

    if (jp_ && ui_->state_->isChecked())
    {
        unsigned int i = 0, j = 0;
        if (sx >= (int)xOffset_)
        {
            i = sx - xOffset_;
            if (ui_->fullscreen_->isChecked() && not ui_->hires_->isChecked())
                i = std::round(i / 3.) - 3;
        }

        if (sy >= (int)yOffset_)
        {
            j = sy - yOffset_;
            if (ui_->fullscreen_->isChecked() && not ui_->hires_->isChecked())
                j = std::round(j / 3.) - 3;
        }

        if (i <= xCurSize_ && j <= yCurSize_)
        {
            unsigned int jai = ui_->jointA_->currentIndex();
            unsigned int jbi = ui_->jointB_->currentIndex();

            auto jab = grid_->getCoords(yCurSize_ - j, i);

            state_->data[jai] = jab.x;
            state_->data[jbi] = jab.y;

            jp_->setState(state_);

            return true;
        }
    }

    return false;
}

void gui::CSpacePanel::update(boost::posix_time::ptime /*last*/, boost::posix_time::ptime /*current*/)
{
    // SE2EZ_DEBUG("CSpacePanel::update()");

    if (imageGen_)
    {
        emit stateChanged();
        if (not grid_->isComputing())
            imageGen_ = false;

        // Lock to write image
        std::unique_lock<std::mutex> slock(mutex_);

        // Copy image contents to QImage that will be blit
        auto cs = grid_->getImage();
        for (unsigned int i = 0; i < xCurSize_; ++i)
        {
            auto *line = (QRgb *)image_.scanLine(i);
            for (unsigned int j = 0; j < yCurSize_; ++j)
            {
                auto px = cs->getPixel(xCurSize_ - i - 1, j);
                line[j] = QColor(px.red, px.green, px.blue).rgb();
            }
        }
    }

    bool update = false;

    // Storing the states from joint panels
    for (const auto &panel : *panels)
    {
        if (panelnames_.emplace(panel.first).second)  // If new panel was added
        {
            auto jpanel = dynamic_cast<gui::JointPanel *>(panel.second);  // check if it is jpanel
            if (jpanel)
            {
                // If the joint panel controls the robot
                if (jpanel->getSignature().compare(robot_->getSignature()) == 0)
                {
                    jnames_.emplace(panel.first);
                    update = true;
                }
            }

            auto ppanel = dynamic_cast<gui::PlanPanel *>(panel.second);  // check if it is jpanel
            if (ppanel)
            {
                // TODO: Fix
                // If the joint panel controls the robot
                // if (jpanel->getSignature().compare(robot_->getSignature()) == 0)
                // {
                pnames_.emplace(panel.first);
                update = true;
                // }
            }
        }
    }

    // update
    if (update)
    {
        ui_->states_->clear();
        ui_->planpanels_->clear();

        // Adding the named states
        ui_->states_->addItem("<none>");
        for (const auto &name : jnames_)
            ui_->states_->addItem(QString(name.c_str()));

        if (jnames_.size())
            ui_->states_->setCurrentIndex(1);

        // Adding the plans
        ui_->planpanels_->addItem("<none>");
        for (const auto &name : pnames_)
            ui_->planpanels_->addItem(QString(name.c_str()));

        if (pnames_.size())
            ui_->planpanels_->setCurrentIndex(1);
    }
}