#include "convergence.h"
#define JMAX 80

ConvergenceData rtbis_with_conv(float (*func)(float), float x1, float x2, float xacc) {
    void nrerror(char error_text[]);

    ConvergenceData data;
    data.size = 0;

    int j;
    float dx, f, fmid, xmid, rtb;

    f = (*func)(x1);
    fmid = (*func)(x2);
    if (f * fmid >= 0.0) nrerror("Root must be bracketed for bisection in rtbis");
    rtb = f < 0.0 ? (dx = x2 - x1, x1) : (dx = x1 - x2, x2);
    for (j = 1; j <= JMAX; j++) {
        fmid = (*func)(xmid = rtb + (dx *= 0.5));
        if (fmid <= 0.0) rtb = xmid;
        data.data[data.size++] = rtb;
        if (fabs(dx) < xacc || fmid == 0.0) {
            return data;
        };
    }
    nrerror("Too many bisections in rtbis");
    return data;
}

ConvergenceData rtflsp_with_conv(float (*func)(float), float x1, float x2, float xacc) {
    void nrerror(char error_text[]);

    ConvergenceData data;
    data.size = 0;

    int j;
    float fl, fh, xl, xh, swap, dx, del, f, rtf;

    fl = (*func)(x1);
    fh = (*func)(x2);
    if (fl * fh > 0.0) nrerror("Root must be bracketed in rtflsp");
    if (fl < 0.0) {
        xl = x1;
        xh = x2;
    } else {
        xl = x2;
        xh = x1;
        swap = fl;
        fl = fh;
        fh = swap;
    }
    dx = xh - xl;
    for (j = 1; j <= JMAX; j++) {
        rtf = xl + dx * fl / (fl - fh);
        f = (*func)(rtf);
        if (f < 0.0) {
            del = xl - rtf;
            xl = rtf;
            fl = f;
        } else {
            del = xh - rtf;
            xh = rtf;
            fh = f;
        }
        dx = xh - xl;
        data.data[data.size++] = rtf;
        if (fabs(del) < xacc || f == 0.0) return data;
    }
    nrerror("Maximum number of iterations exceeded in rtflsp");
    return data;
}

ConvergenceData rtsec_with_conv(float (*func)(float), float x1, float x2, float xacc) {
    void nrerror(char error_text[]);

    ConvergenceData data;
    data.size = 0;

    int j;
    float fl, f, dx, swap, xl, rts;

    fl = (*func)(x1);
    f = (*func)(x2);
    if (fabs(fl) < fabs(f)) {
        rts = x1;
        xl = x2;
        swap = fl;
        fl = f;
        f = swap;
    } else {
        xl = x1;
        rts = x2;
    }
    for (j = 1; j <= JMAX; j++) {
        dx = (xl - rts) * f / (f - fl);
        xl = rts;
        fl = f;
        rts += dx;
        f = (*func)(rts);
        data.data[data.size++] = rts;
        if (fabs(dx) < xacc || f == 0.0) return data;
    }
    nrerror("Maximum number of iterations exceeded in rtsec");
    return data;
}

ConvergenceData rtnewt_with_conv(void (*funcd)(float, float *, float *), float x1, float x2,
                                 float xacc) {
    void nrerror(char error_text[]);

    ConvergenceData data;
    data.size = 0;

    int j;
    float df, dx, f, rtn;

    rtn = 0.5 * (x1 + x2);
    for (j = 1; j <= JMAX; j++) {
        (*funcd)(rtn, &f, &df);
        dx = f / df;
        rtn -= dx;
        if ((x1 - rtn) * (rtn - x2) < 0.0)
            nrerror("Jumped out of brackets in rtnewt");

        data.data[data.size++] = rtn;
        if (fabs(dx) < xacc) return data;
    }
    nrerror("Maximum number of iterations exceeded in rtnewt");
    return data;
}

ConvergenceData rtsafe_with_conv(void (*funcd)(float, float *, float *), float x1, float x2,
                                 float xacc) {
    void nrerror(char error_text[]);

    ConvergenceData data;
    data.size = 0;

    int j;
    float df, dx, dxold, f, fh, fl;
    float temp, xh, xl, rts;

    (*funcd)(x1, &fl, &df);
    (*funcd)(x2, &fh, &df);
    if ((fl > 0.0 && fh > 0.0) || (fl < 0.0 && fh < 0.0))
        nrerror("Root must be bracketed in rtsafe");
    if (fl == 0.0) {
        data.data[data.size++] = x1;
        return data;
    }
    if (fh == 0.0) {
        data.data[data.size++] = x2;
        return data;
    }
    if (fl < 0.0) {
        xl = x1;
        xh = x2;
    } else {
        xh = x1;
        xl = x2;
    }
    rts = 0.5 * (x1 + x2);
    dxold = fabs(x2 - x1);
    dx = dxold;
    (*funcd)(rts, &f, &df);
    for (j = 1; j <= JMAX; j++) {
        if ((((rts - xh) * df - f) * ((rts - xl) * df - f) > 0.0) || (fabs(2.0 * f) > fabs(dxold * df))) {
            dxold = dx;
            dx = 0.5 * (xh - xl);
            rts = xl + dx;
            data.data[data.size++] = rts;

            if (xl == rts) return data;
        } else {
            dxold = dx;
            dx = f / df;
            temp = rts;
            rts -= dx;
            data.data[data.size++] = rts;

            if (temp == rts) return data;
        }
        if (fabs(dx) < xacc) return data;
        (*funcd)(rts, &f, &df);
        if (f < 0.0)
            xl = rts;
        else
            xh = rts;
    }
    nrerror("Maximum number of iterations exceeded in rtsafe");
    return data;
}

ConvergenceData rtmuller_with_conv(float (*func)(float), float x1, float x2, float xacc) {
    void nrerror(char error_text[]);
    int j;
    float fh, fl, fm, fnew, s, xl, r, x3, x0, x_prev, x_curr, x_next;
    float d0, d1, d2, a, b, c;

    ConvergenceData data;
    data.size = 0;

    x0 = x1;
    x_prev = x2;
    x_curr = (x0 + x_prev) / 2.0;

    if (x_prev == x_curr) x_curr += xacc;
    if (x0 == x_curr) x0 -= xacc;

    for (j = 1; j <= 200; j++) {
        d0 = func(x0);
        d1 = func(x_prev);
        d2 = func(x_curr);

        float h1 = x_prev - x0;
        float h2 = x_curr - x_prev;
        float delta1 = (d1 - d0) / h1;
        float delta2 = (d2 - d1) / h2;
        a = (delta2 - delta1) / (h2 + h1);
        b = a * h2 + delta2;
        c = d2;

        float disc = b * b - 4 * a * c;
        if (disc < 0) nrerror("MullerMethodError: Complex roots encountered");

        float denom1 = b + sqrt(disc);
        float denom2 = b - sqrt(disc);

        if (fabs(denom1) < fabs(denom2)) {
            x_next = x_curr - (2 * c) / denom2;
        } else {
            x_next = x_curr - (2 * c) / denom1;
        }
        data.data[data.size++] = x_next;
        if (fabs(x_next - x_curr) < xacc) return data;

        x0 = x_prev;
        x_prev = x_curr;
        x_curr = x_next;
    }
    nrerror("MullerMethodError: Too many iterations");
    return data;
}

#undef JMAX