/* Copyright (c) 2019, Anthony Latorre <tlatorre at uchicago>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.

 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.

 * You should have received a copy of the GNU General Public License along with
 * this program. If not, see <https://www.gnu.org/licenses/>.
 */

#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"
#include "util.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, zebraBank *bev)
{
    /* Read all the PMT banks from the zebra file and update `ev`.
     *
     * Returns 0 on success, -1 on error. */
    int i, rv;
    PMTBank bpmt;
    zebraBank b;
    int id, crate, card, channel;

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

    rv = zebra_get_bank(f,&b,bev->links[KEV_PMT-1]);

    if (rv) {
        fprintf(stderr, "error getting PMT bank: %s\n", zebra_err);
        return -1;
    }

    while (1) {
        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;

        if (!b.next) break;

        rv = zebra_get_bank(f,&b,b.next);

        if (rv) {
            fprintf(stderr, "error getting PMT bank: %s\n", zebra_err);
            return -1;
        }
    }

    return 0;
}

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;
    zebraBank bmast, bmc, bmcgn, 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;
    }

    while (1) {
        rv = zebra_read_next_logical_record(f);

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

        rv = zebra_get_bank(f, &bmast, f->first_bank);

        if (rv) {
            fprintf(stderr, "error getting MAST bank: %s\n", zebra_err);
            goto err;
        }

        if (bmast.links[KMAST_MC-1] == 0) {
            fprintf(stderr, "MAST link is zero!\n");
            goto err;
        }

        rv = zebra_get_bank(f,&bmc,bmast.links[KMAST_MC-1]);

        if (rv) {
            fprintf(stderr, "error getting MC bank: %s\n", zebra_err);
            goto err;
        }

        if (bmast.links[KMC_MCGN-1] == 0) {
            fprintf(stderr, "MCGN link is zero!\n");
            goto err;
        }

        rv = zebra_get_bank(f,&bmcgn,bmc.links[KMC_MCGN-1]);

        if (rv) {
            fprintf(stderr, "error getting MCGN bank: %s\n", zebra_err);
            goto err;
        }

        if (bmcgn.links[KMCGN_MCTK-1] == 0) {
            fprintf(stderr, "MCTK link is zero!\n");
            goto err;
        }

        rv = zebra_get_bank(f,&b,bmcgn.links[KMCGN_MCTK-1]);

        if (rv) {
            fprintf(stderr, "error getting MCTK bank: %s\n", zebra_err);
            goto err;
        }

        if (b.up == 0) {
            fprintf(stderr, "MCVX link is zero!\n");
            goto err;
        }

        rv = zebra_get_bank(f,&b,b.up);

        if (rv) {
            fprintf(stderr, "error getting MCVX bank: %s\n", zebra_err);
            goto err;
        }

        unpack_mcvx(b.data, &bmcvx);

        rv = zebra_get_bank(f,&b,bmast.links[KMAST_EV-1]);

        if (rv) {
            fprintf(stderr, "error getting EV bank: %s\n", zebra_err);
            goto err;
        }

        /* Skip to the last event so we can traverse them in reverse order. The
         * reason for this is that for some reason SNOMAN puts the events in
         * reverse order within each logical record. */
        while (b.next) {
            rv = zebra_get_bank(f,&b,b.next);

            if (rv) {
                fprintf(stderr, "error getting EV bank: %s\n", zebra_err);
                goto err;
            }
        }

        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);
    }

    zebra_close(f);

    return 0;

err:
    zebra_close(f);

    return 1;
}