text: // License: Apache 2.0. See LICENSE file in root directory.
  // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
  
  #include "environment.h"
  #include "device.h"
  
  using namespace librealsense;
  
  std::shared_ptr<matcher> matcher_factory::create(rs2_matchers matcher, std::vector<stream_interface*> profiles)
  {
      switch (matcher)
      {
      case RS2_MATCHER_DI:
          return create_DI_matcher(profiles);
      case RS2_MATCHER_DI_C:
          return create_DI_C_matcher(profiles);
      case RS2_MATCHER_DLR_C:
          return create_DLR_C_matcher(profiles);
      case RS2_MATCHER_DLR:
          return create_DLR_matcher(profiles);
      case RS2_MATCHER_DEFAULT:default:
          LOG_DEBUG("Created default matcher");
          return create_timestamp_matcher(profiles);
          break;
      }
  }
  stream_interface* librealsense::find_profile(rs2_stream stream, int index, std::vector<stream_interface*> profiles)
  {
      auto prof = std::find_if(profiles.begin(), profiles.end(), [&](stream_interface* profile)
      {
          return profile->get_stream_type() == stream && profile->get_stream_index() == index;
      });
  
      if (prof != profiles.end())
          return *prof;
      else
          return nullptr;
  }
  
  std::shared_ptr<matcher> matcher_factory::create_DLR_C_matcher(std::vector<stream_interface*> profiles)
  {
      auto color  = find_profile(RS2_STREAM_COLOR, 0, profiles);
      if (!color)
      {
          LOG_DEBUG("Created default matcher");
          return create_timestamp_matcher(profiles);
      }
  
      return create_timestamp_composite_matcher({ create_DLR_matcher(profiles),
          create_identity_matcher(color) });
  }
  
  std::shared_ptr<matcher> matcher_factory::create_DI_C_matcher(std::vector<stream_interface*> profiles)
  {
      auto color = find_profile(RS2_STREAM_COLOR, 0, profiles);
      if (!color)
      {
          LOG_DEBUG("Created default matcher");
          return create_timestamp_matcher(profiles);
      }
  
      return create_timestamp_composite_matcher({ create_DI_matcher(profiles),
          create_identity_matcher(profiles[2]) });
  }
  
  std::shared_ptr<matcher> matcher_factory::create_DLR_matcher(std::vector<stream_interface*> profiles)
  {
      auto depth = find_profile(RS2_STREAM_DEPTH, 0, profiles);
      auto left = find_profile(RS2_STREAM_INFRARED, 1, profiles);
      auto right = find_profile(RS2_STREAM_INFRARED, 2, profiles);
  
      if (!depth || !left || !right)
      {
          LOG_DEBUG("Created default matcher");
          return create_timestamp_matcher(profiles);
      }
      return create_frame_number_matcher({ depth , left , right });
  }
  
  std::shared_ptr<matcher> matcher_factory::create_DI_matcher(std::vector<stream_interface*> profiles)
  {
      auto depth = find_profile(RS2_STREAM_DEPTH, 0, profiles);
      auto ir = find_profile(RS2_STREAM_INFRARED, 1, profiles);
  
      if (!depth || !ir)
      {
          LOG_DEBUG("Created default matcher");
          return create_timestamp_matcher(profiles);
      }
      return create_frame_number_matcher({ depth , ir });
  }
  
  std::shared_ptr<matcher> matcher_factory::create_frame_number_matcher(std::vector<stream_interface*> profiles)
  {
      std::vector<std::shared_ptr<matcher>> matchers;
      for (auto& p : profiles)
          matchers.push_back(std::make_shared<identity_matcher>(p->get_unique_id(), p->get_stream_type()));
  
      return create_frame_number_composite_matcher(matchers);
  }
  std::shared_ptr<matcher> matcher_factory::create_timestamp_matcher(std::vector<stream_interface*> profiles)
  {
      std::vector<std::shared_ptr<matcher>> matchers;
      for (auto& p : profiles)
          matchers.push_back(std::make_shared<identity_matcher>(p->get_unique_id(), p->get_stream_type()));
  
      return create_timestamp_composite_matcher(matchers);
  }
  
  std::shared_ptr<matcher> matcher_factory::create_identity_matcher(stream_interface *profile)
  {
      return std::make_shared<identity_matcher>(profile->get_unique_id(), profile->get_stream_type());
  }
  
  std::shared_ptr<matcher> matcher_factory::create_frame_number_composite_matcher(std::vector<std::shared_ptr<matcher>> matchers)
  {
      return std::make_shared<frame_number_composite_matcher>(matchers);
  }
  std::shared_ptr<matcher> matcher_factory::create_timestamp_composite_matcher(std::vector<std::shared_ptr<matcher>> matchers)
  {
      return std::make_shared<timestamp_composite_matcher>(matchers);
  }
  
  device::device(std::shared_ptr<context> ctx,
                 const platform::backend_device_group group,
                 bool device_changed_notifications)
      : _context(ctx), _group(group), _is_valid(true),
        _device_changed_notifications(device_changed_notifications)
  {
      _profiles_tags = lazy<std::vector<tagged_profile>>([this]() { return get_profiles_tags(); });
  
      if (_device_changed_notifications)
      {
          auto cb = new devices_changed_callback_internal([this](rs2_device_list* removed, rs2_device_list* added)
          {
              // Update is_valid variable when device is invalid
              std::lock_guard<std::mutex> lock(_device_changed_mtx);
              for (auto& dev_info : removed->list)
              {
                  if (dev_info.info->get_device_data() == _group)
                  {
                      _is_valid = false;
                      return;
                  }
              }
          });
  
          _callback_id = _context->register_internal_device_callback({ cb, [](rs2_devices_changed_callback* p) { p->release(); } });
      }
  }
  
  device::~device()
  {
      if (_device_changed_notifications)
      {
          _context->unregister_internal_device_callback(_callback_id);
      }
      _sensors.clear();
  }
  
  int device::add_sensor(std::shared_ptr<sensor_interface> sensor_base)
  {
      _sensors.push_back(sensor_base);
      return (int)_sensors.size() - 1;
  }
  
  int device::assign_sensor(std::shared_ptr<sensor_interface> sensor_base, uint8_t idx)
  {
      try
      {
          _sensors[idx] = sensor_base;
          return (int)_sensors.size() - 1;
      }
      catch (std::out_of_range)
      {
          throw invalid_value_exception(to_string() << "Cannot assign sensor - invalid subdevice value" << idx);
      }
  }
  
  uvc_sensor& device::get_uvc_sensor(int sub)
  {
      return dynamic_cast<uvc_sensor&>(*_sensors[sub]);
  }
  
  size_t device::get_sensors_count() const
  {
      return static_cast<unsigned int>(_sensors.size());
  }
  
  sensor_interface& device::get_sensor(size_t subdevice)
  {
      try
      {
          return *(_sensors.at(subdevice));
      }
      catch (std::out_of_range)
      {
          throw invalid_value_exception("invalid subdevice value");
      }
  }
  
  size_t device::find_sensor_idx(const sensor_interface& s) const
  {
      int idx = 0;
      for (auto&& sensor : _sensors)
      {
          if (&s == sensor.get()) return idx;
          idx++;
      }
      throw std::runtime_error("Sensor not found!");
  }
  
  const sensor_interface& device::get_sensor(size_t subdevice) const
  {
      try
      {
          return *(_sensors.at(subdevice));
      }
      catch (std::out_of_range)
      {
          throw invalid_value_exception("invalid subdevice value");
      }
  }
  
  void device::hardware_reset()
  {
      throw not_implemented_exception(to_string() << __FUNCTION__ << " is not implemented for this device!");
  }
  
  std::shared_ptr<matcher> librealsense::device::create_matcher(const frame_holder& frame) const
  {
  
      return std::make_shared<identity_matcher>( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type());
  }
  
  std::pair<uint32_t, rs2_extrinsics> librealsense::device::get_extrinsics(const stream_interface& stream) const
  {
      auto stream_index = stream.get_unique_id();
      auto pair = _extrinsics.at(stream_index);
      auto pin_stream = pair.second;
      rs2_extrinsics ext{};
      if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics(*pin_stream, stream, &ext) == false)
      {
          throw std::runtime_error(to_string() << "Failed to fetch extrinsics between pin stream (" << pin_stream->get_unique_id() << ") to given stream (" << stream.get_unique_id() << ")");
      }
      return std::make_pair(pair.first, ext);
  }
  
  void librealsense::device::register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t groupd_index)
  {
      auto iter = std::find_if(_extrinsics.begin(),
                             _extrinsics.end(),
                             [groupd_index](const std::pair<int, std::pair<uint32_t, std::shared_ptr<const stream_interface>>>& p) { return p.second.first == groupd_index; });
      if (iter == _extrinsics.end())
      {
          //First stream to register for this group
          _extrinsics[stream.get_unique_id()] = std::make_pair(groupd_index, stream.shared_from_this());
      }
      else
      {
          //iter->second holds the group_id and the key stream
          _extrinsics[stream.get_unique_id()] = iter->second;
      }
  }
  
  void librealsense::device::tag_profile(stream_profile_interface* profile) const
  {
      for (auto tag : *_profiles_tags)
      {
          auto vp = dynamic_cast<video_stream_profile_interface*>(profile);
          if (vp)
          {
              if ((tag.stream == RS2_STREAM_ANY || vp->get_stream_type() == tag.stream) &&
                  (tag.format == RS2_FORMAT_ANY || vp->get_format() == tag.format) &&
                  (tag.width == -1 || vp->get_width() == tag.width) &&
                  (tag.height == -1 || vp->get_height() == tag.height) &&
                  (tag.fps == -1 || vp->get_framerate() == tag.fps) &&
                  (tag.stream_index == -1 || vp->get_stream_index() == tag.stream_index))
                  profile->tag_profile(tag.tag);
          }
      }
  }