/*
  Copyright (C) Skeletex Research, s.r.o. - All Rights Reserved
  Unauthorized copying of this file, via any medium is strictly prohibited
  Proprietary and confidential
*/
#ifndef UTILS_EXT_CV_H
#define UTILS_EXT_CV_H
#include <glm/glm.hpp>
#include <opencv2/opencv.hpp>
#include <COGS/Color.h>



//! Extensions to the OpenCV library.
namespace cv_ext
{

  inline glm::vec3 ToVec3(const cv::Point3f &vec)
  {
    return glm::vec3(vec.x, vec.y, vec.z);
  }

  inline glm::vec3 ToVec3(const cv::Vec3f &vec)
  {
    return glm::vec3(vec[0], vec[1], vec[2]);
  }

  inline cv::Point3f ToCvPoint3f(const glm::vec3 &vec)
  {
    return cv::Point3f(vec.x, vec.y, vec.z);
  }

  inline cv::Point2f ToCvPoint2f(const glm::vec2 &vec)
  {
    return cv::Point2f(vec.x, vec.y);
  }

  inline cv::Vec3f ToCvVec3f(const glm::vec3 &vec)
  {
    return cv::Vec3f(vec.x, vec.y, vec.z);
  }

  inline cv::Vec2f ToCvVec2f(const glm::vec2 &vec)
  {
    return cv::Vec2f(vec.x, vec.y);
  }

  inline cv::Point3f ToCvPoint3f(const cogs::Color3f &color)
  {
    return cv::Point3f(color.r, color.g, color.b);
  }

  inline glm::mat3 ToMat3(const cv::Matx33d &mat)
  {
    glm::mat3 result(1.0f);
    for (glm::mat3::length_type i = 0; i < 3; ++i)
    {
      for (glm::mat3::length_type j = 0; j < 3; ++j)
      {
        result[i][j] = static_cast<float>(mat(i, j));
      }
    }
    return result;
  }

  inline glm::mat4 ToMat4(const cv::Matx33d &mat)
  {
    glm::mat4 result(1.0f);
    for (glm::mat4::length_type i = 0; i < 3; ++i)
    {
      for (glm::mat4::length_type j = 0; j < 3; ++j)
      {
        result[i][j] = static_cast<float>(mat(i, j));
      }
    }
    return result;
  }

}

#endif /* !UTILS_EXT_CV_H */