#include <stdio.h>
#include "find_peaks.h"
#include "misc.h"
#include "zebra.h"
#include "zdab_utils.h"
#include <stddef.h> /* for size_t */
#include <unistd.h> /* for exit() */
#include "pmt.h"
#include <math.h> /* for M_PI */
#include <errno.h> /* for errno */
#include <string.h> /* for strerror() */
#include "vector.h"

#define EV_RECORD      0x45562020
#define MCTK_RECORD    0x4d43544b
#define MCVX_RECORD    0x4d435658

void plot_hough_transform(double *x, double *y, double *z, size_t n, size_t m)
{
    size_t i, j;

    FILE *pipe = popen("gnuplot -p", "w");

    if (!pipe) {
        fprintf(stderr, "error running gnuplot command: %s\n", strerror(errno));
        exit(1);
    }

    fprintf(pipe, "set title 'Hough Transform'\n");
    /* Not entirely sure what these do, but following the instructions from
     * http://lowrank.net/gnuplot/plot3d2-e.html. */
    fprintf(pipe, "set dgrid3d %zu,%zu\n",n,m);
    fprintf(pipe, "set hidden3d\n");
    fprintf(pipe, "set contour\n");
    fprintf(pipe, "splot '-' u 1:2:3 with lines\n");

    for (i = 0; i < n; i++) {
        for (j = 0; j < m; j++) {
            fprintf(pipe, "%.10g %.10g %.10g\n", x[i], y[j], z[i*m+j]);
        }
    }
    fprintf(pipe,"e\n");

    /* Pause so that you can rotate the 3D graph. */
    fprintf(pipe,"pause mouse keypress\n");

    if (pclose(pipe)) {
        fprintf(stderr, "error closing gnuplot command: %s\n", strerror(errno));
        exit(1);
    }
}

void plot_find_peaks(event *ev, double *peak_theta, double *peak_phi, size_t n)
{
    size_t i;

    FILE *pipe = popen("gnuplot -p", "w");

    if (!pipe) {
        fprintf(stderr, "error running gnuplot command: %s\n", strerror(errno));
        exit(1);
    }

    fprintf(pipe, "set macros\n");
    fprintf(pipe, "load 'viridis.pal'\n");
    fprintf(pipe, "set title 'Hough Transform'\n");
    /* Not entirely sure what these do, but following the instructions from
     * http://lowrank.net/gnuplot/plot3d2-e.html. */
    fprintf(pipe, "set xrange [0:6.28]\n");
    fprintf(pipe, "set yrange [3.14:0]\n");
    fprintf(pipe, "plot '-' u 1:2:3:4 with circles palette fillstyle solid, '-' u 1:2 with circles lc rgb \"red\" lw 2\n");

    for (i = 0; i < MAX_PMTS; i++) {
        if (ev->pmt_hits[i].hit) {
            double r = NORM(pmts[i].pos);
            double theta = acos(pmts[i].pos[2]/r);
            double phi = atan2(pmts[i].pos[1],pmts[i].pos[0]);
            phi = (phi < 0) ? phi + 2*M_PI: phi;
            fprintf(pipe, "%.10g %.10g %.10g %.10g\n", phi, theta, 0.01, ev->pmt_hits[i].qhs);
        }
    }
    fprintf(pipe,"e\n");

    for (i = 0; i < n; i++) {
        fprintf(pipe, "%.10g %.10g\n", peak_phi[i], peak_theta[i]);
    }
    fprintf(pipe,"e\n");

    /* Pause so that you can rotate the 3D graph. */
    fprintf(pipe,"pause mouse keypress\n");

    if (pclose(pipe)) {
        fprintf(stderr, "error closing gnuplot command: %s\n", strerror(errno));
        exit(1);
    }
}

int get_event(zebraFile *f, event *ev, bank *b)
{
    /* Read all the PMT banks from the zebra file and update `ev`.
     *
     * Returns the last return value from get_bank(). */
    int i, rv;
    PMTBank bpmt;
    int id, crate, card, channel;

    for (i = 0; i < MAX_PMTS; i++) {
        ev->pmt_hits[i].hit = 0;
    }

    while (1) {
        rv = next_bank(f, b);

        if (rv == -1) {
            break;
        } else if (rv == 1) {
            /* EOF */
            break;
        }

        switch (b->name) {
        case PMT_RECORD:
            unpack_pmt(b->data, &bpmt);
            card = bpmt.pin/1024;
            crate = (bpmt.pin % 1024)/32;
            channel = bpmt.pin % 32;
            id = crate*512 + card*32 + channel;
            ev->pmt_hits[id].hit = 1;
            ev->pmt_hits[id].t = bpmt.pt;
            ev->pmt_hits[id].qhl = bpmt.phl;
            ev->pmt_hits[id].qhs = bpmt.phs;
            ev->pmt_hits[id].qlx = bpmt.plx;
            ev->pmt_hits[id].flags |= bpmt.pf & KPF_DIS;
            break;
        case EV_RECORD:
        case MAST_RECORD:
            goto end;
        }
    }

end:

    return rv;
}

void usage(void)
{
    fprintf(stderr,"Usage: ./test-find-peaks [options] FILENAME\n");
    fprintf(stderr,"  --plot plot hough transform\n");
    fprintf(stderr,"  -h     display this help message\n");
    exit(1);
}

int main(int argc, char **argv)
{
    int i;
    char *filename = NULL;
    zebraFile *f;
    bank b;
    EVBank bev;
    event ev = {0};
    int rv;
    MCVXBank bmcvx;
    double pos[3];
    double peak_theta[10];
    double peak_phi[10];
    size_t npeaks;
    int plot = 0;

    for (i = 1; i < argc; i++) {
        if (strlen(argv[i]) >= 2 && !strncmp(argv[i], "--", 2)) {
            if (!strcmp(argv[i]+2,"plot")) {
                plot = 1;
                continue;
            }
        } else if (argv[i][0] == '-') {
            switch (argv[i][1]) {
            case 'h':
                usage();
            default:
                fprintf(stderr, "unrecognized option '%s'\n", argv[i]);
                exit(1);
            }
        } else {
            filename = argv[i];
        }
    }

    if (!filename)
        usage();

    load_pmt_info();

    f = zebra_open(filename);

    if (!f) {
        fprintf(stderr, "%s\n", zebra_err);
        return 1;
    }

    int skip = 0;
    while (1) {
        if (!skip)
            rv = next_bank(f, &b);

        skip = 0;

        if (rv == -1) {
            fprintf(stderr, "error getting bank: %s\n", zebra_err);
            goto err;
        } else if (rv == 1) {
            /* EOF */
            break;
        }

        switch (b.name) {
        case MCVX_RECORD:
            /* New MC vertex. */
            if (b.status & KMCVX_CRE) {
                unpack_mcvx(b.data, &bmcvx);
            }
            break;
        case EV_RECORD:
            unpack_ev(b.data, &bev);
            ev.run = bev.run;
            ev.gtid = bev.gtr_id;

            rv = get_event(f,&ev,&b);

            pos[0] = bmcvx.x;
            pos[1] = bmcvx.y;
            pos[2] = bmcvx.z;

            size_t n = 100;
            size_t m = 100;

            double *x = calloc(n,sizeof(double));
            double *y = calloc(m,sizeof(double));
            double *result = calloc(n*m,sizeof(double));

            for (i = 0; i < n; i++) {
                x[i] = i*M_PI/(n-1);
            }

            for (i = 0; i < m; i++) {
                y[i] = i*2*M_PI/(m-1);
            }

            get_hough_transform(&ev,pos,x,y,n,m,result,0,0);

            find_peaks(&ev,pos,n,m,peak_theta,peak_phi,&npeaks,LEN(peak_theta));

            printf("gtid %i\n", ev.gtid);
            for (i = 0; i < npeaks; i++) {
                printf("%.2f %.2f\n", peak_theta[i], peak_phi[i]);
            }

            if (plot)
                plot_find_peaks(&ev,peak_theta,peak_phi,npeaks);

            /* Skip reading in the next bank since get_event() already read in
             * the next bank. */
            skip = 1;
        }
    }

    zebra_close(f);

    return 0;

err:
    zebra_close(f);

    return 1;
}