<commit_before>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2018 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

//! @example

#include <DO/Sara/Core/TicToc.hpp>
#include <DO/Sara/DisjointSets/DisjointSets.hpp>
#include <DO/Sara/FeatureDetectors/EdgeDetector.hpp>
#include <DO/Sara/FeatureDetectors/EdgePostProcessing.hpp>
#include <DO/Sara/FeatureDetectors/EdgeUtilities.hpp>
#include <DO/Sara/Geometry/Algorithms/Polyline.hpp>
#include <DO/Sara/Geometry/Tools/Utilities.hpp>
#include <DO/Sara/Graphics.hpp>
#include <DO/Sara/ImageIO.hpp>
#include <DO/Sara/ImageProcessing.hpp>
#include <DO/Sara/VideoIO.hpp>

#include <future>

#include <omp.h>

using namespace std;
using namespace DO::Sara;

constexpr long double operator"" _percent(long double x)
{
  return x / 100;
}

constexpr long double operator"" _deg(long double x)
{
  return x * M_PI / 180;
}

// ========================================================================== //
// Shape Statistics of the Edge.
// ========================================================================== //
struct OrientedBox
{
  const Eigen::Vector2d& center;
  const Eigen::Matrix2d& axes;
  const Eigen::Vector2d& lengths;

  auto length_ratio() const
  {
    return lengths(0) / lengths(1);
  }

  auto line() const
  {
    return Projective::line(center.homogeneous().eval(),
                            (center + axes.col(0)).homogeneous().eval());
  }

  auto draw(ImageView<Rgb8>& detection, const Rgb8& color,  //
            const Point2d& c1, const double s) const -> void
  {
    const Vector2d u = axes.col(0);
    const Vector2d v = axes.col(1);
    const auto p = std::array<Vector2d, 4>{
        c1 + s * (center + (lengths(0) + 0) * u + (lengths(1) + 0) * v),
        c1 + s * (center - (lengths(0) + 0) * u + (lengths(1) + 0) * v),
        c1 + s * (center - (lengths(0) + 0) * u - (lengths(1) + 0) * v),
        c1 + s * (center + (lengths(0) + 0) * u - (lengths(1) + 0) * v),
    };
    auto pi = std::array<Vector2i, 4>{};
    std::transform(p.begin(), p.end(), pi.begin(),
                   [](const Vector2d& v) { return v.cast<int>(); });

    draw_line(detection, pi[0].x(), pi[0].y(), pi[1].x(), pi[1].y(), color, 2);
    draw_line(detection, pi[1].x(), pi[1].y(), pi[2].x(), pi[2].y(), color, 2);
    draw_line(detection, pi[2].x(), pi[2].y(), pi[3].x(), pi[3].y(), color, 2);
    draw_line(detection, pi[3].x(), pi[3].y(), pi[0].x(), pi[0].y(), color, 2);
  }
};

// ========================================================================== //
// Edge Grouping By Alignment
// ========================================================================== //
using Edge = std::vector<Eigen::Vector2d>;

struct EdgeAttributes
{
  const std::vector<Edge>& edges;
  const std::vector<Eigen::Vector2d>& centers;
  const std::vector<Eigen::Matrix2d>& axes;
  const std::vector<Eigen::Vector2d>& lengths;
};

struct EndPointGraph
{
  const EdgeAttributes& edge_attrs;

  // List of end points.
  std::vector<Point2d> endpoints;
  // Edge IDs to which the end point belongs to.
  std::vector<std::size_t> edge_ids;

  // Connect the end point to another point.
  // Cannot be in the same edge ids.
  Eigen::MatrixXd score;

  EndPointGraph(const EdgeAttributes& attrs)
    : edge_attrs{attrs}
  {
    endpoints.reserve(2 * edge_attrs.edges.size());
    edge_ids.reserve(2 * edge_attrs.edges.size());
    for (auto i = 0u; i < edge_attrs.edges.size(); ++i)
    {
      const auto& e = edge_attrs.edges[i];
      if (e.size() < 2)
        continue;

      if (length(e) < 5)
        continue;

      const auto& theta = std::abs(std::atan2(edge_attrs.axes[i](1, 0),  //
                                              edge_attrs.axes[i](0, 0)));
      if (theta < 5._deg || std::abs(M_PI - theta) < 5._deg)
        continue;

      endpoints.emplace_back(e.front());
      endpoints.emplace_back(e.back());

      edge_ids.emplace_back(i);
      edge_ids.emplace_back(i);
    }

    score = Eigen::MatrixXd(endpoints.size(), endpoints.size());
    score.fill(std::numeric_limits<double>::infinity());
  }

  auto edge(std::size_t i) const -> const Edge&
  {
    const auto& edge_id = edge_ids[i];
    return edge_attrs.edges[edge_id];
  }

  auto oriented_box(std::size_t i) const -> OrientedBox
  {
    const auto& edge_id = edge_ids[i];
    return {
        edge_attrs.centers[edge_id],  //
        edge_attrs.axes[edge_id],     //
        edge_attrs.lengths[edge_id]   //
    };
  }

  auto mark_plausible_alignments() -> void
  {
    // Tolerance of X degrees in the alignment error.
    const auto thres = std::cos(20._deg);

    for (auto i = 0u; i < endpoints.size() / 2; ++i)
    {
      for (auto k = 0; k < 2; ++k)
      {
        const auto& ik = 2 * i + k;
        const auto& p_ik = endpoints[ik];

        const auto& r_ik = oriented_box(ik);
        const auto& c_ik = r_ik.center;

        for (auto j = i + 1; j < endpoints.size() / 2; ++j)
        {
          // The closest and most collinear point.
          for (int l = 0; l < 2; ++l)
          {
            const auto& jl = 2 * j + l;
            const auto& p_jl = endpoints[jl];

            const auto& r_jl = oriented_box(jl);
            const auto& c_jl = r_jl.center;

            // Particular case:
            if ((p_ik - p_jl).squaredNorm() < 1e-3)
            {
              // Check the plausibility that the end points are mutually
              // aligned?
              const auto dir = std::array<Eigen::Vector2d, 2>{
                  (p_ik - c_ik).normalized(),  //
                  (c_jl - p_jl).normalized()   //
              };

              const auto cosine = dir[0].dot(dir[1]);
              if (cosine < thres)
                continue;

              score(ik, jl) = 0;
            }
            else
            {
              const auto dir = std::array<Eigen::Vector2d, 3>{
                  (p_ik - c_ik).normalized(),  //
                  (p_jl - p_ik).normalized(),  //
                  (c_jl - p_jl).normalized()   //
              };
              const auto cosines = std::array<double, 2>{
                  dir[0].dot(dir[1]),
                  dir[1].dot(dir[2]),
              };

              if (cosines[0] + cosines[1] < 2 * thres)
                continue;

              if (Projective::point_to_line_distance(p_ik.homogeneous().eval(),
                                                     r_jl.line()) > 10 &&
                  Projective::point_to_line_distance(p_jl.homogeneous().eval(),
                                                     r_ik.line()) > 10)
                continue;

              // We need this to be as small as possible.
              const auto dist = (p_ik - p_jl).norm();

              // We really need to avoid accidental connections like these
              // situations. Too small edges and too far away, there is little
              // chance it would correspond to a plausible alignment.
              if (length(edge(ik)) + length(edge(jl)) < 20 && dist > 20)
                continue;

              if (dist > 50)
                continue;

              score(ik, jl) = dist;
            }
          }
        }
      }
    }
  }

  auto group() const
  {
    const auto n = score.rows();

    auto ds = DisjointSets(n);

    for (auto i = 0; i < n; ++i)
      ds.make_set(i);

    for (auto i = 0; i < n; ++i)
      for (auto j = i; j < n; ++j)
        if (score(i, j) != std::numeric_limits<double>::infinity())
          ds.join(ds.node(i), ds.node(j));

    auto groups = std::map<std::size_t, std::vector<std::size_t>>{};
    for (auto i = 0; i < n; ++i)
    {
      const auto c = ds.component(i);
      groups[c].push_back(i);
    }

    return groups;
  }
};

int main(int argc, char** argv)
{
  DO::Sara::GraphicsApplication app(argc, argv);
  app.register_user_main(__main);
  return app.exec();
}

int __main(int argc, char** argv)
{
  using namespace std::string_literals;

  const auto video_filepath =
      argc == 2
          ? argv[1]
#ifdef _WIN32
          : "C:/Users/David/Desktop/david-archives/gopro-backup-2/GOPR0542.MP4"s;
#elif __APPLE__
          : "/Users/david/Desktop/Datasets/videos/sample10.mp4"s;
#else
          : "/home/david/Desktop/Datasets/sfm/Family.mp4"s;
#endif

  // OpenMP.
  omp_set_num_threads(omp_get_max_threads());

  // Input and output from Sara.
  VideoStream video_stream(video_filepath);
  auto frame = video_stream.frame();
  const auto downscale_factor = 2;
  auto frame_gray32f = Image<float>{};

  // Show the local extrema.
  create_window(frame.sizes());
  set_antialiasing();

  constexpr float high_threshold_ratio = static_cast<float>(20._percent);
  constexpr float low_threshold_ratio = static_cast<float>(high_threshold_ratio / 2.);
  constexpr float angular_threshold = static_cast<float>(20. / 180. * M_PI);
  const auto sigma = std::sqrt(std::pow(1.2f, 2) - 1);
  const Eigen::Vector2i& p1 = frame.sizes() / 4;  // Eigen::Vector2i::Zero();
  const Eigen::Vector2i& p2 = frame.sizes() * 3 / 4;  // frame.sizes();

  auto ed = EdgeDetector{{
      high_threshold_ratio,  //
      low_threshold_ratio,   //
      angular_threshold      //
  }};

  auto frames_read = 0;
  const auto skip = 0;
  while (true)
  {
    if (!video_stream.read())
    {
      std::cout << "Reached the end of the video!" << std::endl;
      break;
    }
    ++frames_read;
    if (frames_read % (skip + 1) != 0)
      continue;

    // Reduce our attention to the central part of the image.
    tic();
    const auto frame_cropped = crop(frame, p1, p2);
    toc("Crop");

    tic();
    frame_gray32f = frame_cropped.convert<float>();
    toc("Grayscale");

    tic();
    frame_gray32f = gaussian(frame_gray32f, sigma);
    toc("Blur");

    if (downscale_factor > 1)
    {
      tic();
      frame_gray32f = downscale(frame_gray32f, downscale_factor);
      toc("Downscale");
    }

    ed(frame_gray32f);
    auto& edges_refined = ed.pipeline.edges_simplified;

    tic();
    // TODO: split only if the inertias matrix is becoming isotropic.
    edges_refined = split(edges_refined, 10. * M_PI / 180.);
    toc("Edge Split");

    tic();
    // TODO: figure out why the linear directional mean is shaky.
    // auto ldms = std::vector<double>(edges_refined.size());
    // The rectangle approximation.
    auto centers = std::vector<Vector2d>(edges_refined.size());
    auto inertias = std::vector<Matrix2d>(edges_refined.size());
    auto axes = std::vector<Matrix2d>(edges_refined.size());
    auto lengths = std::vector<Vector2d>(edges_refined.size());
#pragma omp parallel for
    for (auto i = 0; i < static_cast<int>(edges_refined.size()); ++i)
    {
      const auto& e = edges_refined[i];
      if (e.size() < 2)
        continue;

      // ldms[i] = linear_directional_mean(e);
      centers[i] = center_of_mass(e);
      inertias[i] = matrix_of_inertia(e, centers[i]);
      const auto svd = inertias[i].jacobiSvd(Eigen::ComputeFullU);
      axes[i] = svd.matrixU();
      lengths[i] = svd.singularValues().cwiseSqrt();
    }
    SARA_CHECK(edges_refined.size());
    toc("Edge Shape Statistics");

    tic();
    const auto edge_attributes = EdgeAttributes{
        edges_refined,  //
        centers,        //
        axes,           //
        lengths         //
    };
    auto endpoint_graph = EndPointGraph{edge_attributes};
    endpoint_graph.mark_plausible_alignments();
    toc("Alignment Computation");

    const Eigen::Vector2d p1d = p1.cast<double>();
    const auto s = static_cast<float>(downscale_factor);

    auto detection = Image<Rgb8>{frame};
    detection.flat_array().fill(Black8);

    for (const auto& e : edges_refined)
      if (e.size() >= 2)
        draw_polyline(detection, e, Blue8, p1d, s);

    // Draw alignment-based connections.
    auto remap = [&](const auto p) -> Vector2d { return p1d + s * p; };
    const auto& score = endpoint_graph.score;
    for (auto i = 0; i < score.rows(); ++i)
    {
      for (auto j = i + 1; j < score.cols(); ++j)
      {
        const auto& pi = endpoint_graph.endpoints[i];
        const auto& pj = endpoint_graph.endpoints[j];

        if (score(i, j) != std::numeric_limits<double>::infinity())
        {
          const auto pi1 = remap(pi).cast<int>();
          const auto pj1 = remap(pj).cast<int>();
          draw_line(detection, pi1.x(), pi1.y(), pj1.x(), pj1.y(), Yellow8, 2);
          draw_circle(detection, pi1.x(), pi1.y(), 3, Yellow8, 3);
          draw_circle(detection, pj1.x(), pj1.y(), 3, Yellow8, 3);
        }
      }
    }

    display(detection);
  }

  return 0;
}
<commit_msg>MAINT: fix path.<commit_after>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2018 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

//! @example

#include <DO/Sara/Core/TicToc.hpp>
#include <DO/Sara/DisjointSets/DisjointSets.hpp>
#include <DO/Sara/FeatureDetectors/EdgeDetector.hpp>
#include <DO/Sara/FeatureDetectors/EdgePostProcessing.hpp>
#include <DO/Sara/FeatureDetectors/EdgeUtilities.hpp>
#include <DO/Sara/Geometry/Algorithms/Polyline.hpp>
#include <DO/Sara/Geometry/Tools/Utilities.hpp>
#include <DO/Sara/Graphics.hpp>
#include <DO/Sara/ImageIO.hpp>
#include <DO/Sara/ImageProcessing.hpp>
#include <DO/Sara/VideoIO.hpp>

#include <future>

#include <omp.h>

using namespace std;
using namespace DO::Sara;

constexpr long double operator"" _percent(long double x)
{
  return x / 100;
}

constexpr long double operator"" _deg(long double x)
{
  return x * M_PI / 180;
}

// ========================================================================== //
// Shape Statistics of the Edge.
// ========================================================================== //
struct OrientedBox
{
  const Eigen::Vector2d& center;
  const Eigen::Matrix2d& axes;
  const Eigen::Vector2d& lengths;

  auto length_ratio() const
  {
    return lengths(0) / lengths(1);
  }

  auto line() const
  {
    return Projective::line(center.homogeneous().eval(),
                            (center + axes.col(0)).homogeneous().eval());
  }

  auto draw(ImageView<Rgb8>& detection, const Rgb8& color,  //
            const Point2d& c1, const double s) const -> void
  {
    const Vector2d u = axes.col(0);
    const Vector2d v = axes.col(1);
    const auto p = std::array<Vector2d, 4>{
        c1 + s * (center + (lengths(0) + 0) * u + (lengths(1) + 0) * v),
        c1 + s * (center - (lengths(0) + 0) * u + (lengths(1) + 0) * v),
        c1 + s * (center - (lengths(0) + 0) * u - (lengths(1) + 0) * v),
        c1 + s * (center + (lengths(0) + 0) * u - (lengths(1) + 0) * v),
    };
    auto pi = std::array<Vector2i, 4>{};
    std::transform(p.begin(), p.end(), pi.begin(),
                   [](const Vector2d& v) { return v.cast<int>(); });

    draw_line(detection, pi[0].x(), pi[0].y(), pi[1].x(), pi[1].y(), color, 2);
    draw_line(detection, pi[1].x(), pi[1].y(), pi[2].x(), pi[2].y(), color, 2);
    draw_line(detection, pi[2].x(), pi[2].y(), pi[3].x(), pi[3].y(), color, 2);
    draw_line(detection, pi[3].x(), pi[3].y(), pi[0].x(), pi[0].y(), color, 2);
  }
};

// ========================================================================== //
// Edge Grouping By Alignment
// ========================================================================== //
using Edge = std::vector<Eigen::Vector2d>;

struct EdgeAttributes
{
  const std::vector<Edge>& edges;
  const std::vector<Eigen::Vector2d>& centers;
  const std::vector<Eigen::Matrix2d>& axes;
  const std::vector<Eigen::Vector2d>& lengths;
};

struct EndPointGraph
{
  const EdgeAttributes& edge_attrs;

  // List of end points.
  std::vector<Point2d> endpoints;
  // Edge IDs to which the end point belongs to.
  std::vector<std::size_t> edge_ids;

  // Connect the end point to another point.
  // Cannot be in the same edge ids.
  Eigen::MatrixXd score;

  EndPointGraph(const EdgeAttributes& attrs)
    : edge_attrs{attrs}
  {
    endpoints.reserve(2 * edge_attrs.edges.size());
    edge_ids.reserve(2 * edge_attrs.edges.size());
    for (auto i = 0u; i < edge_attrs.edges.size(); ++i)
    {
      const auto& e = edge_attrs.edges[i];
      if (e.size() < 2)
        continue;

      if (length(e) < 5)
        continue;

      const auto& theta = std::abs(std::atan2(edge_attrs.axes[i](1, 0),  //
                                              edge_attrs.axes[i](0, 0)));
      if (theta < 5._deg || std::abs(M_PI - theta) < 5._deg)
        continue;

      endpoints.emplace_back(e.front());
      endpoints.emplace_back(e.back());

      edge_ids.emplace_back(i);
      edge_ids.emplace_back(i);
    }

    score = Eigen::MatrixXd(endpoints.size(), endpoints.size());
    score.fill(std::numeric_limits<double>::infinity());
  }

  auto edge(std::size_t i) const -> const Edge&
  {
    const auto& edge_id = edge_ids[i];
    return edge_attrs.edges[edge_id];
  }

  auto oriented_box(std::size_t i) const -> OrientedBox
  {
    const auto& edge_id = edge_ids[i];
    return {
        edge_attrs.centers[edge_id],  //
        edge_attrs.axes[edge_id],     //
        edge_attrs.lengths[edge_id]   //
    };
  }

  auto mark_plausible_alignments() -> void
  {
    // Tolerance of X degrees in the alignment error.
    const auto thres = std::cos(20._deg);

    for (auto i = 0u; i < endpoints.size() / 2; ++i)
    {
      for (auto k = 0; k < 2; ++k)
      {
        const auto& ik = 2 * i + k;
        const auto& p_ik = endpoints[ik];

        const auto& r_ik = oriented_box(ik);
        const auto& c_ik = r_ik.center;

        for (auto j = i + 1; j < endpoints.size() / 2; ++j)
        {
          // The closest and most collinear point.
          for (int l = 0; l < 2; ++l)
          {
            const auto& jl = 2 * j + l;
            const auto& p_jl = endpoints[jl];

            const auto& r_jl = oriented_box(jl);
            const auto& c_jl = r_jl.center;

            // Particular case:
            if ((p_ik - p_jl).squaredNorm() < 1e-3)
            {
              // Check the plausibility that the end points are mutually
              // aligned?
              const auto dir = std::array<Eigen::Vector2d, 2>{
                  (p_ik - c_ik).normalized(),  //
                  (c_jl - p_jl).normalized()   //
              };

              const auto cosine = dir[0].dot(dir[1]);
              if (cosine < thres)
                continue;

              score(ik, jl) = 0;
            }
            else
            {
              const auto dir = std::array<Eigen::Vector2d, 3>{
                  (p_ik - c_ik).normalized(),  //
                  (p_jl - p_ik).normalized(),  //
                  (c_jl - p_jl).normalized()   //
              };
              const auto cosines = std::array<double, 2>{
                  dir[0].dot(dir[1]),
                  dir[1].dot(dir[2]),
              };

              if (cosines[0] + cosines[1] < 2 * thres)
                continue;

              if (Projective::point_to_line_distance(p_ik.homogeneous().eval(),
                                                     r_jl.line()) > 10 &&
                  Projective::point_to_line_distance(p_jl.homogeneous().eval(),
                                                     r_ik.line()) > 10)
                continue;

              // We need this to be as small as possible.
              const auto dist = (p_ik - p_jl).norm();

              // We really need to avoid accidental connections like these
              // situations. Too small edges and too far away, there is little
              // chance it would correspond to a plausible alignment.
              if (length(edge(ik)) + length(edge(jl)) < 20 && dist > 20)
                continue;

              if (dist > 50)
                continue;

              score(ik, jl) = dist;
            }
          }
        }
      }
    }
  }

  auto group() const
  {
    const auto n = score.rows();

    auto ds = DisjointSets(n);

    for (auto i = 0; i < n; ++i)
      ds.make_set(i);

    for (auto i = 0; i < n; ++i)
      for (auto j = i; j < n; ++j)
        if (score(i, j) != std::numeric_limits<double>::infinity())
          ds.join(ds.node(i), ds.node(j));

    auto groups = std::map<std::size_t, std::vector<std::size_t>>{};
    for (auto i = 0; i < n; ++i)
    {
      const auto c = ds.component(i);
      groups[c].push_back(i);
    }

    return groups;
  }
};

int main(int argc, char** argv)
{
  DO::Sara::GraphicsApplication app(argc, argv);
  app.register_user_main(__main);
  return app.exec();
}

int __main(int argc, char** argv)
{
  using namespace std::string_literals;

  const auto video_filepath =
      argc == 2
          ? argv[1]
#ifdef _WIN32
          : "C:/Users/David/Desktop/GOPR0542.MP4"s;
#elif __APPLE__
          : "/Users/david/Desktop/Datasets/videos/sample10.mp4"s;
#else
          : "/home/david/Desktop/Datasets/sfm/Family.mp4"s;
#endif

  // OpenMP.
  omp_set_num_threads(omp_get_max_threads());

  // Input and output from Sara.
  VideoStream video_stream(video_filepath);
  auto frame = video_stream.frame();
  const auto downscale_factor = 2;
  auto frame_gray32f = Image<float>{};

  // Show the local extrema.
  create_window(frame.sizes());
  set_antialiasing();

  constexpr float high_threshold_ratio = static_cast<float>(20._percent);
  constexpr float low_threshold_ratio = static_cast<float>(high_threshold_ratio / 2.);
  constexpr float angular_threshold = static_cast<float>(20. / 180. * M_PI);
  const auto sigma = std::sqrt(std::pow(1.2f, 2) - 1);
  const Eigen::Vector2i& p1 = frame.sizes() / 4;  // Eigen::Vector2i::Zero();
  const Eigen::Vector2i& p2 = frame.sizes() * 3 / 4;  // frame.sizes();

  auto ed = EdgeDetector{{
      high_threshold_ratio,  //
      low_threshold_ratio,   //
      angular_threshold      //
  }};

  auto frames_read = 0;
  const auto skip = 0;
  while (true)
  {
    if (!video_stream.read())
    {
      std::cout << "Reached the end of the video!" << std::endl;
      break;
    }
    ++frames_read;
    if (frames_read % (skip + 1) != 0)
      continue;

    // Reduce our attention to the central part of the image.
    tic();
    const auto frame_cropped = crop(frame, p1, p2);
    toc("Crop");

    tic();
    frame_gray32f = frame_cropped.convert<float>();
    toc("Grayscale");

    tic();
    frame_gray32f = gaussian(frame_gray32f, sigma);
    toc("Blur");

    if (downscale_factor > 1)
    {
      tic();
      frame_gray32f = downscale(frame_gray32f, downscale_factor);
      toc("Downscale");
    }

    ed(frame_gray32f);
    auto& edges_refined = ed.pipeline.edges_simplified;

    tic();
    // TODO: split only if the inertias matrix is becoming isotropic.
    edges_refined = split(edges_refined, 10. * M_PI / 180.);
    toc("Edge Split");

    tic();
    // TODO: figure out why the linear directional mean is shaky.
    // auto ldms = std::vector<double>(edges_refined.size());
    // The rectangle approximation.
    auto centers = std::vector<Vector2d>(edges_refined.size());
    auto inertias = std::vector<Matrix2d>(edges_refined.size());
    auto axes = std::vector<Matrix2d>(edges_refined.size());
    auto lengths = std::vector<Vector2d>(edges_refined.size());
#pragma omp parallel for
    for (auto i = 0; i < static_cast<int>(edges_refined.size()); ++i)
    {
      const auto& e = edges_refined[i];
      if (e.size() < 2)
        continue;

      // ldms[i] = linear_directional_mean(e);
      centers[i] = center_of_mass(e);
      inertias[i] = matrix_of_inertia(e, centers[i]);
      const auto svd = inertias[i].jacobiSvd(Eigen::ComputeFullU);
      axes[i] = svd.matrixU();
      lengths[i] = svd.singularValues().cwiseSqrt();
    }
    SARA_CHECK(edges_refined.size());
    toc("Edge Shape Statistics");

    tic();
    const auto edge_attributes = EdgeAttributes{
        edges_refined,  //
        centers,        //
        axes,           //
        lengths         //
    };
    auto endpoint_graph = EndPointGraph{edge_attributes};
    endpoint_graph.mark_plausible_alignments();
    toc("Alignment Computation");

    const Eigen::Vector2d p1d = p1.cast<double>();
    const auto s = static_cast<float>(downscale_factor);

    auto detection = Image<Rgb8>{frame};
    detection.flat_array().fill(Black8);

    for (const auto& e : edges_refined)
      if (e.size() >= 2)
        draw_polyline(detection, e, Blue8, p1d, s);

    // Draw alignment-based connections.
    auto remap = [&](const auto p) -> Vector2d { return p1d + s * p; };
    const auto& score = endpoint_graph.score;
    for (auto i = 0; i < score.rows(); ++i)
    {
      for (auto j = i + 1; j < score.cols(); ++j)
      {
        const auto& pi = endpoint_graph.endpoints[i];
        const auto& pj = endpoint_graph.endpoints[j];

        if (score(i, j) != std::numeric_limits<double>::infinity())
        {
          const auto pi1 = remap(pi).cast<int>();
          const auto pj1 = remap(pj).cast<int>();
          draw_line(detection, pi1.x(), pi1.y(), pj1.x(), pj1.y(), Yellow8, 2);
          draw_circle(detection, pi1.x(), pi1.y(), 3, Yellow8, 3);
          draw_circle(detection, pj1.x(), pj1.y(), 3, Yellow8, 3);
        }
      }
    }

    display(detection);
  }

  return 0;
}
<|endoftext|>


<commit_before>/*
The OpenTRV project licenses this file to you
under the Apache Licence, Version 2.0 (the "Licence");
you may not use this file except in compliance
with the Licence. You may obtain a copy of the Licence at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing,
software distributed under the Licence is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied. See the Licence for the
specific language governing permissions and limitations
under the Licence.

Author(s) / Copyright (s): Damon Hart-Davis 2016
                           Deniz Erbilgin 2017
*/

/*
 * OTRadValve tests of secure frame operations dependent on OTASEGCM
 * with a particular view to managing stack depth to avoid overflow
 * especially on very limited RAM platforms such as AVR.
 */

// Only enable these tests if the OTAESGCM library is marked as available.
#if defined(EXT_AVAILABLE_ARDUINO_LIB_OTAESGCM)

#include <gtest/gtest.h>
#include <stdio.h>
#include <stdlib.h>

#include <OTV0p2Base.h>
#include <OTAESGCM.h>
#include <OTRadioLink.h>

//static const int AES_KEY_SIZE = 128; // in bits
//static const int GCM_NONCE_LENGTH = 12; // in bytes
//static const int GCM_TAG_LENGTH = 16; // in bytes (default 16, 12 possible)

// All-zeros const 16-byte/128-bit key.
// Can be used for other purposes.
//static const uint8_t zeroBlock[16] = { };

namespace SOSDT {
    // Max stack usage in bytes
    // 20170511
    //           enc, dec, enc*, dec*
    // - DE:     208, 208, 208,  208
    // - DHD:    ???, ???, 358,  ???
    // - Travis: 192, 224, ???,  ???
    // * using a workspace
    #ifndef __APPLE__
    //static constexpr unsigned int maxStackSecureFrameEncode = 328;
    static constexpr unsigned int maxStackSecureFrameDecode = 1024;
    #else
    // On DHD's system, secure frame enc/decode uses 358 bytes (20170511)
    // static constexpr unsigned int maxStackSecureFrameEncode = 1024;
    static constexpr unsigned int maxStackSecureFrameDecode = 1024;
    #endif // __APPLE__

    bool pollIO(bool) {return (false);}
    bool getKeySuccess(uint8_t *key) { memset(key, 0x0,  /*OTV0P2BASE::VOP2BASE_EE_LEN_16BYTE_PRIMARY_BUILDING_KEY*/ 16); return (true); }
    // like Nullframe operation but sets a flag
    volatile bool frameOperationCalledFlag = false;
    OTRadioLink::frameOperator_fn_t setFlagFrameOperation;
    bool setFlagFrameOperation(const OTRadioLink::OTFrameData_T &) { frameOperationCalledFlag = true; return (true);}

    struct minimumSecureFrame
    {
        // Preshared ID prefix; only an initial part/prefix of this goes on the wire in the header.
        static const uint8_t id[];
        // IV/nonce starting with first 6 bytes of preshared ID, then 6 bytes of counter.
        static const uint8_t iv[];
        static const uint8_t oldCounter[];
        // 'O' frame body with some JSON stats.
        static const uint8_t body[];
        // length of secure frame
        static const uint8_t encodedLength;
        // Buffer containing secure frame. Generated using code bellow.
        static const uint8_t buf[];

        // Stuff used to generate a working encodable frame. Taken from SecureFrameTest.cpp
            // All-zeros const 16-byte/128-bit key.
            // Can be used for other purposes.
        //    static const uint8_t zeroBlock[16] = { };
        //    uint8_t buf[OTRadioLink::SecurableFrameHeader::maxSmallFrameSize];
        //    //Example 3: secure, no valve, representative minimum stats {"b":1}).
        //    //Note that the sequence number must match the 4 lsbs of the message count, ie from iv[11].
        //    //and the ID is 0xaa 0xaa 0xaa 0xaa (transmitted) with the next ID bytes 0x55 0x55.
        //    //ResetCounter = 42
        //    //TxMsgCounter = 793
        //    //(Thus nonce/IV: aa aa aa aa 55 55 00 00 2a 00 03 19)
        //    //
        //    //3e cf 94 aa aa aa aa 20 | b3 45 f9 29 69 57 0c b8 28 66 14 b4 f0 69 b0 08 71 da d8 fe 47 c1 c3 53 83 48 88 03 7d 58 75 75 | 00 00 2a 00 03 19 29 3b 31 52 c3 26 d2 6d d0 8d 70 1e 4b 68 0d cb 80
        //    //
        //    //3e  length of header (62) after length byte 5 + (encrypted) body 32 + trailer 32
        //    //cf  'O' secure OpenTRV basic frame
        //    //04  0 sequence number, ID length 4
        //    //aa  ID byte 1
        //    //aa  ID byte 2
        //    //aa  ID byte 3
        //    //aa  ID byte 4
        //    //20  body length 32 (after padding and encryption)
        //    //    Plaintext body (length 8): 0x7f 0x11 { " b " : 1
        //    //    Padded: 7f 11 7b 22 62 22 3a 31 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 17
        //    //b3 45 f9 ... 58 75 75  32 bytes of encrypted body
        //    //00 00 2a  reset counter
        //    //00 03 19  message counter
        //    //29 3b 31 ... 68 0d cb  16 bytes of authentication tag
        //    //80  enc/auth type/format indicator.
        //    const uint8_t encodedLength = OTRadioLink::SimpleSecureFrame32or0BodyTXBase::encodeSecureSmallFrameRaw(buf, sizeof(buf),
        //                                    OTRadioLink::FTS_BasicSensorOrValve,
        //                                    id, 4,
        //                                    body, sizeof(body),
        //                                    iv,
        //                                    OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleEnc_DEFAULT_STATELESS,
        //                                    NULL, zeroBlock);
        //    std::fprintf(stdout, "const uint8_t encodedLength = %u;\n", encodedLength);
        //    std::cout << "const uint8_t buf[] = {\n\t";
        //    for(auto i = 0; i < OTRadioLink::SecurableFrameHeader::maxSmallFrameSize; ++i) {
        //        std::fprintf(stdout, "0x%x, ", buf[i]);
        //        if(7 == (i % 8)) std::cout << "\n\t";
        //    }
        //    std::cout << " };\n";
    };
    // Preshared ID prefix; only an initial part/prefix of this goes on the wire in the header.
    const uint8_t minimumSecureFrame::id[] = { 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x55 };
    // IV/nonce starting with first 6 bytes of preshared ID, then 6 bytes of counter.
    const uint8_t minimumSecureFrame::iv[] = { 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x00, 0x00, 0x2a, 0x00, 0x03, 0x19 };
    const uint8_t minimumSecureFrame::oldCounter[] = { 0x00, 0x00, 0x2a, 0x00, 0x03, 0x18 };
    // 'O' frame body with some JSON stats.
    const uint8_t minimumSecureFrame::body[] = { 0x7f, 0x11, 0x7b, 0x22, 0x62, 0x22, 0x3a, 0x31 };
    // length of secure frame
    const uint8_t minimumSecureFrame::encodedLength = 63;
    // Buffer containing secure frame. Generated using code bellow.
    const uint8_t minimumSecureFrame::buf[] = {
        0x3e, 0xcf, 0x94, 0xaa, 0xaa, 0xaa, 0xaa, 0x20,
        0xb3, 0x45, 0xf9, 0x29, 0x69, 0x57, 0x0c, 0xb8,
        0x28, 0x66, 0x14, 0xb4, 0xf0, 0x69, 0xb0, 0x08,
        0x71, 0xda, 0xd8, 0xfe, 0x47, 0xc1, 0xc3, 0x53,
        0x83, 0x48, 0x88, 0x03, 0x7d, 0x58, 0x75, 0x75,
        0x00, 0x00, 0x2a, 0x00, 0x03, 0x19, 0x29, 0x3b,
        0x31, 0x52, 0xc3, 0x26, 0xd2, 0x6d, 0xd0, 0x8d,
        0x70, 0x1e, 0x4b, 0x68, 0x0d, 0xcb, 0x80 };
}

TEST(SecureOpStackDepth, StackCheckerWorks)
{
    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();
    EXPECT_NE((size_t)0, baseStack);
}

TEST(SecureOpStackDepth, SimpleSecureFrame32or0BodyRXFixedCounterBasic)
{
    const uint8_t counter = 0;
    const uint8_t id = 0;
    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    EXPECT_TRUE(sfrx.updateRXMessageCountAfterAuthentication(&id, &counter));
}

TEST(SecureOpStackDepth, SimpleSecureFrame32or0BodyRXFixedCounterStack)
{
    // Make sure flag is false.
    SOSDT::frameOperationCalledFlag = false;
    // Secure Frame start
    const uint8_t * senderID = SOSDT::minimumSecureFrame::id;
    const uint8_t * msgCounter = SOSDT::minimumSecureFrame::oldCounter;
    const uint8_t * const msgStart = &SOSDT::minimumSecureFrame::buf[1];

    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();

    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    sfrx.setMockIDValue(senderID);
    sfrx.setMockCounterValue(msgCounter);
    const bool test1 = OTRadioLink::decodeAndHandleOTSecureOFrame<OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter,
                                                                  OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleDec_DEFAULT_STATELESS,
                                                                  SOSDT::getKeySuccess,
                                                                  SOSDT::setFlagFrameOperation
                                                                 >(msgStart);

    const size_t maxStack = OTV0P2BASE::MemoryChecks::getMinSP();
    // Uncomment to print stack usage
     std::cout << baseStack - maxStack << "\n";

    EXPECT_TRUE(test1);
    EXPECT_TRUE(SOSDT::frameOperationCalledFlag);
    EXPECT_GT(SOSDT::maxStackSecureFrameDecode, baseStack - maxStack);
}

TEST(SecureOpStackDepth, OTMessageQueueHandlerStackBasic)
{
    // Make sure flag is false.
    SOSDT::frameOperationCalledFlag = false;
    // Secure Frame start
    const uint8_t * senderID = SOSDT::minimumSecureFrame::id;
    const uint8_t * msgCounter = SOSDT::minimumSecureFrame::oldCounter;
//     const uint8_t * const msgStart = &SOSDT::minimumSecureFrame::buf[1];

    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();

    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    sfrx.setMockIDValue(senderID);
    sfrx.setMockCounterValue(msgCounter);

    OTRadioLink::OTMessageQueueHandler<
        SOSDT::pollIO, 4800,
        OTRadioLink::decodeAndHandleOTSecureOFrame<OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter,
                                                                      OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleDec_DEFAULT_STATELESS,
                                                                      SOSDT::getKeySuccess,
                                                                      SOSDT::setFlagFrameOperation
                                                                     >
                                                  > mh;
    OTRadioLink::OTRadioLinkMock rl;
    memcpy(rl.message, SOSDT::minimumSecureFrame::buf, SOSDT::minimumSecureFrame::encodedLength + 1);

    EXPECT_TRUE(mh.handle(false, rl));

    const size_t maxStack = OTV0P2BASE::MemoryChecks::getMinSP();
    // Uncomment to print stack usage
     std::cout << baseStack - maxStack << "\n";

    // EXPECT_TRUE(test1);
    EXPECT_TRUE(SOSDT::frameOperationCalledFlag);
    EXPECT_GT(SOSDT::maxStackSecureFrameDecode, baseStack - maxStack);
}

#endif // ARDUINO_LIB_OTAESGCM
<commit_msg>Stack usage tests + recorded results<commit_after>/*
The OpenTRV project licenses this file to you
under the Apache Licence, Version 2.0 (the "Licence");
you may not use this file except in compliance
with the Licence. You may obtain a copy of the Licence at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing,
software distributed under the Licence is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied. See the Licence for the
specific language governing permissions and limitations
under the Licence.

Author(s) / Copyright (s): Damon Hart-Davis 2016
                           Deniz Erbilgin 2017
*/

/*
 * OTRadValve tests of secure frame operations dependent on OTASEGCM
 * with a particular view to managing stack depth to avoid overflow
 * especially on very limited RAM platforms such as AVR.
 */

// Only enable these tests if the OTAESGCM library is marked as available.
#if defined(EXT_AVAILABLE_ARDUINO_LIB_OTAESGCM)

#include <gtest/gtest.h>
#include <stdio.h>
#include <stdlib.h>

#include <OTV0p2Base.h>
#include <OTAESGCM.h>
#include <OTRadioLink.h>

//static const int AES_KEY_SIZE = 128; // in bits
//static const int GCM_NONCE_LENGTH = 12; // in bytes
//static const int GCM_TAG_LENGTH = 16; // in bytes (default 16, 12 possible)

// All-zeros const 16-byte/128-bit key.
// Can be used for other purposes.
//static const uint8_t zeroBlock[16] = { };

namespace SOSDT {
    // Max stack usage in bytes
    // 20170511
    //           enc, dec, enc*, dec*
    // - DE:     208, 208, 208,  208
    // - DHD:    ???, ???, 358,  ???
    // - Travis: 192, 224, ???,  ???
    // * using a workspace
    #ifndef __APPLE__
    //static constexpr unsigned int maxStackSecureFrameEncode = 328;
    static constexpr unsigned int maxStackSecureFrameDecode = 1024;
    #else
    // On DHD's system, secure frame enc/decode uses 358 bytes (20170511)
    // static constexpr unsigned int maxStackSecureFrameEncode = 1024;
    static constexpr unsigned int maxStackSecureFrameDecode = 1024;
    #endif // __APPLE__

    bool pollIO(bool) {return (false);}
    bool getKeySuccess(uint8_t *key) { memset(key, 0x0,  /*OTV0P2BASE::VOP2BASE_EE_LEN_16BYTE_PRIMARY_BUILDING_KEY*/ 16); return (true); }
    // like Nullframe operation but sets a flag
    volatile bool frameOperationCalledFlag = false;
    OTRadioLink::frameOperator_fn_t setFlagFrameOperation;
    bool setFlagFrameOperation(const OTRadioLink::OTFrameData_T &) { frameOperationCalledFlag = true; return (true);}

    struct minimumSecureFrame
    {
        // Preshared ID prefix; only an initial part/prefix of this goes on the wire in the header.
        static const uint8_t id[];
        // IV/nonce starting with first 6 bytes of preshared ID, then 6 bytes of counter.
        static const uint8_t iv[];
        static const uint8_t oldCounter[];
        // 'O' frame body with some JSON stats.
        static const uint8_t body[];
        // length of secure frame
        static const uint8_t encodedLength;
        // Buffer containing secure frame. Generated using code bellow.
        static const uint8_t buf[];

        // Stuff used to generate a working encodable frame. Taken from SecureFrameTest.cpp
            // All-zeros const 16-byte/128-bit key.
            // Can be used for other purposes.
        //    static const uint8_t zeroBlock[16] = { };
        //    uint8_t buf[OTRadioLink::SecurableFrameHeader::maxSmallFrameSize];
        //    //Example 3: secure, no valve, representative minimum stats {"b":1}).
        //    //Note that the sequence number must match the 4 lsbs of the message count, ie from iv[11].
        //    //and the ID is 0xaa 0xaa 0xaa 0xaa (transmitted) with the next ID bytes 0x55 0x55.
        //    //ResetCounter = 42
        //    //TxMsgCounter = 793
        //    //(Thus nonce/IV: aa aa aa aa 55 55 00 00 2a 00 03 19)
        //    //
        //    //3e cf 94 aa aa aa aa 20 | b3 45 f9 29 69 57 0c b8 28 66 14 b4 f0 69 b0 08 71 da d8 fe 47 c1 c3 53 83 48 88 03 7d 58 75 75 | 00 00 2a 00 03 19 29 3b 31 52 c3 26 d2 6d d0 8d 70 1e 4b 68 0d cb 80
        //    //
        //    //3e  length of header (62) after length byte 5 + (encrypted) body 32 + trailer 32
        //    //cf  'O' secure OpenTRV basic frame
        //    //04  0 sequence number, ID length 4
        //    //aa  ID byte 1
        //    //aa  ID byte 2
        //    //aa  ID byte 3
        //    //aa  ID byte 4
        //    //20  body length 32 (after padding and encryption)
        //    //    Plaintext body (length 8): 0x7f 0x11 { " b " : 1
        //    //    Padded: 7f 11 7b 22 62 22 3a 31 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 17
        //    //b3 45 f9 ... 58 75 75  32 bytes of encrypted body
        //    //00 00 2a  reset counter
        //    //00 03 19  message counter
        //    //29 3b 31 ... 68 0d cb  16 bytes of authentication tag
        //    //80  enc/auth type/format indicator.
        //    const uint8_t encodedLength = OTRadioLink::SimpleSecureFrame32or0BodyTXBase::encodeSecureSmallFrameRaw(buf, sizeof(buf),
        //                                    OTRadioLink::FTS_BasicSensorOrValve,
        //                                    id, 4,
        //                                    body, sizeof(body),
        //                                    iv,
        //                                    OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleEnc_DEFAULT_STATELESS,
        //                                    NULL, zeroBlock);
        //    std::fprintf(stdout, "const uint8_t encodedLength = %u;\n", encodedLength);
        //    std::cout << "const uint8_t buf[] = {\n\t";
        //    for(auto i = 0; i < OTRadioLink::SecurableFrameHeader::maxSmallFrameSize; ++i) {
        //        std::fprintf(stdout, "0x%x, ", buf[i]);
        //        if(7 == (i % 8)) std::cout << "\n\t";
        //    }
        //    std::cout << " };\n";
    };
    // Preshared ID prefix; only an initial part/prefix of this goes on the wire in the header.
    const uint8_t minimumSecureFrame::id[] = { 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x55 };
    // IV/nonce starting with first 6 bytes of preshared ID, then 6 bytes of counter.
    const uint8_t minimumSecureFrame::iv[] = { 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x00, 0x00, 0x2a, 0x00, 0x03, 0x19 };
    const uint8_t minimumSecureFrame::oldCounter[] = { 0x00, 0x00, 0x2a, 0x00, 0x03, 0x18 };
    // 'O' frame body with some JSON stats.
    const uint8_t minimumSecureFrame::body[] = { 0x7f, 0x11, 0x7b, 0x22, 0x62, 0x22, 0x3a, 0x31 };
    // length of secure frame
    const uint8_t minimumSecureFrame::encodedLength = 63;
    // Buffer containing secure frame. Generated using code bellow.
    const uint8_t minimumSecureFrame::buf[] = {
        0x3e, 0xcf, 0x94, 0xaa, 0xaa, 0xaa, 0xaa, 0x20,
        0xb3, 0x45, 0xf9, 0x29, 0x69, 0x57, 0x0c, 0xb8,
        0x28, 0x66, 0x14, 0xb4, 0xf0, 0x69, 0xb0, 0x08,
        0x71, 0xda, 0xd8, 0xfe, 0x47, 0xc1, 0xc3, 0x53,
        0x83, 0x48, 0x88, 0x03, 0x7d, 0x58, 0x75, 0x75,
        0x00, 0x00, 0x2a, 0x00, 0x03, 0x19, 0x29, 0x3b,
        0x31, 0x52, 0xc3, 0x26, 0xd2, 0x6d, 0xd0, 0x8d,
        0x70, 0x1e, 0x4b, 0x68, 0x0d, 0xcb, 0x80 };
}

TEST(SecureOpStackDepth, StackCheckerWorks)
{
    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();
    EXPECT_NE((size_t)0, baseStack);
}

TEST(SecureOpStackDepth, SimpleSecureFrame32or0BodyRXFixedCounterBasic)
{
    const uint8_t counter = 0;
    const uint8_t id = 0;
    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    EXPECT_TRUE(sfrx.updateRXMessageCountAfterAuthentication(&id, &counter));
}

TEST(SecureOpStackDepth, SimpleSecureFrame32or0BodyRXFixedCounterStack)
{
    // Make sure flag is false.
    SOSDT::frameOperationCalledFlag = false;
    // Secure Frame start
    const uint8_t * senderID = SOSDT::minimumSecureFrame::id;
    const uint8_t * msgCounter = SOSDT::minimumSecureFrame::oldCounter;
    const uint8_t * const msgStart = &SOSDT::minimumSecureFrame::buf[1];

    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();

    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    sfrx.setMockIDValue(senderID);
    sfrx.setMockCounterValue(msgCounter);
    const bool test1 = OTRadioLink::decodeAndHandleOTSecureOFrame<OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter,
                                                                  OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleDec_DEFAULT_STATELESS,
                                                                  SOSDT::getKeySuccess,
                                                                  SOSDT::setFlagFrameOperation
                                                                 >(msgStart);

    const size_t maxStack = OTV0P2BASE::MemoryChecks::getMinSP();
    // Uncomment to print stack usage
     std::cout << "decodeAndHandleOTSecureOFrame stack: " << baseStack - maxStack << "\n";

    EXPECT_TRUE(test1);
    EXPECT_TRUE(SOSDT::frameOperationCalledFlag);
    EXPECT_GT(SOSDT::maxStackSecureFrameDecode, baseStack - maxStack);
}

TEST(SecureOpStackDepth, OTMessageQueueHandlerStackBasic)
{
    // Make sure flag is false.
    SOSDT::frameOperationCalledFlag = false;
    // Secure Frame start
    const uint8_t * senderID = SOSDT::minimumSecureFrame::id;
    const uint8_t * msgCounter = SOSDT::minimumSecureFrame::oldCounter;
//     const uint8_t * const msgStart = &SOSDT::minimumSecureFrame::buf[1];

    OTRadioLink::OTRadioLinkMock rl;
    memcpy(rl.message, SOSDT::minimumSecureFrame::buf, SOSDT::minimumSecureFrame::encodedLength + 1);

    // Set up stack usage checks
    OTV0P2BASE::RAMEND = OTV0P2BASE::getSP();
    OTV0P2BASE::MemoryChecks::resetMinSP();
    OTV0P2BASE::MemoryChecks::recordIfMinSP();
    const size_t baseStack = OTV0P2BASE::MemoryChecks::getMinSP();

    OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter &sfrx = OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter::getInstance();
    sfrx.setMockIDValue(senderID);
    sfrx.setMockCounterValue(msgCounter);

    OTRadioLink::OTMessageQueueHandler<
        SOSDT::pollIO, 4800,
        OTRadioLink::decodeAndHandleOTSecureOFrame<OTRadioLink::SimpleSecureFrame32or0BodyRXFixedCounter,
                                                                      OTAESGCM::fixed32BTextSize12BNonce16BTagSimpleDec_DEFAULT_STATELESS,
                                                                      SOSDT::getKeySuccess,
                                                                      SOSDT::setFlagFrameOperation
                                                                     >
                                                  > mh;

    EXPECT_TRUE(mh.handle(false, rl));

    const size_t maxStack = OTV0P2BASE::MemoryChecks::getMinSP();
    // Uncomment to print stack usage
     std::cout << "OTMessageQueueHandler stack: " << baseStack - maxStack << "\n";

    // EXPECT_TRUE(test1);
    EXPECT_TRUE(SOSDT::frameOperationCalledFlag);
    EXPECT_GT(SOSDT::maxStackSecureFrameDecode, baseStack - maxStack);
}

/**
 * Stack usage:
 * Date     | SimpleSecureFrame32or0BodyRXFixedCounterStack (OTMessageQueueHandlerStackBasic) | notes
 * 20170703 | 816 (928) | 928 probably mostly due to use of OTRadioLinkMock.
 */

#endif // ARDUINO_LIB_OTAESGCM
<|endoftext|>


<commit_before>/*
 * glfwInst.cpp
 *
 * Copyright (C) 2008, 2016 MegaMol Team
 * Alle Rechte vorbehalten.
 */

#include "stdafx.h"

#ifndef USE_EGL

#include "gl/glfwInst.h"
#include "vislib/graphics/gl/IncludeAllGL.h"
#include "GLFW/glfw3.h"
#include "vislib/sys/Log.h"
//#include <mutex>

using namespace megamol;
using namespace megamol::console;

std::weak_ptr<gl::glfwInst> gl::glfwInst::inst;

std::shared_ptr<gl::glfwInst> gl::glfwInst::Instance() {
    //static std::mutex instLock;
    //std::lock_guard<std::mutex> lock(instLock);
    std::shared_ptr<gl::glfwInst> i = inst.lock();
    if (!i) {
        i = std::shared_ptr<gl::glfwInst>(new glfwInst());
        inst = i;
    }
    return i;
}

gl::glfwInst::glfwInst() : error(false) {
    try {
        if (::glfwInit() != GL_TRUE) {
            vislib::sys::Log::DefaultLog.WriteError("glfwInit failed");
            error = true;
        }
    } catch (...) {
        vislib::sys::Log::DefaultLog.WriteError("glfwInit failed (exception)");
        error = true;
    }
}

gl::glfwInst::~glfwInst() {
    if (!error) {
        ::glfwTerminate();
    }
}

#endif // USE_EGL
<commit_msg>glfwInst: remove GL header dependency<commit_after>/*
 * glfwInst.cpp
 *
 * Copyright (C) 2008, 2016 MegaMol Team
 * Alle Rechte vorbehalten.
 */

#include "stdafx.h"

#ifndef USE_EGL

#include "gl/glfwInst.h"
#include "GLFW/glfw3.h"
#include "vislib/sys/Log.h"
//#include <mutex>

using namespace megamol;
using namespace megamol::console;

std::weak_ptr<gl::glfwInst> gl::glfwInst::inst;

std::shared_ptr<gl::glfwInst> gl::glfwInst::Instance() {
    //static std::mutex instLock;
    //std::lock_guard<std::mutex> lock(instLock);
    std::shared_ptr<gl::glfwInst> i = inst.lock();
    if (!i) {
        i = std::shared_ptr<gl::glfwInst>(new glfwInst());
        inst = i;
    }
    return i;
}

gl::glfwInst::glfwInst() : error(false) {
    try {
        if (::glfwInit() != GLFW_TRUE) {
            vislib::sys::Log::DefaultLog.WriteError("glfwInit failed");
            error = true;
        }
    } catch (...) {
        vislib::sys::Log::DefaultLog.WriteError("glfwInit failed (exception)");
        error = true;
    }
}

gl::glfwInst::~glfwInst() {
    if (!error) {
        ::glfwTerminate();
    }
}

#endif // USE_EGL
<|endoftext|>


<commit_before>// For conditions of distribution and use, see copyright notice in license.txt

#include "StableHeaders.h"

#include "MumbleVoipModule.h"
#include "LinkPlugin.h"
#include "ServerInfoProvider.h"
#include "ModuleManager.h"
#include "EC_OgrePlaceable.h"
#include "WorldLogicInterface.h"
#include "Entity.h"
#include "ConsoleCommandServiceInterface.h"
#include "EventManager.h"
#include "LinkPlugin.h"
#include "Provider.h"
#include "ApplicationManager.h"
#include "DebugOperatorNew.h"
#include "MemoryLeakCheck.h"
#include "MumbleLibrary.h"

namespace MumbleVoip
{
    std::string MumbleVoipModule::module_name_ = "MumbleVoipModule";

    MumbleVoipModule::MumbleVoipModule()
        : ModuleInterface(module_name_),
          link_plugin_(0),
          server_info_provider_(0),
          in_world_voice_provider_(0),
          time_from_last_update_ms_(0),
          use_camera_position_(false),
          use_native_mumble_client_(false)
    {
        QStringList arguments = QCoreApplication::arguments();
        foreach(QString arg, arguments)
        {
            if (arg == "--use_native_mumble_client")
                use_native_mumble_client_ = true;
        }
    }

    MumbleVoipModule::~MumbleVoipModule()
    {
        SAFE_DELETE(link_plugin_);
        SAFE_DELETE(server_info_provider_);
        SAFE_DELETE(in_world_voice_provider_);
    }

    void MumbleVoipModule::Load()
    {
        if (use_native_mumble_client_)
        {
            server_info_provider_ = new ServerInfoProvider(framework_);
            connect(server_info_provider_, SIGNAL(MumbleServerInfoReceived(ServerInfo)), SLOT(StartMumbleClient(ServerInfo)));
        }
        else
        {
            in_world_voice_provider_ = new InWorldVoice::Provider(framework_);
        }

        link_plugin_ = new LinkPlugin();
    }

    void MumbleVoipModule::Unload()
    {
    }

    void MumbleVoipModule::Initialize()
    {
        InitializeConsoleCommands();
    }

    void MumbleVoipModule::PostInitialize()
    {
        event_category_framework_ = framework_->GetEventManager()->QueryEventCategory("Framework");
        if (event_category_framework_ == 0)
            LogError("Unable to find event category for Framework");
    }

    void MumbleVoipModule::Uninitialize()
    {
        SAFE_DELETE(link_plugin_);
        SAFE_DELETE(server_info_provider_);
        SAFE_DELETE(in_world_voice_provider_);

        if (use_native_mumble_client_)
        {
            if (ApplicationManager::StartCount() > 0)
            {
                // Hack will disconnect from server and trying to connect new one
                ApplicationManager::StartMumbleClient("mumble://user@0.0.0.0?version=1.2.2");
            }
        }
        else
        {
            MumbleLibrary::Stop();
        }
    }

    void MumbleVoipModule::Update(f64 frametime)
    {
        if (link_plugin_ && link_plugin_->IsRunning())
            UpdateLinkPlugin(frametime);

        if (in_world_voice_provider_)
            in_world_voice_provider_->Update(frametime);
    }

    bool MumbleVoipModule::HandleEvent(event_category_id_t category_id, event_id_t event_id, Foundation::EventDataInterface* data)
    {
        if (in_world_voice_provider_)
            in_world_voice_provider_->HandleEvent(category_id, event_id, data);

        if (server_info_provider_)
            server_info_provider_->HandleEvent(category_id, event_id, data);

        return false;
    }

    void MumbleVoipModule::UpdateLinkPlugin(f64 frametime)
    {
        if (!link_plugin_)
            return;

        time_from_last_update_ms_ += 1000*frametime;
        if (time_from_last_update_ms_ < LINK_PLUGIN_UPDATE_INTERVAL_MS_)
            return;
        time_from_last_update_ms_ = 0;

        Vector3df top_vector = Vector3df::UNIT_Z, position, direction;
        if (GetAvatarPosition(position, direction))
            link_plugin_->SetAvatarPosition(position, direction, top_vector);

        if (use_camera_position_)
            if (GetCameraPosition(position, direction))
                link_plugin_->SetCameraPosition(position, direction, top_vector);
        else
            if (GetAvatarPosition(position, direction))
                link_plugin_->SetCameraPosition(position, direction, top_vector);

        link_plugin_->SendData();
    }

    bool MumbleVoipModule::GetAvatarPosition(Vector3df& position, Vector3df& direction)
    {
        using namespace Foundation;
        boost::shared_ptr<WorldLogicInterface> worldLogic = framework_->GetServiceManager()->GetService<WorldLogicInterface>(Service::ST_WorldLogic).lock();
        if (!worldLogic)
            return false;

        Scene::EntityPtr avatar = worldLogic->GetUserAvatarEntity();
        if (!avatar)
            return false;

        boost::shared_ptr<OgreRenderer::EC_OgrePlaceable> ogre_placeable = avatar->GetComponent<OgreRenderer::EC_OgrePlaceable>();
        if (!ogre_placeable)
            return false;

        Quaternion q = ogre_placeable->GetOrientation();
        position = ogre_placeable->GetPosition();
        direction = q*Vector3df::UNIT_X;
        return true;
    }

    bool MumbleVoipModule::GetCameraPosition(Vector3df& position, Vector3df& direction)
    {
        using namespace Foundation;
        boost::shared_ptr<WorldLogicInterface> worldLogic = framework_->GetServiceManager()->GetService<WorldLogicInterface>(Service::ST_WorldLogic).lock();
        if (!worldLogic)
            return false;

        Scene::EntityPtr camera = worldLogic->GetCameraEntity();
        if (!camera)
            return false;

        boost::shared_ptr<OgreRenderer::EC_OgrePlaceable> ogre_placeable = camera->GetComponent<OgreRenderer::EC_OgrePlaceable>();
        if (!ogre_placeable)
            return false;

        Quaternion q = ogre_placeable->GetOrientation();
        position = ogre_placeable->GetPosition();
        direction = q*Vector3df::UNIT_X;
        return true;
    }

    void MumbleVoipModule::InitializeConsoleCommands()
    {
        RegisterConsoleCommand(Console::CreateCommand("mumble link", "Start Mumble link plugin: 'mumble link(user_id, context_id)'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleLink)));
        RegisterConsoleCommand(Console::CreateCommand("mumble unlink", "Stop Mumble link plugin: 'mumble unlink'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleUnlink)));
        RegisterConsoleCommand(Console::CreateCommand("mumble start", "Start Mumble client application: 'mumble start(server_url)'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleStart)));
        RegisterConsoleCommand(Console::CreateCommand("mumble stats", "Show mumble statistics", Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleStats)));

        //RegisterConsoleCommand(Console::CreateCommand("mumble enable vad", "Enable voice activity detector",
        //    Console::Bind(this, &MumbleVoipModule::OnConsoleEnableVoiceActivityDetector)));
        //RegisterConsoleCommand(Console::CreateCommand("mumble disable vad", "Disable voice activity detector",
        //    Console::Bind(this, &MumbleVoipModule::OnConsoleDisableVoiceActivityDetector)));
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleLink(const StringVector &params)
    {
        if (!link_plugin_)
            return Console::ResultFailure("Link plugin is not initialized.");
        if (params.size() != 2)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble link(id, context)'");
        }
        QString id = params[0].c_str();
        QString context = params[1].c_str();

        link_plugin_->SetUserIdentity(id);
        link_plugin_->SetContextId(context);
        link_plugin_->SetApplicationName("Naali viewer");
        link_plugin_->SetApplicationDescription("Naali viewer by realXtend project");
        link_plugin_->Start();

        if (!link_plugin_->IsRunning())
        {
            QString error_message = "Link plugin connection cannot be established. ";
            error_message.append(link_plugin_->GetReason());
            return Console::ResultFailure(error_message.toStdString());
        }

        QString message = QString("Mumbe link plugin started: id=%1 context=%2").arg(id).arg(context);
        return Console::ResultSuccess(message.toStdString());
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleUnlink(const StringVector &params)
    {
        if (!link_plugin_)
            return Console::ResultFailure("Link plugin is not initialized.");
        if (params.size() != 0)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble unlink'");
        }

        if (!link_plugin_->IsRunning())
        {
            return Console::ResultFailure("Mumbe link plugin was not running.");
        }

        link_plugin_->Stop();
        return Console::ResultSuccess("Mumbe link plugin stopped.");
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleStart(const StringVector &params)
    {
        if (params.size() != 1)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble start(server_url)'");
        }
        QString server_url = params[0].c_str();

        try
        {
            ApplicationManager::StartMumbleClient(server_url);
            return Console::ResultSuccess("Mumbe client started.");
        }
        catch(std::exception &e)
        {
            QString error_message = QString("Cannot start Mumble client: %1").arg(QString(e.what()));
            return Console::ResultFailure(error_message.toStdString());
        }
    }

    void MumbleVoipModule::StartMumbleClient(ServerInfo info)
    {
        QUrl murmur_url(QString("mumble://%1/%2").arg(info.server).arg(info.channel)); // setScheme method does not add '//' between scheme and host.
        murmur_url.setUserName(info.user_name);
        murmur_url.setPassword(info.password);
        murmur_url.setQueryItems(QList<QPair<QString,QString> >() << QPair<QString,QString>("version", info.version));

        try
        {
            LogInfo("Starting mumble client.");
            ApplicationManager::StartMumbleClient(murmur_url.toString());

            // it takes some time for a mumble client to setup shared memory for link plugins
            // so we have to wait some time before we can start our link plugin.
            avatar_id_for_link_plugin_ = info.avatar_id;
            context_id_for_link_plugin_ = info.context_id;
            QTimer::singleShot(2000, this, SLOT(StartLinkPlugin()));
        }
        catch(std::exception &e)
        {
            QString messge = QString("Cannot start Mumble client: %1").arg(e.what());
            LogError(messge.toStdString());
            return;
        }
    }

    void MumbleVoipModule::StartLinkPlugin()
    {
        if (!link_plugin_)
            return;

        link_plugin_->SetUserIdentity(avatar_id_for_link_plugin_);
        link_plugin_->SetContextId(context_id_for_link_plugin_);
        link_plugin_->SetApplicationName("Naali viewer");
        link_plugin_->SetApplicationDescription("Naali viewer by realXtend project");
        link_plugin_->Start();

        if (link_plugin_->IsRunning())
        {
            QString message = QString("Mumbe link plugin started: id '%1' context '%2'").arg(avatar_id_for_link_plugin_).arg(context_id_for_link_plugin_);
            LogInfo(message.toStdString());
        }
        else
        {
            QString error_message = QString("Link plugin connection cannot be established. %1 ").arg(link_plugin_->GetReason());
            LogError(error_message.toStdString());
        }
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleStats(const StringVector &params)
    {

        if (in_world_voice_provider_)
        {
            QList<QString> stats = in_world_voice_provider_->Statistics();
            foreach(QString line, stats)
            {
                LogInfo(line.toStdString());
            }
        }
        QString message = QString("");
        return Console::ResultSuccess(message.toStdString());
    }

//    Console::CommandResult MumbleVoipModule::OnConsoleEnableVoiceActivityDetector(const StringVector &params)
//    {
////        connection_manager_->EnableVAD(true);
//        QString message = QString("Voice activity detector enabled.");
//        return Console::ResultSuccess(message.toStdString());
//    }

  //  Console::CommandResult MumbleVoipModule::OnConsoleDisableVoiceActivityDetector(const StringVector &params)
  //  {
  ////      connection_manager_->EnableVAD(false);
  //      QString message = QString("Voice activity detector disabled.");
  //      return Console::ResultSuccess(message.toStdString());
  //  }

} // end of namespace: MumbleVoip

extern "C" void POCO_LIBRARY_API SetProfiler(Foundation::Profiler *profiler);
void SetProfiler(Foundation::Profiler *profiler)
{
    Foundation::ProfilerSection::SetProfiler(profiler);
}

using namespace MumbleVoip;

POCO_BEGIN_MANIFEST(Foundation::ModuleInterface)
    POCO_EXPORT_CLASS(MumbleVoipModule)
POCO_END_MANIFEST
<commit_msg>Moved Mumble console command initialization to PostInitialize. Otherwise they fail since Console service is not up yet.<commit_after>// For conditions of distribution and use, see copyright notice in license.txt

#include "StableHeaders.h"

#include "MumbleVoipModule.h"
#include "LinkPlugin.h"
#include "ServerInfoProvider.h"
#include "ModuleManager.h"
#include "EC_OgrePlaceable.h"
#include "WorldLogicInterface.h"
#include "Entity.h"
#include "ConsoleCommandServiceInterface.h"
#include "EventManager.h"
#include "LinkPlugin.h"
#include "Provider.h"
#include "ApplicationManager.h"
#include "DebugOperatorNew.h"
#include "MemoryLeakCheck.h"
#include "MumbleLibrary.h"

namespace MumbleVoip
{
    std::string MumbleVoipModule::module_name_ = "MumbleVoipModule";

    MumbleVoipModule::MumbleVoipModule()
        : ModuleInterface(module_name_),
          link_plugin_(0),
          server_info_provider_(0),
          in_world_voice_provider_(0),
          time_from_last_update_ms_(0),
          use_camera_position_(false),
          use_native_mumble_client_(false)
    {
        QStringList arguments = QCoreApplication::arguments();
        foreach(QString arg, arguments)
        {
            if (arg == "--use_native_mumble_client")
                use_native_mumble_client_ = true;
        }
    }

    MumbleVoipModule::~MumbleVoipModule()
    {
        SAFE_DELETE(link_plugin_);
        SAFE_DELETE(server_info_provider_);
        SAFE_DELETE(in_world_voice_provider_);
    }

    void MumbleVoipModule::Load()
    {
        if (use_native_mumble_client_)
        {
            server_info_provider_ = new ServerInfoProvider(framework_);
            connect(server_info_provider_, SIGNAL(MumbleServerInfoReceived(ServerInfo)), SLOT(StartMumbleClient(ServerInfo)));
        }
        else
        {
            in_world_voice_provider_ = new InWorldVoice::Provider(framework_);
        }

        link_plugin_ = new LinkPlugin();
    }

    void MumbleVoipModule::Unload()
    {
    }

    void MumbleVoipModule::Initialize()
    {
    }

    void MumbleVoipModule::PostInitialize()
    {
        InitializeConsoleCommands();

        event_category_framework_ = framework_->GetEventManager()->QueryEventCategory("Framework");
        if (event_category_framework_ == 0)
            LogError("Unable to find event category for Framework");
    }

    void MumbleVoipModule::Uninitialize()
    {
        SAFE_DELETE(link_plugin_);
        SAFE_DELETE(server_info_provider_);
        SAFE_DELETE(in_world_voice_provider_);

        if (use_native_mumble_client_)
        {
            if (ApplicationManager::StartCount() > 0)
            {
                // Hack will disconnect from server and trying to connect new one
                ApplicationManager::StartMumbleClient("mumble://user@0.0.0.0?version=1.2.2");
            }
        }
        else
        {
            MumbleLibrary::Stop();
        }
    }

    void MumbleVoipModule::Update(f64 frametime)
    {
        if (link_plugin_ && link_plugin_->IsRunning())
            UpdateLinkPlugin(frametime);

        if (in_world_voice_provider_)
            in_world_voice_provider_->Update(frametime);
    }

    bool MumbleVoipModule::HandleEvent(event_category_id_t category_id, event_id_t event_id, Foundation::EventDataInterface* data)
    {
        if (in_world_voice_provider_)
            in_world_voice_provider_->HandleEvent(category_id, event_id, data);

        if (server_info_provider_)
            server_info_provider_->HandleEvent(category_id, event_id, data);

        return false;
    }

    void MumbleVoipModule::UpdateLinkPlugin(f64 frametime)
    {
        if (!link_plugin_)
            return;

        time_from_last_update_ms_ += 1000*frametime;
        if (time_from_last_update_ms_ < LINK_PLUGIN_UPDATE_INTERVAL_MS_)
            return;
        time_from_last_update_ms_ = 0;

        Vector3df top_vector = Vector3df::UNIT_Z, position, direction;
        if (GetAvatarPosition(position, direction))
            link_plugin_->SetAvatarPosition(position, direction, top_vector);

        if (use_camera_position_)
            if (GetCameraPosition(position, direction))
                link_plugin_->SetCameraPosition(position, direction, top_vector);
        else
            if (GetAvatarPosition(position, direction))
                link_plugin_->SetCameraPosition(position, direction, top_vector);

        link_plugin_->SendData();
    }

    bool MumbleVoipModule::GetAvatarPosition(Vector3df& position, Vector3df& direction)
    {
        using namespace Foundation;
        boost::shared_ptr<WorldLogicInterface> worldLogic = framework_->GetServiceManager()->GetService<WorldLogicInterface>(Service::ST_WorldLogic).lock();
        if (!worldLogic)
            return false;

        Scene::EntityPtr avatar = worldLogic->GetUserAvatarEntity();
        if (!avatar)
            return false;

        boost::shared_ptr<OgreRenderer::EC_OgrePlaceable> ogre_placeable = avatar->GetComponent<OgreRenderer::EC_OgrePlaceable>();
        if (!ogre_placeable)
            return false;

        Quaternion q = ogre_placeable->GetOrientation();
        position = ogre_placeable->GetPosition();
        direction = q*Vector3df::UNIT_X;
        return true;
    }

    bool MumbleVoipModule::GetCameraPosition(Vector3df& position, Vector3df& direction)
    {
        using namespace Foundation;
        boost::shared_ptr<WorldLogicInterface> worldLogic = framework_->GetServiceManager()->GetService<WorldLogicInterface>(Service::ST_WorldLogic).lock();
        if (!worldLogic)
            return false;

        Scene::EntityPtr camera = worldLogic->GetCameraEntity();
        if (!camera)
            return false;

        boost::shared_ptr<OgreRenderer::EC_OgrePlaceable> ogre_placeable = camera->GetComponent<OgreRenderer::EC_OgrePlaceable>();
        if (!ogre_placeable)
            return false;

        Quaternion q = ogre_placeable->GetOrientation();
        position = ogre_placeable->GetPosition();
        direction = q*Vector3df::UNIT_X;
        return true;
    }

    void MumbleVoipModule::InitializeConsoleCommands()
    {
        RegisterConsoleCommand(Console::CreateCommand("mumble link", "Start Mumble link plugin: 'mumble link(user_id, context_id)'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleLink)));
        RegisterConsoleCommand(Console::CreateCommand("mumble unlink", "Stop Mumble link plugin: 'mumble unlink'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleUnlink)));
        RegisterConsoleCommand(Console::CreateCommand("mumble start", "Start Mumble client application: 'mumble start(server_url)'",
            Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleStart)));
        RegisterConsoleCommand(Console::CreateCommand("mumble stats", "Show mumble statistics", Console::Bind(this, &MumbleVoipModule::OnConsoleMumbleStats)));

        //RegisterConsoleCommand(Console::CreateCommand("mumble enable vad", "Enable voice activity detector",
        //    Console::Bind(this, &MumbleVoipModule::OnConsoleEnableVoiceActivityDetector)));
        //RegisterConsoleCommand(Console::CreateCommand("mumble disable vad", "Disable voice activity detector",
        //    Console::Bind(this, &MumbleVoipModule::OnConsoleDisableVoiceActivityDetector)));
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleLink(const StringVector &params)
    {
        if (!link_plugin_)
            return Console::ResultFailure("Link plugin is not initialized.");
        if (params.size() != 2)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble link(id, context)'");
        }
        QString id = params[0].c_str();
        QString context = params[1].c_str();

        link_plugin_->SetUserIdentity(id);
        link_plugin_->SetContextId(context);
        link_plugin_->SetApplicationName("Naali viewer");
        link_plugin_->SetApplicationDescription("Naali viewer by realXtend project");
        link_plugin_->Start();

        if (!link_plugin_->IsRunning())
        {
            QString error_message = "Link plugin connection cannot be established. ";
            error_message.append(link_plugin_->GetReason());
            return Console::ResultFailure(error_message.toStdString());
        }

        QString message = QString("Mumbe link plugin started: id=%1 context=%2").arg(id).arg(context);
        return Console::ResultSuccess(message.toStdString());
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleUnlink(const StringVector &params)
    {
        if (!link_plugin_)
            return Console::ResultFailure("Link plugin is not initialized.");
        if (params.size() != 0)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble unlink'");
        }

        if (!link_plugin_->IsRunning())
        {
            return Console::ResultFailure("Mumbe link plugin was not running.");
        }

        link_plugin_->Stop();
        return Console::ResultSuccess("Mumbe link plugin stopped.");
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleStart(const StringVector &params)
    {
        if (params.size() != 1)
        {
            return Console::ResultFailure("Wrong number of arguments: usage 'mumble start(server_url)'");
        }
        QString server_url = params[0].c_str();

        try
        {
            ApplicationManager::StartMumbleClient(server_url);
            return Console::ResultSuccess("Mumbe client started.");
        }
        catch(std::exception &e)
        {
            QString error_message = QString("Cannot start Mumble client: %1").arg(QString(e.what()));
            return Console::ResultFailure(error_message.toStdString());
        }
    }

    void MumbleVoipModule::StartMumbleClient(ServerInfo info)
    {
        QUrl murmur_url(QString("mumble://%1/%2").arg(info.server).arg(info.channel)); // setScheme method does not add '//' between scheme and host.
        murmur_url.setUserName(info.user_name);
        murmur_url.setPassword(info.password);
        murmur_url.setQueryItems(QList<QPair<QString,QString> >() << QPair<QString,QString>("version", info.version));

        try
        {
            LogInfo("Starting mumble client.");
            ApplicationManager::StartMumbleClient(murmur_url.toString());

            // it takes some time for a mumble client to setup shared memory for link plugins
            // so we have to wait some time before we can start our link plugin.
            avatar_id_for_link_plugin_ = info.avatar_id;
            context_id_for_link_plugin_ = info.context_id;
            QTimer::singleShot(2000, this, SLOT(StartLinkPlugin()));
        }
        catch(std::exception &e)
        {
            QString messge = QString("Cannot start Mumble client: %1").arg(e.what());
            LogError(messge.toStdString());
            return;
        }
    }

    void MumbleVoipModule::StartLinkPlugin()
    {
        if (!link_plugin_)
            return;

        link_plugin_->SetUserIdentity(avatar_id_for_link_plugin_);
        link_plugin_->SetContextId(context_id_for_link_plugin_);
        link_plugin_->SetApplicationName("Naali viewer");
        link_plugin_->SetApplicationDescription("Naali viewer by realXtend project");
        link_plugin_->Start();

        if (link_plugin_->IsRunning())
        {
            QString message = QString("Mumbe link plugin started: id '%1' context '%2'").arg(avatar_id_for_link_plugin_).arg(context_id_for_link_plugin_);
            LogInfo(message.toStdString());
        }
        else
        {
            QString error_message = QString("Link plugin connection cannot be established. %1 ").arg(link_plugin_->GetReason());
            LogError(error_message.toStdString());
        }
    }

    Console::CommandResult MumbleVoipModule::OnConsoleMumbleStats(const StringVector &params)
    {

        if (in_world_voice_provider_)
        {
            QList<QString> stats = in_world_voice_provider_->Statistics();
            foreach(QString line, stats)
            {
                LogInfo(line.toStdString());
            }
        }
        QString message = QString("");
        return Console::ResultSuccess(message.toStdString());
    }

//    Console::CommandResult MumbleVoipModule::OnConsoleEnableVoiceActivityDetector(const StringVector &params)
//    {
////        connection_manager_->EnableVAD(true);
//        QString message = QString("Voice activity detector enabled.");
//        return Console::ResultSuccess(message.toStdString());
//    }

  //  Console::CommandResult MumbleVoipModule::OnConsoleDisableVoiceActivityDetector(const StringVector &params)
  //  {
  ////      connection_manager_->EnableVAD(false);
  //      QString message = QString("Voice activity detector disabled.");
  //      return Console::ResultSuccess(message.toStdString());
  //  }

} // end of namespace: MumbleVoip

extern "C" void POCO_LIBRARY_API SetProfiler(Foundation::Profiler *profiler);
void SetProfiler(Foundation::Profiler *profiler)
{
    Foundation::ProfilerSection::SetProfiler(profiler);
}

using namespace MumbleVoip;

POCO_BEGIN_MANIFEST(Foundation::ModuleInterface)
    POCO_EXPORT_CLASS(MumbleVoipModule)
POCO_END_MANIFEST
<|endoftext|>