#include "zf_common_headfile.h"
#include "zf_common_debug.h"

#include "DOG_path.h"
#include "math.h"

// 
void longest_white_col(struct DOG_PATH* this, uint8 input[MT9V03X_H][MT9V03X_W], uint16 control_point){
	int L_side[2];
	int R_side[2];
	
	int max_white_num = 0;				// аɫ
	int longest_white_X = 0;			// к
	int longest_white_X_cache = 0;		// к껺
	int max_white_col_num = 0;			// 
	int max_white_col_num_cache = 0;	// 
	uint8 max_white_col_find_flag = 0;	// ҵб־λ
	
	for(int X = this -> mid_x;X >= 0;X--)
    {
        if(input[this -> path_start_y-1][X] == 0)    // ɫ
        {
            // 洢ɫ
            L_side[0] = X+1;
            L_side[1] = this -> path_start_y-1;
//			ips200_draw_point(L_side[0], L_side[1]+MT9V03X_H*2, RGB565_RED);
            break;
        }
        if(X == 0)
        {
            // 洢ɫ
            L_side[0] = 0;
            L_side[1] = this -> path_start_y-1;
//			ips200_draw_point(L_side[0], L_side[1]+MT9V03X_H*2, RGB565_RED);
			this -> L_frame_point_num++;
            break;
        }
    }
    // ұ
    for(int X = this -> mid_x;X <= MT9V03X_W-1;X++)
    {
        if(input[this -> path_start_y-1][X] == 0)    // ɫ
        {
            // 洢ɫ
            R_side[0] = X-1;
            R_side[1] = this -> path_start_y-1;
//			ips200_draw_point(R_side[0], R_side[1]+MT9V03X_H*2, RGB565_RED);
            break;
        }
        if(X == MT9V03X_W-1)
        {
            // 洢ɫ
            R_side[0] = MT9V03X_W-1;
            R_side[1] = this -> path_start_y-1;
//			ips200_draw_point(R_side[0], R_side[1]+MT9V03X_H*2, RGB565_RED);
			this -> R_frame_point_num++;
            break;
        }
    }
	
	// mid_x
    this -> mid_x = (L_side[0] + R_side[0])/2;
	
	for(int X = L_side[0];X <= R_side[0];X++)
	{
		for(int Y = this -> path_start_y;Y >= this -> path_start_y-control_point;Y--)
		{
			if(input[Y][X] == 0 || Y == this -> path_start_y-control_point)
			{
				// иİˢ
				if(this -> path_start_y-Y > max_white_num)
				{
					max_white_col_find_flag = 1;
					max_white_num = this -> path_start_y-Y;
					longest_white_X_cache = 0;
					max_white_col_num_cache = 0;
					longest_white_X_cache +=  X;
					max_white_col_num_cache++;
				}
				// һİ
				else if(this -> path_start_y-Y == max_white_num)
				{
					max_white_col_find_flag = 1;
					max_white_num = this -> path_start_y-Y;
					longest_white_X_cache += X;
					max_white_col_num_cache++;
				}
				// ̵İУ
				else
				{
					max_white_col_find_flag = 0;
					if(max_white_col_num_cache > max_white_col_num)
					{
						max_white_col_num = max_white_col_num_cache;
						longest_white_X = longest_white_X_cache;
						max_white_col_num_cache = 0;
						longest_white_X_cache = 0;
					}	
				}
				break;
			}
		}
	}
	if(max_white_col_num_cache > max_white_col_num)
	{
		max_white_col_num = max_white_col_num_cache;
		longest_white_X = longest_white_X_cache;
	}	
	if(max_white_col_num != 0)
		this -> longest_white_col_x = longest_white_X/max_white_col_num;
	if(this -> longest_white_col_x <= 0)
		this -> longest_white_col_x = 0;
	else if(this -> longest_white_col_x >= MT9V03X_W-1)
		this -> longest_white_col_x = MT9V03X_W-1;
}	

// ·ȡ
void path_extract(struct DOG_PATH* this, uint8 input[MT9V03X_H][MT9V03X_W]){
	int16 x,y;
	static int16 mid_x_flag = 0;
	if(this -> mid_x == MT9V03X_W/2)
		mid_x_flag = 0;
	if(mid_x_flag < 10)
	{
		this -> path[0][0] = MT9V03X_W/2;
		mid_x_flag++;
	}
	else
	{
		this -> path[0][0] = this -> path[this -> prediction_point][0];
	}
	this -> path[0][1] = this -> path_start_y;
	for(y = this -> path_start_y-1;y >= this -> path_end_y;y--)
	{
		// ұ
		for(x = this -> path[this -> path_start_y-1-y][0];x < MT9V03X_W;x++)
		{
			if(input[y][x] == 0)
			{
				this -> path[this -> path_start_y-y][0] = x;
				break;
			}
			if(x == MT9V03X_W-1)
			{
				this -> path[this -> path_start_y-y][0] = x;
			}
		}
		// 
		for(x = this -> path[this -> path_start_y-1-y][0];x >= 0;x--)
		{
			if(input[y][x] == 0)
			{
				this -> path_width[this -> path_start_y-y] = this -> path[this -> path_start_y-y][0]-x;
				this -> path[this -> path_start_y-y][0] += x;
				break;
			}
			if(x == 0)
			{
				this -> path_width[this -> path_start_y-y] = this -> path[this -> path_start_y-y][0]-x;
				this -> path[this -> path_start_y-y][0] += x;
			}
		}
		this -> path[this -> path_start_y-y][0] = this -> path[this -> path_start_y-y][0]/2;
		this -> path[this -> path_start_y-y][1] = y;
	}
}

// ȡ
void side_extract(struct DOG_PATH* this, uint8 input[MT9V03X_H][MT9V03X_W]){
	// Ѱ߱
    int seed_grow_dir[8][4] = {{0,1,0,1},{1,1,-1,1},{1,0,-1,0},{1,-1,-1,-1},{0,-1,0,-1},{-1,-1,1,-1},{-1,0,1,0},{-1,1,1,1}};    // X,Y·ʱ  ·˳ʱ 
    int grow_dir_idx = 0;
    // ʼ
    this -> L_side_point_num = 0;
    this -> R_side_point_num = 0;
	this -> L_frame_point_num = 0;
	this -> R_frame_point_num = 0;
    this -> R_side_point_num = 0;
    memset(this -> L_side, 0, sizeof(this -> L_side));
    memset(this -> R_side, 0, sizeof(this -> R_side));

    // Ѱ
    // 
    for(int X = this -> mid_x;X >= 0;X--)
    {
        if(input[this -> side_extract_start_y-1][X] == 0)    // ɫ
        {
            // 洢ɫ
            this -> L_side[0][0] = X+1;
            this -> L_side[0][1] = this -> side_extract_start_y-1;
            break;
        }
        if(X == 0)
        {
            // 洢ɫ
            this -> L_side[0][0] = 0;
            this -> L_side[0][1] = this -> side_extract_start_y-1;
			this -> L_frame_point_num++;
            break;
        }
    }
    // ұ
    for(int X = this -> mid_x;X <= MT9V03X_W-1;X++)
    {
        if(input[this -> side_extract_start_y-1][X] == 0)    // ɫ
        {
            // 洢ɫ
            this -> R_side[0][0] = X-1;
            this -> R_side[0][1] = this -> side_extract_start_y-1;
            break;
        }
        if(X == MT9V03X_W-1)
        {
            // 洢ɫ
            this -> R_side[0][0] = MT9V03X_W-1;
            this -> R_side[0][1] = this -> side_extract_start_y-1;
			this -> R_frame_point_num++;
            break;
        }
    }

    // mid_x
    this -> mid_x = (this -> L_side[0][0] + this -> R_side[0][0])/2;

    // 
    // 
    while(true)
    {
        // ڱ
        if(this -> L_side[this -> L_side_point_num][0] < MT9V03X_W-1 && this -> L_side[this -> L_side_point_num][0] > 0)
        {   
            // ǷԽ
            if(this -> L_side_point_num >= MT9V03X_H*3-2)	/************************ע***************************/
                break;
            // һΪɫΪɫ
            if(input[this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[(grow_dir_idx+1)%8][1]][this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[(grow_dir_idx+1)%8][0]] == 0 && 
                input[this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][1]][this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][0]] == 255)
            {
                // һ
                this -> L_side[this -> L_side_point_num+1][0] = this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[grow_dir_idx][0];
                this -> L_side[this -> L_side_point_num+1][1] = this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[grow_dir_idx][1];
                this -> L_side_point_num+=1;
                grow_dir_idx = 0;
            }
            grow_dir_idx++;
        }
        // ڱ
        else if(grow_dir_idx <= 4)
        {
            // ǷԽ
            if(this -> L_side_point_num >= MT9V03X_H*3-2)	/************************ע***************************/
                break;
            // һΪɫΪɫ
            if( (grow_dir_idx <= 3 && input[this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[(grow_dir_idx+1)%8][1]][this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[(grow_dir_idx+1)%8][0]] == 0 && 
                input[this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][1]][this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][0]] == 255) || 
                (grow_dir_idx == 4 && input[this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][1]][this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][0]] == 255))
            {
                // һ
                this -> L_side[this -> L_side_point_num+1][0] = this -> L_side[this -> L_side_point_num][0]+seed_grow_dir[grow_dir_idx][0];
				this ->  L_side[this -> L_side_point_num+1][1] = this -> L_side[this -> L_side_point_num][1]+seed_grow_dir[grow_dir_idx][1];
                this -> L_side_point_num+=1;
				this -> L_frame_point_num++;
                grow_dir_idx = 0;
            }
            grow_dir_idx++;
        }
        else
            break;
        // ǷԽ
        if(grow_dir_idx >= 8)
            break;
        if(this -> L_side[this -> L_side_point_num][1] < this -> side_extract_end_y)
            break;
    }
    grow_dir_idx = 0;
    // ұ
    while(true)
    {
        // ڱ
        if(this -> R_side[this -> R_side_point_num][0] < MT9V03X_W-1 && this -> R_side[this -> R_side_point_num][0] > 0)
        {   
            // ǷԽ
            if(this -> R_side_point_num >= MT9V03X_H*3-2)	/************************ע***************************/
                break;
            // һΪɫΪɫ
            if(input[this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[(grow_dir_idx+1)%8][3]][this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[(grow_dir_idx+1)%8][2]] == 0 && 
                input[this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][3]][this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][2]] == 255)
            {
                // һ
                this -> R_side[this -> R_side_point_num+1][0] = this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[grow_dir_idx][2];
                this -> R_side[this -> R_side_point_num+1][1] = this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[grow_dir_idx][3];
                this -> R_side_point_num+=1;
                grow_dir_idx = 0;
            }
            grow_dir_idx++;
        }
        // ڱ
        else if(grow_dir_idx <= 4)
        {
            // ǷԽ
            if(this -> R_side_point_num >= MT9V03X_H*3-2)	/************************ע***************************/
                break;
            // һΪɫΪɫ
            if( (grow_dir_idx <= 3 && input[this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[(grow_dir_idx+1)%8][3]][this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[(grow_dir_idx+1)%8][2]] == 0 && 
                input[this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][3]][this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][2]] == 255) || 
                (grow_dir_idx == 4 && input[this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[(grow_dir_idx)%8][3]][this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[(grow_dir_idx)%8][2]] == 255))
            {
                // һ
                this -> R_side[this -> R_side_point_num+1][0] = this -> R_side[this -> R_side_point_num][0]+seed_grow_dir[grow_dir_idx][2];
                this -> R_side[this -> R_side_point_num+1][1] = this -> R_side[this -> R_side_point_num][1]+seed_grow_dir[grow_dir_idx][3];
                this -> R_side_point_num+=1;
				this -> R_frame_point_num++;
                grow_dir_idx = 0;
            }
            grow_dir_idx++;
        }
        else
            break;
        // ǷԽ
        if(grow_dir_idx >= 8)
            break;
        if(this -> R_side[this -> R_side_point_num][1] < this -> side_extract_end_y)
            break;
    }
}

// ߵж
void side_point_kind_judge(struct DOG_PATH* this){
	int i;
    int vector[2][4] = {0}; // ҹյ㹹ɵ
    double vectorScalarProduct[2] = {0};  // ҹյ
    double vectorModule[4] = {0};   // ҹյģ
    double vectorAngle[2] = {0}; // ҹյн(Ƕ)

    // ʼ
    this -> L_bend_point_num = 0;
    this -> R_bend_point_num = 0;
    memset(this -> L_bend_point, 0, sizeof(this -> L_bend_point));
    memset(this -> R_bend_point, 0, sizeof(this -> R_bend_point));

    // Ѱ㷶Χ
    // 
    for(i = this -> point_distance;i <= this -> L_side_point_num-1-this -> point_distance;)
    {
		// Ա߿򴦵ı߽ʶ
		if(this -> L_side[i-this -> point_distance][0] != 0 && this -> L_side[i][0] != 0 && this -> L_side[i+this -> point_distance][0] != 0)
		{
			// ߵһ
			vector[0][0] = (double)(this -> L_side[i-this -> point_distance][0]-this -> L_side[i][0]);
			vector[0][1] = (double)(this -> L_side[i-this -> point_distance][1]-this -> L_side[i][1]);
			// ߵڶ
			vector[1][0] = (double)(this -> L_side[i+this -> point_distance][0]-this -> L_side[i][0]);
			vector[1][1] = (double)(this -> L_side[i+this -> point_distance][1]-this -> L_side[i][1]);

			// жϵ
			vectorScalarProduct[0] = (double)(vector[0][0]*vector[1][0]+vector[0][1]*vector[1][1]);

			// յģ
			vectorModule[0] = sqrt(pow(vector[0][0],2)+pow(vector[0][1],2));
			vectorModule[1] = sqrt(pow(vector[1][0],2)+pow(vector[1][1],2));

			if( vectorModule[0]*vectorModule[1] != 0)
			{
				vectorAngle[0] = acos(vectorScalarProduct[0]/(vectorModule[0]*vectorModule[1]))*(180.0/PI);    // ߶ϵн
			}

			// 㲢洢꣬ǰ᣺겻ٱ߿
			if(abs(vectorAngle[0]) > this -> bend_point_angle_min && abs(vectorAngle[0]) < this -> bend_point_angle_max)
			{
				//  cout << abs(AngleVector[0]) << endl;
				this -> L_bend_point[this -> L_bend_point_num][0] = this -> L_side[i][0];
				this -> L_bend_point[this -> L_bend_point_num][1] = this -> L_side[i][1];
				if(this -> L_bend_point_num < MT9V03X_H*2-1)	/************************ע***************************/
					this -> L_bend_point_num++;
				else
					break;
			}
		}
		i++;
    }
    // ұ
    for(i = this -> point_distance;i <= this -> R_side_point_num-1-this -> point_distance;)
    {
		// Ա߿򴦵ı߽ʶ
		if(this -> R_side[i-this -> point_distance][0] != MT9V03X_W-1 && this -> R_side[i][0] != MT9V03X_W-1 && this -> R_side[i+this -> point_distance][0] != MT9V03X_W-1)
		{
			// ߵһ
			vector[0][2] = (double)(this -> R_side[i-this -> point_distance][0]-this -> R_side[i][0]);
			vector[0][3] = (double)(this -> R_side[i-this -> point_distance][1]-this -> R_side[i][1]);
			// ߵڶ
			vector[1][2] = (double)(this -> R_side[i+this -> point_distance][0]-this -> R_side[i][0]);
			vector[1][3] = (double)(this -> R_side[i+this -> point_distance][1]-this -> R_side[i][1]);

			// жϵ
			vectorScalarProduct[1] = (double)(vector[0][2]*vector[1][2]+vector[0][3]*vector[1][3]);

			// յģ
			vectorModule[2] = sqrt(pow(vector[0][2],2)+pow(vector[0][3],2));
			vectorModule[3] = sqrt(pow(vector[1][2],2)+pow(vector[1][3],2));
		
			if( vectorModule[2]*vectorModule[3] != 0)
			{
				vectorAngle[1] = acos(vectorScalarProduct[1]/(vectorModule[2]*vectorModule[3]))*(180.0/PI);    // ߶ϵн
			}

			// 㲢洢꣬ǰ᣺겻ٱ߿
			if(abs(vectorAngle[1]) > this -> bend_point_angle_min && abs(vectorAngle[1]) < this -> bend_point_angle_max)
			{
				//  cout << abs(AngleVector[0]) << endl;
				this -> R_bend_point[this -> R_bend_point_num][0] = this -> R_side[i][0];
				this -> R_bend_point[this -> R_bend_point_num][1] = this -> R_side[i][1];
				if(this -> R_bend_point_num < MT9V03X_H*2-1)	/************************ע***************************/
					this -> R_bend_point_num++;	
				else
					break;
			}
		}
		i++;
    }
}		

// 캯
void path(struct DOG_PATH* this, uint16 path_start_y, uint16 path_end_y, uint16 side_extract_start_y, uint16 side_extract_end_y, uint16 prediction_point){
	this -> mid_x = MT9V03X_W/2;					// ̬
	this -> path_start_y = path_start_y;			// ·ȡʼ߶
	this -> path_end_y = path_end_y;				// ·ȡ߶
	this -> side_extract_start_y = side_extract_start_y;		// ߿ʼȡ߶
	this -> side_extract_end_y = side_extract_end_y;		// ߽ȡ߶
	memset(this -> path,0,sizeof(this -> path));	// ·xy
	this -> prediction_point = prediction_point;	// Ԥ߶ȣ꽫Ϊһ֡
	this -> longest_white_col_x = 0;				// X
	memset(this -> L_side,0,sizeof(this -> L_side));// 
	memset(this -> R_side,0,sizeof(this -> R_side));// ұ
	this -> L_side_point_num = 0;			// ߵ
	this -> R_side_point_num = 0;			// ұߵ
	this -> L_frame_point_num = 0;			// ߿
	this -> R_frame_point_num = 0;			// ұ߿
	memset(this -> L_bend_point,0,sizeof(this -> L_bend_point));// 
	memset(this -> R_bend_point,0,sizeof(this -> R_bend_point));// ұ
	this -> L_bend_point_num = 0;			// 
	this -> R_bend_point_num = 0;			// ұ
	memset(this -> path_width,0,sizeof(this -> path_width));	// 
	this -> point_distance = 10;				// յ/
	this -> bend_point_angle_min = 0;		// СǶֵ
	this -> bend_point_angle_max = 170;		// Ƕֵ
	
	/* Ա */
	this -> longest_white_col = longest_white_col;
	this -> path_extract = path_extract;
	this -> side_extract = side_extract;
	this -> side_point_kind_judge = side_point_kind_judge;
	
}

// 
void _path(struct DOG_PATH* this){
	this -> mid_x = MT9V03X_W/2;		// ̬
	this -> path_start_y = 0;			// ·ȡʼ߶
	this -> path_end_y = 0;				// ·ȡ߶
	this -> side_extract_start_y = 0;		// ߿ʼȡ߶
	this -> side_extract_end_y = 0;		// ߽ȡ߶
	memset(this -> path,0,sizeof(this -> path));	// ·xy
	this -> prediction_point = 0;	// Ԥ߶ȣ꽫Ϊһ֡
	this -> longest_white_col_x = 0;				// X
	memset(this -> L_side,0,sizeof(this -> L_side));// 
	memset(this -> R_side,0,sizeof(this -> R_side));// ұ
	this -> L_side_point_num = 0;			// ߵ
	this -> R_side_point_num = 0;			// ұߵ
	this -> L_frame_point_num = 0;			// ߿
	this -> R_frame_point_num = 0;			// ұ߿
	memset(this -> L_bend_point,0,sizeof(this -> L_bend_point));// 
	memset(this -> R_bend_point,0,sizeof(this -> R_bend_point));// ұ
	this -> L_bend_point_num = 0;			// 
	this -> R_bend_point_num = 0;			// ұ
	memset(this -> path_width,0,sizeof(this -> path_width));	// 
	this -> point_distance = 0;				// յ/
	this -> bend_point_angle_min = 0;		// СǶֵ
	this -> bend_point_angle_max = 0;		// Ƕֵ
}