#include <math.h>
#include "linear.h"

Linear::Linear(vector<vector<int>> cam_vec_ray, int center_x,int center_y)
{
    this->points_list = cam_vec_ray;
    this->center_x = center_x;
    this->center_y = center_y;

    Linear::bottom_line();
    Linear::third_line();

}

// 直线法表示底边直线
void Linear::bottom_line() {
    int x1 = this->points_list[0][0];
    int y1 = this->points_list[0][1];

    int x2 = this->points_list[1][0];
    int y2 = this->points_list[1][1];

    if(x1 == x2) {
        this->flag = 1; 
    }else if(y2 == y1) {
        this->flag = 2; 
    } else{
        this->flag = 0;
        this->k = (float)(y2-y1)/(x2-x1);
    }
//    printf("%d,%d\n",x1,y1);
//    printf("%d,%d\n",x2,y2);
//    cout << this->flag << endl;
//    cout << "sssss" << endl;
//    cout <<  this->k <<endl;
}

void Linear::third_line()
{
    if(this->flag == 0)
    {
        this->b = this->center_y - this->k * this->center_x;
    }
//    printf("%f,%f",this->b,this->k);
}


vector<float> Linear::inter_points(vector<int> point_m, vector<int> point_n)
{
    int xm = point_m[0];
    int ym = point_m[1];
    int xn = point_n[0];
    int yn = point_n[1];
    vector<float> inter_point_vec;
    float inter_x, inter_y;
    if(xm == xn) {
        inter_x = (float) xm;
        if (this->flag == 2)
            inter_y = (float) this->center_y;
        else
            inter_y = (float) (this->k * inter_x + this->b);
    }else if(ym == yn){
        inter_y = (float)ym;
        if(this->flag == 1)
            inter_x = (float) this->center_x;
        else
            inter_x = (inter_y - this->b)/this->k;
    }else
    {
        float k1 = (float)(ym - yn)/(xm - xn);
        float b1 = ym - k1*xm;
        if(this->flag == 1)
        {
            inter_x = (float) this->center_x;
            inter_y = k1*inter_x + b1;
        } else if (this->flag == 2)
        {
            inter_y = (float) this->center_y;
            inter_x = (inter_y - b1)/k1;
        } else
        {
            inter_x = (this->b - b1)/(k1 - this->k);
            inter_y = k1*inter_x + b1;
        }
    }

    inter_point_vec.push_back(inter_x);
    inter_point_vec.push_back(inter_y);

    return inter_point_vec;

}


double Linear::cal_distance()
{
    printf("cal distance");
    vector<float> s_point_vec = Linear::inter_points(this->points_list[0],this->points_list[3]);
    vector<float> e_point_vec = Linear::inter_points(this->points_list[1],this->points_list[2]);
    printf("%f,%f,other,%f,%f",s_point_vec[0],s_point_vec[1],e_point_vec[0],e_point_vec[1]);

    float pow_x = pow((s_point_vec[0] - e_point_vec[0]),2);
    float pow_y = pow((s_point_vec[1] - e_point_vec[1]),2);
    double distance =  pow((pow_x + pow_y),0.5);

    return distance;
}


double Linear::adjust_distance(int base_distance,int bottom_distance, double center_distance,int angle)
{
    MathUtils mm;
    double rate = cos(mm.angle_to_radian((double)angle));
    if(rate == 0)
    {
        rate = 1e-5;
    }

	double result_distance = base_distance * bottom_distance / (center_distance * rate);
    printf("\n%f::%f\n",rate,result_distance);
//    printf("\na%d%d%f%daa%fhe%f",ragresult_distance);
	return result_distance;
}