#include <cmath>

#include "../gmx_lapack.h"

#include "gromacs/utility/real.h"

#include "lapack_limits.h"

void F77_FUNC(slasd4, SLASD4)(int*   n,
                              int*   i__,
                              float* d__,
                              float* z__,
                              float* delta,
                              float* rho,
                              float* sigma,
                              float* work,
                              int*   info)
{
    int   i__1;
    float d__1;

    float a, b, c__;
    int   j;
    float w, dd[3];
    int   ii;
    float dw, zz[3];
    int   ip1;
    float eta, phi, eps, tau, psi;
    int   iim1, iip1;
    float dphi, dpsi;
    int   iter;
    float temp, prew, sg2lb, sg2ub, temp1, temp2, dtiim, delsq, dtiip;
    int   niter;
    float dtisq;
    int   swtch;
    float dtnsq;
    float delsq2, dtnsq1;
    int   swtch3;
    int   orgati;
    float erretm, dtipsq, rhoinv;

    --work;
    --delta;
    --z__;
    --d__;

    *info = 0;
    if (*n == 1)
    {

        *sigma   = std::sqrt(d__[1] * d__[1] + *rho * z__[1] * z__[1]);
        delta[1] = 1.;
        work[1]  = 1.;
        return;
    }
    if (*n == 2)
    {
        F77_FUNC(slasd5, SLASD5)(i__, &d__[1], &z__[1], &delta[1], rho, sigma, &work[1]);
        return;
    }

    eps    = GMX_FLOAT_EPS;
    rhoinv = 1. / *rho;

    if (*i__ == *n)
    {

        ii    = *n - 1;
        niter = 1;

        temp = *rho / 2.;

        temp1 = temp / (d__[*n] + std::sqrt(d__[*n] * d__[*n] + temp));
        i__1  = *n;
        for (j = 1; j <= i__1; ++j)
        {
            work[j]  = d__[j] + d__[*n] + temp1;
            delta[j] = d__[j] - d__[*n] - temp1;
        }

        psi  = 0.;
        i__1 = *n - 2;
        for (j = 1; j <= i__1; ++j)
        {
            psi += z__[j] * z__[j] / (delta[j] * work[j]);
        }

        c__ = rhoinv + psi;
        w = c__ + z__[ii] * z__[ii] / (delta[ii] * work[ii]) + z__[*n] * z__[*n] / (delta[*n] * work[*n]);

        if (w <= 0.)
        {
            temp1 = std::sqrt(d__[*n] * d__[*n] + *rho);
            temp  = z__[*n - 1] * z__[*n - 1]
                           / ((d__[*n - 1] + temp1) * (d__[*n] - d__[*n - 1] + *rho / (d__[*n] + temp1)))
                   + z__[*n] * z__[*n] / *rho;

            if (c__ <= temp)
            {
                tau = *rho;
            }
            else
            {
                delsq = (d__[*n] - d__[*n - 1]) * (d__[*n] + d__[*n - 1]);
                a     = -c__ * delsq + z__[*n - 1] * z__[*n - 1] + z__[*n] * z__[*n];
                b     = z__[*n] * z__[*n] * delsq;
                if (a < 0.)
                {
                    tau = b * 2. / (std::sqrt(a * a + b * 4. * c__) - a);
                }
                else
                {
                    tau = (a + std::sqrt(a * a + b * 4. * c__)) / (c__ * 2.);
                }
            }
        }
        else
        {
            delsq = (d__[*n] - d__[*n - 1]) * (d__[*n] + d__[*n - 1]);
            a     = -c__ * delsq + z__[*n - 1] * z__[*n - 1] + z__[*n] * z__[*n];
            b     = z__[*n] * z__[*n] * delsq;

            if (a < 0.)
            {
                tau = b * 2. / (std::sqrt(a * a + b * 4. * c__) - a);
            }
            else
            {
                tau = (a + std::sqrt(a * a + b * 4. * c__)) / (c__ * 2.);
            }
        }

        eta = tau / (d__[*n] + std::sqrt(d__[*n] * d__[*n] + tau));

        *sigma = d__[*n] + eta;
        i__1   = *n;
        for (j = 1; j <= i__1; ++j)
        {
            delta[j] = d__[j] - d__[*i__] - eta;
            work[j]  = d__[j] + d__[*i__] + eta;
        }

        dpsi   = 0.;
        psi    = 0.;
        erretm = 0.;
        i__1   = ii;
        for (j = 1; j <= i__1; ++j)
        {
            temp = z__[j] / (delta[j] * work[j]);
            psi += z__[j] * temp;
            dpsi += temp * temp;
            erretm += psi;
        }
        erretm = std::abs(erretm);

        temp   = z__[*n] / (delta[*n] * work[*n]);
        phi    = z__[*n] * temp;
        dphi   = temp * temp;
        erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + std::abs(tau) * (dpsi + dphi);

        w = rhoinv + phi + psi;

        if (std::abs(w) <= eps * erretm)
        {
            goto L240;
        }

        ++niter;
        dtnsq1 = work[*n - 1] * delta[*n - 1];
        dtnsq  = work[*n] * delta[*n];
        c__    = w - dtnsq1 * dpsi - dtnsq * dphi;
        a      = (dtnsq + dtnsq1) * w - dtnsq * dtnsq1 * (dpsi + dphi);
        b      = dtnsq * dtnsq1 * w;
        if (c__ < 0.)
        {
            c__ = std::abs(c__);
        }
        if (std::abs(c__) < GMX_FLOAT_MIN)
        {
            eta = *rho - *sigma * *sigma;
        }
        else if (a >= 0.)
        {
            eta = (a + std::sqrt(std::abs(a * a - b * 4. * c__))) / (c__ * 2.);
        }
        else
        {
            eta = b * 2. / (a - std::sqrt(std::abs(a * a - b * 4. * c__)));
        }

        if (w * eta > 0.)
        {
            eta = -w / (dpsi + dphi);
        }
        temp = eta - dtnsq;
        if (temp > *rho)
        {
            eta = *rho + dtnsq;
        }

        tau += eta;
        eta /= *sigma + std::sqrt(eta + *sigma * *sigma);
        i__1 = *n;
        for (j = 1; j <= i__1; ++j)
        {
            delta[j] -= eta;
            work[j] += eta;
        }

        *sigma += eta;

        dpsi   = 0.;
        psi    = 0.;
        erretm = 0.;
        i__1   = ii;
        for (j = 1; j <= i__1; ++j)
        {
            temp = z__[j] / (work[j] * delta[j]);
            psi += z__[j] * temp;
            dpsi += temp * temp;
            erretm += psi;
        }
        erretm = std::abs(erretm);

        temp   = z__[*n] / (work[*n] * delta[*n]);
        phi    = z__[*n] * temp;
        dphi   = temp * temp;
        erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + std::abs(tau) * (dpsi + dphi);

        w = rhoinv + phi + psi;

        iter = niter + 1;

        for (niter = iter; niter <= 20; ++niter)
        {

            if (std::abs(w) <= eps * erretm)
            {
                goto L240;
            }
            dtnsq1 = work[*n - 1] * delta[*n - 1];
            dtnsq  = work[*n] * delta[*n];
            c__    = w - dtnsq1 * dpsi - dtnsq * dphi;
            a      = (dtnsq + dtnsq1) * w - dtnsq1 * dtnsq * (dpsi + dphi);
            b      = dtnsq1 * dtnsq * w;
            if (a >= 0.)
            {
                eta = (a + std::sqrt(std::abs(a * a - b * 4. * c__))) / (c__ * 2.);
            }
            else
            {
                eta = b * 2. / (a - std::sqrt(std::abs(a * a - b * 4. * c__)));
            }

            if (w * eta > 0.)
            {
                eta = -w / (dpsi + dphi);
            }
            temp = eta - dtnsq;
            if (temp <= 0.)
            {
                eta /= 2.;
            }

            tau += eta;
            eta /= *sigma + std::sqrt(eta + *sigma * *sigma);
            i__1 = *n;
            for (j = 1; j <= i__1; ++j)
            {
                delta[j] -= eta;
                work[j] += eta;
            }

            *sigma += eta;

            dpsi   = 0.;
            psi    = 0.;
            erretm = 0.;
            i__1   = ii;
            for (j = 1; j <= i__1; ++j)
            {
                temp = z__[j] / (work[j] * delta[j]);
                psi += z__[j] * temp;
                dpsi += temp * temp;
                erretm += psi;
            }
            erretm = std::abs(erretm);

            temp   = z__[*n] / (work[*n] * delta[*n]);
            phi    = z__[*n] * temp;
            dphi   = temp * temp;
            erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + std::abs(tau) * (dpsi + dphi);

            w = rhoinv + phi + psi;
        }

        *info = 1;
        goto L240;
    }
    else
    {

        niter = 1;
        ip1   = *i__ + 1;

        delsq  = (d__[ip1] - d__[*i__]) * (d__[ip1] + d__[*i__]);
        delsq2 = delsq / 2.;
        temp   = delsq2 / (d__[*i__] + std::sqrt(d__[*i__] * d__[*i__] + delsq2));
        i__1   = *n;
        for (j = 1; j <= i__1; ++j)
        {
            work[j]  = d__[j] + d__[*i__] + temp;
            delta[j] = d__[j] - d__[*i__] - temp;
        }

        psi  = 0.;
        i__1 = *i__ - 1;
        for (j = 1; j <= i__1; ++j)
        {
            psi += z__[j] * z__[j] / (work[j] * delta[j]);
        }

        phi  = 0.;
        i__1 = *i__ + 2;
        for (j = *n; j >= i__1; --j)
        {
            phi += z__[j] * z__[j] / (work[j] * delta[j]);
        }
        c__ = rhoinv + psi + phi;
        w   = c__ + z__[*i__] * z__[*i__] / (work[*i__] * delta[*i__])
            + z__[ip1] * z__[ip1] / (work[ip1] * delta[ip1]);

        if (w > 0.)
        {

            orgati = 1;
            sg2lb  = 0.;
            sg2ub  = delsq2;
            a      = c__ * delsq + z__[*i__] * z__[*i__] + z__[ip1] * z__[ip1];
            b      = z__[*i__] * z__[*i__] * delsq;
            if (a > 0.)
            {
                tau = b * 2. / (a + std::sqrt(std::abs(a * a - b * 4. * c__)));
            }
            else
            {
                tau = (a - std::sqrt(std::abs(a * a - b * 4. * c__))) / (c__ * 2.);
            }
            eta = tau / (d__[*i__] + std::sqrt(d__[*i__] * d__[*i__] + tau));
        }
        else
        {

            orgati = 0;
            sg2lb  = -delsq2;
            sg2ub  = 0.;
            a      = c__ * delsq - z__[*i__] * z__[*i__] - z__[ip1] * z__[ip1];
            b      = z__[ip1] * z__[ip1] * delsq;
            if (a < 0.)
            {
                tau = b * 2. / (a - std::sqrt(std::abs(a * a + b * 4. * c__)));
            }
            else
            {
                tau = -(a + std::sqrt(std::abs(a * a + b * 4. * c__))) / (c__ * 2.);
            }
            eta = tau / (d__[ip1] + std::sqrt(std::abs(d__[ip1] * d__[ip1] + tau)));
        }

        if (orgati)
        {
            ii     = *i__;
            *sigma = d__[*i__] + eta;
            i__1   = *n;
            for (j = 1; j <= i__1; ++j)
            {
                work[j]  = d__[j] + d__[*i__] + eta;
                delta[j] = d__[j] - d__[*i__] - eta;
            }
        }
        else
        {
            ii     = *i__ + 1;
            *sigma = d__[ip1] + eta;
            i__1   = *n;
            for (j = 1; j <= i__1; ++j)
            {
                work[j]  = d__[j] + d__[ip1] + eta;
                delta[j] = d__[j] - d__[ip1] - eta;
            }
        }
        iim1 = ii - 1;
        iip1 = ii + 1;

        dpsi   = 0.;
        psi    = 0.;
        erretm = 0.;
        i__1   = iim1;
        for (j = 1; j <= i__1; ++j)
        {
            temp = z__[j] / (work[j] * delta[j]);
            psi += z__[j] * temp;
            dpsi += temp * temp;
            erretm += psi;
        }
        erretm = std::abs(erretm);

        dphi = 0.;
        phi  = 0.;
        i__1 = iip1;
        for (j = *n; j >= i__1; --j)
        {
            temp = z__[j] / (work[j] * delta[j]);
            phi += z__[j] * temp;
            dphi += temp * temp;
            erretm += phi;
        }

        w = rhoinv + phi + psi;

        swtch3 = 0;
        if (orgati)
        {
            if (w < 0.)
            {
                swtch3 = 1;
            }
        }
        else
        {
            if (w > 0.)
            {
                swtch3 = 1;
            }
        }
        if (ii == 1 || ii == *n)
        {
            swtch3 = 0;
        }

        temp = z__[ii] / (work[ii] * delta[ii]);
        dw   = dpsi + dphi + temp * temp;
        temp = z__[ii] * temp;
        w += temp;
        erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + std::abs(temp) * 3. + std::abs(tau) * dw;

        if (std::abs(w) <= eps * erretm)
        {
            goto L240;
        }

        if (w <= 0.)
        {
            sg2lb = (sg2lb > tau) ? sg2lb : tau;
        }
        else
        {
            sg2ub = (sg2ub < tau) ? sg2ub : tau;
        }

        ++niter;
        if (!swtch3)
        {
            dtipsq = work[ip1] * delta[ip1];
            dtisq  = work[*i__] * delta[*i__];
            if (orgati)
            {
                d__1 = z__[*i__] / dtisq;
                c__  = w - dtipsq * dw + delsq * (d__1 * d__1);
            }
            else
            {
                d__1 = z__[ip1] / dtipsq;
                c__  = w - dtisq * dw - delsq * (d__1 * d__1);
            }
            a = (dtipsq + dtisq) * w - dtipsq * dtisq * dw;
            b = dtipsq * dtisq * w;
            if (std::abs(c__) < GMX_FLOAT_MIN)
            {
                if (std::abs(a) < GMX_FLOAT_MIN)
                {
                    if (orgati)
                    {
                        a = z__[*i__] * z__[*i__] + dtipsq * dtipsq * (dpsi + dphi);
                    }
                    else
                    {
                        a = z__[ip1] * z__[ip1] + dtisq * dtisq * (dpsi + dphi);
                    }
                }
                eta = b / a;
            }
            else if (a <= 0.)
            {
                eta = (a - std::sqrt(std::abs(a * a - b * 4. * c__))) / (c__ * 2.);
            }
            else
            {
                eta = b * 2. / (a + std::sqrt(std::abs(a * a - b * 4. * c__)));
            }
        }
        else
        {

            dtiim = work[iim1] * delta[iim1];
            dtiip = work[iip1] * delta[iip1];
            temp  = rhoinv + psi + phi;
            if (orgati)
            {
                temp1 = z__[iim1] / dtiim;
                temp1 *= temp1;
                c__ = temp - dtiip * (dpsi + dphi)
                      - (d__[iim1] - d__[iip1]) * (d__[iim1] + d__[iip1]) * temp1;
                zz[0] = z__[iim1] * z__[iim1];
                if (dpsi < temp1)
                {
                    zz[2] = dtiip * dtiip * dphi;
                }
                else
                {
                    zz[2] = dtiip * dtiip * (dpsi - temp1 + dphi);
                }
            }
            else
            {
                temp1 = z__[iip1] / dtiip;
                temp1 *= temp1;
                c__ = temp - dtiim * (dpsi + dphi)
                      - (d__[iip1] - d__[iim1]) * (d__[iim1] + d__[iip1]) * temp1;
                if (dphi < temp1)
                {
                    zz[0] = dtiim * dtiim * dpsi;
                }
                else
                {
                    zz[0] = dtiim * dtiim * (dpsi + (dphi - temp1));
                }
                zz[2] = z__[iip1] * z__[iip1];
            }
            zz[1] = z__[ii] * z__[ii];
            dd[0] = dtiim;
            dd[1] = delta[ii] * work[ii];
            dd[2] = dtiip;
            F77_FUNC(slaed6, SLAED6)(&niter, &orgati, &c__, dd, zz, &w, &eta, info);
            if (*info != 0)
            {
                goto L240;
            }
        }

        if (w * eta >= 0.)
        {
            eta = -w / dw;
        }
        if (orgati)
        {
            temp1 = work[*i__] * delta[*i__];
            temp  = eta - temp1;
        }
        else
        {
            temp1 = work[ip1] * delta[ip1];
            temp  = eta - temp1;
        }
        if (temp > sg2ub || temp < sg2lb)
        {
            if (w < 0.)
            {
                eta = (sg2ub - tau) / 2.;
            }
            else
            {
                eta = (sg2lb - tau) / 2.;
            }
        }

        tau += eta;
        eta /= *sigma + std::sqrt(*sigma * *sigma + eta);

        prew = w;

        *sigma += eta;
        i__1 = *n;
        for (j = 1; j <= i__1; ++j)
        {
            work[j] += eta;
            delta[j] -= eta;
        }

        dpsi   = 0.;
        psi    = 0.;
        erretm = 0.;
        i__1   = iim1;
        for (j = 1; j <= i__1; ++j)
        {
            temp = z__[j] / (work[j] * delta[j]);
            psi += z__[j] * temp;
            dpsi += temp * temp;
            erretm += psi;
        }
        erretm = std::abs(erretm);

        dphi = 0.;
        phi  = 0.;
        i__1 = iip1;
        for (j = *n; j >= i__1; --j)
        {
            temp = z__[j] / (work[j] * delta[j]);
            phi += z__[j] * temp;
            dphi += temp * temp;
            erretm += phi;
        }

        temp   = z__[ii] / (work[ii] * delta[ii]);
        dw     = dpsi + dphi + temp * temp;
        temp   = z__[ii] * temp;
        w      = rhoinv + phi + psi + temp;
        erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + std::abs(temp) * 3. + std::abs(tau) * dw;

        if (w <= 0.)
        {
            sg2lb = (sg2lb > tau) ? sg2lb : tau;
        }
        else
        {
            sg2ub = (sg2ub < tau) ? sg2ub : tau;
        }

        swtch = 0;
        if (orgati)
        {
            if (-w > std::abs(prew) / 10.)
            {
                swtch = 1;
            }
        }
        else
        {
            if (w > std::abs(prew) / 10.)
            {
                swtch = 1;
            }
        }

        iter = niter + 1;

        for (niter = iter; niter <= 20; ++niter)
        {

            if (std::abs(w) <= eps * erretm)
            {
                goto L240;
            }

            if (!swtch3)
            {
                dtipsq = work[ip1] * delta[ip1];
                dtisq  = work[*i__] * delta[*i__];
                if (!swtch)
                {
                    if (orgati)
                    {
                        d__1 = z__[*i__] / dtisq;
                        c__  = w - dtipsq * dw + delsq * (d__1 * d__1);
                    }
                    else
                    {
                        d__1 = z__[ip1] / dtipsq;
                        c__  = w - dtisq * dw - delsq * (d__1 * d__1);
                    }
                }
                else
                {
                    temp = z__[ii] / (work[ii] * delta[ii]);
                    if (orgati)
                    {
                        dpsi += temp * temp;
                    }
                    else
                    {
                        dphi += temp * temp;
                    }
                    c__ = w - dtisq * dpsi - dtipsq * dphi;
                }
                a = (dtipsq + dtisq) * w - dtipsq * dtisq * dw;
                b = dtipsq * dtisq * w;
                if (std::abs(c__) < GMX_FLOAT_MIN)
                {
                    if (std::abs(a) < GMX_FLOAT_MIN)
                    {
                        if (!swtch)
                        {
                            if (orgati)
                            {
                                a = z__[*i__] * z__[*i__] + dtipsq * dtipsq * (dpsi + dphi);
                            }
                            else
                            {
                                a = z__[ip1] * z__[ip1] + dtisq * dtisq * (dpsi + dphi);
                            }
                        }
                        else
                        {
                            a = dtisq * dtisq * dpsi + dtipsq * dtipsq * dphi;
                        }
                    }
                    eta = b / a;
                }
                else if (a <= 0.)
                {
                    eta = (a - std::sqrt(std::abs(a * a - b * 4. * c__))) / (c__ * 2.);
                }
                else
                {
                    eta = b * 2. / (a + std::sqrt(std::abs(a * a - b * 4. * c__)));
                }
            }
            else
            {

                dtiim = work[iim1] * delta[iim1];
                dtiip = work[iip1] * delta[iip1];
                temp  = rhoinv + psi + phi;
                if (swtch)
                {
                    c__   = temp - dtiim * dpsi - dtiip * dphi;
                    zz[0] = dtiim * dtiim * dpsi;
                    zz[2] = dtiip * dtiip * dphi;
                }
                else
                {
                    if (orgati)
                    {
                        temp1 = z__[iim1] / dtiim;
                        temp1 *= temp1;
                        temp2 = (d__[iim1] - d__[iip1]) * (d__[iim1] + d__[iip1]) * temp1;
                        c__   = temp - dtiip * (dpsi + dphi) - temp2;
                        zz[0] = z__[iim1] * z__[iim1];
                        if (dpsi < temp1)
                        {
                            zz[2] = dtiip * dtiip * dphi;
                        }
                        else
                        {
                            zz[2] = dtiip * dtiip * (dpsi - temp1 + dphi);
                        }
                    }
                    else
                    {
                        temp1 = z__[iip1] / dtiip;
                        temp1 *= temp1;
                        temp2 = (d__[iip1] - d__[iim1]) * (d__[iim1] + d__[iip1]) * temp1;
                        c__   = temp - dtiim * (dpsi + dphi) - temp2;
                        if (dphi < temp1)
                        {
                            zz[0] = dtiim * dtiim * dpsi;
                        }
                        else
                        {
                            zz[0] = dtiim * dtiim * (dpsi + (dphi - temp1));
                        }
                        zz[2] = z__[iip1] * z__[iip1];
                    }
                }
                dd[0] = dtiim;
                dd[1] = delta[ii] * work[ii];
                dd[2] = dtiip;
                F77_FUNC(slaed6, SLAED6)(&niter, &orgati, &c__, dd, zz, &w, &eta, info);
                if (*info != 0)
                {
                    goto L240;
                }
            }

            if (w * eta >= 0.)
            {
                eta = -w / dw;
            }
            if (orgati)
            {
                temp1 = work[*i__] * delta[*i__];
                temp  = eta - temp1;
            }
            else
            {
                temp1 = work[ip1] * delta[ip1];
                temp  = eta - temp1;
            }
            if (temp > sg2ub || temp < sg2lb)
            {
                if (w < 0.)
                {
                    eta = (sg2ub - tau) / 2.;
                }
                else
                {
                    eta = (sg2lb - tau) / 2.;
                }
            }

            tau += eta;
            eta /= *sigma + std::sqrt(*sigma * *sigma + eta);

            *sigma += eta;
            i__1 = *n;
            for (j = 1; j <= i__1; ++j)
            {
                work[j] += eta;
                delta[j] -= eta;
            }

            prew = w;

            dpsi   = 0.;
            psi    = 0.;
            erretm = 0.;
            i__1   = iim1;
            for (j = 1; j <= i__1; ++j)
            {
                temp = z__[j] / (work[j] * delta[j]);
                psi += z__[j] * temp;
                dpsi += temp * temp;
                erretm += psi;
            }
            erretm = std::abs(erretm);

            dphi = 0.;
            phi  = 0.;
            i__1 = iip1;
            for (j = *n; j >= i__1; --j)
            {
                temp = z__[j] / (work[j] * delta[j]);
                phi += z__[j] * temp;
                dphi += temp * temp;
                erretm += phi;
            }

            temp = z__[ii] / (work[ii] * delta[ii]);
            dw   = dpsi + dphi + temp * temp;
            temp = z__[ii] * temp;
            w    = rhoinv + phi + psi + temp;
            erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + std::abs(temp) * 3. + std::abs(tau) * dw;
            if (w * prew > 0. && std::abs(w) > std::abs(prew) / 10.)
            {
                swtch = !swtch;
            }

            if (w <= 0.)
            {
                sg2lb = (sg2lb > tau) ? sg2lb : tau;
            }
            else
            {
                sg2ub = (sg2ub < tau) ? sg2ub : tau;
            }
        }

        *info = 1;
    }

L240:
    return;
}
