#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;

    f = fopen(filename, "r");

    min_x = min_y = min_z = FLT_MAX;

    max_x = max_y = max_z = -FLT_MAX;

    while (!feof(f)){
        fread(&x, sizeof(x), 1, f);
        fread(&y, sizeof(y), 1, f);
        fread(&z, sizeof(z), 1, f);
        fread(&l_type, sizeof(l_type), 1, f);

        if (x<min_x){
            min_x = x;
        }
        else if (x>max_x){
            max_x = x;
        }

        if (y<min_y){
            min_y = y;
        }
        else if (y>max_y){
            max_y = y;
        }

        if (z<min_z){
            min_z = z;
        }
        else if (z>max_z){
            max_z = z;
        }
    }

    fclose(f);

    *a_min_x = min_x;
    *a_max_x = max_x;
    *a_min_y = min_y;
    *a_max_y = max_y;
    *a_min_z = min_z;
    *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;

    // 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));//v tomto poli jsou hodnoty 'z' (hodnota nám říká jakou barvu bude mít bod) souřadnic na konkrétním bodu obrázku na pozici xy
    avg_count = (int*)malloc(img->width*img->height*sizeof(int));//hodnoty je potřeba průměrovat, toto pole je pouze na zápis počtu...

    f = fopen(filename, "r");

    //nulování pole
    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, sizeof(fx), 1, f);
        fread(&fy, sizeof(fy), 1, f);
        fread(&fz, sizeof(fz), 1, f);
        fread(&l_type, sizeof(l_type), 1, f);

        fx -= min_x;//odečteme minimální hodnotu x abychom dostali ze souřadnic které jsou mimo indexy pole indexy 0-img->width
        fy -= min_y;//odečteme minimální hodnotu y abychom dostali ze souřadnic které jsou mimo indexy pole indexy 0-img->height

        x = cvRound(fx);//vratí přetypovanou hodnotu z původního float do int-kvůli indexaci pole
        y = cvRound(fy);

        if (x < img->width && y < img->height)//z nějakýho neznámýho důvodu mě to na konci čtení vyhodilo jako indexy strašný hausnumera, pokud to bude chodit bez toho tak to smažte...
        {
            avg[x + y*img->width] += fz;//přičte přečtenou hodnotu souřadnice z
            avg_count[x + y*img->width]++;//inkrementuje počet pro pozdější výpočet průměru
        }
    }

    fclose(f);

    range = 255 / (max_z - min_z);//normalizace hodnot souřadnice z, které jsou ve zdrojovém souboru od hodnoty cca 220 do hodnoty 260, cílová hodnota musí být v rozmezí 0-255, pokud bychom jenom odečetli konstantu tak by byl obrázek málo kontrastní

    //přepočet zetové souřadnice
    for (int i = 0; i<img->width*img->height; i++){
        avg[i] /= avg_count[i];//výpočet průměru
        avg[i] -= min_z;//odečtení minima tím dostaneme hodnotu v rozmezí 0-40
        avg[i] *= range;//přepočet na hodnoty 0-255
    }

    // 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];//vyplní jednotlivé body do obrázku
        }
    }

} //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 + 0.5f), cvRound(delta_y + 0.5f)), IPL_DEPTH_8U, 1);
    show_img = cvCreateImage(cvSize(cvRound(delta_x + 0.5f), cvRound(delta_y + 0.5f)), IPL_DEPTH_8U, 3);
    edge_img = cvCreateImage(cvSize(cvRound(delta_x + 0.5f), cvRound(delta_y + 0.5f)), 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

    while (1) {
        cvShowImage(STEP1_WIN_NAME, show_img);
        //cvShowImage( STEP2_WIN_NAME, edge_img );
        //cvSaveImage( img_file, 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;
}