#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define MAXMASK 100 //for the gauss kernel
#define percent .001 //will change the strength of canny
#define A 1.00000
#define B 1.41421
#define C 2.23606
#define edge 255
#define head 255

// globalized from main
int ** model;
int ** red;
int ** blue;
int ** green;
int ** mEdge; //model edge image
int ** iEdge; //image edge image
double ** distances; //distance transform
int ** closesti;
int ** closestj;
int ** candidates; //head candidates
double ** iXGrad; //Gradients for image and model
double ** iYGrad;
double ** mXGrad;
double ** mYGrad;
int ** finalOutput; //Final image output
int height, width;
int mHeight, mWidth;
int mHeadHeight, mHeadWidth;
int mYCrown, mXCrown;
double headThreshold = 1.5;
double sig = 2.0;

// Functions to allocate/deallocate matrices
double ** doubleMatrix(int rows, int cols) {
    int i, j;
    double ** matrix = (double **) malloc(rows * sizeof(double *));
    for(i = 0; i < rows; i++) {
        matrix[i] = (double *) malloc(cols * sizeof(double));
        for(j = 0; j < cols; j++) {
            matrix[i][j] = 0;
        }
    }
    return matrix;
}
int ** intMatrix(int rows, int cols) {
    int i, j;

    int ** matrix = (int **) malloc(rows * sizeof(int *));
    for(i = 0; i < rows; i++) {
        matrix[i] = (int *) malloc(cols * sizeof(int));
        for(j = 0; j < cols; j++) {
            matrix[i][j] = 0;
        }
    }

    return matrix;
}
void freeMatrix(void ** matrix, int rows) {
    int i;
    for(i = 0; i < rows; i++)
        free(matrix[i]);

    free(matrix);
}

// Writes pixel values to a PGM file
writeFile(char * fileName, int ** pixels, int h, int w) {
    int i, j;
    FILE * output;

    printf("Writing file: %s\n", fileName);
    output = fopen(fileName, "wb");
    if(output == NULL) {
        printf("ERROR: Failed to open %s for writing!\n", fileName);
        exit(1);
    }

    fprintf(output, "P5\n%d %d\n255\n", w, h);
    for(i = 0; i < h; i++)
        for(j = 0; j < w; j++)
            fprintf(output,"%c",(char)pixels[i][j]);

    fclose(output);
}

void getPicPPM(char *inputFileName){
	FILE * input;
	int i, j;
	char c;
	char buffer[100];

	input = fopen(inputFileName, "rb");
    if(input == NULL) {
        printf("ERROR: Failed to open %s for reading.\n",inputFileName);
        exit(1);
    }

	//Check for PPM
    if(getc(input) == 'P' && getc(input) == '6' && getc(input) == '\n')
    {
        ;
    } else {
        printf("ERROR: Input file must be a PPM formated image.\n");
        exit(1);
    }

    // Strip the commented line
    i = 0;
    while((c = getc(input)) == '#')
        while((c = getc(input)) != '\n')
            if(c == EOF) {
                printf("ERROR: Invalid file.\n");
                exit(1);
            }
    buffer[i++] = c;

    // Get the dimensions
    while((buffer[i] = getc(input)) != ' ') i++;
    buffer[i] = '\0';
    width = atoi(buffer);
    fgets(buffer, 100, input);
    height = atoi(buffer);

	red = intMatrix(height, width);
    green = intMatrix(height, width);
    blue = intMatrix(height, width);

    // Get maxlevel (not used)
    fgets(buffer, 100, input);

	// Read in the raw pixel data
    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
            red[i][j] = getc(input);
            green[i][j] = getc(input);
            blue[i][j] = getc(input);
        }
    }
    fclose(input);
}

void getPicPGM(char *inputFileName){
	FILE * input;
	int i, j;
	char c;
	char buffer[100];

	input = fopen(inputFileName, "rb");
    if(input == NULL) {
        printf("ERROR: Failed to open %s for reading.\n",inputFileName);
        exit(1);
    }

	//Check for PGM
    if(getc(input) == 'P' && getc(input) == '5' && getc(input) == '\n')
    {
        ;
    } else {
        printf("ERROR: Input file must be a PGM formated image.\n");
        exit(1);
    }

    // Strip the commented line
    i = 0;
    while((c = getc(input)) == '#')
        while((c = getc(input)) != '\n')
            if(c == EOF) {
                printf("ERROR: Invalid file.\n");
                exit(1);
            }
    buffer[i++] = c;

    // Get the dimensions
    while((buffer[i] = getc(input)) != ' ') i++;
    buffer[i] = '\0';
    mWidth = atoi(buffer);
    fgets(buffer, 100, input);
    mHeight = atoi(buffer);

    // Get maxlevel (not used)
    fgets(buffer, 100, input);

	model = intMatrix(mHeight, mWidth);
	// Read in the raw pixel data
    for(i = 0; i < mHeight; i++) {
        for(j = 0; j < mWidth; j++) {
            model[i][j] = getc(input);
        }
    }
    fclose(input);
}

void canny(int type){
	int i, j, p, q, center, mr;
	double sumxr, sumyr, sumxb, sumyb, sumxg, sumyg, sumx, sumy, magmax;
	int tempH, tempW, more;
	double maskval, direct, area, high, lo;
	double ** magout;
	double ** xGrad;
	double ** yGrad;
	int ** peaks;
	int ** final;
	double maskx[MAXMASK][MAXMASK];
	double masky[MAXMASK][MAXMASK];
	int histogram[255];
	center = (int)(MAXMASK/2);
	mr = (int)(sig * 3); //mask radius

	//Create gaussian curve
	for(p=-mr; p<mr+1; p++){
		for(q=-mr; q<mr+1; q++){
			maskval = (-1*q)*(exp(-1*((p*p)+(q*q))/(2*sig*sig)));
			maskx[p+center][q+center] = maskval;
			maskval = (-1*p)*(exp(-1*((p*p)+(q*q))/(2*sig*sig)));
			masky[p+center][q+center] = maskval;
	} 	}

	//Convolve
	if(type == 5) { //type 5, pgm
		magout = doubleMatrix(mHeight, mWidth);
		mXGrad = doubleMatrix(mHeight, mWidth);
		mYGrad = doubleMatrix(mHeight, mWidth);
		xGrad = doubleMatrix(mHeight, mWidth);
		yGrad = doubleMatrix(mHeight, mWidth);
		peaks = intMatrix(mHeight, mWidth);
		final = intMatrix(mHeight, mWidth);
		mEdge = intMatrix(mHeight, mWidth);
		tempH = mHeight;
		tempW = mWidth;
		magmax = 0;
		for(i=mr; i<mHeight-mr; i++){
			for(j=mr; j<mWidth-mr; j++){
				sumx = 0;
				sumy = 0;
				for(p=-mr; p<mr+1; p++){
					for(q=-mr; q<mr+1; q++){
						sumx += model[i+p][j+q] * maskx[p+center][q+center];
						sumy += model[i+p][j+q] * masky[p+center][q+center];					
				}	}
				magout[i][j] = sqrt((sumx*sumx + sumy*sumy));
			    if (magout[i][j]>magmax) magmax=magout[i][j];
				mXGrad[i][j] = sumx;
				xGrad[i][j] = sumx;
				mYGrad[i][j] = sumy;
				yGrad[i][j] = sumy;
		} 	}
	} else { //type 6, ppm
		magout = doubleMatrix(height, width);
		iXGrad = doubleMatrix(height, width);
		iYGrad = doubleMatrix(height, width);
		xGrad = doubleMatrix(height, width);
		yGrad = doubleMatrix(height, width);
		peaks = intMatrix(height, width);
		final = intMatrix(height, width);
		iEdge = intMatrix(height, width);
		tempH = height;
		tempW = width;
		magmax = 0;
		for(i=mr; i<height-mr; i++){
			for(j=mr; j<width-mr; j++){
				sumxr = 0;
				sumyr = 0;
				sumxb = 0;
				sumyb = 0;
				sumxg = 0;
				sumyg = 0;				
				for(p=-mr; p<mr+1; p++){
					for(q=-mr; q<mr+1; q++){
						sumxr += red[i+p][j+q] * maskx[p+center][q+center];
						sumyr += red[i+p][j+q] * masky[p+center][q+center];
						sumxg += green[i+p][j+q] * maskx[p+center][q+center];
						sumyg += green[i+p][j+q] * masky[p+center][q+center];
						sumxb += blue[i+p][j+q] * maskx[p+center][q+center];
						sumyb += blue[i+p][j+q] * masky[p+center][q+center];						
				}	}
				sumx = sumxr;
				sumy = sumyr;
				if((sqrt((sumx*sumx + sumy*sumy))) < (sqrt((sumxb*sumxb + sumyb*sumyb)))){
					sumx = sumxb;
					sumy = sumyb;
				}
				if((sqrt((sumx*sumx + sumy*sumy))) < (sqrt((sumxg*sumxg + sumyg*sumyg)))){
					sumx = sumxg;
					sumy = sumyg;
				}
				magout[i][j] = sqrt((sumx*sumx + sumy*sumy));
			    if (magout[i][j]>magmax) magmax=magout[i][j];
				iXGrad[i][j] = sumx;
				iYGrad[i][j] = sumy;
				xGrad[i][j] = sumx;
				yGrad[i][j] = sumy;
		} 	}
	}
	//Find the peaks
	for(i=0; i<tempH; i++){
		for(j=0; j<tempW; j++){
            if (xGrad[i][j]==0.0) xGrad[i][j] = 0.000000001;
			direct = (yGrad[i][j]/xGrad[i][j]);
			if((-.414213 <= direct)&&(direct <= .414213)){
				if((magout[i][j] > magout[i][j-1])&&(magout[i][j] > magout[i][j+1]))
					peaks[i][j] = 255;
			} else if((-2.414213 <= direct)&&(direct <= -.414213)){
				if((magout[i][j] > magout[i+1][j-1])&&(magout[i][j] > magout[i-1][j+1]))
					peaks[i][j] = 255;
			} else if((.414213 <= direct)&&(direct <= 2.414213)){
				if((magout[i][j] > magout[i+1][j+1])&&(magout[i][j] > magout[i-1][j-1]))
					peaks[i][j] = 255;
			} else if((magout[i][j] > magout[i+1][j])&&(magout[i][j] > magout[i-1][j])) {
				peaks[i][j] = 255;
			} else {
				peaks[i][j] = 0;
			}
	}	}

	for(i=0; i<255; i++) histogram[i] = 0;

	for(i=0; i<tempH; i++){
		for(j=0; j<tempW; j++){
			histogram[(int)(magout[i][j])]++;
	}	}
	
	area = 0;
	for(i=255; i>-1; i--){
		area +=histogram[i];
		if((percent * tempH * tempW) < area)
			j = i;
	}

	high = j;
	lo = (.35) * j;
	for(i=0; i<tempH; i++){
		for(j=0; j<tempW; j++){
			if(peaks[i][j] == 255){
				if(magout[i][j] > high){
					final[i][j] = 255;
					peaks[i][j] = 0;
				} else if(magout[i][j] < lo)
					peaks[i][j] = 0;
	}	}	}

	more = 1;
	while(more){
		more = 0;
		for(i=0; i<tempH; i++){
			for(j=0; j<tempW; j++){
				if(final[i][j] == 255){
					for(p=-1; p<2; p++){
						for(q=-1; q<2; q++){
							if(peaks[i+p][j+q] == 255){
								final[i+p][j+q] = 255;
								peaks[i+p][j+q] = 0;
								more = 1;
	}	}	}	}	}	}	}
	if(type == 5) writeFile("CannyFinalOutModel.pgm", final, tempH, tempW);
	else writeFile("CannyFinalOut.pgm", final, tempH, tempW);
	if(type == 5){
		for(i=0; i<mHeight; i++)
			for(j=0; j<mWidth; j++)
				mEdge[i][j] = final[i][j];
	} else {
		for(i=0; i<height; i++)
			for(j=0; j<width; j++)
				iEdge[i][j] = final[i][j];
	}
	freeMatrix(magout, tempH);
	freeMatrix(peaks, tempH);
	freeMatrix(final, tempH);
	freeMatrix(xGrad, tempH);
	freeMatrix(yGrad, tempH);
}	

// Finds the distance from each pixel to the closest edge pixel
void findDistances() {
	int ** temp;

	int i, j, x, y;
	// Store the magnitude of the distance to nearest edge pixel here, at i,j
    distances = doubleMatrix(height, width);
	// Store the Y-position of the nearest edge pixel here, at i,j
    closesti = intMatrix(height, width);
	// Store the X-Position of the nearest edge pixel here, at i.j
    closestj = intMatrix(height, width);
	// The integer version of distances, scaled to 0-255
	temp = intMatrix(height, width);
	//init dist matrix


	// This loop merely scales the distances to values the program will use.
	// Temp refits them into values that will look good printed.
	// writeFile... writes the file! distance.pgm will be your graded output
    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
			distances[i][j] = exp(-0.25*distances[i][j]);
			temp[i][j] = (int)((distances[i][j])*255);
    }   }
	writeFile("distance.pgm", temp, height, width);
}

// Uses recursion to trace edges downward from head candidates, marking all as non-candidates
void trackdown(int i, int j, int dir) {
    if(i < 0 || i >= height || j < 0 || j >= width)
        return;
    if(!candidates[i][j] && iEdge[i][j] == 255) {
        candidates[i][j] = 1;
        // These lines may allow more head candidates, if some are being lost
        // The recursion will stop at the side of the head
        trackdown(i+1,j, dir);
        trackdown(i,j+dir, dir);
        trackdown(i+1,j+dir, dir);
    }
}

void normalize() {
    int i, j;
    double tempx, tempy, temp;

    // Normalize gradient vectors from the image
    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
            tempx = iXGrad[i][j];
            tempy = iYGrad[i][j];
            temp = sqrt(tempx*tempx + tempy*tempy);

            iXGrad[i][j] = tempx / temp;
            iYGrad[i][j] = tempy / temp;
    }	}
}

// Marks local maxima as head candidates
void findCandidates() {
    int i, j, id = 0, w, count = 0;
	candidates = intMatrix(height, width);
	normalize();
    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
            // Some funny code to return the middle of a local maxima, if it is flat
            if(!candidates[i][j]  && iEdge[i][j] == 255 && fabs(iYGrad[i][j]) > .95)
                count++;
            else if(count > 0) {
                w = j - count/2 - 1;

                candidates[i][w] = 255;
                trackdown(i,w+1, 1);
                trackdown(i+1,w+1, 1);
                trackdown(i+1,w-1, -1);
                trackdown(i,w-1, -1);
                count = 0;
            }
        }
    }
	for(i=0; i<height; i++){
		for(j=0; j<width; j++){
			if((iEdge[i][j] == 255)&&(candidates[i][j] < 255))
				candidates[i][j] = 128;
	}	}
	writeFile("Candidates.pgm", candidates, height, width);
}

// Uses the head model image to determine the size and position of the head model
void examineModel() {
    int i, j;
    int start, end, count = 0;
    int maxHeight, maxWidth = 0;
    mYCrown = mHeight;

    // Find top-center
	// Looks like the top right to me
    for(j = 0; j < mWidth; j++) {
        for(i = 0; i < mHeight; i++) {
            if(mEdge[i][j] == edge)
                break;
        }
        if(i < mYCrown) {
            mYCrown = i;
            mXCrown = j;
        }
    }

    // Find the size/proportion
    for(i = 0; i < mHeight; i++) {
        start = 0;
        end = 0;
        for(j = 0; j < mWidth; j++) {
            if(mEdge[i][j] == edge && !start)
                start = j;
            else if(mEdge[i][j] == edge)
                end = j;
        }
        if(start != 0 && end - start > maxWidth) {
            maxWidth = end - start;
            maxHeight = i - mYCrown;
        }
        else if(start != 0 && end - start < maxWidth)
            break;
    }
    mHeadHeight = maxHeight;
    mHeadWidth = maxWidth;
    printf("Model: (%d,%d) Size: %dx%d\n", mYCrown, mXCrown, mHeadHeight, mHeadWidth);
}


// Returns whether a point on a scaled model occurs in the larger head model
// Also returns the gradient vector at that point
int scaleModel(int i, int j, double scaley, double scalex, double *normy, double *normx) {
    //, double scaley, double scalex, double * normx, double * normy
    int radiusx = (int)ceil(scalex);
    int radiusy = (int)ceil(scaley);
    int p, q;
    i = (int)(i*scaley);
    j = (int)(j*scalex);

    // Look in the scaled area of the larger head model for an edge point
    // If one is found, then there should be an edge point in the actual image
	// May need to implement bounds checking
    for(p = 0; p < radiusy; p++) {
        for(q = 0; q < radiusx; q++) {
            if(mEdge[i+p][j+q] == edge) {          
				// Also return the gradient vector at this point in the model
                *normx = mXGrad[i+p][j+q];
                *normy = mYGrad[i+p][j+q];
                return 1;
	}	}	}
    return 0;
}

// Searches for the best possible matching of the head model contour for each head candidate
void mapModel() {
    double scalex, scaley;
    int		headHeight, headWidth;
    double	max;
    int		maxHeight, maxWidth;
	int xWinSt, yWinSt; // x and y window start positions
    int		maxI, maxJ;
	double maxDist=0;
    double	sum;
    int		modelPoints;
    int i, j, x, y, a, b;
    double normx, normy, temp;
    int xWinSize, yWinSize; // x and y window sizes
    int closei, closej;
    int count = 0;
	
	for(i=0; i<height; i++){
		for(j=0; j<height; j++){
			if(distances[i][j] > maxDist) maxDist = distances[i][j];
	}	}
	maxDist++;

    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
            if(candidates[i][j] == head) {
                max = 0;
                maxHeight = 0;
                maxWidth = 0;
                maxI = 0;
                maxJ = 0;
				for(headHeight = 10; headHeight < mHeadHeight; headHeight += 3) {
					for(headWidth = 15; headWidth < mHeadWidth; headWidth += 3) {
						if(headWidth/(double)headHeight < 2.5 && headWidth/(double)headHeight > 1.5) {
							sum = 0;
							modelPoints = 0;
							// What is the size of the contour found in relation to our head model?
							scaley = mHeadHeight/(double)headHeight;
							scalex = mHeadWidth/(double)headWidth;
							// Jump to the top left corner of the smaller contour in order to map
							//modelPositionY and X are the crown of the true model
							yWinSt = i - (int)(mYCrown/scaley);
							xWinSt = j - (int)(mXCrown/scalex);
							//modelHeight and Width are the dimensions of the model image
							yWinSize = (int)(mHeight/scaley);
							xWinSize = (int)(mWidth/scalex);
							//printf("a:%d b:%d i+tempy:%d j+temp:%d\n", a, b, (i+tempy), (j+tempx));
							if(yWinSt >= 0 && yWinSt + yWinSize < height && xWinSt >= 0 && xWinSt + xWinSize < width) {
								// Determine the match score for that size/proportion
								for(y = 0; y < yWinSize; y++) { 
									for(x = 0; x < xWinSize; x++) {
										if(scaleModel(y,x,scaley,scalex,&normy,&normx)) {
											modelPoints++;
											closei = closesti[yWinSt+y][xWinSt+x];
											closej = closestj[yWinSt+y][xWinSt+x];
											temp = distances[yWinSt+y][xWinSt+x];
											sum += (maxDist - temp) * fabs(normx * iXGrad[closei][closej] + normy * iYGrad[closei][closej]);
										} // end-if
									} // end-for
								} // end-for
								sum /= (double)modelPoints;
								// Save the best match found so far
								// Shouldnt we be looking for the smallest sum?
								if(sum > max) {
									max = sum;
									maxHeight = headHeight;
									maxWidth = headWidth;
									a=b=0;
									maxI = a;
									maxJ = b;
								}
							} // end-if
						} // end-if
					} // end-for
				} // end-for
				if(max) printf("max = %f\n", max);
				if(max > headThreshold) {
					printf("Location: (%d,%d) \tShift %d,%d \tSize: %dx%d \tScore: %lf\n",i,j,maxI,maxJ,maxHeight,maxWidth,max);
                    // Drawing the head contour with the best match
                    scaley = mHeadHeight/(double)maxHeight;
                    scalex = mHeadWidth/(double)maxWidth;
                    yWinSize = (int)(mHeight/scaley);
                    xWinSize = (int)(mWidth/scalex);

					// shouldnt this be -= ?
                    yWinSt = i - maxI - (int)(mYCrown/scaley);
                    xWinSt = j - maxJ - (int)(mXCrown/scalex);
                    for(y = 0; y < yWinSize; y++) {
                        for(x = 0; x < xWinSize; x++) {
                            if((yWinSt+y >= 0) && (yWinSt+y < height) && (xWinSt+x >= 0) && (xWinSt+x < width) 
								&& scaleModel(y,x,scaley,scalex,&normy,&normx)) {
                                finalOutput[yWinSt+y][xWinSt+x] = 255;
                            }
                        }
                    }
                } // end-if (drawing)
            } // end-if 
        } //end-for j
    } // end-for i
	for(i=0; i<height; i++){
		for(j=0; j<height; j++){
			if(finalOutput[i][j] == 255) printf("q ");
	}	}
}

void writeResults(char * filename) {
    int i, j;
    FILE * output = fopen(filename, "wb");

    if(output == NULL) {
        printf("Unable to open %s for writing.\n", filename);
        exit(1);
    }

    fprintf(output,"P6\n%d %d\n255\n",width,height);
    for(i = 0; i < height; i++) {
        for(j = 0; j < width; j++) {
            if(finalOutput[i][j]){
                fprintf(output,"%c%c%c",255,255,0);
				printf("p ");
			}
            else
				fprintf(output,"%c%c%c",red[i][j],green[i][j],blue[i][j]);

        }
    }
    printf("Wrote %s to file\n", filename);
	fclose(output);
}

int main(int argc, char **argv){
	char *inputFile = argv[1];
	char *outputFile = "Output.ppm";
	int i, j;
	getPicPPM(inputFile);
	canny(6); //find canny of inputeFile
	getPicPGM("model.pgm");
	canny(5); // do you really NEED the gradients from the model?
	findDistances(); //find distance transform on canny
	finalOutput = intMatrix(height, width);
	findCandidates(); //Find all pixels with verticle gradient
	mapModel();
	for(i=0; i<height; i++){
		for(j=0; j<height; j++){
			if(finalOutput[i][j] == 255) printf("q ");
	}	}
	writeResults(outputFile);

	return 0;
}