#include "likelihood.h"
#include <stdio.h>
#include "zebra.h"
#include "Record_Info.h"
#include "event.h"
#include "zdab_utils.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>
#include "scattering.h"
#include "pmt.h"
#include "sno_charge.h"
#include "db.h"
#include "dqxx.h"

#define EV_RECORD    0x45562020 // 'EV  '  (as written to ZDAB file)

double nll(const gsl_vector *v, void *params)
{
    double T, theta, phi, t0;
    double pos[3], dir[3];

    T = gsl_vector_get(v,0);

    /* FIXME: should use a minimization algorithm with bounds. */
    if (T < 0) return 1e9;

    pos[0] = gsl_vector_get(v,1);
    pos[1] = gsl_vector_get(v,2);
    pos[2] = gsl_vector_get(v,3);

    theta = gsl_vector_get(v,4);
    phi = gsl_vector_get(v,5);
    dir[0] = sin(theta)*cos(phi);
    dir[1] = sin(theta)*sin(phi);
    dir[2] = cos(theta);

    t0 = gsl_vector_get(v,6);

    return nll_muon((event *) params, T, pos, dir, t0);
}

int main(int argc, char **argv)
{
    int i;
    zebraFile *f;
    bank b;
    int rv;
    PMTBank bpmt;
    EVBank bev;
    event ev = {0};
    int crate, card, channel;
    int id;

    if (argc < 2) {
        fprintf(stderr, "usage: fit [filename]\n");
        return 1;
    }

    f = zebra_open(argv[1]);

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

    load_pmt_info();

    for (i = 0; i < 10000; i++) {
        ev.pmt_hits[i].hit = 0;
        ev.pmt_hits[i].flags = 0;
    }

    ev.run = -1;

    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    gsl_multimin_function minex_func;

    size_t iter = 0;
    int status;
    double size;

    /* Starting point */
    x = gsl_vector_alloc(7);

    ss = gsl_vector_alloc(7);

    minex_func.n = 7;
    minex_func.f = nll;
    minex_func.params = &ev;

    init_interpolation();
    init_charge();
    dict *db = db_init();

    if (load_file(db, "DQXX_0000010000.dat")) {
        fprintf(stderr, "failed to load DQXX_0000010000.dat: %s\n", db_err);
        exit(1);
    }

    if (dqxx_init(db, &ev)) {
        fprintf(stderr, "failed to initialize DQXX bank: %s\n", dqxx_err);
        exit(1);
    }

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

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

        if (b.name == 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.pn & KPF_DIS;
        } else if (b.name == EV_RECORD) {
            unpack_ev(b.data, &bev);
            if (ev.run != -1) {
                if (ev.run != bev.run || ev.gtid != bev.gtr_id) {
                    /* New event, so we need to fit the old event. */
                    iter = 0;

                    gsl_vector_set(x,0,1000.0);
                    gsl_vector_set(x,1,100.0);
                    gsl_vector_set(x,2,100.0);
                    gsl_vector_set(x,3,100.0);
                    gsl_vector_set(x,4,1.57);
                    gsl_vector_set(x,5,0.0);
                    gsl_vector_set(x,6,120.0);

                    gsl_vector_set(ss,0,10.0);
                    gsl_vector_set(ss,1,10.0);
                    gsl_vector_set(ss,2,10.0);
                    gsl_vector_set(ss,3,10.0);
                    gsl_vector_set(ss,4,0.1);
                    gsl_vector_set(ss,5,0.1);
                    gsl_vector_set(ss,6,1.0);

                    s = gsl_multimin_fminimizer_alloc(T, 7);
                    gsl_multimin_fminimizer_set(s, &minex_func, x, ss);

                    do {
                        iter++;
                        status = gsl_multimin_fminimizer_iterate(s);

                        if (status) break;

                        size = gsl_multimin_fminimizer_size(s);
                        status = gsl_multimin_test_size(size,1e-2);

                        if (status == GSL_SUCCESS) {
                            printf("converged to minimum at\n");
                        }

                        printf("%5zu %.2g %.2g %.2g %.2g %.2g %.2g %.2g f() = %7.3e size = %.3f\n",
                               iter,
                               gsl_vector_get(s->x,0),
                               gsl_vector_get(s->x,1),
                               gsl_vector_get(s->x,2),
                               gsl_vector_get(s->x,3),
                               gsl_vector_get(s->x,4),
                               gsl_vector_get(s->x,5),
                               gsl_vector_get(s->x,6),
                               s->fval,
                               size);
                    } while (status == GSL_CONTINUE && iter < 1000);

                    gsl_multimin_fminimizer_free(s);
                }
            }
            ev.run = bev.run;
            ev.gtid = bev.gtr_id;
        }
    }

    free_interpolation();

    db_free(db);

    gsl_vector_free(x);
    gsl_vector_free(ss);

    zebra_close(f);

    return 0;

err:

    zebra_close(f);
    return 1;
}