#include "line.h"

namespace cv{

Line::Line(){}

Line::~Line(){}

double Line::get_Y_intercept(){
    double slope = (this->end.y-this->begin.y) / (this->end.x-this->begin.x);
    double y_int = this->begin.y - (slope * this->begin.x);
    //std::cout<<"Y-intercept is: " <<y_int<<std::endl;
    return y_int;
}

Line Line::operator + (Line const &obj){
    Line result;
    result.begin.x = begin.x + obj.begin.x;     
    result.end.x =  end.x + obj.end.x;
    result.begin.y = begin.y + obj.begin.y;     
    result.end.y =  end.y + obj.end.y;
    return result;
}

Line Line::operator /(Line const &obj){
    Line result;
    result.begin.x = begin.x / obj.begin.x;  
    result.begin.y = begin.y / obj.begin.y;
    result.end.x = end.x / obj.end.x;
    result.end.y = end.y / obj.end.y;
    return result;
}

Line Line::operator /(double const &number){
    Line result;
    result.begin.x = begin.x / number;  
    result.begin.y = begin.y / number;
    result.end.x = end.x / number;
    result.end.y = end.y / number;
    return result;
}

Point Line::center(){
    cv::Point result;
    result.x = (begin.x + end.x)/2; 
    result.y = (begin.y + end.y)/2;
    return result;
}

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

void filter_straight_Lines(std::vector<Line> &lines, int8_t gap_px, const char flag){
    if(lines.empty() || lines.size() < 2){
        std::cout<<"Empty or Small vector"<<std::endl;
        throw;
    }else{
        std::vector<cv::Line> tmp;
        uint16_t it = 0;
        for(int16_t i = 0; i < (int16_t)lines.size()-it;i++){
            switch (flag) {
                case 'x':
                    if(std::abs(lines.at(i).begin.x-lines.at(i).end.x) <= gap_px){
                        lines.erase(lines.begin()+i);
                        it++;
                    }else{
                        tmp.push_back(lines.at(i));
                    }
                    break;
                case 'y':
                    if(std::abs(lines.at(i).begin.y-lines.at(i).end.y) >= gap_px){ //!!!!!!!!!!!sign
                        lines.erase(lines.begin()+i);
                        it++;
                    }else{
                        tmp.push_back(lines.at(i));
                    }
                    break;
                default:
                    if((std::abs(lines.at(i).begin.x-lines.at(i).end.x) <= gap_px) || (std::abs(lines.at(i).begin.y-lines.at(i).end.y) <= gap_px)){
                        lines.erase(lines.begin()+i);
                        it++;
                    }else{
                        tmp.push_back(lines.at(i));
                    }
            }
        }
        lines = tmp;
    }
}

std::vector<Point> get_Intercept_Point(std::vector<Line> lines){
    std::vector<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){
                Point tmp;
                tmp.x = (b2*c1 - b1*c2)/det;
                tmp.y = (a1*c2 - a2*c1)/det;
                result.push_back(tmp);
            }
        }
    }
    return result;
}

std::vector<Point> get_Intercept_Point(Mat& img, std::vector<Line> lines, bool showPoint){
    std::vector<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){
                Point tmp;
                tmp.x = (b2*c1 - b1*c2)/det;
                tmp.y = (a1*c2 - a2*c1)/det;
                result.push_back(tmp);
                if(showPoint) cv::drawMarker(img, tmp,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 20, 2);
            }
        }
    }
    return result;
}

Line avg_Line(std::vector<Line> lines){
    Line line;
    for(Line &it: lines){
        line = line + it;
    }
    return line / lines.size();
} 

std::vector<std::vector<Line>> sortLines_Y_intercept(std::vector<Line> lines, double const turning_point){
    std::vector<std::vector<Line>> result;
    std::vector<Line> lower;
    std::vector<Line> higher;

    for(auto &line: lines){
        if(line.get_Y_intercept()>=turning_point){
            higher.push_back(line);
        }else{
            lower.push_back(line);
        }
    }
    result.push_back(lower);
    result.push_back(higher);
    return result;
}

void get_Bottom_Line_of_Axis(std::vector<Line> &lines, char const flag, unsigned long const number){
    int16_t max;

    if(flag == 'x' || flag == 'y'){
        if(lines.empty() || lines.size() < number){
            std::cout<<"Emty or Small std::vector<cv::Line>"<<std::endl;
        }else{
            if(flag=='x'){
                max = lines.at(0).begin.x;
            }else{
                max = lines.at(0).begin.y;
            }
        }
        uint16_t it = 0, max_it = 0; 
        for(auto& line: lines){
            if(flag=='x'){
                if(line.begin.x > max){
                    max = line.begin.x;
                    max_it = it;
                }
            }else{
                if(line.begin.y > max){
                    max = line.begin.y;
                    max_it = it;
                }
            }
            it++;
        }
        lines.erase(lines.begin() + max_it);
        if(lines.size() > number) get_Bottom_Line_of_Axis(lines, flag, number);
    }else{
        std::cout<<"Invalid flag -> Enter 'x' or 'y'"<<std::endl;
    }
}

void get_Top_Line_of_Axis(std::vector<Line> &lines, char const flag, unsigned long const number){
    int16_t min;

    if(flag == 'x' || flag == 'y'){
        if(lines.empty() || lines.size() < number){
            std::cout<<"Emty or Small std::vector<cv::Line>"<<std::endl;
        }else{
            if(flag=='x'){
                min = lines.at(0).begin.x;
            }else{
                min = lines.at(0).begin.y;
            }
        }
        uint16_t it = 0, min_it = 0;
        for(auto& line: lines){
            if(flag=='x'){
                if(line.begin.x < min){
                    min = line.begin.x;
                    min_it = it;
                }
            }else{
                if(line.begin.y < min){
                    min = line.begin.y;
                    min_it = it;
                }
            }
            it++;
        }
        lines.erase(lines.begin() + min_it);
        if(lines.size() > number) get_Top_Line_of_Axis(lines, flag, number);
    }else{
        std::cout<<"Invalid flag -> Enter 'x' or 'y'"<<std::endl;
    }
}

}; //end_namespace_cv
