#include "rmoss_util/debug.hpp"
//获取调试开关状态，默认为false，
rmoss_util::get_debug();
//设置静态调试开关
rmoss_util::set_debug(true);

// RMOSS_DEBUG(text); text为一条语句 ，如果静态调试开关为false，括号里面的语句不会被执行。
RMOSS_DEBUG(imshow("dst", dst));
// 用于输出调试信息
RMOSS_DEBUG(std::cout<<"data"<<std::endl);
// 在图像上绘制多边形，用于调试图像
RMOSS_DEBUG(rmoss_util::draw_rotated_rect(img,r));

// task manager
auto get_task_status_cb = [&]() {
    return rmoss_util::TaskStatus::Running;
  };
auto control_task_cb = [&](rmoss_util::TaskCmd cmd) {
    if (cmd == rmoss_util::TaskCmd::Start) {
      // do something
    } else if (cmd == rmoss_util::TaskCmd::Stop) {
      // do something
    } else {
      return false;
    }
    return true;
  };
auto task_manager_ = std::make_shared<rmoss_util::TaskManager>(node_, get_task_status_cb, control_task_cb);

//
void draw_rotated_rect(cv::Mat &img,cv::RotatedRect r,cv::Scalar color=green);
void draw_4points(cv::Mat &img, cv::Point2f *point2fs,cv::Scalar color=green);
void draw_convex_hull(cv::Mat &img,std::vector<cv::Point2f> points,cv::Scalar color=green);

//两点构成直线的倾角，相对常规坐标系，（不同于图像坐标系）常规坐标系y轴方向为向上。
float calc_inclination_angle(cv::Point2f point1, cv::Point2f point2);
//三角形的角度，第一个参数为顶点坐标
float calc_inner_angle(cv::Point2f vertex_point, cv::Point2f point1, cv::Point2f point2);

rmoss_util::MonoMeasureTool mono_location_tool_;
mono_location_tool_.set_camera_info(camera_intrinsic, camera_distortion);
mono_location_tool_.solve_pnp(detected_points, small_armor_points, target_postion);