text: #include "metadata-parser.h"
  #include "archive.h"
  #include <fstream>
  
  #define MIN_DISTANCE 1e-6
  
  namespace librealsense
  {
      std::shared_ptr<sensor_interface> frame::get_sensor() const
      {
          auto res = sensor.lock();
          if (!res)
          {
              auto archive = get_owner();
              if (archive) return archive->get_sensor();
          }
          return res;
      }
      void frame::set_sensor(std::shared_ptr<sensor_interface> s) { sensor = s;}
  
      float3* points::get_vertices()
      {
          auto xyz = (float3*)data.data();
          return xyz;
      }
  
      std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(const frame_holder& texture, float u, float v)
      {
          auto ptr = dynamic_cast<video_frame*>(texture.frame);
          if (ptr == nullptr) {
              throw librealsense::invalid_value_exception("frame must be video frame");
          }
          const int w = ptr->get_width(), h = ptr->get_height();
          int x = std::min(std::max(int(u*w + .5f), 0), w - 1);
          int y = std::min(std::max(int(v*h + .5f), 0), h - 1);
          int idx = x * ptr->get_bpp() / 8 + y * ptr->get_stride();
          const auto texture_data = reinterpret_cast<const uint8_t*>(ptr->get_frame_data());
          return std::make_tuple(texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);
      }
  
      void points::export_to_ply(const std::string& fname, const frame_holder& texture)
      {
          const auto vertices = get_vertices();
          const auto texcoords = get_texture_coordinates();
          std::vector<float3> new_vertices;
          std::vector<std::tuple<uint8_t, uint8_t, uint8_t>> new_tex;
          new_vertices.reserve(get_vertex_count());
          new_tex.reserve(get_vertex_count());
          assert(get_vertex_count());
          for (size_t i = 0; i < get_vertex_count(); ++i)
              if (fabs(vertices[i].x) >= MIN_DISTANCE || fabs(vertices[i].y) >= MIN_DISTANCE ||
                  fabs(vertices[i].z) >= MIN_DISTANCE)
              {
                  new_vertices.push_back(vertices[i]);
                  if (texture)
                  {
                      auto color = get_texcolor(texture, texcoords[i].x, texcoords[i].y);
                      new_tex.push_back(color);
                  }
              }
  
          std::ofstream out(fname);
          out << "ply\n";
          out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/;
          out << "comment pointcloud saved from Realsense Viewer\n";
          out << "element vertex " << new_vertices.size() << "\n";
          out << "property float" << sizeof(float) * 8 << " x\n";
          out << "property float" << sizeof(float) * 8 << " y\n";
          out << "property float" << sizeof(float) * 8 << " z\n";
          if (texture)
          {
              out << "property uchar red\n";
              out << "property uchar green\n";
              out << "property uchar blue\n";
          }
          out << "end_header\n";
          out.close();
  
          out.open(fname, std::ios_base::app | std::ios_base::binary);
          for (int i = 0; i < new_vertices.size(); ++i)
          {
              // we assume little endian architecture on your device
              out.write(reinterpret_cast<const char*>(&(new_vertices[i].x)), sizeof(float));
              out.write(reinterpret_cast<const char*>(&(new_vertices[i].y)), sizeof(float));
              out.write(reinterpret_cast<const char*>(&(new_vertices[i].z)), sizeof(float));
  
              if (texture)
              {
                  uint8_t x, y, z;
                  std::tie(x, y, z) = new_tex[i];
                  out.write(reinterpret_cast<const char*>(&x), sizeof(uint8_t));
                  out.write(reinterpret_cast<const char*>(&y), sizeof(uint8_t));
                  out.write(reinterpret_cast<const char*>(&z), sizeof(uint8_t));
              }
          }
      }
  
      size_t points::get_vertex_count() const
      {
          return data.size() / (sizeof(float3) + sizeof(int2));
      }
  
      float2* points::get_texture_coordinates()
      {
          auto xyz = (float3*)data.data();
          auto ijs = (float2*)(xyz + get_vertex_count());
          return ijs;
      }
  
      // Defines general frames storage model
      template<class T>
      class frame_archive : public std::enable_shared_from_this<frame_archive<T>>, public archive_interface
      {
          std::atomic<uint32_t>* max_frame_queue_size;
          std::atomic<uint32_t> published_frames_count;
          small_heap<T, RS2_USER_QUEUE_SIZE> published_frames;
  
          callbacks_heap callback_inflight;
  
          std::vector<T> freelist; // return frames here
          std::atomic<bool> recycle_frames;
          int pending_frames = 0;
          std::recursive_mutex mutex;
          std::shared_ptr<platform::time_service> _time_service;
          std::shared_ptr<metadata_parser_map> _metadata_parsers = nullptr;
  
          std::weak_ptr<sensor_interface> _sensor;
          std::shared_ptr<sensor_interface> get_sensor() const override { return _sensor.lock(); }
          void set_sensor(std::shared_ptr<sensor_interface> s) override { _sensor = s; }
  
          T alloc_frame(const size_t size, const frame_additional_data& additional_data, bool requires_memory)
          {
              T backbuffer;
              //const size_t size = modes[stream].get_image_size(stream);
              {
                  std::lock_guard<std::recursive_mutex> guard(mutex);
  
                  if (requires_memory)
                  {
                      // Attempt to obtain a buffer of the appropriate size from the freelist
                      for (auto it = begin(freelist); it != end(freelist); ++it)
                      {
                          if (it->data.size() == size)
                          {
                              backbuffer = std::move(*it);
                              freelist.erase(it);
                              break;
                          }
                      }
                  }
  
                  // Discard buffers that have been in the freelist for longer than 1s
                  for (auto it = begin(freelist); it != end(freelist);)
                  {
                      if (additional_data.timestamp > it->additional_data.timestamp + 1000) it = freelist.erase(it);
                      else ++it;
                  }
              }
  
              if (requires_memory)
              {
                  backbuffer.data.resize(size, 0); // TODO: Allow users to provide a custom allocator for frame buffers
              }
              backbuffer.additional_data = additional_data;
              return backbuffer;
          }
  
          frame_interface* track_frame(T& f)
          {
              std::unique_lock<std::recursive_mutex> lock(mutex);
  
              auto published_frame = f.publish(this->shared_from_this());
              if (published_frame)
              {
                  published_frame->acquire();
                  return published_frame;
              }
  
              LOG_DEBUG("publish(...) failed");
              return nullptr;
          }
  
          void unpublish_frame(frame_interface* frame)
          {
              if (frame)
              {
                  auto f = (T*)frame;
                  log_frame_callback_end(f);
                  std::unique_lock<std::recursive_mutex> lock(mutex);
  
                  frame->keep();
  
                  if (recycle_frames)
                  {
                      freelist.push_back(std::move(*f));
                  }
                  lock.unlock();
  
                  if (f->is_fixed())
                      published_frames.deallocate(f);
                  else
                      delete f;
              }
          }
  
          void keep_frame(frame_interface* frame)
          {
              --published_frames_count;
          }
  
          frame_interface* publish_frame(frame_interface* frame)
          {
              auto f = (T*)frame;
  
              unsigned int max_frames = *max_frame_queue_size;
  
              if (published_frames_count >= max_frames
                  && max_frames)
              {
                  LOG_DEBUG("User didn't release frame resource.");
                  return nullptr;
              }
              auto new_frame = (max_frames ? published_frames.allocate() : new T());
  
              if (new_frame)
              {
                  if (max_frames) new_frame->mark_fixed();
              }
              else
              {
                  new_frame = new T();
              }
  
              ++published_frames_count;
              *new_frame = std::move(*f);
  
              return new_frame;
          }
  
          void log_frame_callback_end(T* frame) const
          {
              if (frame && frame->get_stream())
              {
                  auto callback_ended = _time_service?_time_service->get_time():0;
                  auto callback_warning_duration = 1000 / (frame->get_stream()->get_framerate() + 1);
                  auto callback_duration = callback_ended - frame->get_frame_callback_start_time_point();
  
                  LOG_DEBUG("CallbackFinished," << librealsense::get_string(frame->get_stream()->get_stream_type()) << "," << frame->get_frame_number()
                      << ",DispatchedAt," << callback_ended);
  
                  if (callback_duration > callback_warning_duration)
                  {
                      LOG_DEBUG("Frame Callback [" << librealsense::get_string(frame->get_stream()->get_stream_type())
                               << "#" << std::dec << frame->additional_data.frame_number
                               << "] overdue. (Duration: " << callback_duration
                               << "ms, FPS: " << frame->get_stream()->get_framerate() << ", Max Duration: " << callback_warning_duration << "ms)");
                  }
              }
          }
  
          std::shared_ptr<metadata_parser_map> get_md_parsers() const { return _metadata_parsers; };
  
          friend class frame;
  
      public:
          explicit frame_archive(std::atomic<uint32_t>* in_max_frame_queue_size,
                               std::shared_ptr<platform::time_service> ts,
                               std::shared_ptr<metadata_parser_map> parsers)
              : max_frame_queue_size(in_max_frame_queue_size),
                mutex(), recycle_frames(true), _time_service(ts),
                _metadata_parsers(parsers)
          {
              published_frames_count = 0;
          }
  
          callback_invocation_holder begin_callback()
          {
              return { callback_inflight.allocate(), &callback_inflight };
          }
  
          void release_frame_ref(frame_interface* ref)
          {
              ref->release();
          }
  
          frame_interface* alloc_and_track(const size_t size, const frame_additional_data& additional_data, bool requires_memory)
          {
              auto frame = alloc_frame(size, additional_data, requires_memory);
              return track_frame(frame);
          }
  
          void flush()
          {
              published_frames.stop_allocation();
              callback_inflight.stop_allocation();
              recycle_frames = false;
  
              auto callbacks_inflight = callback_inflight.get_size();
              if (callbacks_inflight > 0)
              {
                  LOG_WARNING(callbacks_inflight << " callbacks are still running on some other threads. Waiting until all callbacks return...");
              }
              // wait until user is done with all the stuff he chose to borrow
              callback_inflight.wait_until_empty();
  
              {
                  std::lock_guard<std::recursive_mutex> guard(mutex);
                  freelist.clear();
              }
  
              pending_frames = published_frames.get_size();
              if (pending_frames > 0)
              {
                  LOG_INFO("The user was holding on to "
                      << std::dec << pending_frames << " frames after stream 0x"
                      << std::hex << this << " stopped" << std::dec);
              }
              // frames and their frame refs are not flushed, by design
          }
  
          ~frame_archive()
          {
              if (pending_frames > 0)
              {
                  LOG_INFO("All frames from stream 0x"
                      << std::hex << this << " are now released by the user");
              }
          }
  
      };
  
      std::shared_ptr<archive_interface> make_archive(rs2_extension type,
                                                      std::atomic<uint32_t>* in_max_frame_queue_size,
                                                      std::shared_ptr<platform::time_service> ts,
                                                      std::shared_ptr<metadata_parser_map> parsers)
      {
          switch(type)
          {
          case RS2_EXTENSION_VIDEO_FRAME :
              return std::make_shared<frame_archive<video_frame>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_COMPOSITE_FRAME :
              return std::make_shared<frame_archive<composite_frame>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_MOTION_FRAME:
              return std::make_shared<frame_archive<motion_frame>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_POINTS:
              return std::make_shared<frame_archive<points>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_DEPTH_FRAME:
              return std::make_shared<frame_archive<depth_frame>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_POSE_FRAME:
              return std::make_shared<frame_archive<pose_frame>>(in_max_frame_queue_size, ts, parsers);
  
          case RS2_EXTENSION_DISPARITY_FRAME:
              return std::make_shared<frame_archive<disparity_frame>>(in_max_frame_queue_size, ts, parsers);
  
          default:
              throw std::runtime_error("Requested frame type is not supported!");
          }
      }
  }
  
  void frame::release()
  {
      if (ref_count.fetch_sub(1) == 1)
      {
          on_release();
          owner->unpublish_frame(this);
      }
  }
  
  void frame::keep()
  {
      if (!_kept.exchange(true))
      {
          owner->keep_frame(this);
      }
  }
  
  frame_interface* frame::publish(std::shared_ptr<archive_interface> new_owner)
  {
      owner = new_owner;
      _kept = false;
      return owner->publish_frame(this);
  }
  
  rs2_metadata_type frame::get_frame_metadata(const rs2_frame_metadata_value& frame_metadata) const
  {
      auto md_parsers = owner->get_md_parsers();
  
      if (!md_parsers)
          throw invalid_value_exception(to_string() << "metadata not available for "
                                        << get_string(get_stream()->get_stream_type())<<" stream");
  
      auto it = md_parsers.get()->find(frame_metadata);
      if (it == md_parsers.get()->end())          // Possible user error - md attribute is not supported by this frame type
          throw invalid_value_exception(to_string() << get_string(frame_metadata)
                                        << " attribute is not applicable for "
                                        << get_string(get_stream()->get_stream_type()) << " stream ");
  
      // Proceed to parse and extract the required data attribute
      return it->second->get(*this);
  }
  
  bool frame::supports_frame_metadata(const rs2_frame_metadata_value& frame_metadata) const
  {
      auto md_parsers = owner->get_md_parsers();
  
      // verify preconditions
      if (!md_parsers)
          return false;                         // No parsers are available or no metadata was attached
  
      auto it = md_parsers.get()->find(frame_metadata);
      if (it == md_parsers.get()->end())          // Possible user error - md attribute is not supported by this frame type
          return false;
  
      return it->second->supports(*this);
  }
  
  const byte* frame::get_frame_data() const
  {
      const byte* frame_data = data.data();
  
      if (on_release.get_data())
      {
          frame_data = static_cast<const byte*>(on_release.get_data());
      }
  
      return frame_data;
  }
  
  rs2_timestamp_domain frame::get_frame_timestamp_domain() const
  {
      return additional_data.timestamp_domain;
  }
  
  rs2_time_t frame::get_frame_timestamp() const
  {
      return additional_data.timestamp;
  }
  
  unsigned long long frame::get_frame_number() const
  {
      return additional_data.frame_number;
  }
  
  rs2_time_t frame::get_frame_system_time() const
  {
      return additional_data.system_time;
  }
  
  void frame::update_frame_callback_start_ts(rs2_time_t ts)
  {
      additional_data.frame_callback_started = ts;
  }
  
  rs2_time_t frame::get_frame_callback_start_time_point() const
  {
      return additional_data.frame_callback_started;
  }
  
  void frame::log_callback_start(rs2_time_t timestamp)
  {
      update_frame_callback_start_ts(timestamp);
      LOG_DEBUG("CallbackStarted," << std::dec << librealsense::get_string(get_stream()->get_stream_type()) << "," << get_frame_number() << ",DispatchedAt," << timestamp);
  }
  
  void frame::log_callback_end(rs2_time_t timestamp) const
  {
      auto callback_warning_duration = 1000.f / (get_stream()->get_framerate() + 1);
      auto callback_duration = timestamp - get_frame_callback_start_time_point();
  
      LOG_DEBUG("CallbackFinished," << librealsense::get_string(get_stream()->get_stream_type()) << "," << get_frame_number() << ",DispatchedAt," << timestamp);
  
      if (callback_duration > callback_warning_duration)
      {
          LOG_INFO("Frame Callback " << librealsense::get_string(get_stream()->get_stream_type())
                   << "#" << std::dec << get_frame_number()
                   << "overdue. (Duration: " << callback_duration
                   << "ms, FPS: " << get_stream()->get_framerate() << ", Max Duration: " << callback_warning_duration << "ms)");
      }
  }
  