source: finroc_plugins_composite_ports/tInterfaceBase.cpp @ 10:475d7a686fd6

//
// You received this file as part of Finroc
// A framework for intelligent robot control
//
// Copyright (C) Finroc GbR (finroc.org)
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program 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, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//----------------------------------------------------------------------
/*!\file    plugins/composite_ports/tInterfaceBase.cpp
 *
 * \author  Max Reichardt
 *
 * \date    2020-01-14
 *
 */
//----------------------------------------------------------------------
#include "plugins/composite_ports/tInterfaceBase.h"

//----------------------------------------------------------------------
// External includes (system with <>, local with "")
//----------------------------------------------------------------------
#include "core/tFrameworkElementTags.h"
#include "core/tRuntimeEnvironment.h"
#include "rrlib/util/string.h"

//----------------------------------------------------------------------
// Internal includes with ""
//----------------------------------------------------------------------
#include "plugins/composite_ports/internal/tInterfaceTypeInfo.h"
#include "plugins/composite_ports/internal/type_traits.h"

//----------------------------------------------------------------------
// Debugging
//----------------------------------------------------------------------
#include <cassert>

//----------------------------------------------------------------------
// Namespace usage
//----------------------------------------------------------------------

//----------------------------------------------------------------------
// Namespace declaration
//----------------------------------------------------------------------
namespace finroc
{
namespace composite_ports
{

//----------------------------------------------------------------------
// Forward declarations / typedefs / enums
//----------------------------------------------------------------------
typedef core::tFrameworkElementFlag tFlag;

//----------------------------------------------------------------------
// Const values
//----------------------------------------------------------------------
const std::string cINTERFACE_TAG_SINGLE_BACKEND_INSTANTIATION = "finroc.composite_ports.single_backend_instantiation";

//----------------------------------------------------------------------
// Implementation
//----------------------------------------------------------------------

namespace
{

const core::tFrameworkElement* GetAggregator(const core::tFrameworkElement& element, core::tFrameworkElementFlags* extend_with_default_ports_flags = nullptr)
{
  if (element.GetFlag(tFlag::EDGE_AGGREGATOR) && (!element.IsPort()))
  {
    if (element.GetFlag(tFlag::INTERFACE) && extend_with_default_ports_flags)
    {
      (*extend_with_default_ports_flags) |= static_cast<const core::tPortGroup&>(element).GetDefaultPortFlags();
      if (element.GetFlag(tFlag::SENSOR_DATA))
      {
        (*extend_with_default_ports_flags) |= tFlag::SENSOR_DATA;
      }
      if (element.GetFlag(tFlag::CONTROLLER_DATA))
      {
        (*extend_with_default_ports_flags) |= tFlag::CONTROLLER_DATA;
      }
      if (element.GetFlag(tFlag::INTERFACE_FOR_INPUTS) || element.GetFlag(tFlag::PARAMETER_INTERFACE))
      {
        (*extend_with_default_ports_flags) |= data_ports::cDEFAULT_INPUT_PORT_FLAGS;
      }
      if (element.GetFlag(tFlag::INTERFACE_FOR_OUTPUTS))
      {
        (*extend_with_default_ports_flags) |= data_ports::cDEFAULT_OUTPUT_PORT_FLAGS;
      }
      if (element.GetFlag(tFlag::PROXY_INTERFACE))
      {
        (*extend_with_default_ports_flags) |= tFlag::EMITS_DATA | tFlag::ACCEPTS_DATA;
      }
    }
    return &element;
  }
  return element.GetParent() ? GetAggregator(*element.GetParent(), extend_with_default_ports_flags) : nullptr;
}

core::tAbstractPortCreationInfo MakeCreationInfo(const rrlib::rtti::tType& interface_type, const std::string& name, core::tFrameworkElement& parent_interface)
{
  core::tAbstractPortCreationInfo result;
  result.data_type = interface_type;
  result.parent = &parent_interface;
  result.flags |= tFlag::INTERFACE;
  GetAggregator(parent_interface, &result.flags);
  result.name = name;
  assert(result.flags.Get(tFlag::EMITS_DATA) || result.flags.Get(tFlag::ACCEPTS_DATA));
  return result;
}

//void ReplaceAny(std::string& s, const std::string& seach_string, const std::string& replace_with)  // TODO
//{
//  auto index = s.find(seach_string);
//  while (index != std::string::npos)
//  {
//    s.replace(index, seach_string.length(), replace_with);
//    index = s.find(seach_string);
//  }
//}

}

void tInterfaceBase::operator()(core::tAbstractPortCreationInfo& creation_info) const
{
  if ((!Backend()) || (Backend() && Backend()->convenience_port_type))
  {
    creation_info.flags |= tFlag::READY; // tComponent::cNO_PORT_NAME_BUILDER;
  }
  if (!Backend())
  {
    creation_info.flags |= tFlag::DELETED;
    return;
  }
  creation_info.parent = &Backend()->primary_backend;
  return;
}


tInterfaceBase::tBackend::tBackend(const rrlib::rtti::tType& interface_type, core::tFrameworkElement* parent_component, core::tFrameworkElement* parent, int primary_relation_id, const std::string& name, bool convenience_port_type, const tConnectFunction& custom_connect_function, tCreateMissingComponentInterfaceFunction create_missing_component_interface_function) :
  tAbstractPort(MakeCreationInfo(interface_type, name, *parent)),
  parent_component(parent_component),
  primary_backend(*this),
  custom_connect_function(custom_connect_function),
  convenience_port_type(convenience_port_type),
  primary_relation_id(primary_relation_id),
  create_missing_component_interface_function(create_missing_component_interface_function)
{
  all_backends.emplace_back(this);
  relation_backend_mapping.emplace_back(primary_relation_id, this);
}

tInterfaceBase::tBackend::tBackend(core::tAbstractPortCreationInfo& creation_info, tBackend& primary) :
  tAbstractPort(creation_info),
  primary_backend(primary),
  convenience_port_type(false),
  primary_relation_id(primary.primary_relation_id),
  create_missing_component_interface_function(nullptr)
{
}

void tInterfaceBase::tBackend::ConnectChildPortsByName(tBackend& destination, const core::tConnectOptions& connect_options)
{
  for (auto port = this->ChildPortsBegin(); port != this->ChildPortsEnd(); ++port)
  {
    auto other_port = destination.GetChild(port->GetName());
    if (other_port && other_port->IsPort())
    {
      port->ConnectTo(static_cast<core::tAbstractPort&>(*other_port), connect_options);
    }
  }
}

tInterfaceBase::tBackend* tInterfaceBase::tBackend::GetBackend(int primary_relation_id)
{
  if (&primary_backend != this)
  {
    return primary_backend.GetBackend(primary_relation_id);
  }

  for (auto & entry : relation_backend_mapping)
  {
    if (entry.first == primary_relation_id)
    {
      return entry.second;
    }
  }

  core::tFrameworkElement* parent = nullptr;
  if (GetParent() && typeid(*GetParent()).name() == typeid(tBackend).name())
  {
    parent = static_cast<tBackend&>(*GetParent()).GetBackend(primary_relation_id);
  }
  else if (core::tFrameworkElementTags::IsTagged(*this->GetParent(), cINTERFACE_TAG_SINGLE_BACKEND_INSTANTIATION))
  {
    parent = this->GetParent();
  }
  else
  {
    // Actually process relation
    std::vector<int> relation_list;
    tFrameworkElement* component_interface = this->GetParent();
    int relation_id = primary_relation_id;
    if ((!component_interface) || (!internal::IsComponent(component_interface->GetParent())))
    {
      FINROC_LOG_PRINT(WARNING, "Generic creation of port composite interfaces with multiple port types only works on components");
      throw std::runtime_error("Generic creation of port composite interfaces with multiple port types only works on components");
    }

    const core::tFrameworkElementFlags relevant_flags = tFlag::SENSOR_DATA | tFlag::CONTROLLER_DATA | tFlag::INTERFACE_FOR_INPUTS | tFlag::INTERFACE_FOR_OUTPUTS | tFlag::PROXY_INTERFACE | tFlag::PARAMETER_INTERFACE;

    // Compute opcode sequence
    {
      while (relation_id)
      {
        int opcode = relation_id & 0xFF;
        relation_list.push_back(opcode);
        relation_id = relation_id >> 8;
      }
      std::reverse(relation_list.begin(), relation_list.end());
    }

    for (int opcode : relation_list)
    {
      core::tFrameworkElementFlags original_flags = component_interface->GetAllFlags();
      core::tFrameworkElementFlags target_set_flags = core::tFrameworkElementFlags(original_flags.Raw() & relevant_flags.Raw());
      core::tFrameworkElementFlags target_unset_flags = core::tFrameworkElementFlags((~original_flags.Raw()) & relevant_flags.Raw());

      typedef internal::tPortTypeRelation tOpCode;
      switch (opcode)
      {
      case tOpCode::ePTR_COUNTER_PART:
      case tOpCode::ePTR_COUNTER_PART_PARAMETER_IF_INPUT:
        if (original_flags.Get(tFlag::CONTROLLER_DATA) && (!original_flags.Get(tFlag::SENSOR_DATA)))
        {
          target_set_flags.Set(tFlag::SENSOR_DATA);
          target_set_flags.Set(tFlag::CONTROLLER_DATA, false);
          target_unset_flags.Set(tFlag::CONTROLLER_DATA);
          target_unset_flags.Set(tFlag::SENSOR_DATA, false);
        }
        if (original_flags.Get(tFlag::SENSOR_DATA) && (!original_flags.Get(tFlag::CONTROLLER_DATA)))
        {
          target_set_flags.Set(tFlag::CONTROLLER_DATA);
          target_set_flags.Set(tFlag::SENSOR_DATA, false);
          target_unset_flags.Set(tFlag::SENSOR_DATA);
          target_unset_flags.Set(tFlag::CONTROLLER_DATA, false);
        }
        if (original_flags.Get(tFlag::INTERFACE_FOR_INPUTS) && (!original_flags.Get(tFlag::INTERFACE_FOR_OUTPUTS)))
        {
          target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
          target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
          target_set_flags.Set(tFlag::PARAMETER_INTERFACE, false);
          target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
          target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
        }
        if (original_flags.Get(tFlag::INTERFACE_FOR_OUTPUTS) && (!original_flags.Get(tFlag::INTERFACE_FOR_INPUTS)))
        {
          if (opcode == tOpCode::ePTR_COUNTER_PART_PARAMETER_IF_INPUT)
          {
            target_set_flags = tFlag::PARAMETER_INTERFACE;
            target_unset_flags = core::tFrameworkElementFlags();
          }
          else
          {
            target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
            target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
            target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
            target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
          }
        }
        break;
      case internal::tPortTypeRelation::ePTR_INPUT:
        target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
        target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
        target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
        target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
        break;
      case internal::tPortTypeRelation::ePTR_OUTPUT:
        target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
        target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
        target_set_flags.Set(tFlag::PARAMETER_INTERFACE, false);
        target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
        target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
        break;
      case internal::tPortTypeRelation::ePTR_PARAMETER:
        target_set_flags = tFlag::PARAMETER_INTERFACE;
        target_unset_flags = core::tFrameworkElementFlags();
        break;
      default:
        break;
      }

      core::tFrameworkElement* new_candidate = nullptr;
      size_t candidate_name_length_difference = std::numeric_limits<size_t>::max();
      auto component = component_interface->GetParent();
      for (auto it = component->ChildrenBegin(); it != component->ChildrenEnd(); ++it)
      {
        if ((!it->IsPort()) && it->GetFlag(tFlag::INTERFACE) && (it->GetAllFlags().Raw() & target_set_flags.Raw()) == target_set_flags.Raw() && (it->GetAllFlags().Raw() & target_unset_flags.Raw()) == 0)
        {
          size_t name_length_difference = std::abs(static_cast<long>(component_interface->GetName().length()) - static_cast<long>(it->GetName().length()));
          if (name_length_difference < candidate_name_length_difference)
          {
            new_candidate = &*it;
            candidate_name_length_difference = name_length_difference;
          }
        }
      }

      // Not-yet-created component interfaces - until there is a generic way to do this
      if ((!new_candidate) && create_missing_component_interface_function)
      {
        new_candidate = (*create_missing_component_interface_function)(component_interface->GetParent(), target_set_flags);
      }

      if (new_candidate)
      {
        component_interface = new_candidate;
      }
      else
      {
        std::stringstream message;
        message << "Generic creation of port composite interface failed: No interface with relation " << make_builder::GetEnumString(static_cast<tOpCode>(opcode)) << " found below " << *component_interface->GetParent();
        FINROC_LOG_PRINT(WARNING, message.str());
        throw std::runtime_error(message.str());
      }
    }

    parent = component_interface;
  }

  for (auto & entry : relation_backend_mapping)
  {
    if (entry.second->GetParent() == parent)
    {
      auto backend = entry.second;
      relation_backend_mapping.emplace_back(primary_relation_id, backend);
      return backend;
    }
  }

  // Create new backend below this parent
  core::tAbstractPortCreationInfo secondary_creation_info;
  secondary_creation_info.data_type = this->GetDataType();
  secondary_creation_info.name = GetName();
  secondary_creation_info.parent = parent;
  secondary_creation_info.flags = core::tFrameworkElementFlags(tFlag::INTERFACE);
  GetAggregator(*parent, &secondary_creation_info.flags);
  all_backends.push_back(new tBackend(secondary_creation_info, *this));
  relation_backend_mapping.emplace_back(primary_relation_id, all_backends.back());
  return all_backends.back();
}

core::tFrameworkElementFlags tInterfaceBase::tBackend::GetDefaultPortFlags(bool generic_port_backend) const
{
  core::tFrameworkElementFlags result;
  if (generic_port_backend)
  {
    GetAggregator(*this, &result);
  }
  else
  {
    auto aggregator = GetAggregator(*this);
    if (aggregator && aggregator->GetFlag(tFlag::INTERFACE))
    {
      if (aggregator->GetFlag(tFlag::PROXY_INTERFACE))
      {
        result |= (tFlag::ACCEPTS_DATA | tFlag::EMITS_DATA | tFlag::PUSH_STRATEGY);
      }
      result |= static_cast<const core::tPortGroup&>(*aggregator).GetDefaultPortFlags();
    }
    const int cRELEVANT_FLAGS = (tFlag::SHARED | tFlag::SENSOR_DATA_PORT | tFlag::CONTROLLER_DATA_PORT).Raw();
    result |= core::tFrameworkElementFlags(this->GetAllFlags().Raw() & cRELEVANT_FLAGS);
  }
  return result;
}

void tInterfaceBase::tBackend::OnConnect(tAbstractPort& partner, bool partner_is_destination)
{
  if (primary_backend.operation_on_all_elements_pending)
  {
    return;
  }

  tBackend& other = static_cast<tBackend&>(partner);
  bool connect_all_backends = this->IsPrimaryBackend() && other.IsPrimaryBackend();

  primary_backend.operation_on_all_elements_pending = true;
  try
  {
    if (primary_backend.custom_connect_function)
    {
      if (partner_is_destination)
      {
        primary_backend.custom_connect_function(*this, other);
      }
      else
      {
        primary_backend.custom_connect_function(other, *this);
      }
      primary_backend.operation_on_all_elements_pending = false;
      return;
    }

    // Connect all matching backends and ports
    for (auto this_backend : primary_backend.relation_backend_mapping)
    {
      for (auto other_backend : other.primary_backend.primary_backend.relation_backend_mapping)
      {
        if (this_backend.first - primary_backend.primary_relation_id == other_backend.first - other.primary_backend.primary_relation_id && (connect_all_backends || this_backend.second == this || other_backend.second == &other))
        {
          core::tConnectionFlag direction = (this->IsOutputPort() == this_backend.second->IsOutputPort() ? partner_is_destination : (!partner_is_destination)) ? core::tConnectionFlag::DIRECTION_TO_DESTINATION : core::tConnectionFlag::DIRECTION_TO_SOURCE;
          this_backend.second->ConnectTo(*other_backend.second, direction);
          this_backend.second->ConnectChildPortsByName(*other_backend.second, direction);
        }
      }
    }
  }
  catch (const std::exception& e)
  {
    FINROC_LOG_PRINT(WARNING, "Connecting failed: ", e);
  }
  primary_backend.operation_on_all_elements_pending = false;
}

void tInterfaceBase::tBackend::OnDisconnect(tAbstractPort& partner, bool partner_is_destination)
{
  bool& pending = primary_backend.primary_backend.operation_on_all_elements_pending;
  tBackend& other = static_cast<tBackend&>(partner).primary_backend;
  if (pending || other.operation_on_all_elements_pending || partner_is_destination)
  {
    return;
  }

  pending = true;
  try
  {
    // Update other backend connection info
    for (auto this_backend : this->primary_backend.all_backends)
    {
      for (auto other_backend : other.primary_backend.all_backends)
      {
        this_backend->DisconnectFrom(*other_backend);
      }
    }

    // Disconnect all ports to partner backends (this should also cover cases with custom connect function)
    for (auto backend : primary_backend.all_backends)
    {
      for (auto port = backend->ChildPortsBegin(); port != backend->ChildPortsEnd(); ++port)
      {
        for (auto partner = port->IncomingConnectionsBegin(); partner != port->IncomingConnectionsEnd(); ++partner)
        {
          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Source().GetParent()))
          {
            port->DisconnectFrom(partner->Source());
          }
        }
        for (auto partner = port->OutgoingConnectionsBegin(); partner != port->OutgoingConnectionsEnd(); ++partner)
        {
          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Destination().GetParent()))
          {
            port->DisconnectFrom(partner->Destination());
          }
        }
      }
    }
  }
  catch (const std::exception& e)
  {
    FINROC_LOG_PRINT(WARNING, "Disconnecting failed: ", e);
  }
  pending = false;
}

void tInterfaceBase::tBackend::OnInitialization()
{
  bool& pending = primary_backend.operation_on_all_elements_pending;
  if (!pending)
  {
    pending = true;
    for (auto backend : primary_backend.all_backends)
    {
      if (backend != this)
      {
        backend->Init();
      }
    }
    pending = false;
  }
}

void tInterfaceBase::tBackend::OnManagedDelete()
{
  rrlib::thread::tLock lock1(GetStructureMutex());

  if (!core::tRuntimeEnvironment::ShuttingDown())
  {
    bool& pending = primary_backend.operation_on_all_elements_pending;
    if (!pending)
    {
      pending = true;
      for (auto backend : primary_backend.all_backends)
      {
        if (backend != this)
        {
          backend->ManagedDelete();
        }
      }
      pending = false;
    }
  }

  tAbstractPort::OnManagedDelete();
}


//----------------------------------------------------------------------
// End of namespace declaration
//----------------------------------------------------------------------
}
}