#include "likelihood.h"
#include <stdio.h>
#include "zebra.h"
#include "Record_Info.h"
#include "event.h"
#include "zdab_utils.h"
#include "scattering.h"
#include "pmt.h"
#include "sno_charge.h"
#include "db.h"
#include "dqxx.h"
#include <nlopt.h>
#include <math.h> /* for sin(), cos(), etc. */
#include <sys/time.h> /* for gettimeofday() */

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

double nll(unsigned int n, const double *x, double *grad, void *params)
{
    double T, theta, phi, t0;
    double pos[3], dir[3];
    static size_t iter;
    double fval;
    double z1[1], z2[1];
    struct timeval tv_start, tv_stop;

    T = x[0];

    pos[0] = x[1];
    pos[1] = x[2];
    pos[2] = x[3];

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

    t0 = x[6];

    z1[0] = x[7];
    z2[0] = x[8];

    gettimeofday(&tv_start, NULL);
    fval = nll_muon((event *) params, T, pos, dir, t0, z1, z2, 1);
    gettimeofday(&tv_stop, NULL);

    long long elapsed = (tv_stop.tv_sec - tv_start.tv_sec)*1000 + (tv_stop.tv_usec - tv_start.tv_usec)/1000;

    printf("%5zu %10.2f %7.2f %7.2f %7.2f %4.2f %4.2f %5.2f %4.2f %4.2f f() = %7.3e took %lld ms\n",
           iter++,
           x[0],
           x[1],
           x[2],
           x[3],
           x[4],
           x[5],
           x[6],
           x[7],
           x[8],
           fval,
           elapsed);

    return fval;
}

int fit_event(event *ev, double *fmin)
{
    double x[9], ss[9], lb[9], ub[9];
    int rv;

    nlopt_opt opt = nlopt_create(NLOPT_LN_BOBYQA, 9);
    nlopt_set_min_objective(opt,nll,ev);
    nlopt_set_ftol_abs(opt, 1e-5);

    x[0] = 1200.0;
    x[1] = 0.0;
    x[2] = 0.0;
    x[3] = 0.0;
    x[4] = 1.57;
    x[5] = 0.0;
    x[6] = 130.0;
    x[7] = 0.0;
    x[8] = 0.0;

    ss[0] = 10.0;
    ss[1] = 10.0;
    ss[2] = 10.0;
    ss[3] = 10.0;
    ss[4] = 0.1;
    ss[5] = 0.1;
    ss[6] = 1.0;
    ss[7] = 0.1;
    ss[8] = 0.1;

    lb[0] = 1.0;
    lb[1] = -1000.0;
    lb[2] = -1000.0;
    lb[3] = -1000.0;
    lb[4] = 0.0;
    lb[5] = 0.0;
    lb[6] = 0.0;
    lb[7] = -10.0;
    lb[8] = -10.0;

    ub[0] = 100000.0;
    ub[1] = 1000.0;
    ub[2] = 1000.0;
    ub[3] = 1000.0;
    ub[4] = M_PI;
    ub[5] = 2*M_PI;
    ub[6] = 400.0;
    ub[7] = 10.0;
    ub[8] = 10.0;

    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_upper_bounds(opt, ub);

    nlopt_set_initial_step(opt, ss);

    rv = nlopt_optimize(opt,x,fmin);

    nlopt_destroy(opt);

    return rv;
}

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;
    double fmin;

    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 < MAX_PMTS; i++) {
        ev.pmt_hits[i].hit = 0;
        ev.pmt_hits[i].flags = 0;
    }

    ev.run = -1;

    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. */
                    fit_event(&ev,&fmin);
                }
            }
            ev.run = bev.run;
            ev.gtid = bev.gtr_id;

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

    free_interpolation();

    db_free(db);

    zebra_close(f);

    return 0;

err:

    zebra_close(f);
    return 1;
}