#include <stdio.h>
#include <math.h>
#include <stdlib.h>
//#include "com_fun.h"

typedef unsigned char	uchar;

//Image is a structure for PGM format;
struct PgmImage
{
	int colorLevel;
	int row;
	int col;
	uchar** img;			// for pgm using

	PgmImage(int _row,int _col);
	PgmImage(int _row,int _col, uchar color);
	PgmImage(PgmImage* _img);
	PgmImage(char* filename);
	PgmImage();
	~PgmImage();

	void Edge(float ht,float lt,float sigma);
	void VerticalEdge(float ht,float lt,float sigma);
	void Connect(int r, int c, float lt, float** Im2, float** Im4);
	void WritePGM(char* filename);
	void ReadPGM(char* filename);
	void Copy(PgmImage* _img);
	void Clear();
	void Clear(uchar color);

	void InfLine(int x1,int y1,int x2,int y2);
	void Line(int x1,int y1,int x2,int y2, int color);
	void Line(int x1,int y1,int x2,int y2, int color1, int color2);
	void Yline(int x1,int y1,int x2,int y2, int color);
	void Double(PgmImage* dst);
	void DrawHorizon();
	PgmImage* AddMargin(int margin, uchar color);

	void DrawPoint(int x, int y);
	void DrawCross(int x, int y, uchar color);
	void WritePixel(int x, int y, uchar color);
};


PgmImage::PgmImage(int _row,int _col) 
{
	int r,c;
	row=_row;
	col=_col;
	img = new uchar*[row];		//assign memory to source and target arrays
	
	for (r=0; r<row; r++)		//initialize image to 0
	{
		img[r]=new uchar[col];
		for (c=0; c<col; c++)
		{
			img[r][c] = 0;
		}
	}
}

PgmImage::PgmImage(int _row,int _col, uchar color) 
{
	int r,c;
	row=_row;
	col=_col;
	img = new uchar*[row];		//assign memory to source and target arrays
	
	for (r=0; r<row; r++)		//initialize image to 0
	{
		img[r]=new uchar[col];
		for (c=0; c<col; c++)
		{
			img[r][c] = color;
		}
	}
}

PgmImage::PgmImage(PgmImage *_img)
{
	int r,c;
	row=_img->row;
	col=_img->col;
	img = new uchar*[row];		//assign memory to source and target arrays
	
	for (r=0; r<row; r++)		//initialize image to 0
	{
		img[r]=new uchar[col];
		for (c=0; c<col; c++)
		{
			img[r][c] = _img->img[r][c];
		}
	}
}

PgmImage::PgmImage()
{
	row=-1;
	col=-1;
	img=NULL;
}

PgmImage::PgmImage(char *filename)
{
	ReadPGM(filename);
}

PgmImage::~PgmImage()
{
	int i;

	if(row!=-1)
	{
		for(i=0; i<row; i++)
		{
			delete img[i];
		}
		delete img;
	}
}

void PgmImage::Copy(PgmImage* _img)
{
	int r,c;

	for (r=0; r<row; r++)		//initialize image to 0
	{
		for (c=0; c<col; c++)
		{
			img[r][c] = _img->img[r][c];
		}
	}
}

void PgmImage::Clear()
{
	int r,c;

	for (r=0; r<row; r++)		//initialize image to 0
	{
		for (c=0; c<col; c++)
		{
			img[r][c] = 0;
		}
	}
}

void PgmImage::Clear(uchar color)
{
	int r,c;

	for (r=0; r<row; r++)		//initialize image to 0
	{
		for (c=0; c<col; c++)
		{
			img[r][c] = color;
		}
	}
}

void PgmImage::ReadPGM(char *filename)
{
	FILE *fp;
	int r,c;
	char buff[256];
	int gray;
	int pgmtype;

	if ((fp = fopen(filename,"rb")) == NULL)
	{
		printf("File does not exist!\n");	//bad source file name
		exit(0);
	}
	else
	{
		if(fgetc(fp)!='P')			//check if the source file is in correct format
		{	
			printf("Not a PGM file!\n");
			fclose(fp);
			return;
		}
		else
		{	
			fgets(buff,256,fp);		//inputs the first line in the image (starting from 'P')
			if(buff[0]=='2')
			{
				pgmtype=2;	
			}
			else
			{
				pgmtype=5;
			}

			while(1)
			{
				fgets(buff,256,fp);	//comments are stored in char array comments
				if(buff[0]!='#')
				{
					sscanf(buff,"%d %d",&col,&row);
					break;
				}
			}
			
			fgets(buff,256,fp);		//read max grey level
			sscanf(buff,"%d",&colorLevel);
			
			img = new uchar*[row];		//assign memory to source and target arrays

			for (r=0; r<row; r++)
			{
				img[r]=new uchar[col];
			}
			
			if(pgmtype==5)					//binary file
			{
				for (r=0; r<row; r++)	//read image from the file and loads in the array
					fread(img[r],sizeof(uchar),col,fp);
			}
			else
			{
				for (r=0;r<row;r++)		//ascii file
				{
					for(c=0;c<col-1;c++)
					{
						fscanf(fp,"%d",&gray);
						img[r][c]=gray;
					}
					fscanf(fp,"%d\n",&gray);
					img[r][col-1]=gray;
				}
			}
			fclose(fp);					//close the file pointer
		}		
	}
}


void PgmImage::WritePGM(char *filename)
{
	int r;
	
	FILE *fptr;
	
	char rowSize[10];
	char colSize[10];
	char Color[10];
	
	sprintf(rowSize,"%d",row);
	sprintf(colSize,"%d",col);
	sprintf(Color,"%d",255);
	
	fptr = fopen(filename,"wb");		//create target file
	
	fputs("P5",fptr);					//format for pgm
	fputc('\n',fptr);
	
	fputs("#this is a pgm file in binaray format",fptr);	//comments
	fputc('\n',fptr); 
	
	fputs(colSize,fptr);				//write number of rows and cols and max value of grey level
	fputc(' ',fptr);
	
	fputs(rowSize,fptr);
	fputc('\n',fptr);
	
	fputs(Color,fptr);		
	fputc('\n',fptr);
	
	for (r=0; r<row; r++)				//write the file
		fwrite(img[r],sizeof(uchar),col,fptr);
	//	for (c=0; c<col; c++)
	//		fputc((uchar)(img[r][c]) ,fptr);
	fclose(fptr);						//close the file pointer
}

float roundf(float a) 
{
	float b=(float)floor(a);
	if(a-b>0.5)
		return b+1;
	else
		return b;
}

double round(double a) 
{
	double b=floor(a);
	if(a-b>0.5)
		return b+1;
	else
		return b;
}

//sg1 sigma for spatial
//sg2 sigma for range
void Bilateral_Smooth(PgmImage *input, PgmImage *output, float sg1, float sg2)
{
	int r,c,i,j,row, col;
	row=input->row;
	col=input->col;

	float **gsm1;
	float *gsm2;
	int r1,r2,d1,d2;

	float ss=sg1*sg1;
	r1=(int)round(sqrt(-log(0.01)*2*ss));
	d1=2*r1+1;
	
	gsm1= new float*[d1];		//assign memory to source and target arrays
	for (i=0; i<d1; i++)
	{
		gsm1[i]=new float[d1];
	}

	for(i=-r1;i<=r1;i++)
	{
		for(j=-r1;j<=r1;j++)
		{
			gsm1[i+r1][j+r1]=(float)exp(-(i*i+j*j)/(2.0*ss));
		}
	}
	
	ss=sg2*sg2;
	r2=(int)round(sqrt(-log(0.01)*2*ss));
	d2=2*r2+1;

	gsm2= new float[d2];		//assign memory to source and target arrays

	for(i=-r2;i<=r2;i++)
	{
		gsm2[i+r2]=(float)exp(-(i*i)/(2.0*ss));
	}

	for(r=0; r<row; r++)
	{
		for(c=0; c<col; c++)
		{	
			float sum=0;
			float gs=0;
			for (i=0; i<d1; i++)		//convolve the mask
			{
				for (j=0; j<d1; j++)
				{
					int ri=r+i-r1;
					int cj=c+j-r1;
					if(ri>=0 && ri<row && cj>=0 && cj<col)
					{
						int det=abs(input->img[r][c]-input->img[ri][cj]);
						if(det>r2)
							det=r2;
						float weight=gsm1[i][j]*gsm2[det+r2];
						sum = sum + input->img[ri][cj]*weight;
						gs = gs + weight;
					}
				}
			}
			output->img[r][c]=(int)roundf(sum/gs);
		}
	}
}

void Mean_Shift_Smooth(PgmImage *input, PgmImage *output, float sg1, float sg2)
{
	int row=input->row;
	int col=input->col;

	int i,j,k,r,c;

	float **gsm1;
	float *gsm2;
	int r1,r2,d1,d2;

	float ss=sg1*sg1;
	r1=(int)round(sqrt(-log(0.01)*2.0*ss));
	d1=2*r1+1;
	
	gsm1= new float*[d1];		//assign memory to source and target arrays
	for (i=0; i<d1; i++)
	{
		gsm1[i]=new float[d1];
	}

	for(i=-r1;i<=r1;i++)
	{
		for(j=-r1;j<=r1;j++)
		{
			gsm1[i+r1][j+r1]=(float)exp(-(i*i+j*j)/(2.0*ss));
		}
	}

	ss=sg2*sg2;
	r2=(int)round(sqrt(-log(0.01)*2.0*ss));
	d2=2*r2+1;

	gsm2= new float[d2];		//assign memory to source and target arrays
	for(i=-r2;i<=r2;i++)
	{
		gsm2[i+r2]=(float)exp(-(i*i)/(2.0*ss));
	}
	printf("mean_shift: r1: %d \tr2: %d\n",r1,r2);

	for(r=0; r<row; r++)
	{
		for(c=0; c<col; c++)
		{	
			int x=c;
			int y=r;
			int color=input->img[y][x];
			for(k=0;k<5;k++)
			{
				float sc=0;
				float sx=0;
				float sy=0;
				float gs=0;
				for (i=0; i<d1; i++)		//convolve the mask
				{
					for (j=0; j<d1; j++)
					{
						int ri=y+i-r1;
						int cj=x+j-r1;
						//int d=roundf(sqrt(ri*ri+cj*cj));
						if(ri>=0 && ri<row && cj>=0 && cj<col)
						{
							//int det=abs(pgm->img[y][x]-pgm->img[ri][cj]);
							int det=abs(color-input->img[ri][cj]);
							if(det>r2)
								det=r2;
							float weight=gsm1[i][j]*gsm2[det+r2];
							sc = sc + input->img[ri][cj]*weight;
							sx = sx + cj*weight;
							sy = sy + ri*weight;
							gs = gs + weight;
						}
					}
				}
				color=(int)roundf(sc/gs);
				x=(int)roundf(sx/gs);
				y=(int)roundf(sy/gs);
			}
			output->img[r][c]=color;
		}
	}
}



void main()
{
	PgmImage *org1=new PgmImage("scene_00.pgm");
	int row=org1->row;
	int col=org1->col;

	PgmImage *img1=new PgmImage(row,col);
	PgmImage *img2=new PgmImage(row,col);
	
	Bilateral_Smooth(org1,img1,2,6);
	Mean_Shift_Smooth(org1,img2,2,6);

	img1->WritePGM("smooth1.pgm");
	img2->WritePGM("smooth2.pgm");
}