dynamics.c

Go to the documentation of this file.

/*
 * This work is licensed under a Creative Commons 
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * 
 *    http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 * 
 * You are free:
 * 
 *    to Share - to copy, distribute, display, and perform the work
 *    to Remix - to make derivative works
 * 
 * Under the following conditions:
 * 
 *    Attribution. You must attribute the work in the manner specified by the
 *    author or licensor (but not in any way that suggests that they endorse you
 *    or your use of the work).
 * 
 *    Noncommercial. You may not use this work for commercial purposes.
 * 
 *    Share Alike. If you alter, transform, or build upon this work, you may
 *    distribute the resulting work only under the same or similar license to
 *    this one.
 * 
 * For any reuse or distribution, you must make clear to others the license
 * terms of this work. The best way to do this is by including this header.
 * 
 * Any of the above conditions can be waived if you get permission from the
 * copyright holder.
 * 
 * Apart from the remix rights granted under this license, nothing in this
 * license impairs or restricts the author's moral rights.
 */


#include <jwsc/config.h>

#include <stdlib.h>
#include <stdio.h>
#include <inttypes.h>
#include <math.h>
#include <assert.h>

#include "jwsc/base/error.h"
#include "jwsc/base/limits.h"
#include "jwsc/vector/vector.h"
#include "jwsc/vector/vector_math.h"
#include "jwsc/vector/vector_io.h"
#include "jwsc/matrix/matrix.h"
#include "jwsc/prob/pdf.h"
#include "jwsc/prob/mv_pdf.h"
#include "jwsc/stat/dynamics.h"


Error* stochastic_dynamics_1_d
(
    uint32_t iterations,
    double   delta_t,
    double   alpha,
    uint32_t kick,
    void*    params,
    void     (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*   (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_d* sample, const Vector_d* momenta, void* params)
)
{
    uint32_t D;

    Vector_d* p = NULL;
    Vector_d* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_d(&delta_t_v, D, delta_t);

    err = stochastic_dynamics_2_d(iterations, delta_t_v, alpha, kick, params,
            get_pt_from_params, grad_energy_func, accept_sample);

    free_vector_d(p);
    free_vector_d(delta_t_v);

    return err;
}


Error* stochastic_dynamics_2_d
(
    uint32_t        iterations,
    const Vector_d* delta_t,
    double          alpha,
    uint32_t        kick,
    void*           params,
    void            (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*          (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_d* sample, const Vector_d* momenta, void* params)
)
{
    uint32_t i;
    uint32_t d, D;

    Vector_d* g  = NULL;
    Vector_d* p  = NULL;
    Vector_d* p_stoch  = NULL;
    Vector_d* pp = NULL;
    Vector_d* q  = NULL;
    Vector_d* gauss = NULL;
    Error*    err;

    get_pt_from_params(&q, params);
    D = q->num_elts;

    sample_standard_mv_gaussian_pdf_d(&p, D);

    if ((err = grad_energy_func(&g, params)))
    {
        free_vector_d(g);
        free_vector_d(p);
        free_vector_d(q);
        free_vector_d(gauss);
        return err;
    }
    for (d = 0; d < D; d++)
    {
        g->elts[ d ] *= 0.5 * delta_t->elts[ d ];
    }
    subtract_vectors_d(&p, p, g);
    copy_vector_d(&p_stoch, p);

    for (i = 0; i < iterations; i++)
    {
        if (kick > 0 && ((i+1) % kick == 0))
        {
            copy_vector_d(&p_stoch, p);
        }

        copy_vector_d(&pp, p_stoch);
        for (d = 0; d < D; d++)
        {
            pp->elts[ d ] *= delta_t->elts[ d ];
        }
        add_vectors_d(&q, q, pp);

        if ((err = accept_sample(q, p_stoch, params)))
        {
            free_vector_d(g);
            free_vector_d(p);
            free_vector_d(p_stoch);
            free_vector_d(pp);
            free_vector_d(q);
            free_vector_d(gauss);
            return err;
        }

        /* The params might range limit the point, so get it again. */
        get_pt_from_params(&q, params);

        if ((err = grad_energy_func(&g, params)))
        {
            free_vector_d(g);
            free_vector_d(p);
            free_vector_d(p_stoch);
            free_vector_d(pp);
            free_vector_d(q);
            free_vector_d(gauss);
            return err;
        }
        for (d = 0; d < D; d++)
        {
            g->elts[ d ] *= delta_t->elts[ d ];
        }
        subtract_vectors_d(&p, p, g);
        subtract_vectors_d(&p_stoch, p_stoch, g);

        multiply_vector_by_scalar_d(&p_stoch, p_stoch, alpha);
        sample_standard_mv_gaussian_pdf_d(&gauss, D);
        multiply_vector_by_scalar_d(&gauss, gauss, sqrt(1 - alpha*alpha));
        add_vectors_d(&p_stoch, p_stoch, gauss);
    }

    // Ignore the last half-step update of the momenta.

    free_vector_d(g);
    free_vector_d(p);
    free_vector_d(p_stoch);
    free_vector_d(pp);
    free_vector_d(q);
    free_vector_d(gauss);

    return NULL;
}

Error* hybrid_monte_carlo_1_d
(
    uint32_t iterations,
    uint32_t leap_iter,
    double   delta_t,
    double   alpha,
    uint32_t kick,
    void*    params,
    void     (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*   (*set_params_from_pt)(const Vector_d* p, void* params),
    Error*   (*energy_func)(double* energy_out, void* params),
    Error*   (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_d* sample, const Vector_d* momenta, void* params),
    Error*   (*reject_sample)(const Vector_d* sample, const Vector_d* momenta, void* params)
)
{
    uint32_t D;

    Vector_d* p = NULL;
    Vector_d* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_d(&delta_t_v, D, delta_t);

    err = hybrid_monte_carlo_2_d(iterations, leap_iter, delta_t_v, alpha,
            kick, params, get_pt_from_params, set_params_from_pt, energy_func,
            grad_energy_func, accept_sample, reject_sample);

    free_vector_d(p);
    free_vector_d(delta_t_v);

    return err;
}

Error* hybrid_monte_carlo_2_d
(
    uint32_t        iterations,
    uint32_t        leap_iter,
    const Vector_d* delta_t,
    double          alpha,
    uint32_t        kick,
    void*           params,
    void            (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*          (*set_params_from_pt)(const Vector_d* p, void* params),
    Error*          (*energy_func)(double* energy_out, void* params),
    Error*          (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_d* sample, const Vector_d* momenta, void* params),
    Error*          (*reject_sample)(const Vector_d* sample, const Vector_d* momenta, void* params)
)
{
    uint32_t i, l;
    uint32_t d, D;
    double   lambda;
    double   E, K;
    double   E_prop, K_prop;
    double   log_alpha;
    double   log_u;

    Vector_d* g = NULL;
    Vector_d* p = NULL;
    Vector_d* p_stoch = NULL;
    Vector_d* p_prop = NULL;
    Vector_d* p_stoch_prop = NULL;
    Vector_d* pp_prop = NULL;
    Vector_d* q = NULL;
    Vector_d* q_prop = NULL;
    Vector_d* gauss = NULL;
    Vector_d* delta_t_0 = NULL;
    Error*    err;

    get_pt_from_params(&q, params);
    D = q->num_elts;

    sample_standard_mv_gaussian_pdf_d(&p, D);

    if ((err = energy_func(&E, params)))
    {
        free_vector_d(p);
        free_vector_d(q);
        return err;
    }

    copy_vector_d(&p_stoch, p);
    calc_vector_dot_prod_d(&K, p, p);
    K *= 0.5;

    copy_vector_d(&delta_t_0, delta_t);

    for (i = 0; i < iterations; i++)
    {
        copy_vector_d(&q_prop, q);
        copy_vector_d(&p_prop, p);
        copy_vector_d(&p_stoch_prop, p_stoch);

        lambda = (sample_uniform_pdf_d(0, 1) < 0.5) ? -1 : 1;
        for (d = 0; d < D; d++)
        {
            delta_t->elts[ d ] = delta_t_0->elts[ d ] * lambda;
        }

        if ((err = grad_energy_func(&g, params)))
        {
            free_vector_d(g);
            free_vector_d(p); free_vector_d(p_prop); free_vector_d(pp_prop);
            free_vector_d(p_stoch); free_vector_d(p_stoch_prop);
            free_vector_d(q); free_vector_d(q_prop);
            free_vector_d(gauss);
            free_vector_d(delta_t_0);
            return err;
        }
        for (d = 0; d < D; d++)
        {
            g->elts[ d ] *= 0.5*delta_t->elts[ d ];
        }
        subtract_vectors_d(&p_prop, p_prop, g);
        subtract_vectors_d(&p_stoch_prop, p_stoch_prop, g);

        for (l = 0; l < leap_iter; l++)
        {
            copy_vector_d(&pp_prop, p_stoch_prop);
            for (d = 0; d < D; d++)
            {
                pp_prop->elts[ d ] *= delta_t->elts[ d ];
            }
            add_vectors_d(&q_prop, q_prop, pp_prop);

            if ((err = set_params_from_pt(q_prop, params)) ||
                (err = grad_energy_func(&g, params)))
            {
                assert(set_params_from_pt(q, params) == NULL);
                free_vector_d(g);
                free_vector_d(p); free_vector_d(p_prop); free_vector_d(pp_prop);
                free_vector_d(p_stoch); free_vector_d(p_stoch_prop);
                free_vector_d(q); free_vector_d(q_prop);
                free_vector_d(gauss);
                free_vector_d(delta_t_0);
                return err;
            }

            /* The params might range limit the point, so get it again. */
            get_pt_from_params(&q_prop, params);

            for (d = 0; d < D; d++)
            {
                g->elts[ d ] *= delta_t->elts[ d ];
            }
            subtract_vectors_d(&p_prop, p_prop, g);
            subtract_vectors_d(&p_stoch_prop, p_stoch_prop, g);

            multiply_vector_by_scalar_d(&p_stoch_prop, p_stoch_prop, alpha);
            sample_standard_mv_gaussian_pdf_d(&gauss, D);
            multiply_vector_by_scalar_d(&gauss, gauss, sqrt(1 - alpha*alpha));
            add_vectors_d(&p_stoch_prop, p_stoch_prop, gauss);
        }

        if ((err = energy_func(&E_prop, params)) ||
            (err = set_params_from_pt(q, params)))
        {
            free_vector_d(g);
            free_vector_d(p); free_vector_d(p_prop); free_vector_d(pp_prop);
            free_vector_d(p_stoch); free_vector_d(p_stoch_prop);
            free_vector_d(q); free_vector_d(q_prop);
            free_vector_d(gauss);
            free_vector_d(delta_t_0);
            return err;
        }

        calc_vector_dot_prod_d(&K_prop, p_stoch_prop, p_stoch_prop);
        K_prop *= 0.5;

        log_alpha = - E_prop - K_prop + E + K;
        log_u     = log(sample_uniform_pdf_d(JWSC_MIN_LOG_ARG, 1.0));

        if (log_u <= log_alpha)
        {
            copy_vector_d(&q, q_prop);
            copy_vector_d(&p, p_prop);
            copy_vector_d(&p_stoch, p_stoch_prop);

            if ((err = accept_sample(q, p_stoch, params)))
            {
                free_vector_d(g);
                free_vector_d(p); free_vector_d(p_prop); free_vector_d(pp_prop);
                free_vector_d(p_stoch); free_vector_d(p_stoch_prop);
                free_vector_d(q); free_vector_d(q_prop);
                free_vector_d(gauss);
                free_vector_d(delta_t_0);
                return err;
            }

            /* The params might range limit the point, so get it again. */
            get_pt_from_params(&q, params);

            E = E_prop;

            if (kick > 0 && ((i+1) % kick == 0))
            {
                copy_vector_d(&p_stoch, p);
                calc_vector_dot_prod_d(&K, p_stoch, p_stoch);
                K *= 0.5;
            }
            else
            {
                K = K_prop;
            }
        }
        else if (reject_sample)
        {
            if ((err = reject_sample(q_prop, p_stoch_prop, params)))
            {
                free_vector_d(g);
                free_vector_d(p); free_vector_d(p_prop); free_vector_d(pp_prop);
                free_vector_d(p_stoch); free_vector_d(p_stoch_prop);
                free_vector_d(q); free_vector_d(q_prop);
                free_vector_d(gauss);
                free_vector_d(delta_t_0);
                return err;
            }
        }
    }

    free_vector_d(g);
    free_vector_d(p);
    free_vector_d(p_stoch);
    free_vector_d(p_prop);
    free_vector_d(p_stoch_prop);
    free_vector_d(pp_prop);
    free_vector_d(q);
    free_vector_d(q_prop);
    free_vector_d(gauss);
    free_vector_d(delta_t_0);

    return NULL;
}

Error* langevin_1_f
(
    uint32_t iterations,
    float    delta_t,
    void*    params,
    void     (*get_pt_from_params)(Vector_f** p_out, const void* params),
    Error*   (*grad_energy_func)(Vector_f** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_f* sample, void* params)
)
{
    uint32_t D;

    Vector_f* p = NULL;
    Vector_f* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_f(&delta_t_v, D, delta_t);

    err = langevin_2_f(iterations, delta_t_v, params, get_pt_from_params,
            grad_energy_func, accept_sample);

    free_vector_f(p);
    free_vector_f(delta_t_v);

    return err;
}

Error* langevin_2_f
(
    uint32_t        iterations,
    const Vector_f* delta_t,
    void*           params,
    void            (*get_pt_from_params)(Vector_f** p_out, const void* params),
    Error*          (*grad_energy_func)(Vector_f** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_f* sample, void* params)
)
{
    uint32_t i;
    uint32_t n;
    float    dx;
    float    dt;
    float    gauss;

    Vector_f* g = NULL;
    Vector_f* s = NULL;
    Error*    err;

    for (i = 0; i < iterations; i++)
    {
        if ((err = grad_energy_func(&g, params)))
        {
            free_vector_f(g);
            free_vector_f(s);
            return err;
        }

        get_pt_from_params(&s, params);

        assert(s->num_elts == g->num_elts);
        assert(s->num_elts == delta_t->num_elts);

        for (n = 0; n < s->num_elts; n++)
        {
            dx    = g->elts[ n ];
            dt    = delta_t->elts[ n ];
            gauss = sample_standard_gaussian_pdf_f(-6.0, 6.0);
            s->elts[ n ] -= dt*(0.5*dt*dx + gauss);
        }

        if ((err = accept_sample(s, params)))
        {
            free_vector_f(g);
            free_vector_f(s);
            return err;
        }
    }

    free_vector_f(g);
    free_vector_f(s);

    return NULL;
}

Error* langevin_1_d
(
    uint32_t iterations,
    double   delta_t,
    void*    params,
    void     (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*   (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_d* sample, void* params)
)
{
    uint32_t D;

    Vector_d* p = NULL;
    Vector_d* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_d(&delta_t_v, D, delta_t);

    err = langevin_2_d(iterations, delta_t_v, params, get_pt_from_params,
            grad_energy_func, accept_sample);

    free_vector_d(p);
    free_vector_d(delta_t_v);

    return err;
}

Error* langevin_2_d
(
    uint32_t        iterations,
    const Vector_d* delta_t,
    void*           params,
    void            (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*          (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_d* sample, void* params)
)
{
    uint32_t i;
    uint32_t n;
    double   dx;
    double   dt;
    double   gauss;

    Vector_d* g = NULL;
    Vector_d* s = NULL;
    Error*    err;

    for (i = 0; i < iterations; i++)
    {
        if ((err = grad_energy_func(&g, params)))
        {
            free_vector_d(g);
            free_vector_d(s);
            return err;
        }

        get_pt_from_params(&s, params);

        assert(s->num_elts == g->num_elts);
        assert(s->num_elts == delta_t->num_elts);

        for (n = 0; n < s->num_elts; n++)
        {
            dx    = g->elts[ n ];
            dt    = delta_t->elts[ n ];
            gauss = sample_standard_gaussian_pdf_d(-6.0, 6.0);
            s->elts[ n ] -= dt*(0.5*dt*dx + gauss);
        }

        if ((err = accept_sample(s, params)))
        {
            free_vector_d(g);
            free_vector_d(s);
            return err;
        }
    }

    free_vector_d(g);
    free_vector_d(s);

    return NULL;
}

static Error* hyperdynamics_hessian_eigenvalue_f
(
    float*          e_out, 
    const Vector_f* v,
    float           eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*energy_func)(float*, void*)
)
{
    float a, b, c;

    Vector_f* eta_v   = NULL;
    Vector_f* p       = NULL;
    Vector_f* q       = NULL;
    Error*    err;

    get_pt_from_params(&p, params);

    multiply_vector_by_scalar_f(&eta_v, v, eta);

    assert(add_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        return err;
    }

    assert(subtract_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    if ((err = energy_func(&c, params)))
    {
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        return err;
    }

    *e_out = (a + b - 2.0*c) / (eta*eta);

    free_vector_f(eta_v);
    free_vector_f(p);
    free_vector_f(q);

    return NULL;
}

static Error* hyperdynamics_hessian_eigenvalue_d
(
    double*         e_out, 
    const Vector_d* v,
    double          eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*energy_func)(double*, void*)
)
{
    double a, b, c;

    Vector_d* eta_v   = NULL;
    Vector_d* p       = NULL;
    Vector_d* q       = NULL;
    Error*    err;

    get_pt_from_params(&p, params);

    multiply_vector_by_scalar_d(&eta_v, v, eta);

    assert(add_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        return err;
    }

    assert(subtract_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    if ((err = energy_func(&c, params)))
    {
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        return err;
    }

    *e_out = (a + b - 2.0*c) / (eta*eta);

    free_vector_d(eta_v);
    free_vector_d(p);
    free_vector_d(q);

    return NULL;
}

static Error* hyperdynamics_hessian_eigenvector_f
(
    Vector_f**      v_out, 
    float           eta, 
    float           gamma,
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*energy_func)(float*, void*),
    Error*          (*grad_energy_func)(Vector_f**, void*)
)
{
    uint32_t n;
    uint32_t N;
    uint8_t  done;

    float    e;
    float    delta_e;
    float    g;
    float    e_dv_dp;
    float    e_dv_prev_dp;
    float    beta;

    Vector_f* v;
    Vector_f* v_cp    = NULL;
    Vector_f* delta_v = NULL;
    Vector_f* eta_v   = NULL;
    Vector_f* p       = NULL;
    Vector_f* q       = NULL;
    Vector_f* a       = NULL;
    Vector_f* b       = NULL;
    Error*    err     = NULL;
    Vector_f* e_dv    = NULL;
    Vector_f* e_dv_prev = NULL;
    Vector_f* e_dv_diff = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    create_vector_f(v_out, N);
    v = *v_out;

    e    = 1.0e+10;
    done = 0;

    while (!done)
    {
        get_pt_from_params(&p, params);

        multiply_vector_by_scalar_f(&eta_v, v, eta);

        assert(add_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(set_params_from_pt(p, params) == NULL);

        assert(copy_vector_f(&delta_v, v) == NULL);

        assert(subtract_vectors_f(&e_dv, a, b) == NULL);
        multiply_vector_by_scalar_f(&e_dv, e_dv, 1 / eta);

        if (!e_dv_prev)
        {
            copy_vector_f(&e_dv_prev, e_dv);
            calc_vector_dot_prod_f(&e_dv_prev_dp, e_dv_prev, e_dv_prev);
        }

        subtract_vectors_f(&e_dv_diff, e_dv, e_dv_prev);
        calc_vector_dot_prod_f(&e_dv_dp, e_dv, e_dv_diff);

        beta = (e_dv_prev_dp > 0) && (e_dv_dp / e_dv_prev_dp) > 0 
            ? e_dv_dp / e_dv_prev_dp : 0;

        multiply_vector_by_scalar_f(&e_dv_prev, e_dv_prev, beta);
        add_vectors_f(&e_dv, e_dv, e_dv_prev);

        g       = gamma;
        delta_e = e;
        done    = 0;

        while (!done)
        {
            copy_vector_f(&v_cp, v);

            for (n = 0; n < N; n++)
            {
                v_cp->elts[ n ] -= g * e_dv->elts[ n ];
            }
            normalize_vector_mag_f(&v_cp, v_cp);

            if ((err = hyperdynamics_hessian_eigenvalue_f(&e, v_cp, eta, 
                            params, get_pt_from_params, set_params_from_pt, 
                            energy_func)))
            {
                break;
            }

            // TEMP
            //fprintf(stdout, "e1 : %10.3e %10.3e %10.3e\n", 
            //        e, (delta_e - e), g);

            g *= 0.5;

            if (g < 1.0e-16 || (delta_e - e) >= 0)
            {
                done = 1;
            }
        }
        if (err) 
        {
            break;
        }

        copy_vector_f(&v, v_cp);

        delta_e -= e;

        done = 1;
        for (n = 0; n < N; n++)
        {
            delta_v->elts[ n ] 
                = (fabs(fabs(delta_v->elts[ n ]) - fabs(v->elts[ n ])) < 1.0e-6)
                ?  fabs(fabs(delta_v->elts[ n ]) - fabs(v->elts[ n ]))
                :  fabs(delta_v->elts[ n ] - v->elts[ n ]);

            done = (done && (delta_v->elts[ n ] <= 1.0e-6));
        }

        // TEMP
        //fprintf(stdout, "dv : ");
        //write_formatted_vector_fp_f(delta_v, "%10.3e", stdout);
        //fprintf(stdout, "v  : ");
        //write_formatted_vector_fp_f(v, "%10.3e", stdout);

        copy_vector_f(&e_dv_prev, e_dv);
        calc_vector_dot_prod_f(&e_dv_prev_dp, e_dv_prev, e_dv_prev);
    }

    free_vector_f(v_cp);
    free_vector_f(delta_v);
    free_vector_f(eta_v);
    free_vector_f(p);
    free_vector_f(q);
    free_vector_f(a);
    free_vector_f(b);
    free_vector_f(e_dv);
    free_vector_f(e_dv_prev);
    free_vector_f(e_dv_diff);

    if (err)
    {
        free_vector_f(*v_out); *v_out = NULL;
    }

    return err;
}

static Error* hyperdynamics_hessian_eigenvector_d
(
    Vector_d**      v_out, 
    double          eta, 
    double          gamma,
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*energy_func)(double*, void*),
    Error*          (*grad_energy_func)(Vector_d**, void*)
)
{
    uint32_t n;
    uint32_t N;
    uint8_t  done;

    double   e;
    double   delta_e;
    double   g;
    double   e_dv_dp;
    double   e_dv_prev_dp;
    double   beta;

    Vector_d* v;
    Vector_d* v_cp    = NULL;
    Vector_d* delta_v = NULL;
    Vector_d* eta_v   = NULL;
    Vector_d* p       = NULL;
    Vector_d* q       = NULL;
    Vector_d* a       = NULL;
    Vector_d* b       = NULL;
    Error*    err     = NULL;
    Vector_d* e_dv    = NULL;
    Vector_d* e_dv_prev = NULL;
    Vector_d* e_dv_diff = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    create_vector_d(v_out, N);
    v = *v_out;

    e    = 1.0e+10;
    done = 0;

    while (!done)
    {
        get_pt_from_params(&p, params);

        multiply_vector_by_scalar_d(&eta_v, v, eta);

        assert(add_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(set_params_from_pt(p, params) == NULL);

        assert(copy_vector_d(&delta_v, v) == NULL);

        assert(subtract_vectors_d(&e_dv, a, b) == NULL);
        multiply_vector_by_scalar_d(&e_dv, e_dv, 1 / eta);

        if (!e_dv_prev)
        {
            copy_vector_d(&e_dv_prev, e_dv);
            calc_vector_dot_prod_d(&e_dv_prev_dp, e_dv_prev, e_dv_prev);
        }

        subtract_vectors_d(&e_dv_diff, e_dv, e_dv_prev);
        calc_vector_dot_prod_d(&e_dv_dp, e_dv, e_dv_diff);

        beta = (e_dv_prev_dp > 0) && (e_dv_dp / e_dv_prev_dp) > 0 
            ? e_dv_dp / e_dv_prev_dp : 0;

        multiply_vector_by_scalar_d(&e_dv_prev, e_dv_prev, beta);
        add_vectors_d(&e_dv, e_dv, e_dv_prev);

        g       = gamma;
        delta_e = e;
        done    = 0;

        while (!done)
        {
            copy_vector_d(&v_cp, v);

            for (n = 0; n < N; n++)
            {
                v_cp->elts[ n ] -= g * e_dv->elts[ n ];
            }
            normalize_vector_mag_d(&v_cp, v_cp);

            if ((err = hyperdynamics_hessian_eigenvalue_d(&e, v_cp, eta, 
                            params, get_pt_from_params, set_params_from_pt, 
                            energy_func)))
            {
                break;
            }

            // TEMP
            //fprintf(stdout, "e1 : %10.3e %10.3e %10.3e\n", 
            //        e, (delta_e - e), g);

            g *= 0.5;

            if (g < 1.0e-128 || (delta_e - e) >= 0)
            {
                done = 1;
            }
        }
        if (err) 
        {
            break;
        }

        copy_vector_d(&v, v_cp);

        delta_e -= e;

        done = 1;
        for (n = 0; n < N; n++)
        {
            delta_v->elts[ n ] 
                = (fabs(fabs(delta_v->elts[ n ]) - fabs(v->elts[ n ])) < 1.0e-8)
                ?  fabs(fabs(delta_v->elts[ n ]) - fabs(v->elts[ n ]))
                :  fabs(delta_v->elts[ n ] - v->elts[ n ]);

            done = (done && (delta_v->elts[ n ] <= 1.0e-8));
        }

        // TEMP
        //fprintf(stdout, "dv : ");
        //write_formatted_vector_fp_d(delta_v, "%10.3e", stdout);
        //fprintf(stdout, "v  : ");
        //write_formatted_vector_fp_d(v, "%10.3e", stdout);

        copy_vector_d(&e_dv_prev, e_dv);
        calc_vector_dot_prod_d(&e_dv_prev_dp, e_dv_prev, e_dv_prev);
    }

    free_vector_d(v_cp);
    free_vector_d(delta_v);
    free_vector_d(eta_v);
    free_vector_d(p);
    free_vector_d(q);
    free_vector_d(a);
    free_vector_d(b);
    free_vector_d(e_dv);
    free_vector_d(e_dv_prev);
    free_vector_d(e_dv_diff);

    if (err)
    {
        free_vector_d(*v_out); *v_out = NULL;
    }

    return err;
}

static Error* hyperdynamics_dx_hessian_eigenvalue_f
(
    Vector_f**      e_dx_out, 
    const Vector_f* v,
    float           eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*grad_energy_func)(Vector_f**, void*)
)
{
    uint32_t n;
    uint32_t N;

    Vector_f* eta_v   = NULL;
    Vector_f* a       = NULL;
    Vector_f* b       = NULL;
    Vector_f* c       = NULL;
    Vector_f* p       = NULL;
    Vector_f* q       = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    multiply_vector_by_scalar_f(&eta_v, v, eta);

    assert(add_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = grad_energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        free_vector_f(a); free_vector_f(b); free_vector_f(c);
        free_vector_f(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    assert(subtract_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = grad_energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        free_vector_f(a); free_vector_f(b); free_vector_f(c);
        free_vector_f(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    if ((err = grad_energy_func(&c, params)))
    {
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        free_vector_f(a); free_vector_f(b); free_vector_f(c);
        free_vector_f(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    create_vector_f(e_dx_out, N);
    for (n = 0; n < N; n++)
    {
        (*e_dx_out)->elts[ n ] = (a->elts[n] + b->elts[n] - 2.0*c->elts[n]) / 
                                 (eta*eta);
    }

    free_vector_f(eta_v);
    free_vector_f(p);
    free_vector_f(q);
    free_vector_f(a);
    free_vector_f(b);
    free_vector_f(c);

    return NULL;
}

static Error* hyperdynamics_dx_hessian_eigenvalue_d
(
    Vector_d**      e_dx_out, 
    const Vector_d* v,
    double          eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*grad_energy_func)(Vector_d**, void*)
)
{
    uint32_t n;
    uint32_t N;

    Vector_d* eta_v   = NULL;
    Vector_d* a       = NULL;
    Vector_d* b       = NULL;
    Vector_d* c       = NULL;
    Vector_d* p       = NULL;
    Vector_d* q       = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    multiply_vector_by_scalar_d(&eta_v, v, eta);

    assert(add_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = grad_energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        free_vector_d(a); free_vector_d(b); free_vector_d(c);
        free_vector_d(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    assert(subtract_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = grad_energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        free_vector_d(a); free_vector_d(b); free_vector_d(c);
        free_vector_d(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    if ((err = set_params_from_pt(p, params)) ||
        (err = grad_energy_func(&c, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        free_vector_d(a); free_vector_d(b); free_vector_d(c);
        free_vector_d(*e_dx_out); *e_dx_out = NULL;
        return err;
    }

    create_vector_d(e_dx_out, N);
    for (n = 0; n < N; n++)
    {
        (*e_dx_out)->elts[ n ] = (a->elts[n] + b->elts[n] - 2.0*c->elts[n]) / 
                                 (eta*eta);
    }

    free_vector_d(eta_v);
    free_vector_d(p);
    free_vector_d(q);
    free_vector_d(a);
    free_vector_d(b);
    free_vector_d(c);

    return NULL;
}

static Error* hyperdynamics_hessgrad_eigenvalue_f
(
    float*          e_out,
    float           lambda,
    const Vector_f* v,
    float           eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*energy_func)(float*, void*),
    uint8_t         positive
)
{
    float e;
    float a, b;

    Vector_f* eta_v = NULL;
    Vector_f* p     = NULL;
    Vector_f* q     = NULL;
    Error*    err;

    if ((err = hyperdynamics_hessian_eigenvalue_f(&e, v, eta, params,
                    get_pt_from_params, set_params_from_pt, energy_func)))
    {
        free_vector_f(eta_v);
        free_vector_f(p);
        free_vector_f(q);
        return err;
    }

    get_pt_from_params(&p, params);

    multiply_vector_by_scalar_f(&eta_v, v, eta);

    assert(add_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p);
        free_vector_f(q);
        return err;
    }

    assert(subtract_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(eta_v);
        free_vector_f(p);
        free_vector_f(q);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    if (positive)
    {
        e += (0.25*lambda / (eta*eta)) * (a - b) * (a - b);
    }
    else
    {
        e -= (0.25*lambda / (eta*eta)) * (a - b) * (a - b);
    }

    *e_out = e;

    free_vector_f(eta_v);
    free_vector_f(p);
    free_vector_f(q);

    return NULL;
}

static Error* hyperdynamics_hessgrad_eigenvalue_d
(
    double*         e_out,
    double          lambda,
    const Vector_d* v,
    double          eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*energy_func)(double*, void*),
    uint8_t         positive
)
{
    double e;
    double a, b;

    Vector_d* eta_v = NULL;
    Vector_d* p     = NULL;
    Vector_d* q     = NULL;
    Error*    err;

    if ((err = hyperdynamics_hessian_eigenvalue_d(&e, v, eta, params,
                    get_pt_from_params, set_params_from_pt, energy_func)))
    {
        free_vector_d(eta_v);
        free_vector_d(p);
        free_vector_d(q);
        return err;
    }

    get_pt_from_params(&p, params);

    multiply_vector_by_scalar_d(&eta_v, v, eta);

    assert(add_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p);
        free_vector_d(q);
        return err;
    }

    assert(subtract_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(eta_v);
        free_vector_d(p);
        free_vector_d(q);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    if (positive)
    {
        e += (0.25*lambda / (eta*eta)) * (a - b) * (a - b);
    }
    else
    {
        e -= (0.25*lambda / (eta*eta)) * (a - b) * (a - b);
    }

    *e_out = e;

    free_vector_d(eta_v);
    free_vector_d(p);
    free_vector_d(q);

    return NULL;
}

static Error* hyperdynamics_proj_hessian_eigenvector_f
(
    float* g1p_out,
    float  e_pos,
    float  e_neg,
    float  lambda
)
{
    double d;

    assert(fabs(lambda) > 1.0e-8);

    d = (e_pos - e_neg) / lambda;
    //assert(d > 0);
    if (d < 1.0e-8)
    {
        d = 1.0e-8;
    }

    *g1p_out = sqrt(0.5 * d);

    return NULL;
}

static Error* hyperdynamics_proj_hessian_eigenvector_d
(
    double* g1p_out,
    double  e_pos,
    double  e_neg,
    double  lambda
)
{
    double d;

    assert(fabs(lambda) > 1.0e-8);

    d = (e_pos - e_neg) / lambda;
    //assert(d > 0);
    if (d < 1.0e-8)
    {
        d = 1.0e-8;
    }

    *g1p_out = sqrt(0.5 * d);

    return NULL;
}

static Error* hyperdynamics_hessgrad_eigenvector_f
(
    float*          lambda_out,
    Vector_f**      v_pos_out,
    Vector_f**      v_neg_out,
    float           eta, 
    float           gamma,
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*energy_func)(float*, void*),
    Error*          (*grad_energy_func)(Vector_f**, void*)
)
{
    uint32_t n;
    uint32_t N;
    uint8_t  done;
    float    g1p;
    float    lambda;
    float    lambda_cp;
    float    e_pos;
    float    e_neg;
    float    e_pos_dlambda;
    float    e_neg_dlambda;
    float    e_pos_dlambda_diff;
    float    e_neg_dlambda_diff;
    float    e_pos_dlambda_prev;
    float    e_neg_dlambda_prev;
    float    c, d;
    float    delta_g1p;
    float    delta_e;
    float    delta_lambda;
    float    g;
    float    e_pos_dv_dp;
    float    e_neg_dv_dp;
    float    e_pos_dv_prev_dp;
    float    e_neg_dv_prev_dp;
    float    e_pos_dlambda_dp;
    float    e_neg_dlambda_dp;
    float    e_pos_dlambda_prev_dp;
    float    e_neg_dlambda_prev_dp;
    float    e_pos_dv_beta;
    float    e_neg_dv_beta;
    float    e_pos_dlambda_beta;
    float    e_neg_dlambda_beta;

    Vector_f* v_pos;
    Vector_f* v_neg;
    Vector_f* v_pos_cp    = NULL;
    Vector_f* v_neg_cp    = NULL;
    Vector_f* delta_v_pos = NULL;
    Vector_f* delta_v_neg = NULL;
    Vector_f* eta_v       = NULL;
    Vector_f* e_pos_dv    = NULL;
    Vector_f* e_neg_dv    = NULL;
    Vector_f* e_pos_dv_diff = NULL;
    Vector_f* e_neg_dv_diff = NULL;
    Vector_f* e_pos_dv_prev = NULL;
    Vector_f* e_neg_dv_prev = NULL;
    Vector_f* p           = NULL;
    Vector_f* q           = NULL;
    Vector_f* a           = NULL;
    Vector_f* b           = NULL;
    Error*    err         = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    create_vector_f(v_pos_out, N);
    v_pos = *v_pos_out;

    create_vector_f(v_neg_out, N);
    v_neg = *v_neg_out;

    create_vector_f(&e_pos_dv, N);
    create_vector_f(&e_neg_dv, N);

    lambda = *lambda_out;

    /* May not be the best initialization for these. */
    g1p   = 0;
    e_pos = 0;
    e_neg = 0;

    done = 0;

    while (!done)
    {
        get_pt_from_params(&p, params);

        /* Positive lambda */
        multiply_vector_by_scalar_f(&eta_v, v_pos, eta);

        assert(add_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)) ||
            (err = energy_func(&c, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)) ||
            (err = energy_func(&d, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        for (n = 0; n < N; n++)
        {
            e_pos_dv->elts[n] = (a->elts[n] - b->elts[n]) / eta +
                                (0.5*lambda / eta) *
                                (c - d) * (a->elts[n] + b->elts[n]);
        }
        e_pos_dlambda = (0.5*(c - d) / eta) * (0.5*(c - d) / eta);

        if (!e_pos_dv_prev)
        {
            copy_vector_f(&e_pos_dv_prev, e_pos_dv);
            calc_vector_dot_prod_f(&e_pos_dv_prev_dp, e_pos_dv_prev, 
                    e_pos_dv_prev);
            e_pos_dlambda_prev = e_pos_dlambda;
            e_pos_dlambda_prev_dp = e_pos_dlambda_prev * e_pos_dlambda_prev;
        }

        subtract_vectors_f(&e_pos_dv_diff, e_pos_dv, e_pos_dv_prev);
        calc_vector_dot_prod_f(&e_pos_dv_dp, e_pos_dv, e_pos_dv_diff);

        e_pos_dlambda_diff = e_pos_dlambda - e_pos_dlambda_prev;
        e_pos_dlambda_dp = e_pos_dlambda * e_pos_dlambda_diff;

        e_pos_dv_beta = 
            (e_pos_dv_prev_dp > 0) && (e_pos_dv_dp / e_pos_dv_prev_dp) > 0 
            ? e_pos_dv_dp / e_pos_dv_prev_dp : 0;

        e_pos_dlambda_beta = (e_pos_dlambda_prev_dp > 0) && 
            (e_pos_dlambda_dp / e_pos_dlambda_prev_dp) > 0 
            ? e_pos_dlambda_dp / e_pos_dlambda_prev_dp : 0;

        multiply_vector_by_scalar_f(&e_pos_dv_prev, e_pos_dv_prev, 
                e_pos_dv_beta);
        add_vectors_f(&e_pos_dv, e_pos_dv, e_pos_dv_prev);

        e_pos_dlambda += e_pos_dlambda_beta * e_pos_dlambda_prev;


        /* Negative lambda */
        multiply_vector_by_scalar_f(&eta_v, v_neg, eta);

        assert(add_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)) ||
            (err = energy_func(&c, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_f(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)) ||
            (err = energy_func(&d, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        for (n = 0; n < N; n++)
        {
            e_neg_dv->elts[n] = (a->elts[n] - b->elts[n]) / eta -
                                (0.5*lambda / eta) *
                                (c - d) * (a->elts[n] + b->elts[n]);
        }
        e_neg_dlambda = -1.0 * (0.5*(c - d) / eta) * (0.5*(c - d) / eta);

        if (!e_neg_dv_prev)
        {
            copy_vector_f(&e_neg_dv_prev, e_neg_dv);
            calc_vector_dot_prod_f(&e_neg_dv_prev_dp, e_neg_dv_prev, 
                    e_neg_dv_prev);
            e_neg_dlambda_prev = e_neg_dlambda;
            e_neg_dlambda_prev_dp = e_neg_dlambda_prev * e_neg_dlambda_prev;
        }

        subtract_vectors_f(&e_neg_dv_diff, e_neg_dv, e_neg_dv_prev);
        calc_vector_dot_prod_f(&e_neg_dv_dp, e_neg_dv, e_neg_dv_diff);

        e_neg_dlambda_diff = e_neg_dlambda - e_neg_dlambda_prev;
        e_neg_dlambda_dp = e_neg_dlambda * e_neg_dlambda_diff;

        e_neg_dv_beta = 
            (e_neg_dv_prev_dp > 0) && (e_neg_dv_dp / e_neg_dv_prev_dp) > 0 
            ? e_neg_dv_dp / e_neg_dv_prev_dp : 0;

        e_neg_dlambda_beta = (e_neg_dlambda_prev_dp > 0) && 
            (e_neg_dlambda_dp / e_neg_dlambda_prev_dp) > 0 
            ? e_neg_dlambda_dp / e_neg_dlambda_prev_dp : 0;

        multiply_vector_by_scalar_f(&e_neg_dv_prev, e_neg_dv_prev, 
                e_neg_dv_beta);
        add_vectors_f(&e_neg_dv, e_neg_dv, e_neg_dv_prev);

        e_neg_dlambda += e_neg_dlambda_beta * e_neg_dlambda_prev;


        assert(set_params_from_pt(p, params) == NULL);


        assert(copy_vector_f(&delta_v_pos, v_pos) == NULL);
        assert(copy_vector_f(&delta_v_neg, v_neg) == NULL);
        delta_g1p    = g1p;
        delta_lambda = lambda;
        delta_e      = e_pos + e_neg;
        done         = 0;
        g            = gamma;

        while (!done)
        {
            lambda_cp = lambda;

            lambda_cp -= g * e_pos_dlambda;
            lambda_cp -= g * e_neg_dlambda;

            copy_vector_f(&v_pos_cp, v_pos);
            copy_vector_f(&v_neg_cp, v_neg);

            for (n = 0; n < N; n++)
            {
                v_pos_cp->elts[ n ] -= g * e_pos_dv->elts[ n ];
                v_neg_cp->elts[ n ] -= g * e_neg_dv->elts[ n ];
            }
            normalize_vector_mag_f(&v_pos_cp, v_pos_cp);
            normalize_vector_mag_f(&v_neg_cp, v_neg_cp);

            if ((err = hyperdynamics_hessgrad_eigenvalue_f(&e_pos, lambda_cp, 
                            v_pos_cp, eta, params, get_pt_from_params, 
                            set_params_from_pt, energy_func, 1)) ||
                (err = hyperdynamics_hessgrad_eigenvalue_f(&e_neg, lambda_cp, 
                            v_neg_cp, eta, params, get_pt_from_params, 
                            set_params_from_pt, energy_func, 0)))
            {
                break;
            }

            // TEMP
            //fprintf(stdout, "g1p: %10.3e %10.3e %10.3e %10.3e\n", 
            //        e_pos, e_neg, (delta_e - (e_pos+e_neg)), g);

            g *= 0.5;

            if (g < 1.0e-16 || (delta_e - (e_pos+e_neg)) >= 0)
            {
                done = 1;
            }
        }
        if (err) 
        {
            break;
        }
        lambda = lambda_cp;

        copy_vector_f(&v_pos, v_pos_cp);
        copy_vector_f(&v_neg, v_neg_cp);

        delta_lambda = fabs(delta_lambda - lambda);
        delta_e = fabs(delta_e - (e_pos + e_neg));

        hyperdynamics_proj_hessian_eigenvector_f(&g1p, e_pos, e_neg, lambda);
        delta_g1p = fabs(delta_g1p - g1p);

        done = 1;
        for (n = 0; n < N; n++)
        {
            delta_v_pos->elts[ n ] 
                = (fabs(fabs(delta_v_pos->elts[ n ]) - 
                   fabs(v_pos->elts[ n ])) < 1.0e-6)
                ?  fabs(fabs(delta_v_pos->elts[ n ]) - fabs(v_pos->elts[ n ]))
                :  fabs(delta_v_pos->elts[ n ] - v_pos->elts[ n ]);

            delta_v_neg->elts[ n ] 
                = (fabs(fabs(delta_v_neg->elts[ n ]) - 
                   fabs(v_neg->elts[ n ])) < 1.0e-6)
                ?  fabs(fabs(delta_v_neg->elts[ n ]) - fabs(v_neg->elts[ n ]))
                :  fabs(delta_v_neg->elts[ n ] - v_neg->elts[ n ]);

            done = (done && (delta_v_pos->elts[ n ] <= 1.0e-6)
                         && (delta_v_neg->elts[ n ] <= 1.0e-6));
        }
        done = (done && (delta_lambda <= 1.0e-6));
        done = (done || (delta_g1p <= 1.0e-4));

        copy_vector_f(&e_pos_dv_prev, e_pos_dv);
        calc_vector_dot_prod_f(&e_pos_dv_prev_dp, e_pos_dv_prev, e_pos_dv_prev);

        e_pos_dlambda_prev = e_pos_dlambda;
        e_pos_dlambda_prev_dp = e_pos_dlambda_prev * e_pos_dlambda_prev;

        copy_vector_f(&e_neg_dv_prev, e_neg_dv);
        calc_vector_dot_prod_f(&e_neg_dv_prev_dp, e_neg_dv_prev, e_neg_dv_prev);

        e_neg_dlambda_prev = e_neg_dlambda;
        e_neg_dlambda_prev_dp = e_neg_dlambda_prev * e_neg_dlambda_prev;
    }

    *lambda_out = lambda;

    free_vector_f(v_pos_cp);
    free_vector_f(v_neg_cp);
    free_vector_f(delta_v_pos);
    free_vector_f(delta_v_neg);
    free_vector_f(eta_v);
    free_vector_f(e_pos_dv);
    free_vector_f(e_neg_dv);
    free_vector_f(e_pos_dv_diff);
    free_vector_f(e_neg_dv_diff);
    free_vector_f(e_pos_dv_prev);
    free_vector_f(e_neg_dv_prev);
    free_vector_f(p);
    free_vector_f(q);
    free_vector_f(a);
    free_vector_f(b);

    if (err)
    {
        free_vector_f(*v_pos_out); *v_pos_out = NULL;
        free_vector_f(*v_neg_out); *v_neg_out = NULL;
    }

    return err;
}

static Error* hyperdynamics_hessgrad_eigenvector_d
(
    double*         lambda_out,
    Vector_d**      v_pos_out,
    Vector_d**      v_neg_out,
    double          eta, 
    double          gamma,
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*energy_func)(double*, void*),
    Error*          (*grad_energy_func)(Vector_d**, void*)
)
{
    uint32_t n;
    uint32_t N;
    uint8_t  done;
    double   g1p;
    double   lambda;
    double   lambda_cp;
    double   e_pos;
    double   e_neg;
    double   e_pos_dlambda;
    double   e_neg_dlambda;
    double   e_pos_dlambda_diff;
    double   e_neg_dlambda_diff;
    double   e_pos_dlambda_prev;
    double   e_neg_dlambda_prev;
    double   c, d;
    double   delta_g1p;
    double   delta_e;
    double   delta_lambda;
    double   g;
    double   e_pos_dv_dp;
    double   e_neg_dv_dp;
    double   e_pos_dv_prev_dp;
    double   e_neg_dv_prev_dp;
    double   e_pos_dlambda_dp;
    double   e_neg_dlambda_dp;
    double   e_pos_dlambda_prev_dp;
    double   e_neg_dlambda_prev_dp;
    double   e_pos_dv_beta;
    double   e_neg_dv_beta;
    double   e_pos_dlambda_beta;
    double   e_neg_dlambda_beta;

    Vector_d* v_pos;
    Vector_d* v_neg;
    Vector_d* v_pos_cp    = NULL;
    Vector_d* v_neg_cp    = NULL;
    Vector_d* delta_v_pos = NULL;
    Vector_d* delta_v_neg = NULL;
    Vector_d* eta_v       = NULL;
    Vector_d* e_pos_dv    = NULL;
    Vector_d* e_neg_dv    = NULL;
    Vector_d* e_pos_dv_diff = NULL;
    Vector_d* e_neg_dv_diff = NULL;
    Vector_d* e_pos_dv_prev = NULL;
    Vector_d* e_neg_dv_prev = NULL;
    Vector_d* p           = NULL;
    Vector_d* q           = NULL;
    Vector_d* a           = NULL;
    Vector_d* b           = NULL;
    Error*    err         = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    create_vector_d(v_pos_out, N);
    v_pos = *v_pos_out;

    create_vector_d(v_neg_out, N);
    v_neg = *v_neg_out;

    create_vector_d(&e_pos_dv, N);
    create_vector_d(&e_neg_dv, N);

    lambda = *lambda_out;

    /* May not be the best initialization for these. */
    e_pos = 0;
    e_neg = 0;
    g1p   = 0;

    done = 0;

    while (!done)
    {
        get_pt_from_params(&p, params);

        /* Positive lambda */
        multiply_vector_by_scalar_d(&eta_v, v_pos, eta);

        assert(add_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)) ||
            (err = energy_func(&c, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)) ||
            (err = energy_func(&d, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        for (n = 0; n < N; n++)
        {
            e_pos_dv->elts[n] = (a->elts[n] - b->elts[n]) / eta +
                                (0.5*lambda / eta) *
                                (c - d) * (a->elts[n] + b->elts[n]);
        }
        e_pos_dlambda = (0.5*(c - d) / eta) * (0.5*(c - d) / eta);

        if (!e_pos_dv_prev)
        {
            copy_vector_d(&e_pos_dv_prev, e_pos_dv);
            calc_vector_dot_prod_d(&e_pos_dv_prev_dp, e_pos_dv_prev, 
                    e_pos_dv_prev);
            e_pos_dlambda_prev = e_pos_dlambda;
            e_pos_dlambda_prev_dp = e_pos_dlambda_prev * e_pos_dlambda_prev;
        }

        subtract_vectors_d(&e_pos_dv_diff, e_pos_dv, e_pos_dv_prev);
        calc_vector_dot_prod_d(&e_pos_dv_dp, e_pos_dv, e_pos_dv_diff);

        e_pos_dlambda_diff = e_pos_dlambda - e_pos_dlambda_prev;
        e_pos_dlambda_dp = e_pos_dlambda * e_pos_dlambda_diff;

        e_pos_dv_beta = 
            (e_pos_dv_prev_dp > 0) && (e_pos_dv_dp / e_pos_dv_prev_dp) > 0 
            ? e_pos_dv_dp / e_pos_dv_prev_dp : 0;

        e_pos_dlambda_beta = (e_pos_dlambda_prev_dp > 0) && 
            (e_pos_dlambda_dp / e_pos_dlambda_prev_dp) > 0 
            ? e_pos_dlambda_dp / e_pos_dlambda_prev_dp : 0;

        multiply_vector_by_scalar_d(&e_pos_dv_prev, e_pos_dv_prev, 
                e_pos_dv_beta);
        add_vectors_d(&e_pos_dv, e_pos_dv, e_pos_dv_prev);

        e_pos_dlambda += e_pos_dlambda_beta * e_pos_dlambda_prev;


        /* Negative lambda */
        multiply_vector_by_scalar_d(&eta_v, v_neg, eta);

        assert(add_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&a, params)) ||
            (err = energy_func(&c, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        assert(subtract_vectors_d(&q, p, eta_v) == NULL);

        if ((err = set_params_from_pt(q, params)) ||
            (err = grad_energy_func(&b, params)) ||
            (err = energy_func(&d, params)))
        {
            assert(set_params_from_pt(p, params) == NULL);
            break;
        }

        for (n = 0; n < N; n++)
        {
            e_neg_dv->elts[n] = (a->elts[n] - b->elts[n]) / eta -
                                (0.5*lambda / eta) *
                                (c - d) * (a->elts[n] + b->elts[n]);
        }
        e_neg_dlambda = -1.0 * (0.5*(c - d) / eta) * (0.5*(c - d) / eta);

        if (!e_neg_dv_prev)
        {
            copy_vector_d(&e_neg_dv_prev, e_neg_dv);
            calc_vector_dot_prod_d(&e_neg_dv_prev_dp, e_neg_dv_prev, 
                    e_neg_dv_prev);
            e_neg_dlambda_prev = e_neg_dlambda;
            e_neg_dlambda_prev_dp = e_neg_dlambda_prev * e_neg_dlambda_prev;
        }

        subtract_vectors_d(&e_neg_dv_diff, e_neg_dv, e_neg_dv_prev);
        calc_vector_dot_prod_d(&e_neg_dv_dp, e_neg_dv, e_neg_dv_diff);

        e_neg_dlambda_diff = e_neg_dlambda - e_neg_dlambda_prev;
        e_neg_dlambda_dp = e_neg_dlambda * e_neg_dlambda_diff;

        e_neg_dv_beta = 
            (e_neg_dv_prev_dp > 0) && (e_neg_dv_dp / e_neg_dv_prev_dp) > 0 
            ? e_neg_dv_dp / e_neg_dv_prev_dp : 0;

        e_neg_dlambda_beta = (e_neg_dlambda_prev_dp > 0) && 
            (e_neg_dlambda_dp / e_neg_dlambda_prev_dp) > 0 
            ? e_neg_dlambda_dp / e_neg_dlambda_prev_dp : 0;

        multiply_vector_by_scalar_d(&e_neg_dv_prev, e_neg_dv_prev, 
                e_neg_dv_beta);
        add_vectors_d(&e_neg_dv, e_neg_dv, e_neg_dv_prev);

        e_neg_dlambda += e_neg_dlambda_beta * e_neg_dlambda_prev;


        assert(set_params_from_pt(p, params) == NULL);


        assert(copy_vector_d(&delta_v_pos, v_pos) == NULL);
        assert(copy_vector_d(&delta_v_neg, v_neg) == NULL);
        delta_g1p    = g1p;
        delta_lambda = lambda;
        delta_e      = e_pos + e_neg;
        done         = 0;
        g            = gamma;

        while (!done)
        {
            lambda_cp = lambda;

            lambda_cp -= g * e_pos_dlambda;
            lambda_cp -= g * e_neg_dlambda;

            copy_vector_d(&v_pos_cp, v_pos);
            copy_vector_d(&v_neg_cp, v_neg);

            for (n = 0; n < N; n++)
            {
                v_pos_cp->elts[ n ] -= g * e_pos_dv->elts[ n ];
                v_neg_cp->elts[ n ] -= g * e_neg_dv->elts[ n ];
            }
            normalize_vector_mag_d(&v_pos_cp, v_pos_cp);
            normalize_vector_mag_d(&v_neg_cp, v_neg_cp);

            if ((err = hyperdynamics_hessgrad_eigenvalue_d(&e_pos, lambda_cp, 
                            v_pos_cp, eta, params, get_pt_from_params, 
                            set_params_from_pt, energy_func, 1)) ||
                (err = hyperdynamics_hessgrad_eigenvalue_d(&e_neg, lambda_cp, 
                            v_neg_cp, eta, params, get_pt_from_params, 
                            set_params_from_pt, energy_func, 0)))
            {
                break;
            }

            g *= 0.5;

            if (g < 1.0e-128 || (delta_e - (e_pos+e_neg)) >= 0)
            {
                done = 1;
            }
        }
        if (err) 
        {
            break;
        }
        lambda = lambda_cp;

        copy_vector_d(&v_pos, v_pos_cp);
        copy_vector_d(&v_neg, v_neg_cp);

        delta_lambda = fabs(delta_lambda - lambda);
        delta_e = fabs(delta_e - (e_pos + e_neg));

        hyperdynamics_proj_hessian_eigenvector_d(&g1p, e_pos, e_neg, lambda);
        delta_g1p = fabs(delta_g1p - g1p);

        done = 1;
        for (n = 0; n < N; n++)
        {
            delta_v_pos->elts[ n ] 
                = (fabs(fabs(delta_v_pos->elts[ n ]) - 
                   fabs(v_pos->elts[ n ])) < 1.0e-8)
                ?  fabs(fabs(delta_v_pos->elts[ n ]) - fabs(v_pos->elts[ n ]))
                :  fabs(delta_v_pos->elts[ n ] - v_pos->elts[ n ]);

            delta_v_neg->elts[ n ] 
                = (fabs(fabs(delta_v_neg->elts[ n ]) - 
                   fabs(v_neg->elts[ n ])) < 1.0e-8)
                ?  fabs(fabs(delta_v_neg->elts[ n ]) - fabs(v_neg->elts[ n ]))
                :  fabs(delta_v_neg->elts[ n ] - v_neg->elts[ n ]);

            done = (done && (delta_v_pos->elts[ n ] <= 1.0e-8)
                         && (delta_v_neg->elts[ n ] <= 1.0e-8));
        }
        done = (done && (delta_lambda <= 1.0e-8));
        done = (done || (delta_g1p <= 1.0e-4));

        copy_vector_d(&e_pos_dv_prev, e_pos_dv);
        calc_vector_dot_prod_d(&e_pos_dv_prev_dp, e_pos_dv_prev, e_pos_dv_prev);

        e_pos_dlambda_prev = e_pos_dlambda;
        e_pos_dlambda_prev_dp = e_pos_dlambda_prev * e_pos_dlambda_prev;

        copy_vector_d(&e_neg_dv_prev, e_neg_dv);
        calc_vector_dot_prod_d(&e_neg_dv_prev_dp, e_neg_dv_prev, e_neg_dv_prev);

        e_neg_dlambda_prev = e_neg_dlambda;
        e_neg_dlambda_prev_dp = e_neg_dlambda_prev * e_neg_dlambda_prev;
    }

    *lambda_out = lambda;

    free_vector_d(v_pos_cp);
    free_vector_d(v_neg_cp);
    free_vector_d(delta_v_pos);
    free_vector_d(delta_v_neg);
    free_vector_d(eta_v);
    free_vector_d(e_pos_dv);
    free_vector_d(e_neg_dv);
    free_vector_d(e_pos_dv_diff);
    free_vector_d(e_neg_dv_diff);
    free_vector_d(e_pos_dv_prev);
    free_vector_d(e_neg_dv_prev);
    free_vector_d(p);
    free_vector_d(q);
    free_vector_d(a);
    free_vector_d(b);

    if (err)
    {
        free_vector_d(*v_pos_out); *v_pos_out = NULL;
        free_vector_d(*v_neg_out); *v_neg_out = NULL;
    }

    return err;
}

static Error* hyperdynamics_dx_hessgrad_eigenvalue_f
(
    Vector_f**      e_lambda_dx_out, 
    float           lambda, 
    const Vector_f* v, 
    float           eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_f**, const void*),
    Error*          (*set_params_from_pt)(const Vector_f*, void*),
    Error*          (*energy_func)(float*, void*),
    Error*          (*grad_energy_func)(Vector_f**, void*),
    uint8_t         positive
)
{
    uint32_t n;
    uint32_t N;
    float    a, b;

    Vector_f* e_lambda_dx;
    Vector_f* e_dx  = NULL;
    Vector_f* eta_v = NULL;
    Vector_f* p     = NULL;
    Vector_f* q     = NULL;
    Vector_f* c     = NULL;
    Vector_f* d     = NULL;
    Error*    err;

    if ((err = hyperdynamics_dx_hessian_eigenvalue_f(&e_dx, v, eta, params,
                    get_pt_from_params, set_params_from_pt, grad_energy_func)))
    {
        return err;
    }

    get_pt_from_params(&p, params);
    N = p->num_elts;

    multiply_vector_by_scalar_f(&eta_v, v, eta);

    assert(add_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)) ||
        (err = grad_energy_func(&c, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(e_dx);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        free_vector_f(c); free_vector_f(d);
        return err;
    }

    assert(subtract_vectors_f(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)) ||
        (err = grad_energy_func(&d, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_f(e_dx);
        free_vector_f(eta_v);
        free_vector_f(p); free_vector_f(q);
        free_vector_f(c); free_vector_f(d);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    create_vector_f(e_lambda_dx_out, N);
    e_lambda_dx = *e_lambda_dx_out;
    for (n = 0; n < N; n++)
    {
        e_lambda_dx->elts[ n ] = e_dx->elts[ n ];

        if (positive)
        {
            e_lambda_dx->elts[ n ] += 0.5*lambda * (a - b) * 
                                      (c->elts[n] - d->elts[n]) / (eta*eta);
        }
        else
        {
            e_lambda_dx->elts[ n ] -= 0.5*lambda * (a - b) * 
                                      (c->elts[n] - d->elts[n]) / (eta*eta);
        }
    }

    free_vector_f(e_dx);
    free_vector_f(eta_v);
    free_vector_f(p);
    free_vector_f(q);
    free_vector_f(c);
    free_vector_f(d);

    return NULL;
}

static Error* hyperdynamics_dx_hessgrad_eigenvalue_d
(
    Vector_d**      e_lambda_dx_out, 
    double          lambda, 
    const Vector_d* v, 
    double          eta, 
    void*           params,
    void            (*get_pt_from_params)(Vector_d**, const void*),
    Error*          (*set_params_from_pt)(const Vector_d*, void*),
    Error*          (*energy_func)(double*, void*),
    Error*          (*grad_energy_func)(Vector_d**, void*),
    uint8_t         positive
)
{
    uint32_t n;
    uint32_t N;
    double   a, b;

    Vector_d* e_lambda_dx;
    Vector_d* e_dx  = NULL;
    Vector_d* eta_v = NULL;
    Vector_d* p     = NULL;
    Vector_d* q     = NULL;
    Vector_d* c     = NULL;
    Vector_d* d     = NULL;
    Error*    err;

    if ((err = hyperdynamics_dx_hessian_eigenvalue_d(&e_dx, v, eta, params,
                    get_pt_from_params, set_params_from_pt, grad_energy_func)))
    {
        return err;
    }

    get_pt_from_params(&p, params);
    N = p->num_elts;

    multiply_vector_by_scalar_d(&eta_v, v, eta);

    assert(add_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&a, params)) ||
        (err = grad_energy_func(&c, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(e_dx);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        free_vector_d(c); free_vector_d(d);
        return err;
    }

    assert(subtract_vectors_d(&q, p, eta_v) == NULL);

    if ((err = set_params_from_pt(q, params)) ||
        (err = energy_func(&b, params)) ||
        (err = grad_energy_func(&d, params)))
    {
        assert(set_params_from_pt(p, params) == NULL);
        free_vector_d(e_dx);
        free_vector_d(eta_v);
        free_vector_d(p); free_vector_d(q);
        free_vector_d(c); free_vector_d(d);
        return err;
    }

    assert(set_params_from_pt(p, params) == NULL);

    create_vector_d(e_lambda_dx_out, N);
    e_lambda_dx = *e_lambda_dx_out;
    for (n = 0; n < N; n++)
    {
        e_lambda_dx->elts[ n ] = e_dx->elts[ n ];

        if (positive)
        {
            e_lambda_dx->elts[ n ] += 0.5*lambda * (a - b) * 
                                      (c->elts[n] - d->elts[n]) / (eta*eta);
        }
        else
        {
            e_lambda_dx->elts[ n ] -= 0.5*lambda * (a - b) * 
                                      (c->elts[n] - d->elts[n]) / (eta*eta);
        }
    }

    free_vector_d(e_dx);
    free_vector_d(eta_v);
    free_vector_d(p);
    free_vector_d(q);
    free_vector_d(c);
    free_vector_d(d);

    return NULL;
}

static Error* hyperdynamics_dx_proj_hessian_eigenvector_f
(
    Vector_f**      g1p_dx_out,
    const Vector_f* e_pos_dx,
    const Vector_f* e_neg_dx,
    float           g1p,
    float           lambda
)
{
    uint32_t n;
    uint32_t N;

    N = e_pos_dx->num_elts;
    create_vector_f(g1p_dx_out, N);

    for (n = 0; n < N; n++)
    {
        (*g1p_dx_out)->elts[ n ] = (e_pos_dx->elts[n] - e_neg_dx->elts[n]) /
                                   (g1p*4.0*lambda);
    }

    return NULL;
}

static Error* hyperdynamics_dx_proj_hessian_eigenvector_d
(
    Vector_d**      g1p_dx_out,
    const Vector_d* e_pos_dx,
    const Vector_d* e_neg_dx,
    double          g1p,
    double          lambda
)
{
    uint32_t n;
    uint32_t N;

    N = e_pos_dx->num_elts;
    create_vector_d(g1p_dx_out, N);

    for (n = 0; n < N; n++)
    {
        (*g1p_dx_out)->elts[ n ] = (e_pos_dx->elts[n] - e_neg_dx->elts[n]) /
                                   (g1p*4.0*lambda);
    }

    return NULL;
}

Error* langevin_hyperdynamics_1_f
(
    uint32_t iterations,
    float    delta_t,
    float    gamma,
    float    eta,
    float    h,
    float    d,
    void*    params,
    void     (*get_pt_from_params)(Vector_f** p_out, const void* params),
    Error*   (*set_params_from_pt)(const Vector_f* p, void* params),
    Error*   (*energy_func)(float* energy_out, void* params),
    Error*   (*grad_energy_func)(Vector_f** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_f* sample, void* params, float energy_bias)
)
{
    uint32_t D;

    Vector_f* p = NULL;
    Vector_f* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_f(&delta_t_v, D, delta_t);

    err = langevin_hyperdynamics_2_f(iterations, delta_t_v, gamma, eta, h, d,
            params, get_pt_from_params, set_params_from_pt, energy_func,
            grad_energy_func, accept_sample);

    free_vector_f(p);
    free_vector_f(delta_t_v);

    return err;
}

Error* langevin_hyperdynamics_2_f
(
    uint32_t        iterations,
    const Vector_f* delta_t,
    float           gamma,
    float           eta,
    float           h,
    float           d,
    void*           params,
    void            (*get_pt_from_params)(Vector_f** p_out, const void* params),
    Error*          (*set_params_from_pt)(const Vector_f* p, void* params),
    Error*          (*energy_func)(float* energy_out, void* params),
    Error*          (*grad_energy_func)(Vector_f** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_f* sample, void* params, float energy_bias)
)
{
    uint32_t i;
    uint32_t n;
    uint32_t N;
    float    gauss;
    float    lambda;
    float    e;
    float    e_pos;
    float    e_neg;
    float    g1p;
    float    Vb;
    float    Vb_dx;
    float    dt;

    Vector_f* p        = NULL;
    Vector_f* v        = NULL;
    Vector_f* v_pos    = NULL;
    Vector_f* v_neg    = NULL;
    Vector_f* e_dx     = NULL;
    Vector_f* e_pos_dx = NULL;
    Vector_f* e_neg_dx = NULL;
    Vector_f* g1p_dx   = NULL;
    Vector_f* V_dx     = NULL;
    Error*    err      = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    /* Randomly initialize lambda and the eigenvectors. */
    create_random_vector_f(&v, N, -1, 1);
    normalize_vector_mag_f(&v, v);

    create_random_vector_f(&v_pos, N, -1, 1);
    normalize_vector_mag_f(&v_pos, v_pos);

    create_random_vector_f(&v_neg, N, -1, 1);
    normalize_vector_mag_f(&v_neg, v_neg);

    lambda = sample_uniform_pdf_f(-1.0e3, 1.0e3);

    for (i = 0; i < iterations; i++)
    {
        if ((err = hyperdynamics_hessian_eigenvector_f(&v, eta, gamma, 
                        params, get_pt_from_params, set_params_from_pt, 
                        energy_func, grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessian_eigenvalue_f(&e, v, eta, params,
                        get_pt_from_params, set_params_from_pt, energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessian_eigenvalue_f(&e_dx, v, eta, params,
                        get_pt_from_params, set_params_from_pt, 
                        grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvector_f(&lambda, &v_pos, &v_neg,
                        eta, gamma, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvalue_f(&e_pos, lambda, v_pos,
                        eta, params, get_pt_from_params, set_params_from_pt, 
                        energy_func, 1)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvalue_f(&e_neg, lambda, v_neg,
                        eta, params, get_pt_from_params, set_params_from_pt, 
                        energy_func, 0)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessgrad_eigenvalue_f(&e_pos_dx, lambda, 
                        v_pos, eta, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func, 1)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessgrad_eigenvalue_f(&e_neg_dx, lambda, 
                        v_neg, eta, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func, 0)))
        {
            break;
        }

        if ((err = hyperdynamics_proj_hessian_eigenvector_f(&g1p, e_pos, e_neg, 
                        lambda)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_proj_hessian_eigenvector_f(&g1p_dx, 
                        e_pos_dx, e_neg_dx, g1p, lambda)))
        {
            break;
        }

        if ((err = grad_energy_func(&V_dx, params)))
        {
            break;
        }

        get_pt_from_params(&p, params);

        Vb = 0.5*h * (1 + e / sqrt(e*e + g1p*g1p/(d*d)));

        for (n = 0; n < N; n++)
        {
            Vb_dx = (h / (2*sqrt((e*e) + ((g1p*g1p)/(d*d))))) * 
                    (e_dx->elts[n] - e*(e*e_dx->elts[n] + 
                                        (g1p/(d*d))*g1p_dx->elts[n]) / 
                     (e*e + (g1p*g1p)/(d*d)));

            dt    = delta_t->elts[ n ];
            gauss = sample_standard_gaussian_pdf_f(-6.0, 6.0);

            p->elts[ n ] -= dt*(0.5*dt*(V_dx->elts[n]+Vb_dx) + gauss);
        }

        if ((err = accept_sample(p, params, Vb)))
        {
            break;
        }
    }

    free_vector_f(p);
    free_vector_f(v);
    free_vector_f(v_pos);
    free_vector_f(v_neg);
    free_vector_f(e_dx);
    free_vector_f(e_pos_dx);
    free_vector_f(e_neg_dx);
    free_vector_f(g1p_dx);
    free_vector_f(V_dx);

    return err;
}

Error* langevin_hyperdynamics_1_d
(
    uint32_t iterations,
    double   delta_t,
    double   gamma,
    double   eta,
    double   h,
    double   d,
    void*    params,
    void     (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*   (*set_params_from_pt)(const Vector_d* p, void* params),
    Error*   (*energy_func)(double* energy_out, void* params),
    Error*   (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*   (*accept_sample)(const Vector_d* sample, void* params, double energy_bias)
)
{
    uint32_t D;

    Vector_d* p = NULL;
    Vector_d* delta_t_v = NULL;
    Error*    err;

    get_pt_from_params(&p, params);
    D = p->num_elts;

    create_init_vector_d(&delta_t_v, D, delta_t);

    err = langevin_hyperdynamics_2_d(iterations, delta_t_v, gamma, eta, h, d,
            params, get_pt_from_params, set_params_from_pt, energy_func,
            grad_energy_func, accept_sample);

    free_vector_d(p);
    free_vector_d(delta_t_v);

    return err;
}

Error* langevin_hyperdynamics_2_d
(
    uint32_t        iterations,
    const Vector_d* delta_t,
    double          gamma,
    double          eta,
    double          h,
    double          d,
    void*           params,
    void            (*get_pt_from_params)(Vector_d** p_out, const void* params),
    Error*          (*set_params_from_pt)(const Vector_d* p, void* params),
    Error*          (*energy_func)(double* energy_out, void* params),
    Error*          (*grad_energy_func)(Vector_d** grad_out, void* params),
    Error*          (*accept_sample)(const Vector_d* sample, void* params, double energy_bias)
)
{
    uint32_t i;
    uint32_t n;
    uint32_t N;
    double   gauss;
    double   lambda;
    double   e;
    double   e_pos;
    double   e_neg;
    double   g1p;
    double   Vb;
    double   Vb_dx;
    double   dt;

    Vector_d* p        = NULL;
    Vector_d* v        = NULL;
    Vector_d* v_pos    = NULL;
    Vector_d* v_neg    = NULL;
    Vector_d* e_dx     = NULL;
    Vector_d* e_pos_dx = NULL;
    Vector_d* e_neg_dx = NULL;
    Vector_d* g1p_dx   = NULL;
    Vector_d* V_dx     = NULL;
    Error*    err      = NULL;

    get_pt_from_params(&p, params);
    N = p->num_elts;

    /* Randomly initialize lambda and the eigenvectors. */
    create_random_vector_d(&v, N, -1, 1);
    normalize_vector_mag_d(&v, v);

    create_random_vector_d(&v_pos, N, -1, 1);
    normalize_vector_mag_d(&v_pos, v_pos);

    create_random_vector_d(&v_neg, N, -1, 1);
    normalize_vector_mag_d(&v_neg, v_neg);

    lambda = sample_uniform_pdf_d(-1.0e3, 1.0e3);

    for (i = 0; i < iterations; i++)
    {
        if ((err = hyperdynamics_hessian_eigenvector_d(&v, eta, gamma, 
                        params, get_pt_from_params, set_params_from_pt, 
                        energy_func, grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessian_eigenvalue_d(&e, v, eta, params,
                        get_pt_from_params, set_params_from_pt, energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessian_eigenvalue_d(&e_dx, v, eta, params,
                        get_pt_from_params, set_params_from_pt, 
                        grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvector_d(&lambda, &v_pos, &v_neg,
                        eta, gamma, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvalue_d(&e_pos, lambda, v_pos,
                        eta, params, get_pt_from_params, set_params_from_pt, 
                        energy_func, 1)))
        {
            break;
        }

        if ((err = hyperdynamics_hessgrad_eigenvalue_d(&e_neg, lambda, v_neg,
                        eta, params, get_pt_from_params, set_params_from_pt, 
                        energy_func, 0)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessgrad_eigenvalue_d(&e_pos_dx, lambda, 
                        v_pos, eta, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func, 1)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_hessgrad_eigenvalue_d(&e_neg_dx, lambda, 
                        v_neg, eta, params, get_pt_from_params, 
                        set_params_from_pt, energy_func, grad_energy_func, 0)))
        {
            break;
        }

        if ((err = hyperdynamics_proj_hessian_eigenvector_d(&g1p, e_pos, e_neg, 
                        lambda)))
        {
            break;
        }

        if ((err = hyperdynamics_dx_proj_hessian_eigenvector_d(&g1p_dx, 
                        e_pos_dx, e_neg_dx, g1p, lambda)))
        {
            break;
        }

        if ((err = grad_energy_func(&V_dx, params)))
        {
            break;
        }

        get_pt_from_params(&p, params);

        Vb = 0.5*h * (1 + e / sqrt(e*e + g1p*g1p/(d*d)));

        for (n = 0; n < N; n++)
        {
            Vb_dx = (h / (2*sqrt((e*e) + ((g1p*g1p)/(d*d))))) * 
                    (e_dx->elts[n] - e*(e*e_dx->elts[n] + 
                                        (g1p/(d*d))*g1p_dx->elts[n]) / 
                     (e*e + (g1p*g1p)/(d*d)));

            dt    = delta_t->elts[ n ];
            gauss = sample_standard_gaussian_pdf_d(-6.0, 6.0);

            p->elts[ n ] -= dt*(0.5*dt*(V_dx->elts[n]+Vb_dx) + gauss);
        }

        if ((err = accept_sample(p, params, Vb)))
        {
            break;
        }
    }

    free_vector_d(p);
    free_vector_d(v);
    free_vector_d(v_pos);
    free_vector_d(v_neg);
    free_vector_d(e_dx);
    free_vector_d(e_pos_dx);
    free_vector_d(e_neg_dx);
    free_vector_d(g1p_dx);
    free_vector_d(V_dx);

    return err;
}