#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#define NAMESIZE 80

#define IMAGE1_NAME "c:\\LukasKanade\\GaussianPyramid\\d1\\flower_frame_0070.pgm"
#define IMAGE2_NAME "c:\\LukasKanade\\GaussianPyramid\\d1\\flower_frame_0071.pgm"

/////////////////////////GLOBAL HAPPY FUN TIMES!////////////////////////////////

// 2D arrays of size [height(rows), width(cols)]
double **image1,     // Image 1 array
       **image2;     // Image 2 array
double **image1_dx,  // Image 1 Gradients
       **image1_dy,
       **image2_dx,  // Image 2 Gradients
       **image2_dy,
       **average_x,  // Average of image1_dx and image2_dx
       **average_y,  // Average of image1_dy and image2_dy  
       **gauss_1,    // Gaussian of image 1
       **gauss_2,    // Gaussian of image 2
       **gauss_diff, // Difference between gauss_1 and gauss_2
       **field1,     // These are the vector fields (?)
       **field2;     
       
// 2D arrays of size [kernal_size, kernal_size] 
double **gauss_kernel, // Standard Gaussian
       **xkernel,      // dx gauss kernal (x mask)
       **ykernel;      // dy gauss kernal (y mask)

double **xkernel180,**ykernel180;
       
// Other random 2D arrays
double a[2][2],
       b[2][1],
       product[2][1];
       
int cols, rows, sigma = 1;
int square = 7; // Area parameter for Lucas-Kanade matrices a and b
   
////////////////////////////////////////////////////////////////////////////////    
int main()
{
    int i, j, p, q, m, n; // Random for loop indicies
    int kernel_size, 
        k;   // gauss_kernel dimensions (I think this is the radius)
    double sum, sum1x, sum1y, sum1g, sum2x, sum2y, sum2g, determinate, tmp, inverse;
    char image1_name[NAMESIZE], image2_name[NAMESIZE], trash[NAMESIZE];
    FILE *img1, *img2;
    FILE *out1, *out2, *out3, *out4, *out5, *out6, *out7;
    double *swap1, *swap2;
	double M_PI;
	int i0,j0;
	char debug;

	M_PI=4*atan(1.0);

    
    kernel_size = (6 * sigma + 1);
    k = (kernel_size - 1) / 2;

    // Try to open the first file
    if( (img1 = fopen(IMAGE1_NAME, "rb")) == NULL) // File was not found
    {
        printf("\nCannot open file \"%s\", closing program...\n", IMAGE1_NAME);
        system("pause");
        return 0;
    }

    // Try to open the second file
    if( (img2 = fopen(IMAGE2_NAME, "rb")) == NULL) // File was not found
    {
        printf("\nCannot open file \"%s\", closing program...\n", IMAGE2_NAME);
        system("pause");
        return 0;
    }    

    // Assume that both images are of the same diminsions, and are .pgm formatted
    // As such, shave off the .pgm header and extract the cols and rows data from the first image
    fgets(trash, 80, img1);
    fgets(trash, 80, img1);
    
    // Extract the cols and rows, assuming no comment in header
    sscanf(trash, "%d%d", &cols, &rows);
    
    fgets(trash, 80, img1);
                
    if( strcmp(trash, "255\n") != 0 ) // There was a comment
    {
        sscanf(trash, "%d%d", &cols, &rows); // The second line was a comment, try the third line
        fgets(trash, 80, img1); // Clear the next line
    }
    
    // Remove the .pgm header of the second image
    fgets(trash, 80, img2);
    fgets(trash, 80, img2);
    fgets(trash, 80, img2);
    if( strcmp(trash, "255\n") != 0 ) // If there was a comment, clear the next line
        fgets(trash, 80, img2);
        
/////////////////////////////////// TEST CODE //////////////////////////////////
    printf("\ncols = %d, rows = %d\n", cols, rows);
    printf("pi = %1.20lf\n", M_PI);
////////////////////////////////////////////////////////////////////////////////

///////////////////// DYNAMICALLY CREATE THE ARRAYS!!! W00T!! //////////////////

// Get enough memory to hold the addresses of the rows     
    // 2D arrays of size [height(rows), width(cols)]
    image1     = malloc( rows * sizeof(double*) ); 
    image2     = malloc( rows * sizeof(double*) ); 
    image1_dx  = malloc( rows * sizeof(double*) ); 
    image1_dy  = malloc( rows * sizeof(double*) ); 
    image2_dx  = malloc( rows * sizeof(double*) ); 
    image2_dy  = malloc( rows * sizeof(double*) ); 
    average_x  = malloc( rows * sizeof(double*) );
    average_y  = malloc( rows * sizeof(double*) );
    gauss_1    = malloc( rows * sizeof(double*) );
    gauss_2    = malloc( rows * sizeof(double*) );
    gauss_diff = malloc( rows * sizeof(double*) );
    field1     = malloc( rows * sizeof(double*) );
    field2     = malloc( rows * sizeof(double*) );
    
    // 2D arrays of size [kernal_size, kernal_size]
    gauss_kernel = malloc( kernel_size * sizeof(double*) );
    xkernel      = malloc( kernel_size * sizeof(double*) );
    ykernel      = malloc( kernel_size * sizeof(double*) );

	xkernel180      = malloc( kernel_size * sizeof(double*) );
    ykernel180      = malloc( kernel_size * sizeof(double*) );
    
// Get enough memory to hold the number of columns in each row
    // 2D arrays of size [height(rows), width(cols)]
    for(i = 0; i < rows; ++i)                         
    {   
        image1[i]     = calloc( cols , sizeof(double) ); 
        image2[i]     = calloc( cols , sizeof(double) ); 
        image1_dx[i]  = calloc( cols , sizeof(double) ); 
        image1_dy[i]  = calloc( cols , sizeof(double) ); 
        image2_dx[i]  = calloc( cols , sizeof(double) ); 
        image2_dy[i]  = calloc( cols , sizeof(double) ); 
        average_x[i]  = calloc( cols , sizeof(double) );
        average_y[i]  = calloc( cols , sizeof(double) );
        gauss_1[i]    = calloc( cols , sizeof(double) );
        gauss_2[i]    = calloc( cols , sizeof(double) );
        gauss_diff[i] = calloc( cols , sizeof(double) );
        field1[i]     = calloc( cols , sizeof(double) );
        field2[i]     = calloc( cols , sizeof(double) );
    }
    // 2D arrays of size [kernal_size, kernal_size]
    for(i = 0; i < kernel_size; i++)
    {
        gauss_kernel[i] = calloc( kernel_size , sizeof(double) );
        xkernel[i]      = calloc( kernel_size , sizeof(double) );
        ykernel[i]      = calloc( kernel_size , sizeof(double) );
		xkernel180[i]      = calloc( kernel_size , sizeof(double) );
        ykernel180[i]      = calloc( kernel_size , sizeof(double) );
    }
    
////////////////////////////////////////////////////////////////////////////////

    // Read in the image file into the pic array
    for(i = 0; i < rows; i++)
    { 
        for(j = 0; j < cols; j++)
        {
            image1[i][j] =  getc(img1);
            image2[i][j] =  getc(img2);

			image1[i][j] /= 255.0;
			image2[i][j] /= 255.0;
        }
    }                                                                          


    // Create all the kernals/masks(?) that we will need                                                          
    for(i = 0; i < kernel_size; i++)
    {
        for(j = 0; j < kernel_size; j++)
        {
            xkernel[i][j] = -( (j-k) / ( 2* M_PI * pow(sigma, 3) ) ) * 
                            exp( -( pow((i-k), 2) + pow((j-k), 2) ) / ( 2 * pow(sigma, 2) ) );
                          
            ykernel[i][j] = -( (i-k) / ( 2* M_PI * pow(sigma, 3) ) ) * 
                            exp( -( pow((i-k), 2) + pow((j-k), 2) ) / ( 2 * pow(sigma, 2) ) ); 
			
			i0= kernel_size-1-i;
			j0= kernel_size-1-j;

			xkernel180[i0][j0]=xkernel[i][j];
			ykernel180[i0][j0]=ykernel[i][j];
            
            gauss_kernel[i][j] = ( 1 / ( 2 * M_PI * pow(sigma, 2) ) ) * 
                            exp( -( pow((i-k), 2) + pow((j-k), 2) ) / ( 2 * pow(sigma, 2) ) );
        }
    }




    // Convolute in the x and y direction
    for(i = k; i < rows - k; i++)
    { 
        for(j = k; j < cols - k; j++)
        {
            sum1x = 0;
            sum1y = 0;
            sum2x = 0;
            sum2y = 0;
            
            for(p = -k; p <= k; p++)
            {
                for(q = -k; q <= k; q++)
                {
                    sum1x += image1[i+p][j+q] * xkernel180[-p+k][-q+k];
                    sum1y += image1[i+p][j+q] * ykernel180[-p+k][-q+k];
                    sum2x += image2[i+p][j+q] * xkernel180[-p+k][-q+k];
                    sum2y += image2[i+p][j+q] * ykernel180[-p+k][-q+k];
                }
            }
             
            image1_dx[i][j] = sum1x;
            image1_dy[i][j] = sum1y;
            image2_dx[i][j] = sum2x;
            image2_dy[i][j] = sum2y;
        }
    }

///////////////////// TESTING OUTPUT FOR THE CONVOLUTION ///////////////////////
    out1 = fopen("img1x.txt", "w");
    out2 = fopen("img1y.pgm", "wb");
    out3 = fopen("xkernel.txt", "w");
    out4 = fopen("ykernel.txt", "w");
    out5 = fopen("gausskernel.txt", "w");
    out6 = fopen("img2x.pgm", "wb");
    out7 = fopen("img2y.pgm", "wb");
    
    //fprintf(out1, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out2, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out6, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out7, "P5\n%d %d\n255\n", cols, rows);
    
    for(i = 0; i < rows; i++)
    {
        for(j = 0; j < cols; j++)
        { 
             fprintf(out1, "%lf\n", image1_dx[i][j]);
             //fputc(((int)image1_dx[i][j]), out1);
             fputc(((int)image1_dy[i][j]), out2);
             fputc(((int)image2_dx[i][j]), out6);
             fputc(((int)image2_dy[i][j]), out7);             
        }
    }
    
    for(i = 0; i < kernel_size; i++)
    {
        for(j = 0; j < kernel_size; j++)
        {
            fprintf(out3, "%2.7f ", xkernel180[i][j]);
            if(ykernel180[i][j] >= 0)
                fprintf(out4, " %2.7f ", ykernel180[i][j]);
            else
                fprintf(out4, "%2.7f ", ykernel180[i][j]);
            fprintf(out5, "%2.7f ", gauss_kernel[i][j]);
        }
        
        fputc('\n', out3);
        fputc('\n', out4);
        fputc('\n', out5);
    }
   
    fclose(out1);
    fclose(out2);
    fclose(out3);
    fclose(out4);
    fclose(out5);
    fclose(out6);
    fclose(out7);
////////////////////////////////////////////////////////////////////////////////


    // Calculate average_x and average_y
    for(i = 0; i < rows; i++)
    {
        for(j = 0; j < cols; j++)
        {
            average_x[i][j] = (image1_dx[i][j] + image2_dx[i][j]) / 2;
            average_y[i][j] = (image1_dy[i][j] + image2_dy[i][j]) / 2;
        }
    }


    // Convolute of the gaussian kernel and image 1 and 2
    for(i = k; i < rows - k; i++)
    { 
        for(j = k; j < cols - k; j++)
        {
            sum1g = 0;
            sum2g = 0;
            
            for(p = -k; p <= k; p++)
            {
                for(q = -k; q <= k; q++)
                {
                    sum1g += image1[i+p][j+q] * gauss_kernel[p+k][q+k];
                    sum2g += image2[i+p][j+q] * gauss_kernel[p+k][q+k];
                }
            }
            
            gauss_1[i][j] = sum1g;
            gauss_2[i][j] = sum2g;
            gauss_diff[i][j] = sum2g - sum1g; // Gauss_2 - Gauss_1
        }
    }
///////////////////// TESTING OUTPUT FOR THE CONVOLUTION ///////////////////////
    out1 = fopen("gauss_1.pgm", "wb");
    out2 = fopen("gauss_2.pgm", "wb");
    out3 = fopen("gauss_diff.pgm", "wb");
    out4 = fopen("average_x.pgm", "wb");
    out5 = fopen("average_y.pgm", "wb");
    
    fprintf(out1, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out2, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out3, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out4, "P5\n%d %d\n255\n", cols, rows);
    fprintf(out5, "P5\n%d %d\n255\n", cols, rows); 
       
    for(i = 0; i < rows; i++)
    {
        for(j = 0; j < cols; j++)
        {
             fputc(((int)gauss_1[i][j]), out1);
             fputc(((int)gauss_2[i][j]), out2);
             fputc(((int)gauss_diff[i][j]), out3);
             fputc(((int)average_x[i][j]), out4);
             fputc(((int)average_y[i][j]), out5);
        }
    }

    fclose(out1);
    fclose(out2);
    fclose(out3);
    fclose(out4);
    fclose(out5);
////////////////////////////////////////////////////////////////////////////////

    
    for(i = k; i < rows - k; i++)
    { 
        for(j = k; j < cols - k; j++)
        {
            sum1g = 0;
            sum2g = 0;
            
            // Zero out a and b
            for(m = 0; m < 2; m++)
            {
                for(n = 0; n < 2; n++)
                {
                    a[m][n] = 0.0;
                }
                b[m][0] = 0.0;
            }
            
            
            for(p = (i - k); p <= (i + k); p++)
            {
                for(q = (j - k); q <= (j + k); q++)
                { 
                    // Compute the sum of (A^T)*(A) over the window  
                    a[0][0] += average_x[p][q] * average_x[p][q];
                    a[0][1] += average_x[p][q] * average_y[p][q];
                    a[1][0] += average_x[p][q] * average_y[p][q];
                    a[1][1] += average_y[p][q] * average_y[p][q];
                    
                    b[0][0] += average_x[p][q] * gauss_diff[p][q];
                    b[1][0] += average_y[p][q] * gauss_diff[p][q];
                }
            }
                  
                  
        // Find the inverse of a and multiply that by b
            
            // inverse of a 2x2 matrix (m) =
            // ( 1 / |determinate(m)| ) * |  z -x |
            //                            | -y  w |
            
            // determinate of a 2x2 matrix = 
            // | w x |
            // | y z |  = wz - yx
            
            determinate = ( a[0][0] * a[1][1] ) - ( a[1][0] * a[0][1] );
			if(i==6 && j==6)
				{
				debug =1;
				}
            
        ////////////////////////////////////////////////////////////////////////
        // How to handle this case?
            if(determinate == 0) // Then a is not invertable
            {
                field1[i][j] = 0;
                field2[i][j] = 0;
                continue;
            }       
        ////////////////////////////////////////////////////////////////////////
        
            // Get a into form
            // |  z -x |
            // | -y  w |
            
            // swap w with z
            tmp = a[0][0];
            a[0][0] = a[1][1];
            a[1][1] = tmp;
            a[0][1] = -a[0][1]; // negate x
            a[1][0] = -a[1][0]; // negate y
            
            if(determinate < 0) // Absolute value of determinate
            {
                determinate *= -1.0;
            }
            
            inverse = 1.0 / determinate;

            // a = (inverse of a)
            // inverse of a 2x2 matrix =
            // ( 1 / |determinate(a)| ) * |  z -x |
            //                            | -y  w |
            for(m = 0; m < 2; m++)
                for(n = 0; n < 2; n++)
                    a[m][n] *= inverse;
           
            // b = (-b)
            // Negating b
            b[0][0] = -b[0][0];
            b[1][0] = -b[1][0];
            
            // product = (inverse of A) * b = (2x2 * 2x1 = 2x1) 
            // Code for multiplying a 2x2 matrix by a 2x1 matrix
            // This for loop is used to move down the rows of matrix 1
            for(m = 0; m < 2; ++m)
            {
                // Since there is only 1 column in matrix 2, nothing is needed to move arcoss it
                
                sum = 0;
                // This for loop is used to move the column of matrix 1 and row of matrix 2
                for(n = 0; n < 2; ++n)
                {
                    sum += a[m][n] * b[n][0];
                }
                
                product[m][0] = sum;
            }
        
            field1[i][j] = product[0][0];
            field2[i][j] = product[1][0];           
        }
    }


////////////////////////////// TEST CODE ///////////////////////////////////////
    out1 = fopen("field1.txt", "w");
    out2 = fopen("field2.txt", "w");
    
    for(i = 0; i < rows; i++)
    {
        for(j = 0; j < cols; j++)
        {
            fprintf(out1, "%f\n", field1[i][j]);
            fprintf(out2, "%f\n", field2[i][j]);
        }
    }
    
    fclose(out1);
    fclose(out2);
/*
////////////////////////////////////////////////////////////////////////////////     
    
    
    // Flip field1 and field2 from top to bottom along a horizontal center axis
    for(i = 0; i < rows/2; i++)
    {
        swap1 = field1[i];
        swap2 = field2[i];
        
        field1[i] = field1[rows - (i + 1)];
        field2[i] = field2[rows - (i + 1)];
        
        field1[rows - (i + 1)] = swap1;
        field2[rows - (i + 1)] = swap2;
    }
    
////////////////////////////// TEST CODE ///////////////////////////////////////
    out1 = fopen("field1flip.txt", "w");
    out2 = fopen("field2flip.txt", "w");
    
    for(i = 0; i < rows; i++)
    {
        for(j = 0; j < cols; j++)
        {
            fprintf(out1, "%f\n", field1[i][j]);
            fprintf(out2, "%f\n", field2[i][j]);
        }
    }
    
    fclose(out1);
    fclose(out2);
*/
////////////////////////////////////////////////////////////////////////////////  
    
// Time to clean up after myself
    fclose(img1);
    fclose(img2);

    // FREEDOM, gotta love manual garbage collection!
    for(i = 0; i < rows; ++i)                         
    {
        free(image1[i]);
        free(image2[i]);
        free(image1_dx[i]);
        free(image1_dy[i]);
        free(image2_dx[i]);
        free(image2_dy[i]);
        free(average_x[i]);
        free(average_y[i]);
        free(gauss_1[i]);
        free(gauss_2[i]);
        free(gauss_diff[i]);
        free(field1[i]);
        free(field2[i]);
    }    
    free(image1);
    free(image2);
    free(image1_dx);
    free(image1_dy);
    free(image2_dx);
    free(image2_dy);
    free(average_x);
    free(average_y);
    free(gauss_1);
    free(gauss_2);
    free(gauss_diff);
    free(field1);
    free(field2); 

    system("pause");
    return 0;
}