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

#include <cv.h>
#include <highgui.h>

#include "building.h"
#include "functools.h"
#include "utils.h"

#define LINE_SIZE 1024

#define SKIP_LINE( no ) if ( no == NULL ) { printf( "Skipping\n" ); continue; }

#define H 0
#define S 1
#define V 2

#define STEP1_WIN_NAME "STEP 1"
#define STEP2_WIN_NAME "STEP 2"
#define ZOOM           1

#define RESAMPLE       1.0f

typedef struct MouseProbe {
    int x;
    int y;

    IplImage *img;
    IplImage *show_img;
    IplImage *edge_img;
} MouseProbe;

// variables


// function declarations
void flood_fill( IplImage *src_img, IplImage *dst_img, IplImage *height_img, const int x, const int y );


/**
 * Callback pro kliknuti mysi.
 * param - obsahuje poiter na libovolna data, ktera mu predame pri vytvareni callbacku
 */
void mouse_probe_handler( int event, int x, int y, int flags, void* param ) {
    MouseProbe *probe = (MouseProbe*)param;

    switch ( event ) {

    case CV_EVENT_LBUTTONDOWN:
        printf( "Clicked LEFT at: [ %d, %d ]\n", x, y );
        flood_fill( probe->edge_img, probe->show_img, probe->img, x, y );
        break;

    case CV_EVENT_RBUTTONDOWN:
        printf( "Clicked RIGHT at: [ %d, %d ]\n", x, y );
        break;
    }
}


void create_windows( const int width, const int height ) {
    cvNamedWindow( STEP1_WIN_NAME, 0 );
    cvNamedWindow( STEP2_WIN_NAME, 0 );

    cvResizeWindow( STEP1_WIN_NAME, width*ZOOM, height*ZOOM );
    cvResizeWindow( STEP2_WIN_NAME, width*ZOOM, height*ZOOM );

} // create_windows


/**
 * Provede flood fill ze zadaneho bodu (x, y) pro okolni body pokud obsahuji stejnou hodnotu,
 * jaka prisla v argumentu value. Funkce rekurzivne vola sama sebe pro sve 4-okoli.
 * src_img - obraz na kterem se bude provadet vyplnovani
 * dst_img - obraz, do ktereho zaznamename vyplneni
 * value - hodnota, pro kterou provedeme vyplneni
 */
void fill_step( IplImage *src_img, IplImage *dst_img, IplImage *height_img, const int x, const int y, const uchar value, Building *building ) {
    int width, height;
    int z;

} //fill_step


/**
 * Provede flood fill ze zadaneho bodu (x, y). Funkce si zapamatuje hodnotu na souradnici (x, y)
 * a vyplnuje okoli pomoci funkce fill_step tak dlouho, dokud je hodnota v okolnich bodech stejna.
 * Vyplnovani provadejte na nejakem docasnem obraze, aby nedoslo k poskozeni puvodniho obrazu.
 * src_img - obraz na kterem se bude provadet vyplnovani
 * dst_img - obraz, do ktereho zaznamename vyplneni
 */
void flood_fill( IplImage *src_img, IplImage *dst_img, IplImage *height_img, const int x, const int y ) {
    int width, height;
    IplImage *tmp_ff_img;

} //flood_fill


void make_binary_file( const char *input_filename, const char *out_filename ) {
    FILE *f_in, *f_out;
    char line[ LINE_SIZE ];
    char *no = NULL;
    long line_no = 0;
    float x, y, z;

    f_in = fopen( input_filename, "r" );
    f_out = fopen( out_filename, "wb" );

    while ( fgets( line, LINE_SIZE, f_in ) != NULL ) {
        if ( line_no % 1000000 == 0 ) {
            printf( "Processing line: %ld Million\n", line_no / 1000000 );
        }

        no = (char*)strtok( line, " " );
        SKIP_LINE( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no );
        x = (float)atof( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no )
        y = (float)atof( no );
        no = (char*)strtok( NULL, " " );
        SKIP_LINE( no );
        z = (float)atof( no );

        fwrite( &x, sizeof( x ), 1, f_out );
        fwrite( &y, sizeof( y ), 1, f_out );
        fwrite( &z, sizeof( z ), 1, f_out );

        line_no++;
    }

    printf( "Closing file: %s\n", input_filename );
    printf( "Closing file: %s\n", out_filename );
    fclose( f_in );
    fclose( f_out );
} //make_binary_file


/**
 * Zjisti minimalni a maximalni souradnice v zadanem souboru.
 * Nezapomente, ze soubor tvori S-JTSK souradnice.
 */
void get_min_max( const char *filename, float *a_min_x, float *a_max_x, float *a_min_y, float *a_max_y, float *a_min_z, float *a_max_z ) {
    FILE *f = NULL;
    float x, y, z;
    float min_x, min_y, min_z, max_x, max_y, max_z;
    int l_type;

    // nacist binarni soubor, ziskat min a max pro x,y,z

    f = fopen(filename, "rb");

    fread(&max_x,sizeof(float),1,f);
    fread(&max_y,sizeof(float),1,f);
    fread(&max_z,sizeof(float),1,f);
    fread(&l_type,sizeof(int),1,f);
    min_x = max_x;
    min_y = max_y;
    min_z = max_z;

    while (feof(f)==0)
    {
        fread(&x,sizeof(float),1,f);
        fread(&y,sizeof(float),1,f);
        fread(&z,sizeof(float),1,f);
        fread(&l_type,sizeof(int),1,f);

        if (x > max_x)
        {
            max_x = x;
        }
        if (y > max_y)
        {
            max_y = y;
        }
        if (z > max_z)
        {
            max_z = z;
        }
        if (x < min_x)
        {
            min_x = x;
        }
        if (y < min_y)
        {
            min_y = y;
        }
        if (z < min_z)
        {
            min_z = z;
        }
    }

    fclose(f);

    *a_min_x = min_x;
    *a_min_y = min_y;
    *a_min_z = min_z;
    *a_max_x = max_x;
    *a_max_y = max_y;
    *a_max_z = max_z;


} //get_min_max


/**
 * Naplni obraz daty z lidaru.
 * Vsechny lidarove body jsou ukladany do pole, ktere ma rozmery obrazu. Pote je jednotlivym pixelum prirazena
 * hodnota jako prumer hodnot z odpovidajiciho prvku pole. Timto jednoduchym pristupem vsak dochazi ke ztrate dat.
 * filename - soubor s binarnimi daty
 * img - vystupni obrazek
 */
void fill_image( const char *filename, IplImage *img, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z ) {
    FILE *f = NULL;
    int delta_x, delta_y, delta_z;
    float fx, fy, fz;
    int x, y, l_type;
    int stride;
    int num_points = 0;
    float range = 0.0f;
    float *avg = NULL;
    int *avg_count = NULL;

    // zjistime sirku a vysku obrazu
    delta_x = img->width;
    delta_y = img->height;

    // naalokujeme pomocna pole, ve kterych budeme ukaladat hodnoty z lidaru
    // a pocet techto hodnot pro kazdy pixel
    avg = (float*)malloc((img->width)*(img->height)*(sizeof(float)));
    avg_count = (int*)malloc((img->width)*(img->height)*(sizeof(int)));

    //otevreni souboru
    f = fopen(filename, "r");

    //vynulovani poli
    for (int i = 0; i < (img->width)*(img->height); i++)
    {
        avg[i] = 0.0f;
        avg_count[i] = 0;
    }

    // projdeme soubor a hodnoty priradime do poli
    while(!feof(f))
    {
        fread(&fx, 1, sizeof(float), f);
        fread(&fy, 1, sizeof(float), f);
        fread(&fz, 1, sizeof(float), f);
        fread(&l_type, 1, sizeof(int), f);

        fx -= min_x;
        fy -= min_y;

        x = cvRound(fx);
        y = cvRound(fy);

        /*printf("%d",x);
        printf("%d",y);*/

        if ((x < (img->width)) && (y < (img->height)))
        {
            avg[x + y * img->width] += fz;
            avg_count[x + y * img->width]++;
        }
    }

    //zavreni souboru
    fclose(f);

    // pro normalizaci
    range = 255/(max_z - min_z);

    //prepocet z-tove souradnice
    for(int i = 0; i < ((img->width)*(img->height)); i++)
    {
        avg[i] /= avg_count[i]; //prumer
        avg[i] -= min_z;
        avg[i] *= range;
    }

    // hodnoty z pomocneho pole priradime do obrazu
    for ( y = 0; y < (img->height); y++ ) {
        for ( x = 0; x < (img->width); x++ ) {
            CV_IMAGE_ELEM( img, uchar, y, x ) = (uchar)avg[(x + (y * (img->width)))];
        }
    }

} //fill_image

void make_edges( const IplImage *src_img, IplImage *sobel_img ) {
    cvCanny( src_img, sobel_img, 1, 80 );
}

/**
 * Prevede hodnoty obrazu na pouze 2 hodnoty. Hranici mozno nastavit experimentalne.
 */
void binarize_image( IplImage *img ) {
    int x, y;
    uchar value;

}


void process_lidar( const char *txt_file, const char *bin_file, const char *img_file ) {
    float min_x, max_x, min_y, max_y, min_z, max_z;
    float delta_x, delta_y, delta_z;
    MouseProbe *probe;

    IplImage *img = NULL;      // obraz pro vstup lidarovych dat
    IplImage *show_img = NULL; // obraz pro kresleni nalezenych ploch
    IplImage *edge_img = NULL; // obraz pro hrany

    probe = (MouseProbe*)malloc( sizeof( probe[ 0 ] ) );

    //make_binary_file( txt_file, bin_file );
    get_min_max( bin_file, &min_x, &max_x, &min_y, &max_y, &min_z, &max_z );

    printf( "min x: %f, max x: %f\n", min_x, max_x );
    printf( "min y: %f, max y: %f\n", min_y, max_y );
    printf( "min z: %f, max z: %f\n", min_z, max_z );

    delta_x = max_x - min_x;
    delta_y = max_y - min_y;
    delta_z = max_z - min_z;

    printf( "delta x: %f\n", delta_x );
    printf( "delta y: %f\n", delta_y );
    printf( "delta z: %f\n", delta_z );

    // vytvorime obrazky podle informari ze souboru
    img = cvCreateImage( cvSize( cvRound (delta_x) , cvRound (delta_y) ), IPL_DEPTH_8U, 1 );
    show_img = cvCreateImage( cvSize( cvRound (delta_x) , cvRound (delta_y) ), IPL_DEPTH_8U, 3 );
    edge_img = cvCreateImage( cvSize( cvRound (delta_x) , cvRound (delta_y) ), IPL_DEPTH_8U, 1 );

    create_windows( img->width, img->height );

    //cvSetMouseCallback( STEP1_WIN_NAME, mouse_probe_handler, probe );
    //cvSetMouseCallback( STEP2_WIN_NAME, mouse_probe_handler, probe );
    probe->img = img;
    probe->show_img = show_img;
    probe->edge_img = edge_img;


    //printf( "Image w=%d, h=%d\n", img->width, img->height );

    // naplnime vstupni obraz daty z lidaru
    fill_image( bin_file, img, min_x, max_x, min_y, max_y, min_z, max_z );
    cvCvtColor( img, show_img, CV_GRAY2RGB );

    // vytvorime obraz hran
    //make_edges( img, edge_img );

    // muzeme obraz hran binarizovat, ale v prvni fazi to neni nutne
    //binarize_image( edge_img );
    // dilatace, eroze

    // v cyklu cekame na odezvu uzivatele (klikani uzivatele)
    // pokud zmackneme klavesu 'q', program konci

    //cvSaveImage( img_file, img );
    while ( 1 ) {
        cvShowImage( STEP1_WIN_NAME, show_img );
        //cvShowImage( STEP2_WIN_NAME, edge_img );
        int key = cvWaitKey( 10 );
        if ( key == 'q' )
        {
            break;
        }
    }


    cvDestroyWindow( STEP1_WIN_NAME );
    cvDestroyWindow( STEP2_WIN_NAME );

    cvReleaseImage( &img );
    cvReleaseImage( &show_img );
    cvReleaseImage( &edge_img );
}


int main( int argc, char *argv[] ) {
    char *txt_file, *bin_file, *img_file;

    if ( argc < 4 ) {
        printf( "Not enough parameters.\n" );
        exit( 1 );
    }

    txt_file = argv[ 1 ];
    bin_file = argv[ 2 ];
    img_file = argv[ 3 ];

    process_lidar( txt_file, bin_file, img_file );

    return 0;
}