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

#define COLS 96
#define ROWS 58
#define SCALE 10

#define ARROW_MAG 8.0

#define FILENAME "linetest1.pgm"
#define INPUT "flower_frame_0070.pgm"

#define X_NAME "c:\\LukasKanade\\LK\\field1.txt"
#define Y_NAME "c:\\LukasKanade\\LK\\field2.txt"

struct image {
    int    **image;  // Image to have lines drawn on
    double **xcomp;  // Holds the x-component of the vector at position (x,y) = xcoord[y][x]
    double **ycomp;  // Holds the y-component of the vector at position (x,y) = ycoord[y][x]
    int    height;   // Diminsions of the image/coordinate maps 
    int    width;    
    double **angle;
    double **magnitude;
    double max_mag;
};

typedef struct image image;

void load_image(image *);
void load_coords(image *pic,char* x_name,char* y_name); // LOCKED

void draw_line(image *, int, int, int, int);
void write_image(image *, char *);
void line_prep(image *);
void draw_arrowhead(image *, int, int, int, int);
void angle_mag(image *);

int main()
{
    int i;
    image pic;
    
    // Set up the image for use
    load_image(&pic);
    
    pic.xcomp = malloc( ROWS * sizeof(double*) );
    pic.ycomp = malloc( ROWS * sizeof(double*) );
    pic.angle = malloc( ROWS * sizeof(double*) );
    pic.magnitude = malloc( ROWS * sizeof(double*) );

#if 0  
    pic.image = malloc( ROWS * SCALE * sizeof(int*) );
    for(i = 0; i < ROWS*SCALE; i++)
    {
        pic.image[i] = calloc( COLS * SCALE, sizeof(int) );
    }
#endif
	
    for(i = 0; i < ROWS; i++)
    {
       pic.xcomp[i] = calloc( COLS, sizeof(double) );
       pic.ycomp[i] = calloc( COLS, sizeof(double) );
       pic.angle[i] = calloc( COLS, sizeof(double) );
       pic.magnitude[i] = calloc( COLS, sizeof(double) );
    }
    
    load_coords(&pic,X_NAME,Y_NAME);

/*
    pic.image = malloc( ROWS * sizeof(int*) );
    for(i = 0; i < ROWS; i++)
    {
        pic.image[i] = calloc( COLS, sizeof(int) );
    }
    
    // H
    draw_line(&pic, 10, 10, 10, 100);
    draw_line(&pic, 50, 10, 50, 100); 
    draw_line(&pic, 10, 55, 50, 55);
    
    // i
    draw_line(&pic, 70, 100, 70, 50);
    draw_line(&pic, 70, 41, 70, 40);
    draw_line(&pic, 69, 41, 69, 40);
    draw_line(&pic, 71, 41, 71, 40);
    draw_line(&pic, 70, 40, 70, 39);

    // Draws a poorly designed star
    draw_line(&pic, 120, 210, 179, 50);
    draw_line(&pic, 179, 50, 236, 210);
    draw_line(&pic, 236, 210, 105, 105);    
    draw_line(&pic, 105, 105, 251, 105);
    draw_line(&pic, 251, 105, 120, 210);    
*/   
    angle_mag(&pic);
    line_prep(&pic);
    
    write_image(&pic, FILENAME);
    
    // Free the allocated memory
    for(i = 0; i < ROWS; i++)
    {
       free(pic.xcomp[i]);
       free(pic.ycomp[i]);
       free(pic.angle[i]);
       free(pic.magnitude[i]);
    }    
    
    free(pic.xcomp);
    free(pic.ycomp);
    free(pic.angle);
    free(pic.magnitude);
    
    system("pause");    
    return 0;
}

void load_image(image *pic) 
{
    FILE *input;
    char c;
    char comment[50];
    int x, y, i;
    
    input = fopen(INPUT, "rb");
    
    c = getc(input); // 'P'
    c = getc(input); // '5
    fscanf(input, " "); // '\n'

    c = fgetc(input); 
    while(c == '#') 
    {
        fgets(comment,40,input);
        while(comment[strlen(comment)-1]!='\n') 
        {
            fgets(comment,40,input);
        }
        fscanf(input," ");
        c = fgetc(input);
    }
    ungetc(c,input); // Put the last char that was taken back onto the stream

    fscanf(input," %d %d ", &pic -> width, &pic -> height);
    
	c = fgetc(input);
    while(c == '#') 
    {
        fgets(comment,40,input);
        while(comment[strlen(comment)-1]!='\n') 
        {
            fgets(comment,40,input);
        }
        fscanf(input," ");
        c = fgetc(input);
    }
    ungetc(c,input);

    fscanf(input," %d",&x); // 255
    fgetc(input);           // '\n'

    pic -> image = malloc( pic -> height * sizeof(int*) );
    for(i = 0; i < pic -> height; i++)
    {
       pic -> image[i] = calloc( pic -> width, sizeof(int) );
    }
    
    for(x = 0; x < pic -> height; x++) 
    {
        for(y = 0; y < pic -> width; y++) 
        {
            fscanf(input,"%c", &pic -> image[x][y]);      
        }
    }
             
    fclose(input);
}


void load_coords(image *pic,char* x_name,char* y_name) // LOCKED
{
    int i, j;
    FILE *xfield, *yfield;
    
    xfield = fopen(x_name, "r");
    yfield = fopen(y_name, "r");
    
    for(i = 0; i < ROWS; i++)
    {
        for(j = 0; j < COLS; j++)
        {
             fscanf(xfield, "%lf", &(pic -> xcomp[i][j]) );
             fscanf(yfield, "%lf", &(pic -> ycomp[i][j]) );
        }
    }

    fclose(xfield);
    fclose(yfield);
}
    

void draw_line(image *w00t, int startx, int starty, int endx, int endy)
{
    int i, x = startx, y = starty , temp, dx_abs, dy_abs, last_x, last_y, swap = 0;
    double slope, dx, dy;

    if (startx == endx && starty == endy) return;// Line of zero length. 
      
    dy = endy - starty;
    dx = endx - startx;
        
    //printf("\ndy = %f, dx = %f\n", dy, dx);    
    
    if(dy < 0) dy_abs = dy * -1;
    else       dy_abs = dy;
    
    if(dx < 0) dx_abs = dx * -1;
    else       dx_abs = dx;    

    // Is line more horizontal than vertical? 
    if (dy_abs < dx_abs)
    {
        //printf("\nHorizontal Line\n");
        
        if(dx == 0) slope = 0;
        else        slope = dy/dx;                 
        
        //printf("slope (dy/dx) = %lf\n", slope);
        
        // Put points in increasing order by column. 
        if (startx > endx) // swap 
        {
	        temp = starty; starty = endy; endy = temp;
	        temp = startx; startx = endx; endx = temp;
	        swap = 1;
        }
        
        for (x = startx; x <= endx; x++)
        {
            last_y = y;
            y = starty + (x - startx) * slope;
            
            //printf("\nstartx = %d, starty = %d\n", startx, starty);
            //printf("\nendx = %d, endy = %d\n", endx, endy);
            //printf("\nx = %d, y = %d\n", x, y);
            
            if(y >= ROWS*SCALE || y < 0) return;
            if(x >= COLS*SCALE || x < 0) return;
            
            w00t -> image[y][x] = 255.0;
            last_x = x;
        }
        
    } else {
           
        //printf("\nVertical Line\n");
        
        if(dy == 0) slope = 0;
        else        slope = dx/dy;
        
        //printf("slope (dx/dy) = %lf\n", slope);
        
        if (starty > endy) // swap
        {               	        
	        temp = starty; starty = endy; endy = temp;
	        temp = startx; startx = endx; endx = temp;
	        swap = 1;
        }
        
        for (y = starty; y <= endy; y++)
        {
            last_x = x;
            x = startx + (y - starty) * slope;
            
            //printf("\nstartx = %d, starty = %d\n", startx, starty);
            //printf("\nendx = %d, endy = %d\n", endx, endy);
            //printf("\nx = %d, y = %d\n", x, y);
            
            if(x >= COLS*SCALE || x < 0) return;
            if(y >= ROWS*SCALE || y < 0) return;
                        
	        w00t -> image[y][x] = 255.0;
	        last_y = y;
        }
    }

    if(swap == 1)
    {
        draw_arrowhead(w00t, last_x, last_y, startx, starty);
    } else {
        draw_arrowhead(w00t, last_x, last_y, endx, endy);
    }

}


void write_image(image *img, char *filename) // LOCKED
{
    FILE *output;
    int x,y;
    
    output = fopen(filename,"wb");

    printf("\nfilename = %s\n", filename);        
    printf("\nimg(width, height) = (%d, %d)\n", COLS*SCALE, ROWS*SCALE);// img -> width, img -> height);
    
    fprintf(output,"P5\n%d %d\n255\n", COLS*SCALE, ROWS*SCALE);//img -> width, img -> height);
    
    for(x = 0; x < ROWS*SCALE; x++)//img -> height; x++) 
    {
        for(y = 0; y < COLS*SCALE; y++)//img -> width; y++) 
        {
            fputc((img -> image[x][y]), output);
        }
    }
  
    fclose(output);
}

// Set-up function for draw_line
void line_prep(image *pic)
{
    int x, y, loop = 0;
    int endx, endy;

    printf("\nMax_Mag = %f\n", pic -> max_mag);
    system("pause");
/*
    // Normalize the vector fields with respect to the maximum magnitude
    for(y = 0; y < ROWS; y++)
    {
        for(x = 0; x < COLS; x++)
        {
            pic -> xcomp[y][x] *= ceil(20.0 / pic -> max_mag);
            pic -> ycomp[y][x] *= ceil(20.0 / pic -> max_mag);
        }
    }
*/  
    for(y = SCALE; y < ROWS*SCALE - SCALE; y+=SCALE)
    {
        for(x = SCALE; x < COLS*SCALE - SCALE; x+=SCALE)
        {             
            //printf("\nx = %d, y = %d\n", x, y);
#if 0           
            if(pic -> magnitude[(y/SCALE)-1][(x/SCALE)-1] < 1)
            {
                pic -> image[y][x] = 255.0;
                continue;
            }
#endif           
            endx = x + floor(pic -> xcomp[(y/SCALE)-1][(x/SCALE)-1]);
            endy = y + floor(pic -> ycomp[(y/SCALE)-1][(x/SCALE)-1]);  
            
            // Nothing to draw here        
            if(endx == 0 && endy == 0) continue; 
            if(endx >= COLS*SCALE || endx < 0) continue;
            if(endy >= ROWS*SCALE || endy < 0) continue;       
      
            draw_line(pic, x, y, endx, endy);
        }
    }
}


void draw_arrowhead(image *pic, int pre_x, int pre_y, int endx, int endy)
{   
    //printf("\nBeginning of Arrowhead\n");
    
    //printf("\nendy + 1 = %d\n", endy + 1);
    
    if( (endx + 1 != COLS*SCALE && endx - 1 != 0) && (endy + 1 != ROWS*SCALE && endy - 1 != 0) )
    {
        pic -> image[endy][endx+1] = 255;
        pic -> image[endy][endx-1] = 255;
        pic -> image[endy+1][endx] = 255;
        pic -> image[endy-1][endx] = 255;
    } else return;
    
    if(pic -> image[endy][endx] != 255.0) pic -> image[endy][endx] = 255.0;
    
    //printf("\nEnd of Arrowhead\n");
}



void angle_mag(image *w00t)
{
    int x, y;
    double angle, magnitude, max = 0;
    
    for(y = 0; y < ROWS; y++)
    {
        for(x = 0; x < COLS; x++)
        {
            angle = atan2( w00t -> ycomp[y][x], w00t -> xcomp[y][x]);
            magnitude = sqrt( pow(w00t -> xcomp[y][x], 2.0) + pow(w00t -> ycomp[y][x], 2.0) );
            
            if(magnitude > max) max = magnitude;
                        
            w00t -> magnitude[y][x] = magnitude;
            w00t -> angle[y][x] = angle;

			w00t->xcomp[y][x]*=ARROW_MAG;
			w00t->ycomp[y][x]*=ARROW_MAG;
        }
    }
    
    w00t -> max_mag = max;
    
    //printf("\nmax_mag = %lf\n", w00t -> max_mag);
}