#ifndef MAT_METHODS_H
#define MAT_METHODS_H

#pragma once
#include <stdio.h>
#include <opencv2/opencv.hpp>    
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/aruco.hpp>
#include <iostream>

#include <Eigen/Geometry>
#include <vector>
#include <random>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <utility>
#include <boost/program_options.hpp>

#include "libs/tcpc/tcpc_defs.h"
#include "line.h"

namespace tcpc{
//deprecated
cv::Point getTCP(cv::Mat& inputBIN, std::vector<cv::Line> lines, bool showALL = true);

sHSV_Mat_Pattern detectComponents(cv::Mat inputFrame, sHSV_boundaries boundaries);

cv::Mat detectComponent(cv::Mat inputFrame, sHSV_components boundaries);

std::vector<cv::Point> findContours(cv::Mat inputFrame);

std::vector<cv::Point> getSignificantContours(cv::Mat inputFrame, sHSV_components boundariesHSV, sThresholds cannyThresholds, uint8_t dilateThickness=4);

cv::Point getGravityCenter(std::vector<cv::Point> convex_hull);

uint16_t mm_to_px(cv::Point begin, cv::Point end);

cv::Point getLowestPoint(std::vector<cv::Point> points, const char axis);

cv::Point getHighestPoint(std::vector<cv::Point> points, const char axis);

cv::Point getMiddlePoint(cv::Point begin, cv::Point end);

cv::Point getLowestMiddle(std::vector<cv::Point> points, const char axis);

cv::Point getHighestMiddle(std::vector<cv::Point> points, const char axis);

void triangulatePoint(cv::Point2f front_point, cv::Point2f side_point, cv::Point3f& triangulated_point, cv::Mat Q_matrix);

void triangulatePoint(cv::Point2f front_point, cv::Point2f side_point, cv::Mat& triangulated_point, cv::Mat Q_matrix);

void getPxOffsets(cv::Point center, cv::Point point, int32_t& xOffset, int32_t& yOffset);

void getPxOffset(cv::Point center, cv::Point point, int32_t& offset, const char flag);

void rigidTransform(cv::Point3f camera_3D_Point, cv::Point3f robot_3D_Point, cv::Mat &Rot_matrix, cv::Mat &trans_vector);

void rigidTransform(std::vector<cv::Point3f> camera_3D_Point, std::vector<cv::Point3f> robot_3D_Point, cv::Mat &Rot_matrix, cv::Mat &trans_vector);

cv::Mat toGray(cv::Mat inputMat);

cv::Point detect_ArUco_gravity(cv::Mat inputMat, cv::Mat K_matrix, cv::Mat Dist_coeffs, cv::Vec3d& R_vec, cv::Vec3d& t_vec);

cv::Point detect_ArUco_gravity(cv::Mat inputMat, cv::Mat K_matrix, cv::Mat Dist_coeffs);

void filterPointsbyGravity(std::vector<cv::Point>& points, std::string flag);

ri::Point estimateTCP(std::vector<ri::Point>detected_points);


std::pair<ri::Point, double> getGripperPosition(std::vector<ri::Point> calibPositions, ri::Point rotation, int max_iterations);
double gripperOffsetLoss(std::vector<ri::Point> calibPositions, Eigen::Matrix4d gripperMatrix);
Eigen::Matrix4d pointToMatrix(ri::Point pt);
double calibQuality(std::vector<ri::Point> calibPositions);
Eigen::Vector3d computeReferencePoint(std::vector<ri::Point> calibPositions, ri::Point gripper);

};//end_namespace_tcpc
#endif // MAT_METHODS_H
