#include "mat_methods.h"

namespace tcpc{

sHSV_Mat_Pattern detectComponents(cv::Mat inputFrame, sHSV_boundaries boundaries){
    sHSV_Mat_Pattern result;
    cv::Mat HSVframe;
    cv::cvtColor(inputFrame, HSVframe, CV_BGR2HSV);
    cv::inRange(HSVframe, cv::Scalar(boundaries.side.Hue.Lower, boundaries.side.Saturation.Lower, boundaries.side.Value.Lower),
                          cv::Scalar(boundaries.side.Hue.Upper, boundaries.side.Saturation.Upper, boundaries.side.Value.Upper), result.side);
    cv::inRange(HSVframe, cv::Scalar(boundaries.front.Hue.Lower, boundaries.front.Saturation.Lower, boundaries.front.Value.Lower),
                          cv::Scalar(boundaries.front.Hue.Upper, boundaries.front.Saturation.Upper, boundaries.front.Value.Upper), result.front );

    cv::Mat kernel = cv::Mat::ones(3,3, CV_32F);

    cv::morphologyEx(result.side, result.side, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(result.side, result.side, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(result.front, result.front, cv::MORPH_DILATE, kernel);
    cv::morphologyEx(result.front, result.front, cv::MORPH_DILATE, kernel);

    return result;
}

cv::Mat detectComponent(cv::Mat inputFrame, sHSV_components boundaries){
    cv::Mat result, HSVframe;
    cv::cvtColor(inputFrame, HSVframe, CV_BGR2HSV);
    cv::inRange(HSVframe, cv::Scalar(boundaries.Hue.Lower, boundaries.Saturation.Lower, boundaries.Value.Lower),
                          cv::Scalar(boundaries.Hue.Upper, boundaries.Saturation.Upper, boundaries.Value.Upper), result);

    cv::Mat kernel = cv::Mat::ones(3,3, CV_32F);
    cv::morphologyEx(result, result, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(result, result, cv::MORPH_DILATE, kernel);

    return result;
}

std::vector<cv::Point> findContours(cv::Mat inputFrame){
    std::vector<std::vector<cv::Point> > contours;
	int max_area = 0;
	std::vector<cv::Point> large_contour;
	std::vector<cv::Point> convex_hull;
    cv::findContours(inputFrame, contours, cv::RETR_LIST,cv::CHAIN_APPROX_SIMPLE);

    // find max area contours
	for (unsigned int i = 0; i < contours.size(); ++i) {
		int area = (int)cv::contourArea(contours[i]);
		if (area > max_area) {
			large_contour = contours[i];
			max_area = area;
		}
	}
    if (max_area == 0) throw std::runtime_error("Not possible to detect contours");
	
	// simplify large contours
    cv::approxPolyDP(cv::Mat(large_contour), large_contour, 5, true);//5
	
	// convex hull
	cv::convexHull(large_contour, convex_hull, false);
    if (convex_hull.size() < 3 ) throw std::runtime_error("Not possible to detect contours");

    return convex_hull;
}

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

    cv::Mat patternHSV = detectComponent(inputFrame, boundariesHSV);

    cv::Canny(patternHSV, morphPattern, cannyThresholds.Low, cannyThresholds.High);
    cv::dilate(morphPattern, morphPattern, cv::Mat(), cv::Point(-1, -1), dilateThickness, 1, 1);
    return tcpc::findContours(morphPattern);
}

cv::Point getGravityCenter(std::vector<cv::Point> convex_hull){
    cv::Moments mo = cv::moments(convex_hull);
	return cv::Point(mo.m10/mo.m00 , mo.m01/mo.m00);
}

uint16_t mm_to_px(cv::Point begin, cv::Point end){
    double x_dif = begin.x - end.x;
    double y_dif = begin.y - end.y;

    return std::sqrt((x_dif * x_dif)+(y_dif * y_dif));
}

void filterPointsbyGravity(std::vector<cv::Point>& points, std::string flag){
    if (points.size()==0) throw std::runtime_error("Empty vector to filter points");
    if (flag != "<x" && flag != ">x" && flag != "<y" && flag != ">y") throw std::runtime_error("unsuported flag");

    cv::Point gravity = getGravityCenter(points);
    for(int i=0;i<int(points.size());i++){
       if(flag=="<x"){
           if(points.at(i).x>gravity.x){
               points.erase(points.begin()+i);
               i--;
           }
       }
       if(flag==">x"){
           if(points.at(i).x<gravity.x){
               points.erase(points.begin()+i);
               i--;
           }
       }
       if(flag=="<y"){
           if(points.at(i).y>gravity.y){
               points.erase(points.begin()+i);
               i--;
           }
       }
       if(flag==">y"){
           if(points.at(i).y<gravity.y){
               points.erase(points.begin()+i);
               i--;
           }
       }
    }
}

cv::Point getLowestPoint(std::vector<cv::Point> points, const char axis){
    cv::Point min;
    switch (axis) {
        case 'y':
            min.y = points.at(0).y;
            for(cv::Point& point: points){
                if(point.y < min.y) min = point;
            }
        break;
        case 'x':
            min.x = points.at(0).x;
            for(cv::Point& point: points){
                if(point.x < min.x) min = point;
            }
        break;
    }
    return min;
}

cv::Point getHighestPoint(std::vector<cv::Point> points, const char axis){
    cv::Point max;
    switch (axis) {
        case 'y':
            max.y = points.at(0).y;
            for(cv::Point& point: points){
                if(point.y > max.y) max = point;
            }
        break;
        case 'x':
            max.x = points.at(0).x;
            for(cv::Point& point: points){
                if(point.x > max.x) max = point;
            }
        break;
    }
    return max;
}

cv::Point getMiddlePoint(cv::Point begin, cv::Point end){
    cv::Point result;
    result.x = (begin.x + end.x)/2;
    result.y = (begin.y + end.y)/2;

    return result;
}

cv::Point getLowestMiddle(std::vector<cv::Point> points, const char axis){
    cv::Point middle;
    std::vector<cv::Point> filteredPoints;
    cv::Point gravity = getGravityCenter(points);
    switch (axis) {
        case 'y':
            for(cv::Point& point: points){
                if(point.y < gravity.y) filteredPoints.push_back(point);
            }
            if(filteredPoints.size() < 2) throw std::runtime_error("Not possible to calculate middle of points");
            else if(filteredPoints.size() == 2) return getMiddlePoint(filteredPoints.at(0), filteredPoints.at(1));
            else{
                uint8_t iter = 0;
                uint16_t x = 0, y = 0;
                for(uint8_t i=0;i<filteredPoints.size();i++){
                    x += filteredPoints.at(i).x;
                    y += filteredPoints.at(i).y;
                    iter++;
                }
                x /= iter;
                y /= iter;
                middle.x = x;
                middle.y = y;
            }
        break;
        case 'x':
            for(cv::Point& point: points){
                if(point.x < gravity.x) filteredPoints.push_back(point);
            }
            if(filteredPoints.size() < 2) throw std::runtime_error("Not possible to calculate middle of points");
            else if(filteredPoints.size() == 2) return getMiddlePoint(filteredPoints.at(0), filteredPoints.at(1));
            else{
                uint8_t iter = 0;
                uint16_t x = 0, y = 0;
                for(uint8_t i=0;i<filteredPoints.size();i++){
                    x += filteredPoints.at(i).x;
                    y += filteredPoints.at(i).y;
                    iter++;
                }
                x /= iter;
                y /= iter;
                middle.x = x;
                middle.y = y;
            }
        break;
    }
    return middle;
}

cv::Point getHighestMiddle(std::vector<cv::Point> points, const char axis){
    cv::Point middle;
    std::vector<cv::Point> filteredPoints;
    cv::Point gravity = getGravityCenter(points);
    switch (axis) {
        case 'y':
            for(cv::Point& point: points){
                if(point.y > gravity.y) filteredPoints.push_back(point);
            }
            if(filteredPoints.size() < 2) throw std::runtime_error("Not possible to calculate middle of points");
            else if(filteredPoints.size() == 2) return getMiddlePoint(filteredPoints.at(0), filteredPoints.at(1));
            else{
                uint8_t iter = 0;
                uint16_t x = 0, y = 0;
                for(uint8_t i=0;i<filteredPoints.size();i++){
                    x += filteredPoints.at(i).x;
                    y += filteredPoints.at(i).y;
                    iter++;
                }
                x /= iter;
                y /= iter;
                middle.x = x;
                middle.y = y;
            }
        break;
        case 'x':
            for(cv::Point& point: points){
                if(point.x > gravity.x) filteredPoints.push_back(point);
            }
            if(filteredPoints.size() < 2) throw std::runtime_error("Not possible to calculate middle of points");
            else if(filteredPoints.size() == 2) return getMiddlePoint(filteredPoints.at(0), filteredPoints.at(1));
            else{
                uint8_t iter = 0;
                uint16_t x = 0, y = 0;
                for(uint8_t i=0;i<filteredPoints.size();i++){
                    x += filteredPoints.at(i).x;
                    y += filteredPoints.at(i).y;
                    iter++;
                }
                x /= iter;
                y /= iter;
                middle.x = x;
                middle.y = y;
            }
        break;
    }
    return middle;
}

void getPxOffsets(cv::Point center, cv::Point point, int32_t& xOffset, int32_t& yOffset){
    xOffset = center.x - point.x;
    yOffset = center.y - point.y;
}

void getPxOffset(cv::Point center, cv::Point point, int32_t& offset, const char flag){
    switch (flag) {
        case 'x':
            offset = center.x - point.x;
            break;
        case 'y':
            offset = center.y - point.y;
            break;
        default:
            throw std::runtime_error("getPxOffset have bad flag");
    }
}

void triangulatePoint(cv::Point2f front_point, cv::Point2f side_point, cv::Point3f& triangulated_point, cv::Mat Q_matrix){
    float disparity = front_point.x - side_point.x;
    cv::Vec4f to_transform(front_point.x,
                           front_point.y,
                           disparity,
                           1);
    Q_matrix.convertTo(Q_matrix,CV_32F);
    cv::Mat viewed = Q_matrix*cv::Mat(to_transform);
    triangulated_point.x = viewed.at<float>(0)/viewed.at<float>(3);
    triangulated_point.y = viewed.at<float>(1)/viewed.at<float>(3);
    triangulated_point.z = viewed.at<float>(2)/viewed.at<float>(3);
}

void triangulatePoint(cv::Point2f front_point, cv::Point2f side_point, cv::Mat& triangulated_point, cv::Mat Q_matrix){
    int disparity = front_point.x - side_point.x;
    cv::Vec4f to_transform(front_point.x,
                           front_point.y,
                           disparity,
                           1);
    Q_matrix.convertTo(Q_matrix,CV_32F);
    cv::Mat viewed = Q_matrix*cv::Mat(to_transform);
    triangulated_point.at<float>(0) = viewed.at<float>(0)/viewed.at<float>(3);
    triangulated_point.at<float>(1) = viewed.at<float>(1)/viewed.at<float>(3);
    triangulated_point.at<float>(2) = viewed.at<float>(2)/viewed.at<float>(3);
}

void rigidTransform(cv::Point3f camera_3D_Point, cv::Point3f robot_3D_Point, cv::Mat &Rot_matrix, cv::Mat &trans_vector){
    cv::Mat w, u, vt;
    cv::Point3f Mcamera_3D_Point;
    cv::Point3f Mrobot_3D_Point;
    
    float centroid_Camera = 0;//(camera_3D_Point.x + camera_3D_Point.y + camera_3D_Point.z)/3;
    float centroid_Robot = 0;//(robot_3D_Point.x + robot_3D_Point.y + robot_3D_Point.z)/3;
    
    Mcamera_3D_Point.x = camera_3D_Point.x - centroid_Camera;
    Mcamera_3D_Point.y = camera_3D_Point.y - centroid_Camera;
    Mcamera_3D_Point.z = camera_3D_Point.z - centroid_Camera;
    Mrobot_3D_Point.x = robot_3D_Point.x - centroid_Robot;
    Mrobot_3D_Point.y = robot_3D_Point.y - centroid_Robot;
    Mrobot_3D_Point.z = robot_3D_Point.z - centroid_Robot;

//    cv::Vec3f H(Mcamera_3D_Point.x*Mrobot_3D_Point.x,
//                Mcamera_3D_Point.y*Mrobot_3D_Point.y,
//                Mcamera_3D_Point.z*Mrobot_3D_Point.z);
    cv::Mat H = cv::Mat(cv::Vec3f(Mcamera_3D_Point))*cv::Mat(cv::Vec3f(Mrobot_3D_Point).t());
    cv::SVD::compute(H, w, u, vt);

    Rot_matrix = vt.t() * u.t();

    if(cv::determinant(Rot_matrix)<0){
        for(int i = 0;i<3;i++){
            vt.at<float>(2,i) *= (-1);
            Rot_matrix = vt.t() * u.t();
        }
    }
    trans_vector = (-Rot_matrix*cv::Mat(cv::Vec3f(Mcamera_3D_Point)))+cv::Mat(cv::Vec3f(Mrobot_3D_Point));
}

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 w, u, vt;
    std::vector<cv::Point3f> Mcamera_3D_Point;
    std::vector<cv::Point3f> Mrobot_3D_Point;

    if(camera_3D_Point.size() != robot_3D_Point.size()){
        throw std::runtime_error("camera_3D_Point.size() != robot_3D_Point.size()");
    }
    cv::Point3f centroid_Camera = {0,0,0}, centroid_Robot = {0,0,0};

    for(cv::Point3f& point: camera_3D_Point){
        centroid_Camera.x += point.x;
        centroid_Camera.y += point.y;
        centroid_Camera.z += point.z;
        Mcamera_3D_Point.push_back(point);
    }
    centroid_Camera.x /= camera_3D_Point.size();
    centroid_Camera.y /= camera_3D_Point.size();
    centroid_Camera.z /= camera_3D_Point.size();

    for(cv::Point3f& point: robot_3D_Point){
        centroid_Robot.x += point.x;
        centroid_Robot.y += point.y;
        centroid_Robot.z += point.z;
        Mrobot_3D_Point.push_back(point);
    }
    centroid_Robot.x /= camera_3D_Point.size();
    centroid_Robot.y /= camera_3D_Point.size();
    centroid_Robot.z /= camera_3D_Point.size();

    for(unsigned long i=0;i<camera_3D_Point.size();i++){
        Mcamera_3D_Point.at(i).x -= centroid_Camera.x;
        Mcamera_3D_Point.at(i).y -= centroid_Camera.y;
        Mcamera_3D_Point.at(i).z -= centroid_Camera.z;
        Mrobot_3D_Point.at(i).x -= centroid_Robot.x;
        Mrobot_3D_Point.at(i).y -= centroid_Robot.y;
        Mrobot_3D_Point.at(i).z -= centroid_Robot.z;
    }

    cv::Mat H = cv::Mat(Mcamera_3D_Point).reshape(1).t()*cv::Mat(Mrobot_3D_Point).reshape(1);
    cv::SVD::compute(H, w, u, vt);

    Rot_matrix = vt.t() * u.t();

    if(cv::determinant(Rot_matrix)<0){
        for(int i = 0;i<3;i++){
            vt.at<float>(2,i) *= (-1);
            Rot_matrix = vt.t() * u.t();
        }
    }
    trans_vector = -Rot_matrix*cv::Mat(centroid_Camera)+cv::Mat(centroid_Robot);
}

cv::Mat toGray(cv::Mat inputMat){
    cv::Mat result;
    if(inputMat.channels()==1){
        inputMat.convertTo(result,CV_8UC1, 1/255.0);
    }else if(inputMat.channels()==3){
        if(inputMat.type()==18){
            inputMat.convertTo(result,CV_8UC3, 1/255.0);
        }else inputMat.copyTo(result);

        cv::cvtColor(result,result,cv::COLOR_BGR2GRAY);

    }else throw std::runtime_error("Unsupported Image format");
    return result;
}

cv::Point detect_ArUco_gravity(cv::Mat inputMat, cv::Mat K_matrix, cv::Mat Dist_coeffs, cv::Vec3d& R_vec, cv::Vec3d& t_vec){
    std::vector<int> markerIds;
    std::vector<std::vector<cv::Point2f>> markerCorners, rejectedCandidates;
    cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    cv::aruco::detectMarkers(inputMat, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

    if(markerCorners.size()==0) throw std::runtime_error("Not possible to detect ArUco Marker ID# 21");
    std::vector<cv::Vec3d> rvecs, tvecs;
    cv::aruco::estimatePoseSingleMarkers(markerCorners, 25, K_matrix, Dist_coeffs, rvecs, tvecs);

    int rowF = 0, colF = 0;
    for(int i = 0; i<4; i++){
      rowF += markerCorners.at(0).at(i).x;
      colF += markerCorners.at(0).at(i).y;

    }

    rowF /= 4;
    colF /= 4;

    cv::Point gravity(rowF,colF);
    R_vec = rvecs.at(0);
    t_vec = tvecs.at(0);

    return gravity;
}

cv::Point detect_ArUco_gravity(cv::Mat inputMat, cv::Mat K_matrix, cv::Mat Dist_coeffs){
    std::vector<int> markerIds;
    std::vector<std::vector<cv::Point2f>> markerCorners, rejectedCandidates;
    cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    cv::aruco::detectMarkers(inputMat, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

    if(markerCorners.size()==0) throw std::runtime_error("Not possible to detect ArUco Marker ID# 21");
    std::vector<cv::Vec3d> rvecs, tvecs;
    cv::aruco::estimatePoseSingleMarkers(markerCorners, 25, K_matrix, Dist_coeffs, rvecs, tvecs);

    int rowF = 0, colF = 0;
    for(int i = 0; i<4; i++){
      rowF += markerCorners.at(0).at(i).x;
      colF += markerCorners.at(0).at(i).y;

    }

    rowF /= 4;
    colF /= 4;

    return cv::Point(rowF,colF);
}

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

    std::vector<cv::Point> intersec_Point = get_Intercept_Point(inputBIN, lines, showALL);
    cv::Point center_tmp = get_Center(lines.at(0).end, lines.at(1).begin);
    if(showALL){
        cv::drawMarker(inputBIN, center_tmp,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 20, 2);
        cv::line(inputBIN, intersec_Point.at(0), center_tmp, cv::Scalar(0,0,255), 3, cv::LINE_AA);
    }
    //TO DO - Prusecik stredu mezi primkama s prusecikem primek a TCP
    /*
    cv::LineIterator it(inputBIN, intersecPoint.at(0), midPoint, 4);
    for( int j=0; j<it.count; ++j,++it ) {
        if((int)(*it)[0]==255){
            result = it.pos();
            break;
        }
    }
    if(showALL) cv::drawMarker(inputBIN, result,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 20, 2);
    result = intersecPoint.at(0);*/
    return /*result*/center_tmp;
}

}; //end_namespace_tcpc
