#include "vision_method.h"

cv::Point getMiddleofPoints(cv::Point first, cv::Point second){
    cv::Point result;
    result.x = (first.x + second.x)/2; 
    result.y = (first.y + second.y)/2;
    return result;
}

void getMin(std::vector<cv::Line> &lines){
    int16_t max;

    if(lines.empty() || lines.size() < 2){
        //throw
    }else{
        max = lines.at(0).begin.x; 
    }
    uint16_t it = 0, max_it = 0; 
    for(auto& line: lines){
        if(line.begin.x > max){
            max = line.begin.x;
            max_it = it;
        }
        it++;
    }
    lines.erase(lines.begin()+max_it);
    if(lines.size()>2) getMin(lines);
}

std::vector<cv::Point> getIntersectionPointofLines(cv::Mat& img, std::vector<cv::Line> lines, bool showPoint){
    std::vector<cv::Point> result;
    //a*x + b*y = c 
    for(uint8_t i = 0 ; i < lines.size(); i++){
        for(uint8_t j = 0 + i ; j < lines.size(); j++){
            double a1 = lines.at(i).end.y - lines.at(i).begin.y;
            double b1 = lines.at(i).begin.x - lines.at(i).end.x;
            double c1 = a1*lines.at(i).begin.x + b1*lines.at(i).begin.y;

            double a2 = lines.at(j).end.y - lines.at(j).begin.y;
            double b2 = lines.at(j).begin.x - lines.at(j).end.x;
            double c2 = a2*lines.at(j).begin.x + b2*lines.at(j).begin.y;

            double det = a1*b2 -a2*b1;

            if(det != 0 && i != j){
                cv::Point tmp;
                tmp.x = (b2*c1 - b1*c2)/det;
                tmp.y = (a1*c2 - a2*c1)/det;
                result.push_back(tmp);
                //std::cout<<tmp<<std::endl;
                if(showPoint) cv::drawMarker(img, tmp,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 20, 2);
            }
        }
    }
    return result;
}

std::vector<cv::Line> Hough_transform(cv::Mat& input){
    std::vector<cv::Vec4i> lines;
    std::vector<cv::Line> result;

    HoughLinesP(input, lines, 1, CV_PI/180, 100, 200, 20);
    for( size_t i = 0; i < lines.size(); i++ ){
        cv::Line line;
        cv::Vec4i vector = lines[i];
        line.begin.x = vector[0];
        line.begin.y = vector[1];
        line.end.x = vector[2];
        line.end.y = vector[3];
        //std::cout<<line.begin<<line.end<<std::endl;
        result.push_back(line);
    }
    getMin(result);
    return result;
}   

std::vector<cv::Line> Hough_transform(cv::Mat& input, cv::Mat& output, bool showALL){
    std::vector<cv::Vec4i> lines;
    std::vector<cv::Line> result;

    HoughLinesP(input, lines, 1, CV_PI/180, 100, 200, 20);
    for( size_t i = 0; i < lines.size(); i++ ){
        cv::Line line;
        cv::Vec4i vector = lines[i];
        line.begin.x = vector[0];
        line.begin.y = vector[1];
        line.end.x = vector[2];
        line.end.y = vector[3];
        //std::cout<<line.begin<<line.end<<std::endl;
        if(showALL) cv::line( output, line.begin, line.end, cv::Scalar(0,0,255), 3, cv::LINE_AA);
        result.push_back(line);
    }
    getMin(result);
    if(!showALL){
        for(cv::Line& line: result) cv::line( output, line.begin, line.end, cv::Scalar(0,0,255), 3, cv::LINE_AA);
    }
    return result;
}   

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

    std::vector<cv::Point> intersecPoint = getIntersectionPointofLines(inputBIN, lines, showALL);
    cv::Point midPoint = getMiddleofPoints(lines.at(0).begin, lines.at(1).begin);
    if(showALL){
        cv::drawMarker(inputBIN, midPoint,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 20, 2);
        cv::line(inputBIN, intersecPoint.at(0), midPoint, cv::Scalar(0,0,255), 3, cv::LINE_AA);
    }

    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);
    return result;
} 
