void create_pattern(Mat pattern)
{
	int size=pattern.rows, rx=20, ry=40;
	ellipse(pattern,Point(size/2-rx,size/2-ry),Size(70,50),0,0,360,255,2,8,0);
}

void initialize_table(int ***grad,int *gradCount, int resolution, int resolution_max)
{
	// assuming maximum number of coordinates for each angle is resolution_max
	for(int x=0;x<resolution;x++)
	{
		grad[x]=new int*[resolution_max];
		for(int y=0;y<resolution_max;y++)
		{
			grad[x][y]=new int[2];
		}	
	}
	for(int x=0;x<resolution;x++)
	{
		gradCount[x]=0;
		for(int y=0;y<resolution_max;y++)
		{
			grad[x][y][0]=0;
			grad[x][y][1]=0;
		}
	}	
}

void gradient_operator(int **gradxOp, int **gradyOp, int opSize)
{
	for(int x=0;x<opSize;x++)
	{
		gradxOp[x]=new int[opSize];
		gradyOp[x]=new int[opSize];
	}
	gradxOp[0][1]=-1;
	gradxOp[0][1]=-2;
	gradxOp[0][2]=-1;
	gradxOp[1][1]=0;
	gradxOp[1][1]=0;
	gradxOp[1][2]=0;
	gradxOp[2][1]=1;
	gradxOp[2][1]=2;
	gradxOp[2][2]=1;
	gradyOp[0][1]=-1;
	gradyOp[0][1]=0;
	gradyOp[0][2]=1;
	gradyOp[1][1]=-2;
	gradyOp[1][1]=0;
	gradyOp[1][2]=2;
	gradyOp[2][1]=-1;
	gradyOp[2][1]=0;
	gradyOp[2][2]=1;
}

void generate_pattern_table(Mat pattern,int ***grad,int *gradCount,int **gradxOp,int **gradyOp,int opSize)
{
	double gradx=0, grady=0;
	int size=pattern.rows;
	for(int x=opSize/2;x<size-opSize/2;x++)
	{
//		cout<<"Hello"<<endl;
		for(int y=opSize/2;y<size-opSize/2;y++)
		{
//			cout<<x<<","<<y<<endl;
			if(pattern.at<uint8_t>(x,y)==255)
			{
				gradx=0;
				grady=0;
				// finding gradient
				for(int i=-opSize/2;i<=opSize/2;i++)
				{
					for(int j=-opSize/2;j<=opSize/2;j++)
					{
						gradx+=gradxOp[i+opSize/2][j+opSize/2]*pattern.at<uint8_t>(i+x,y+j);
						grady+=gradyOp[i+opSize/2][j+opSize/2]*pattern.at<uint8_t>(i+x,y+j);
					}
				}
				// size/2 is the assumed center of pattern
				grad[179+(int)round(atan2(grady,gradx)*180/PI)][(int)gradCount[179+(int)round(atan2(grady,gradx)*180/PI)]][0]=x-size/2;
				grad[179+(int)round(atan2(grady,gradx)*180/PI)][(int)gradCount[179+(int)round(atan2(grady,gradx)*180/PI)]][1]=y-size/2;				

				gradCount[179+(int)round(atan2(grady,gradx)*180/PI)]+=1;
			}
		}
	}
}

void draw_test_image(Mat image)
{
	int posx=180, posy=190, rx=20, ry=40;
	ellipse(image,Point(180,190),Size(70,50),0,0,360,255,2,7,0);
	ellipse(image,Point(200,250),Size(70,50),0,0,360,255,2,8,0);
	ellipse(image,Point(280,290),Size(70,50),0,0,360,255,2,8,0);
	ellipse(image,Point(300,250),Size(70,50),0,0,360,255,2,8,0);
//	rectangle(image,Point(posx-rx,posy-ry),Point(posx+rx,posy+ry),255,2,8,0);
//	rectangle(image,Point(190-rx,220-ry),Point(190+rx,220+ry),255,2,8,0);
//	rectangle(image,Point(250-2*rx,350-2*ry),Point(250+2*rx,350+2*ry),255,2,8,0);
//	rectangle(image,Point(200-4*rx,200-4*ry),Point(200+4*rx,200+4*ry),255,2,8,0);	
	GaussianBlur(image,image,Size(3,3),3,3,0);
	int temp;
	for(int x=0;x<image.rows;x++)
	{
		for(int y=0;y<image.cols;y++)
		{
			image.at<uint8_t>(x,y)=image.at<uint8_t>(x,y)>126?255:0;
			temp=rand()%100;
			if(temp<=2)
			{
				image.at<uint8_t>(x,y)=255;
			}
			else if(temp>=90)
			{
				image.at<uint8_t>(x,y)=0;
			}
		}
	}
}

void generalized_hough_transform(int ***cand, int scale_number, int *scale_value, int size, Mat image, int ***grad,int *gradCount, int **gradxOp,int **gradyOp, int opSize)
{
	for(int x=0;x<size;x++)
	{
		cand[x]=new int*[size];
		for(int y=0;y<size;y++)
		{
			cand[x][y]=new int[scale_number];
			for(int z=0;z<scale_number;z++)
			{
				cand[x][y][z]=0;
			}
		}
	}
	
	int temp, gradx=0, grady=0;
	for(int x=opSize/2;x<image.rows-opSize/2;x++)
	{
		for(int y=opSize/2;y<image.cols-opSize/2;y++)
		{
			if(image.at<uint8_t>(x,y)==255)
			{
				gradx=0;
				grady=0;
				for(int i=-opSize/2;i<=opSize/2;i++)
				{
					for(int j=-opSize/2;j<=opSize/2;j++)
					{
						gradx+=gradxOp[i+opSize/2][j+opSize/2]*image.at<uint8_t>(i+x,y+j);
						grady+=gradyOp[i+opSize/2][j+opSize/2]*image.at<uint8_t>(i+x,y+j);
					}
				}
				temp=179+round(atan2(grady,gradx)*180/PI);
				for(int z=0;z<gradCount[temp];z++)
				{
					// voting for centers corresponding to current pixel
					for(int i=0;i<scale_number;i++)
					{
						if(x-scale_value[i]*grad[temp][z][0]<size && x-scale_value[i]*grad[temp][z][0]>=0 && y-scale_value[i]*grad[temp][z][1]>=0 && y-scale_value[i]*grad[temp][z][1]<size)
						{
						cand[x-scale_value[i]*grad[temp][z][0]][y-scale_value[i]*grad[temp][z][1]][i]++;
						}
					}
				}
			}	
		}
	}
}

void maximum_array(int &max, int ***cand, int size1, int size2, int scale_pos)
{
	for(int x=0;x<size1;x++)
	{
		for(int y=0;y<size2;y++)
		{
			if(cand[x][y][scale_pos]>max)
			{
				max=cand[x][y][scale_pos];
			}
//			if(cand[x][y]>0)
//			{
//				cout<<x<<","<<y<<","<<cand[x][y]<<endl;
//			}
		}
	}
}

void draw_crossbar(Mat detected_image,Mat image,int ***cand,int size1,int size2,int scale_pos,int thresh, int *scale_value)
{
	// drawing crossbar at identified location
//	int **crosspoints=new int*[size1];
//	for(int x=0;x<size1;x++)
//	{
//		crosspoints[x]=new int[size2];
//		for(int y=0;y<size2;y++)
//		{
//			crosspoints[x][y]=0;
//		}
//	}
	int ptx=0,pty=0;
	int min_dist=10;
	int max_x=0, max_y=0, max;	
	int crossbar_size=20;//ensure crossbar_size<min_dist
	for(int x=min_dist;x<size1-min_dist;x++)
	{
		for(int y=min_dist;y<size2-min_dist;y++)
		{
			if(cand[x][y][scale_pos]>=thresh)
			{
//				ptx=x;
//				pty=y;
				max_x=x;
				max_y=y;
				max=cand[x][y][scale_pos];
				for(int i=x-min_dist;i<x+min_dist;i++)
				{
					for(int j=y-min_dist;j<y+min_dist;j++)
					{
						if(max<=cand[i][j][scale_pos])
						{
							max=cand[i][j][scale_pos];
							max_x=i;
							max_y=j;
						}
					}
				}
				if(max_x==x && max_y==y)
				{
//					rectangle(detected_image,Point(y-scale_value[scale_pos]*20,x-scale_value[scale_pos]*40),Point(y+scale_value[scale_pos]*20,x+scale_value[scale_pos]*40),Scalar(255,0,0),2,8,0);
//					ellipse(detected_image,Point(y,x),Size(70,50),0,0,360,255,2,8,0);					
					for(int i=x-crossbar_size;i<=x+crossbar_size;i++)
					{	
						for(int j=y-crossbar_size;j<=y+crossbar_size;j++)
						{
							if(i==x || j==y)
							{
								detected_image.at<Vec3b>(i,j)[2]=255;
							}
						}
					}	
				}
			}
		}
	}
}