/*
 * Copyright 2017 Anton Khirnov <anton@khirnov.net>
 *
 * 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 <http://www.gnu.org/licenses/>.
 */

#include "config.h"

#include <errno.h>
#include <float.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>

#include <cblas.h>

#include <threadpool.h>

#include "basis.h"
#include "common.h"
#include "internal.h"
#include "log.h"
#include "nlsolve.h"
#include "td_constraints.h"
#include "teukolsky_data.h"

static const enum BasisFamily basis_sets[NB_EQUATIONS][2] = {
    { BASIS_FAMILY_SB_EVEN,  BASIS_FAMILY_COS_EVEN },
    { BASIS_FAMILY_SB_EVEN,  BASIS_FAMILY_COS_EVEN },
    { BASIS_FAMILY_SB_EVEN,  BASIS_FAMILY_COS_EVEN },
};

static void log_callback(TDLogger *log, int level, const char *fmt, va_list vl)
{
    TDContext *td = log->opaque;
    td->log_callback(td, level, fmt, vl);
}

static int teukolsky_init_check_options(TDContext *td)
{
    TDPriv *s = td->priv;
    int ret;

    if (!td->nb_threads) {
        long val = 0;

#ifdef _SC_NPROCESSORS_ONLN
        val = sysconf(_SC_NPROCESSORS_ONLN);
#endif

        if (val <= 0)
            val = 1;

        td->nb_threads = val;
    }

    ret = tp_init(&s->tp, td->nb_threads);
    if (ret < 0)
        return ret;

    s->logger.log    = log_callback;
    s->logger.opaque = td;

    s->basis_order[0][0] = td->nb_coeffs[0];
    s->basis_order[0][1] = td->nb_coeffs[1];
    s->basis_order[1][0] = td->nb_coeffs[0];
    s->basis_order[1][1] = td->nb_coeffs[1];
    s->basis_order[2][0] = td->nb_coeffs[0];
    s->basis_order[2][1] = td->nb_coeffs[1];

    ret = posix_memalign((void**)&s->coeffs, 32,
                         sizeof(*s->coeffs) * NB_EQUATIONS * td->nb_coeffs[0] * td->nb_coeffs[1]);
    if (ret)
        return -ENOMEM;
    memset(s->coeffs, 0, sizeof(*s->coeffs) * NB_EQUATIONS * td->nb_coeffs[0] * td->nb_coeffs[1]);

    if (td->solution_branch > 0) {
        ret = posix_memalign((void**)&s->coeffs_tmp, 32,
                             sizeof(*s->coeffs_tmp) * NB_EQUATIONS * td->nb_coeffs[0] * td->nb_coeffs[1]);
        if (ret)
            return -ENOMEM;
        memset(s->coeffs_tmp, 0, sizeof(*s->coeffs_tmp) * NB_EQUATIONS * td->nb_coeffs[0] * td->nb_coeffs[1]);
    }

    for (int i = 0; i < NB_EQUATIONS; i++)
        td->coeffs[i] = s->coeffs + i * td->nb_coeffs[0] * td->nb_coeffs[1];

    for (int i = 0; i < ARRAY_ELEMS(basis_sets); i++)
        for (int j = 0; j < ARRAY_ELEMS(basis_sets[i]); j++) {
            double sf;

            ret = tdi_basis_init(&s->basis[i][j], basis_sets[i][j], td->basis_scale_factor[j]);
            if (ret < 0)
                return ret;
        }

    return 0;
}

int tdi_ce_alloc(const TDContext *td, const unsigned int *nb_coords,
                 const double * const *coords,
                 double amplitude, TDConstraintEvalContext **pce)
{
    TDPriv *priv = td->priv;
    TDConstraintEvalContext *ce;
    int ret;

    ret = tdi_constraint_eval_alloc(&ce, td->family);
    if (ret < 0) {
        tdi_log(&priv->logger, 0, "Error allocating the constraints evaluator\n");
        return ret;
    }

    ce->logger       = priv->logger;
    ce->amplitude    = amplitude;
    ce->nb_coords[0] = nb_coords[0];
    ce->nb_coords[1] = nb_coords[1];
    ce->coords[0]    = coords[0];
    ce->coords[1]    = coords[1];

    ret = tdi_constraint_eval_init(ce);
    if (ret < 0) {
        tdi_constraint_eval_free(&ce);
        tdi_log(&priv->logger, 0, "Error initializing the constraints evaluator\n");
        return ret;
    }

    *pce = ce;
    return 0;
}

static int nlsolve_alloc(const TDContext *td, NLSolveContext **pnl)
{
    TDPriv *s = td->priv;
    NLSolveContext *nl;
    int ret;

    ret = tdi_nlsolve_context_alloc(&nl, ARRAY_ELEMS(basis_sets));
    if (ret < 0) {
        tdi_log(&s->logger, 0, "Error allocating the non-linear solver\n");
        return ret;
    }

    nl->logger  = s->logger;
    nl->tp      = s->tp;
    nl->maxiter = td->max_iter;
    nl->atol    = td->atol;

    memcpy(nl->basis,       s->basis,       sizeof(s->basis));
    memcpy(nl->solve_order, s->basis_order, sizeof(s->basis_order));

    ret = tdi_nlsolve_context_init(nl);
    if (ret < 0) {
        tdi_log(&s->logger, 0, "Error initializing the non-linear solver\n");
        goto fail;
    }

    *pnl = nl;
    return 0;
fail:
    tdi_nlsolve_context_free(&nl);
    return ret;
}

static int teukolsky_constraint_eval(void *opaque, unsigned int eq_idx,
                                     const unsigned int *colloc_grid_order,
                                     const double * const *colloc_grid,
                                     const double * const (*vars)[PSSOLVE_DIFF_ORDER_NB],
                                     double *dst)
{
    TDConstraintEvalContext *ce = opaque;
    const double *amplitude = opaque;

    return tdi_constraint_eval(ce, eq_idx, vars, dst);
}

int td_solve(TDContext *td, double *coeffs_init[3])
{
    TDPriv *s = td->priv;
    NLSolveContext *nl;
    TDConstraintEvalContext *ce;
    double a0;
    int ret;

    ret = teukolsky_init_check_options(td);
    if (ret < 0)
        return ret;

    ret = nlsolve_alloc(td, &nl);
    if (ret < 0)
        goto fail;

    ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], 0.0, &ce);
    if (ret < 0)
        goto fail;
    if (fabs(td->amplitude) >= ce->a_diverge) {
        tdi_log(&s->logger, 0,
                "Amplitude A=%16.16g is above the point A_{max}=%g, no solutions "
                "are known to exist there. Set solution_branch=1 to get to the "
                "second branch where mass increases with decreasing amplitude\n",
                td->amplitude, ce->a_converge);
    }
    a0 = SGN(td->amplitude) * ce->a_converge;
    tdi_constraint_eval_free(&ce);

    if (coeffs_init) {
        for (int i = 0; i < 3; i++) {
            memcpy(td->coeffs[i], coeffs_init[i], sizeof(*td->coeffs[i]) *
                   td->nb_coeffs[0] * td->nb_coeffs[1]);
        }
    }

    if (td->solution_branch == 0 || coeffs_init) {
        // direct solve with default (flat space) or user-provided initial guess
        ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], td->amplitude, &ce);
        if (ret < 0)
            goto fail;

        ret = tdi_nlsolve_solve(nl, teukolsky_constraint_eval, NULL,
                                ce, s->coeffs, 0);
        tdi_constraint_eval_free(&ce);
        if (ret < 0) {
            tdi_log(&s->logger, 0, "tdi_nlsolve_solve() failed: %d\n", ret);
            goto fail;
        }
    } else {
        // second branch requested and no user-provided initial guess
        // execute two lower-branch solutions and extrapolate to get to the upper branch
        const int N = NB_EQUATIONS * td->nb_coeffs[0] * td->nb_coeffs[1];
        const double delta = 1e-3;
        const double a1 = a0 * (1.0 - delta);

        double cur_amplitude, new_amplitude, step;

        ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], a0, &ce);
        if (ret < 0)
            goto fail;

        ret = tdi_nlsolve_solve(nl, teukolsky_constraint_eval, NULL,
                                ce, s->coeffs, 0);
        tdi_constraint_eval_free(&ce);
        if (ret < 0)
            goto fail;

        ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], a1, &ce);
        if (ret < 0)
            goto fail;

        ret = tdi_nlsolve_solve(nl, teukolsky_constraint_eval, NULL,
                                ce, s->coeffs_tmp, 0);
        tdi_constraint_eval_free(&ce);
        if (ret < 0)
            goto fail;

        cblas_daxpy(N, -1.0, s->coeffs,     1, s->coeffs_tmp, 1);
        cblas_daxpy(N, -1.0, s->coeffs_tmp, 1, s->coeffs,     1);

        // obtain solution for a1 in the upper branch
        ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], a1, &ce);
        if (ret < 0)
            goto fail;

        ret = tdi_nlsolve_solve(nl, teukolsky_constraint_eval, NULL,
                                ce, s->coeffs, 0);
        tdi_constraint_eval_free(&ce);
        if (ret < 0) {
            tdi_log(&s->logger, 0, "Failed to get into the upper branch\n");
            goto fail;
        }

        cur_amplitude = a1;
        step = (td->amplitude - cur_amplitude) / 128.0;

        while (fabs(cur_amplitude - td->amplitude) > DBL_EPSILON) {
            new_amplitude = cur_amplitude + step;
            if (SGN(step) != SGN(td->amplitude - new_amplitude))
                new_amplitude = td->amplitude;
            tdi_log(&s->logger, 2, "Trying amplitude %g\n", new_amplitude);

            ret = tdi_ce_alloc(td, td->nb_coeffs, nl->colloc_grid[0], new_amplitude, &ce);
            if (ret < 0)
                goto fail;

            memcpy(s->coeffs_tmp, s->coeffs, sizeof(*s->coeffs) * N);
            ret = tdi_nlsolve_solve(nl, teukolsky_constraint_eval, NULL,
                                    ce, s->coeffs_tmp, 1);
            tdi_constraint_eval_free(&ce);
            if (ret == -EDOM) {
                step *= 0.5;
                if (fabs(step) < 1e-5)
                    goto fail;
                continue;
            } else if (ret < 0)
                goto fail;

            if (ret <= nl->maxiter / 2)
                step *= 1.75;

            cur_amplitude = new_amplitude;
            memcpy(s->coeffs, s->coeffs_tmp, sizeof(*s->coeffs) * N);
        }
    }
finish:
    ret = 0;
    tdi_nlsolve_print_stats(nl);
fail:
    tdi_nlsolve_context_free(&nl);

    return ret;
}

static void log_default_callback(const TDContext *td, int level, const char *fmt, va_list vl)
{
    vfprintf(stderr, fmt, vl);
}

TDContext *td_context_alloc(void)
{
    TDContext *td = calloc(1, sizeof(*td));

    if (!td)
        return NULL;

    td->nb_threads = 0;

    td->family          = TD_FAMILY_TIME_ANTISYM_CUBIC;
    td->solution_branch = 0;
    td->amplitude       = 1.0;

    td->nb_coeffs[0] = 32;
    td->nb_coeffs[1] = 16;

    td->basis_scale_factor[0] = 3.0;
    td->basis_scale_factor[1] = 3.0;

    td->max_iter = 16;
    td->atol     = 1e-12;

    td->log_callback = log_default_callback;

    td->priv = calloc(1, sizeof(TDPriv));
    if (!td->priv) {
        free(td);
        return NULL;
    }

    return td;
}

void td_context_free(TDContext **ptd)
{
    TDContext *td = *ptd;
    TDPriv *s;

    if (!td)
        return;

    s = td->priv;

    tp_free(&s->tp);

    for (int i = 0; i < ARRAY_ELEMS(s->basis); i++)
        for (int j = 0; j < ARRAY_ELEMS(s->basis[i]); j++)
            tdi_basis_free(&s->basis[i][j]);

    free(s->coeffs);
    free(s->coeffs_tmp);
    free(s->coeffs_lapse);

    free(td->priv);
    free(td);
    *ptd = NULL;
}