text: /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2012, Willow Garage, Inc.
   *  All rights reserved.
   *
   *  Redistribution and use in source and binary forms, with or without
   *  modification, are permitted provided that the following conditions
   *  are met:
   *
   *   * Redistributions of source code must retain the above copyright
   *     notice, this list of conditions and the following disclaimer.
   *   * Redistributions in binary form must reproduce the above
   *     copyright notice, this list of conditions and the following
   *     disclaimer in the documentation and/or other materials provided
   *     with the distribution.
   *   * Neither the name of Willow Garage nor the names of its
   *     contributors may be used to endorse or promote products derived
   *     from this software without specific prior written permission.
   *
   *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   *  POSSIBILITY OF SUCH DAMAGE.
   *********************************************************************/
  
  /* Author: Dave Coleman */
  
  // SA
  #include "end_effectors_widget.h"
  // Qt
  #include <QFormLayout>
  #include <QMessageBox>
  #include <QApplication>
  
  namespace moveit_setup_assistant
  {
  // ******************************************************************************************
  // Constructor
  // ******************************************************************************************
  EndEffectorsWidget::EndEffectorsWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
    : SetupScreenWidget(parent), config_data_(config_data)
  {
    // Basic widget container
    QVBoxLayout *layout = new QVBoxLayout();
  
    // Top Header Area ------------------------------------------------
  
    HeaderWidget *header =
        new HeaderWidget("End Effectors", "Setup grippers and other end effectors for your robot", this);
    layout->addWidget(header);
  
    // Create contents screens ---------------------------------------
  
    effector_list_widget_ = createContentsWidget();
    effector_edit_widget_ = createEditWidget();
  
    // Create stacked layout -----------------------------------------
    stacked_layout_ = new QStackedLayout(this);
    stacked_layout_->addWidget(effector_list_widget_);  // screen index 0
    stacked_layout_->addWidget(effector_edit_widget_);  // screen index 1
  
    // Create Widget wrapper for layout
    QWidget *stacked_layout_widget = new QWidget(this);
    stacked_layout_widget->setLayout(stacked_layout_);
  
    layout->addWidget(stacked_layout_widget);
  
    // Finish Layout --------------------------------------------------
    this->setLayout(layout);
  }
  
  // ******************************************************************************************
  // Create the main content widget
  // ******************************************************************************************
  QWidget *EndEffectorsWidget::createContentsWidget()
  {
    // Main widget
    QWidget *content_widget = new QWidget(this);
  
    // Basic widget container
    QVBoxLayout *layout = new QVBoxLayout(this);
  
    // Table ------------ ------------------------------------------------
  
    data_table_ = new QTableWidget(this);
    data_table_->setColumnCount(4);
    data_table_->setSortingEnabled(true);
    data_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
    connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int)));
    connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int)));
    layout->addWidget(data_table_);
  
    // Set header labels
    QStringList header_list;
    header_list.append("End Effector Name");
    header_list.append("Group Name");
    header_list.append("Parent Link");
    header_list.append("Parent Group");
    data_table_->setHorizontalHeaderLabels(header_list);
  
    // Bottom Buttons --------------------------------------------------
  
    QHBoxLayout *controls_layout = new QHBoxLayout();
  
    // Spacer
    QWidget *spacer = new QWidget(this);
    spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
    controls_layout->addWidget(spacer);
  
    // Edit Selected Button
    btn_edit_ = new QPushButton("&Edit Selected", this);
    btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
    btn_edit_->setMaximumWidth(300);
    btn_edit_->hide();  // show once we know if there are existing poses
    connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
    controls_layout->addWidget(btn_edit_);
    controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
  
    // Delete Selected Button
    btn_delete_ = new QPushButton("&Delete Selected", this);
    connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteSelected()));
    controls_layout->addWidget(btn_delete_);
    controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
  
    // Add end effector Button
    QPushButton *btn_add = new QPushButton("&Add End Effector", this);
    btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
    btn_add->setMaximumWidth(300);
    connect(btn_add, SIGNAL(clicked()), this, SLOT(showNewScreen()));
    controls_layout->addWidget(btn_add);
    controls_layout->setAlignment(btn_add, Qt::AlignRight);
  
    // Add layout
    layout->addLayout(controls_layout);
  
    // Set layout -----------------------------------------------------
    content_widget->setLayout(layout);
  
    return content_widget;
  }
  
  // ******************************************************************************************
  // Create the edit widget
  // ******************************************************************************************
  QWidget *EndEffectorsWidget::createEditWidget()
  {
    // Main widget
    QWidget *edit_widget = new QWidget(this);
    // Layout
    QVBoxLayout *layout = new QVBoxLayout();
  
    // Simple form -------------------------------------------
    QFormLayout *form_layout = new QFormLayout();
    // form_layout->setContentsMargins( 0, 15, 0, 15 );
    form_layout->setRowWrapPolicy(QFormLayout::WrapAllRows);
  
    // Name input
    effector_name_field_ = new QLineEdit(this);
    form_layout->addRow("End Effector Name:", effector_name_field_);
  
    // Group input
    group_name_field_ = new QComboBox(this);
    group_name_field_->setEditable(false);
    form_layout->addRow("End Effector Group:", group_name_field_);
    connect(group_name_field_, SIGNAL(currentIndexChanged(const QString &)), this,
            SLOT(previewClickedString(const QString &)));
  
    // Parent Link input
    parent_name_field_ = new QComboBox(this);
    parent_name_field_->setEditable(false);
    form_layout->addRow("Parent Link (usually part of the arm):", parent_name_field_);
  
    // Parent Group input
    parent_group_name_field_ = new QComboBox(this);
    parent_group_name_field_->setEditable(false);
    form_layout->addRow("Parent Group (optional):", parent_group_name_field_);
  
    layout->addLayout(form_layout);
  
    // Bottom Buttons --------------------------------------------------
  
    QHBoxLayout *controls_layout = new QHBoxLayout();
    controls_layout->setContentsMargins(0, 25, 0, 15);
  
    // Spacer
    QWidget *spacer = new QWidget(this);
    spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
    controls_layout->addWidget(spacer);
  
    // Save
    QPushButton *btn_save_ = new QPushButton("&Save", this);
    btn_save_->setMaximumWidth(200);
    connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing()));
    controls_layout->addWidget(btn_save_);
    controls_layout->setAlignment(btn_save_, Qt::AlignRight);
  
    // Cancel
    QPushButton *btn_cancel_ = new QPushButton("&Cancel", this);
    btn_cancel_->setMaximumWidth(200);
    connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing()));
    controls_layout->addWidget(btn_cancel_);
    controls_layout->setAlignment(btn_cancel_, Qt::AlignRight);
  
    // Add layout
    layout->addLayout(controls_layout);
  
    // Set layout -----------------------------------------------------
    edit_widget->setLayout(layout);
  
    return edit_widget;
  }
  
  // ******************************************************************************************
  // Show edit screen for creating a new effector
  // ******************************************************************************************
  void EndEffectorsWidget::showNewScreen()
  {
    // Remember that this is a new effector
    current_edit_effector_.clear();
  
    // Clear previous data
    effector_name_field_->setText("");
    parent_name_field_->clearEditText();
    group_name_field_->clearEditText();         // actually this just chooses first option
    parent_group_name_field_->clearEditText();  // actually this just chooses first option
  
    // Switch to screen
    stacked_layout_->setCurrentIndex(1);
  
    // Announce that this widget is in modal mode
    Q_EMIT isModal(true);
  }
  
  // ******************************************************************************************
  // Edit whatever element is selected
  // ******************************************************************************************
  void EndEffectorsWidget::editDoubleClicked(int row, int column)
  {
    editSelected();
  }
  
  // ******************************************************************************************
  // Preview whatever element is selected
  // ******************************************************************************************
  void EndEffectorsWidget::previewClicked(int row, int column)
  {
    // Get list of all selected items
    QList<QTableWidgetItem *> selected = data_table_->selectedItems();
  
    // Check that an element was selected
    if (!selected.size())
      return;
  
    // Find the selected in datastructure
    srdf::Model::EndEffector *effector = findEffectorByName(selected[0]->text().toStdString());
  
    // Unhighlight all links
    Q_EMIT unhighlightAll();
  
    // Highlight group
    Q_EMIT highlightGroup(effector->component_group_);
  }
  
  // ******************************************************************************************
  // Preview the planning group that is selected
  // ******************************************************************************************
  void EndEffectorsWidget::previewClickedString(const QString &name)
  {
    // Don't highlight if we are on the overview end effectors screen. we are just populating drop down box
    if (stacked_layout_->currentIndex() == 0)
      return;
  
    // Unhighlight all links
    Q_EMIT unhighlightAll();
  
    // Highlight group
    Q_EMIT highlightGroup(name.toStdString());
  }
  
  // ******************************************************************************************
  // Edit whatever element is selected
  // ******************************************************************************************
  void EndEffectorsWidget::editSelected()
  {
    // Get list of all selected items
    QList<QTableWidgetItem *> selected = data_table_->selectedItems();
  
    // Check that an element was selected
    if (!selected.size())
      return;
  
    // Get selected name and edit it
    edit(selected[0]->text().toStdString());
  }
  
  // ******************************************************************************************
  // Edit effector
  // ******************************************************************************************
  void EndEffectorsWidget::edit(const std::string &name)
  {
    // Remember what we are editing
    current_edit_effector_ = name;
  
    // Find the selected in datastruture
    srdf::Model::EndEffector *effector = findEffectorByName(name);
  
    // Set effector name
    effector_name_field_->setText(effector->name_.c_str());
  
    // Set effector parent link
    int index = parent_name_field_->findText(effector->parent_link_.c_str());
    if (index == -1)
    {
      QMessageBox::critical(this, "Error Loading", "Unable to find parent link in drop down box");
      return;
    }
    parent_name_field_->setCurrentIndex(index);
  
    // Set group:
    index = group_name_field_->findText(effector->component_group_.c_str());
    if (index == -1)
    {
      QMessageBox::critical(this, "Error Loading", "Unable to find group name in drop down box");
      return;
    }
    group_name_field_->setCurrentIndex(index);
  
    // Set parent group:
    index = parent_group_name_field_->findText(effector->parent_group_.c_str());
    if (index == -1)
    {
      QMessageBox::critical(this, "Error Loading", "Unable to find parent group name in drop down box");
      return;
    }
    parent_group_name_field_->setCurrentIndex(index);
  
    // Switch to screen
    stacked_layout_->setCurrentIndex(1);
  
    // Announce that this widget is in modal mode
    Q_EMIT isModal(true);
  }
  
  // ******************************************************************************************
  // Populate the combo dropdown box with avail group names
  // ******************************************************************************************
  void EndEffectorsWidget::loadGroupsComboBox()
  {
    // Remove all old groups
    group_name_field_->clear();
    parent_group_name_field_->clear();
    parent_group_name_field_->addItem("");  // optional setting
  
    // Add all group names to combo box
    for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
         group_it != config_data_->srdf_->groups_.end(); ++group_it)
    {
      group_name_field_->addItem(group_it->name_.c_str());
      parent_group_name_field_->addItem(group_it->name_.c_str());
    }
  }
  
  // ******************************************************************************************
  // Populate the combo dropdown box with avail parent links
  // ******************************************************************************************
  void EndEffectorsWidget::loadParentComboBox()
  {
    // Remove all old groups
    parent_name_field_->clear();
  
    // Get all links in robot model
    std::vector<const robot_model::LinkModel *> link_models = config_data_->getRobotModel()->getLinkModels();
  
    // Add all links to combo box
    for (std::vector<const robot_model::LinkModel *>::const_iterator link_it = link_models.begin();
         link_it < link_models.end(); ++link_it)
    {
      parent_name_field_->addItem((*link_it)->getName().c_str());
    }
  }
  
  // ******************************************************************************************
  // Find the associated data by name
  // ******************************************************************************************
  srdf::Model::EndEffector *EndEffectorsWidget::findEffectorByName(const std::string &name)
  {
    // Find the group state we are editing based on the effector name
    srdf::Model::EndEffector *searched_group = NULL;  // used for holding our search results
  
    for (std::vector<srdf::Model::EndEffector>::iterator effector_it = config_data_->srdf_->end_effectors_.begin();
         effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it)
    {
      if (effector_it->name_ == name)  // string match
      {
        searched_group = &(*effector_it);  // convert to pointer from iterator
        break;                             // we are done searching
      }
    }
  
    // Check if effector was found
    if (searched_group == NULL)  // not found
    {
      QMessageBox::critical(this, "Error Saving", "An internal error has occured while saving. Quitting.");
      QApplication::quit();
    }
  
    return searched_group;
  }
  
  // ******************************************************************************************
  // Delete currently editing item
  // ******************************************************************************************
  void EndEffectorsWidget::deleteSelected()
  {
    // Get list of all selected items
    QList<QTableWidgetItem *> selected = data_table_->selectedItems();
  
    // Check that an element was selected
    if (!selected.size())
      return;
  
    // Get selected name and edit it
    current_edit_effector_ = selected[0]->text().toStdString();
  
    // Confirm user wants to delete group
    if (QMessageBox::question(this, "Confirm End Effector Deletion",
                              QString("Are you sure you want to delete the end effector '")
                                  .append(current_edit_effector_.c_str())
                                  .append("'?"),
                              QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
    {
      return;
    }
  
    // Delete effector from vector
    for (std::vector<srdf::Model::EndEffector>::iterator effector_it = config_data_->srdf_->end_effectors_.begin();
         effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it)
    {
      // check if this is the group we want to delete
      if (effector_it->name_ == current_edit_effector_)  // string match
      {
        config_data_->srdf_->end_effectors_.erase(effector_it);
        break;
      }
    }
  
    // Reload main screen table
    loadDataTable();
    config_data_->changes |= MoveItConfigData::END_EFFECTORS;
  }
  
  // ******************************************************************************************
  // Save editing changes
  // ******************************************************************************************
  void EndEffectorsWidget::doneEditing()
  {
    // Get a reference to the supplied strings
    const std::string effector_name = effector_name_field_->text().toStdString();
  
    // Used for editing existing groups
    srdf::Model::EndEffector *searched_data = NULL;
  
    // Check that name field is not empty
    if (effector_name.empty())
    {
      QMessageBox::warning(this, "Error Saving", "A name must be specified for the end effector!");
      return;
    }
  
    // Check if this is an existing group
    if (!current_edit_effector_.empty())
    {
      // Find the group we are editing based on the goup name string
      searched_data = findEffectorByName(current_edit_effector_);
    }
  
    // Check that the effector name is unique
    for (std::vector<srdf::Model::EndEffector>::const_iterator data_it = config_data_->srdf_->end_effectors_.begin();
         data_it != config_data_->srdf_->end_effectors_.end(); ++data_it)
    {
      if (data_it->name_.compare(effector_name) == 0)  // the names are the same
      {
        // is this our existing effector? check if effector pointers are same
        if (&(*data_it) != searched_data)
        {
          QMessageBox::warning(
              this, "Error Saving",
              QString("An end-effector named '").append(effector_name.c_str()).append("'already exists!"));
          return;
        }
      }
    }
  
    // Check that a group was selected
    if (group_name_field_->currentText().isEmpty())
    {
      QMessageBox::warning(this, "Error Saving", "A group that contains the links of the end-effector must be chosen!");
      return;
    }
  
    // Check that a parent link was selected
    if (parent_name_field_->currentText().isEmpty())
    {
      QMessageBox::warning(this, "Error Saving", "A parent link must be chosen!");
      return;
    }
  
    const robot_model::JointModelGroup *jmg =
        config_data_->getRobotModel()->getJointModelGroup(group_name_field_->currentText().toStdString());
    /*
    if (jmg->hasLinkModel(parent_name_field_->currentText().toStdString()))
    {
      QMessageBox::warning( this, "Error Saving", QString::fromStdString("Group " +
    group_name_field_->currentText().toStdString() + " contains the link " +
    parent_name_field_->currentText().toStdString() + ". However, the parent link of the end-effector should not belong to
    the group for the end-effector itself."));
      return;
    }
    */
    if (!parent_group_name_field_->currentText().isEmpty())
    {
      jmg = config_data_->getRobotModel()->getJointModelGroup(parent_group_name_field_->currentText().toStdString());
      if (!jmg->hasLinkModel(parent_name_field_->currentText().toStdString()))
      {
        QMessageBox::warning(this, "Error Saving",
                             QString::fromStdString("The specified parent group '" +
                                                    parent_group_name_field_->currentText().toStdString() +
                                                    "' must contain the specified parent link '" +
                                                    parent_name_field_->currentText().toStdString() + "'."));
        return;
      }
    }
  
    config_data_->changes |= MoveItConfigData::END_EFFECTORS;
  
    // Save the new effector name or create the new effector ----------------------------
    bool isNew = false;
  
    if (searched_data == NULL)  // create new
    {
      isNew = true;
  
      searched_data = new srdf::Model::EndEffector();
    }
  
    // Copy name data ----------------------------------------------------
    searched_data->name_ = effector_name;
    searched_data->parent_link_ = parent_name_field_->currentText().toStdString();
    searched_data->component_group_ = group_name_field_->currentText().toStdString();
    searched_data->parent_group_ = parent_group_name_field_->currentText().toStdString();
  
    // Insert new effectors into group state vector --------------------------
    if (isNew)
    {
      config_data_->srdf_->end_effectors_.push_back(*searched_data);
    }
  
    // Finish up ------------------------------------------------------
  
    // Reload main screen table
    loadDataTable();
  
    // Switch to screen
    stacked_layout_->setCurrentIndex(0);
  
    // Announce that this widget is not in modal mode
    Q_EMIT isModal(false);
  }
  
  // ******************************************************************************************
  // Cancel changes
  // ******************************************************************************************
  void EndEffectorsWidget::cancelEditing()
  {
    // Switch to screen
    stacked_layout_->setCurrentIndex(0);
  
    // Re-highlight the currently selected end effector group
    previewClicked(0, 0);  // the number parameters are actually meaningless
  
    // Announce that this widget is not in modal mode
    Q_EMIT isModal(false);
  }
  
  // ******************************************************************************************
  // Load the end effectors into the table
  // ******************************************************************************************
  void EndEffectorsWidget::loadDataTable()
  {
    // Disable Table
    data_table_->setUpdatesEnabled(false);  // prevent table from updating until we are completely done
    data_table_->setDisabled(true);         // make sure we disable it so that the cellChanged event is not called
    data_table_->clearContents();
  
    // Set size of datatable
    data_table_->setRowCount(config_data_->srdf_->end_effectors_.size());
  
    // Loop through every end effector
    int row = 0;
    for (std::vector<srdf::Model::EndEffector>::const_iterator data_it = config_data_->srdf_->end_effectors_.begin();
         data_it != config_data_->srdf_->end_effectors_.end(); ++data_it)
    {
      // Create row elements
      QTableWidgetItem *data_name = new QTableWidgetItem(data_it->name_.c_str());
      data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
      QTableWidgetItem *group_name = new QTableWidgetItem(data_it->component_group_.c_str());
      group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
      QTableWidgetItem *parent_name = new QTableWidgetItem(data_it->parent_link_.c_str());
      group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
      QTableWidgetItem *parent_group_name = new QTableWidgetItem(data_it->parent_group_.c_str());
      parent_group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
  
      // Add to table
      data_table_->setItem(row, 0, data_name);
      data_table_->setItem(row, 1, group_name);
      data_table_->setItem(row, 2, parent_name);
      data_table_->setItem(row, 3, parent_group_name);
  
      // Increment counter
      ++row;
    }
  
    // Reenable
    data_table_->setUpdatesEnabled(true);  // prevent table from updating until we are completely done
    data_table_->setDisabled(false);       // make sure we disable it so that the cellChanged event is not called
  
    // Resize header
    data_table_->resizeColumnToContents(0);
    data_table_->resizeColumnToContents(1);
    data_table_->resizeColumnToContents(2);
    data_table_->resizeColumnToContents(3);
  
    // Show edit button if applicable
    if (config_data_->srdf_->end_effectors_.size() > 0)
      btn_edit_->show();
  }
  
  // ******************************************************************************************
  // Called when setup assistant navigation switches to this screen
  // ******************************************************************************************
  void EndEffectorsWidget::focusGiven()
  {
    // Show the current effectors screen
    stacked_layout_->setCurrentIndex(0);
  
    // Load the data to the tree
    loadDataTable();
  
    // Load the avail groups to the combo box
    loadGroupsComboBox();
    loadParentComboBox();
  }
  
  }  // namespace
  