!   grpnet_ordinal.f90 - group elastic net (ordinal / polr)
!   Nathaniel E. Helwig (helwig@umn.edu)
!   Department of Psychology and School of Statistics
!   University of Minnesota
!   Date: 2025-12-12


! INPUTS/OUTPUTS
!   nobs = number of observations (N)
!   nvars = number of variables (P)
!   nresp = number of response classes for y (M)
!   x = grouped predictor matrix (N,P)
!       note: (x.1, x.2, ..., x.K) where x.k is (N,P.k) with sum(P.k) = P
!   y = response vector with entries in {1,...,M} (N)
!   w = observation weight vector (N)
!       note: if min(w) = max(w), then weights are ignored
!   off = offset vector (N)
!   ngrps = number of groups (K)
!   gsize = number of coefs in each group (K)
!           note: (P.1, P.2, ..., P.K) with sum(P.k) = P
!   pw = penalty weight vector (K)   (outputs xsdev)
!        note: set pw(k) = 0 to leave k-th group unpenalized
!   alpha = weight for L1 and L2 penalities (alpha = 1 for lasso/mcp/scad, alpha = 0 for ridge)
!           note: setting alpha between (0,1) gives elastic net
!   nlam = number of lambdas
!   lambda = sequence of lambda values
!            note: if max(lambda) <=0, then lambda.max is data dependent
!   lmr = lambda minimum ratio (lambda.min = lmr * lambda.max)
!         note: unless lambda is provided, lambda.max is data dependent
!   penid = penalty id: 1 = lasso, 2 = mcp, 3 = scad
!   gamma = additional hyper-parameter for mcd and scad penalities
!           note: gamma > 1 for mcp and gamma > 2 for scad
!   eps = convergence tolerance
!   maxit = maximum number of iterations
!   standardize = integer (0: no standardization, 1: yes standardization)
!   intercept = integer (0: no intercept, 1: yes intercept)
!   ibeta = output matrix of intercepts (nresp-1,nlam)
!   betas = output matrix of coefficients (nvars,nlam)
!   iters = output vector of iterations for each lambda (nlam)
!   nzgrps = number of non-zero groups for each lambda (nlam)
!   nzcoef = number of non-zero coefficients for each lambda (nlam)
!   edfs = effective degrees of freedom for each lambda (nlam)
!   devs = residual deviance for each lambda (nlam)
!   nulldev = null deviance


SUBROUTINE grpnet_ordinal(nobs, nvars, nresp, x, y, w, off, ngrps, gsize, pw, &
                          alpha, nlam, lambda, lmr, penid, gamma, eps, maxit, &
                          standardize, intercept, ibeta, betas, iters, &
                          nzgrps, nzcoef, edfs, devs, nulldev)

    IMPLICIT NONE

! --------------- ARGUMENTS --------------- !
    INTEGER nobs, nvars, nresp, y(nobs), ngrps, gsize(ngrps), nlam, penid, maxit
    INTEGER standardize, intercept, iters(nlam), nzgrps(nlam), nzcoef(nlam)
    DOUBLE PRECISION x(nobs, nvars), w(nobs), off(nobs), pw(ngrps)
    DOUBLE PRECISION alpha, lambda(nlam), lmr, gamma, eps, ibeta(nresp-1,nlam)
    DOUBLE PRECISION betas(nvars, nlam), edfs(nlam), devs(nlam), nulldev
! --------------- ARGUMENTS --------------- !


! --------------- LOCAL DEFINITIONS --------------- !
    INTEGER i, j, k, l, iter, violations, makelambda, weighted
    INTEGER active(ngrps), strong(ngrps), ia(ngrps), ib(ngrps), gid
    INTEGER yfreq(nresp)
    DOUBLE PRECISION ycumsum(nresp)
    DOUBLE PRECISION wmin, wmax, rnglam, minlam, maxlam, maxdif, macheps
    DOUBLE PRECISION r(nobs), difbeta(nvars), beta(nvars), zvec(nvars)
    DOUBLE PRECISION grad(nvars), gradnorm(ngrps), ctol, shrink, twolam
    DOUBLE PRECISION penone, pentwo, xmean(nvars), xsdev(ngrps), xev(ngrps)
    DOUBLE PRECISION znorm, bnorm, eta(nobs), mu(nobs), yprob(nresp-1)
    DOUBLE PRECISION shift(nresp-1), bigtheta(nresp+1), fit0(nobs)
    DOUBLE PRECISION fit1(nobs), Pi0(nobs), Pi1(nobs), xmb(nlam)
    DOUBLE PRECISION, ALLOCATABLE :: xtx(:,:)
! --------------- LOCAL DEFINITIONS --------------- !


! --------------- MACHINE EPSILON --------------- !
    macheps = EPSILON(eps)
! --------------- MACHINE EPSILON --------------- !


! --------------- CHECK WEIGHTS --------------- !
    wmin = MINVAL(w)
    wmax = MAXVAL(w)
    IF (wmax > wmin) THEN
        weighted = 1
        w = nobs * w / SUM(w)     ! normalize so SUM(w) = nobs
        w = SQRT(w)
        DO i=1,nobs
            x(i,:) = w(i) * x(i,:)
        END DO
    ELSE
        weighted = 0
        w = 1.0D0
    END IF
! --------------- CHECK WEIGHTS --------------- !


! --------------- GROUP INDICES --------------- !
    gid = 0
    DO k=1,ngrps
        ia(k) = gid + 1
        gid = gid + gsize(k)
        ib(k) = gid
    END DO
! --------------- GROUP INDICES --------------- !


! --------------- CENTER AND SCALE --------------- !
    IF (intercept == 1) THEN
        DO j=1,nvars
            xmean(j) = SUM(x(:,j)) / nobs
            x(:,j) = x(:,j) - xmean(j)
        END DO
    END IF
    xsdev = 1.0D0
    IF (standardize == 1) THEN
        DO k=1,ngrps
            xsdev(k) = SQRT( SUM(x(:, ia(k):ib(k))**2) / (nobs * gsize(k)) )
            IF (xsdev(k) > macheps) THEN
                x(:,ia(k):ib(k)) = x(:,ia(k):ib(k)) / xsdev(k)
            ELSE
                xsdev(k) = 1.0D0
            END IF
        END DO
    END IF
! --------------- CENTER AND SCALE --------------- !


! --------------- GET MAX EIGENVALUE --------------- !
    DO k=1,ngrps
        IF (gsize(k) == 1) THEN
            xev(k) = SUM(x(:,ia(k))**2) / nobs
        ELSE
            ALLOCATE(xtx(gsize(k), gsize(k)))
            xtx = MATMUL(TRANSPOSE(x(:,ia(k):ib(k))), x(:,ia(k):ib(k))) / nobs
            CALL grpnet_maxeigval(xtx, gsize(k), xev(k))
            DEALLOCATE(xtx)
        END IF
    END DO
    xev = xev / 2.0D0
! --------------- GET MAX EIGENVALUE --------------- !


! --------------- MISCELLANEOUS INITIALIZATIONS --------------- !
    maxlam = MAXVAL(lambda)
    makelambda = 0
    iter = 0
    strong = 0
    active = 0
    nzgrps = 0
    nzcoef = 0
    zvec = 0.0D0
    ibeta = 0.0D0
    beta = 0.0D0
    grad = 0.0D0
    gradnorm = 0.0D0
    twolam = 0.0D0
    devs = 0.0D0
    eta = off
    maxdif = 0.0D0
! --------------- MISCELLANEOUS INITIALIZATIONS --------------- !


! --------------- SHIFT INITIALIZATION --------------- !
    yfreq = 0
    ycumsum = 0
    yfreq(1) = COUNT(y == 1)
    ycumsum(1) = yfreq(1) + 0.0D0
    DO k=2,nresp
        yfreq(k) = COUNT(y == k)
        ycumsum(k) = ycumsum(k-1) + yfreq(k)
    END DO
    yprob = 1.0D0 - (ycumsum(1:(nresp-1)) / nobs)
    DO k=1,nresp-1
        IF (yprob(k) < 1.0E-6) THEN
            yprob(k) = 1.0E-6
        ELSE IF (yprob(k) > (1.0D0 - 1.0E-6)) THEN
            yprob(k) = 1.0D0 - 1.0E-6
        END IF
    END DO
    shift = LOG(yprob / (1.0D0 - yprob))
! --------------- SHIFT INITIALIZATION --------------- !


! --------------- RESID INITIALIZATION --------------- !
    bigtheta(1) = 100.0D0
    bigtheta(2:nresp) = shift
    bigtheta(nresp+1) = -100.0D0
    fit0 = eta + bigtheta(y)
    fit1 = eta + bigtheta(y+1)
    Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
    Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
    r = w * (1.0D0 - Pi0 - Pi1)
! --------------- RESID INITIALIZATION --------------- !


! --------------- GENERATE LAMBDA --------------- !
    IF (maxlam <= macheps) THEN

        makelambda = 1
        i = 1

        ! find unpenalized groups !
        DO k=1,ngrps
            IF (pw(k) <= macheps) THEN
                active(k) = 1
                nzgrps(i) = nzgrps(i) + 1
                nzcoef(i) = nzcoef(i) + gsize(k)
            END IF
        END DO
        ! find unpenalized groups !

        ! iterate until active coefficients converge !
        IF (nzgrps(i) > 0) THEN
            DO WHILE(iter < maxit)

                ! update iter and reset counters
                ctol = 0.0D0
                iter = iter + 1

                ! update shifts
                CALL grpnet_ordinal_int(nobs, nresp, eta, y, w**2, yfreq, shift, maxdif)
                ctol = MAX(maxdif, ctol)

                ! define r
                bigtheta(1) = 100.0D0
                bigtheta(2:nresp) = shift
                bigtheta(nresp+1) = -100.0D0
                fit0 = eta + bigtheta(y)
                fit1 = eta + bigtheta(y+1)
                Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
                Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
                r = w * (1.0D0 - Pi0 - Pi1)

                ! update active groups
                DO k=1,ngrps
                    IF(active(k) == 0) CYCLE
                    grad(ia(k):ib(k)) = MATMUL(r, x(:,ia(k):ib(k))) / nobs
                    zvec(ia(k):ib(k)) = beta(ia(k):ib(k)) + grad(ia(k):ib(k)) / xev(k)
                    difbeta(ia(k):ib(k)) = zvec(ia(k):ib(k)) - beta(ia(k):ib(k))
                    maxdif = MAXVAL( ABS(difbeta(ia(k):ib(k))) / (1.0D0 + ABS(beta(ia(k):ib(k)))) )
                    beta(ia(k):ib(k)) = beta(ia(k):ib(k)) + difbeta(ia(k):ib(k))
                    eta = eta + MATMUL(x(:,ia(k):ib(k)), difbeta(ia(k):ib(k))) / w
                    fit0 = eta + bigtheta(y)
                    fit1 = eta + bigtheta(y+1)
                    Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
                    Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
                    r = w * (1.0D0 - Pi0 - Pi1)
                    ctol = MAX(maxdif , ctol)
                END DO ! k=1,ngrps

                ! convergence check
                IF(ctol < eps) EXIT

            END DO ! WHILE(iter < maxit)

        ELSE

            ! intercept only
            DO WHILE(iter < maxit)
                ctol = 0.0D0
                iter = iter + 1
                CALL grpnet_ordinal_int(nobs, nresp, eta, y, w**2, yfreq, shift, maxdif)
                ctol = MAX(maxdif, ctol)
                IF(ctol < eps) EXIT
            END DO ! WHILE(iter < maxit)

            ! define r
            bigtheta(1) = 100.0D0
            bigtheta(2:nresp) = shift
            bigtheta(nresp+1) = -100.0D0
            fit0 = eta + bigtheta(y)
            fit1 = eta + bigtheta(y+1)
            Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
            Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
            r = w * (1.0D0 - Pi0 - Pi1)

        END IF ! (nzgrps(1) > 0)
        ! iterate until active coefficients converge !

        ! create lambda sequence !
        DO k=1,ngrps
            IF (pw(k) > macheps) THEN
                grad(ia(k):ib(k)) = MATMUL(r, x(:,ia(k):ib(k))) / nobs
                gradnorm(k) = SQRT(SUM(grad(ia(k):ib(k))**2)) / pw(k)
            END IF
        END DO
        IF (alpha > macheps) THEN
            maxlam = MAXVAL(gradnorm / alpha)
        ELSE
            maxlam = MAXVAL(gradnorm / 1.0E-3)
            makelambda = 0
        END IF
        minlam = lmr * maxlam
        lambda(i) = maxlam
        maxlam = LOG(maxlam)
        minlam = LOG(minlam)
        rnglam = maxlam - minlam
        DO k=2,nlam
            lambda(k) = EXP(maxlam - rnglam * (k - 1) / (nlam - 1))
        END DO
        ! create lambda sequence !

        ! calculate deviance !
        mu = Pi0 - Pi1
        devs(i) = -2.0D0 * SUM(w**2 * LOG(mu))
        ! calculate deviance !

        ! save results !
        ibeta(:,i) = shift
        betas(:,i) = beta
        iters(i) = iter
        nzgrps(i) = nzgrps(i) + intercept
        nzcoef(i) = nzcoef(i) + intercept * (nresp-1)
        edfs(i) = DBLE(nzcoef(i))
        ! save results !

    END IF
! --------------- GENERATE LAMBDA --------------- !


! --------------- ITERATIVE WORK --------------- !
    DO i=1,nlam

        ! initializations !
        IF (i == 1 .AND. makelambda == 1) CYCLE
        IF (i > 1) THEN
            shift = ibeta(:,i-1)
            beta = betas(:,i-1)
            twolam = alpha * (2.0D0 * lambda(i) - lambda(i-1))
        ELSE
            grad = MATMUL(r, x) / nobs
        END IF
        ! initializations !

        ! strong rule initialization !
        DO k=1,ngrps
            gradnorm(k) = SQRT(SUM(grad(ia(k):ib(k))**2))
            IF (gradnorm(k) + 1.0E-8 > pw(k) * twolam) THEN
                strong(k) = 1
            ELSE
                strong(k) = 0
            END IF
        END DO
        ! strong rule initialization !

        ! iterate until strong set converges !
        iter = 0
        DO WHILE(iter < maxit)

            ! iterate until active set converges !
            DO WHILE(iter < maxit)

                ! iterate until active coefficients converge !
                DO WHILE(iter < maxit)

                    ! update iter and reset counters
                    iter = iter + 1
                    nzgrps(i) = 0
                    edfs(i) = 0.0D0
                    ctol = 0.0D0

                    ! update shifts
                    CALL grpnet_ordinal_int(nobs, nresp, eta, y, w**2, yfreq, shift, maxdif)
                    ctol = MAX(maxdif, ctol)

                    ! define r
                    bigtheta(1) = 100.0D0
                    bigtheta(2:nresp) = shift
                    bigtheta(nresp+1) = -100.0D0
                    fit0 = eta + bigtheta(y)
                    fit1 = eta + bigtheta(y+1)
                    Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
                    Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
                    r = w * (1.0D0 - Pi0 - Pi1)

                    ! update active groups
                    DO k=1,ngrps
                        IF(active(k) == 0) CYCLE
                        penone = alpha * lambda(i) * pw(k) / xev(k)
                        pentwo = (1.0D0 - alpha) * lambda(i) * pw(k) / xev(k)
                        grad(ia(k):ib(k)) = MATMUL(r, x(:,ia(k):ib(k))) / nobs
                        zvec(ia(k):ib(k)) = beta(ia(k):ib(k)) + grad(ia(k):ib(k)) / xev(k)
                        znorm = SQRT(SUM(zvec(ia(k):ib(k))**2))
                        bnorm = SQRT(SUM(beta(ia(k):ib(k))**2))
                        CALL grpnet_penalty(znorm, penid, penone, pentwo, gamma, shrink)
                        IF(shrink == 0.0D0 .AND. bnorm == 0.0D0) CYCLE
                        difbeta(ia(k):ib(k)) = shrink * zvec(ia(k):ib(k)) - beta(ia(k):ib(k))
                        maxdif = MAXVAL( ABS(difbeta(ia(k):ib(k))) / (1.0D0 + ABS(beta(ia(k):ib(k)))) )
                        beta(ia(k):ib(k)) = beta(ia(k):ib(k)) + difbeta(ia(k):ib(k))
                        eta = eta + MATMUL(x(:,ia(k):ib(k)), difbeta(ia(k):ib(k))) / w
                        fit0 = eta + bigtheta(y)
                        fit1 = eta + bigtheta(y+1)
                        Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
                        Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
                        r = w * (1.0D0 - Pi0 - Pi1)
                        ctol = MAX(maxdif , ctol)
                        IF(shrink > 0.0D0) THEN
                            nzgrps(i) = nzgrps(i) + 1
                            edfs(i) = edfs(i) + gsize(k) * shrink
                        ENDIF
                    END DO ! k=1,ngrps

                    ! convergence check
                    IF(ctol < eps) EXIT

                END DO ! WHILE(iter < maxit) - inner
                ! iterate until active coefficients converge !

                ! check inactive groups in strong set !
                violations = 0
                DO k=1,ngrps
                    IF(strong(k) == 0 .OR. active(k) == 1) CYCLE
                    grad(ia(k):ib(k)) = MATMUL(r, x(:,ia(k):ib(k))) / nobs
                    gradnorm(k) = SQRT(SUM(grad(ia(k):ib(k))**2))
                    IF (gradnorm(k) > alpha * lambda(i) * pw(k)) THEN
                        active(k) = 1
                        violations = violations + 1
                    END IF
                END DO ! k=1,ngrps
                IF(violations == 0) EXIT
                ! check inactive groups in strong set !

            END DO ! WHILE(iter < maxit) - middle
            ! iterate until active set converges !

            ! check groups in weak set !
            violations = 0
            DO k=1,ngrps
                IF (strong(k) == 1) CYCLE
                grad(ia(k):ib(k)) = MATMUL(r, x(:,ia(k):ib(k))) / nobs
                gradnorm(k) = SQRT(SUM(grad(ia(k):ib(k))**2))
                IF (gradnorm(k) + 1e-8 > alpha * lambda(i) * pw(k)) THEN
                    strong(k) = 1
                    active(k) = 1
                    violations = violations + 1
                END IF
            END DO
            IF(violations == 0) EXIT
            ! check groups in weak set !

        END DO ! WHILE(iter < maxit) - outer
        ! iterate until strong set converges !

        ! calculate nzcoef !
        DO k=1,ngrps
            IF(active(k) == 0) CYCLE
            DO l=1,gsize(k)
                IF (ABS(beta(ia(k) + l - 1)) > macheps) THEN
                    nzcoef(i) = nzcoef(i) + 1
                END IF
            END DO
        END DO
        ! calculate nzcoef !

        ! calculate deviance !
        mu = Pi0 - Pi1
        devs(i) = -2.0D0 * SUM(w**2 * LOG(mu))
        ! calculate deviance !

        ! save results !
        ibeta(:,i) = shift
        betas(:,i) = beta
        iters(i) = iter
        nzgrps(i) = nzgrps(i) + intercept
        nzcoef(i) = nzcoef(i) + intercept
        edfs(i) = edfs(i) + DBLE(intercept)
        ! save results !

    END DO
! --------------- ITERATIVE WORK --------------- !


! --------------- POST PROCESSING --------------- !
    IF (standardize == 1) THEN
        DO k=1,ngrps
            betas(ia(k):ib(k),:) = betas(ia(k):ib(k),:) / xsdev(k)
        END DO
    END IF
    IF (intercept == 1) THEN
        xmb = MATMUL(xmean, betas)
        DO i=1,nlam
            ibeta(:,i) = ibeta(:,i) - xmb(i)
        END DO
    END IF
    IF(makelambda == 1 .AND. nzgrps(1) == 0) THEN
        shift = ibeta(:,1)
    ELSE
        shift = log(yprob / (1 - yprob))
        eta = 0.0D0
        DO WHILE(iter < maxit)
            ctol = 0.0D0
            CALL grpnet_ordinal_int(nobs, nresp, eta, y, w**2, yfreq, shift, maxdif)
            ctol = MAX(maxdif, ctol)
            IF(ctol < eps) EXIT
        END DO ! WHILE(iter < maxit)
    END IF
    bigtheta(1) = 100.0D0
    bigtheta(2:nresp) = shift
    bigtheta(nresp+1) = -100.0D0
    fit0 = bigtheta(y)
    fit1 = bigtheta(y+1)
    Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
    Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
    mu = Pi0 - Pi1
    nulldev = -2.0D0 * SUM(w**2 * LOG(mu))
    pw = xsdev
! --------------- POST PROCESSING --------------- !

END SUBROUTINE


SUBROUTINE grpnet_ordinal_int(nobs, nresp, eta, y, w, yfreq, theta, maxdif)
    IMPLICIT NONE

! --------------- ARGUMENTS --------------- !
    INTEGER nobs, nresp, y(nobs), yfreq(nresp)
    DOUBLE PRECISION eta(nobs), w(nobs), theta(nresp-1), maxdif
! --------------- ARGUMENTS --------------- !

! --------------- LOCAL DEFINITIONS --------------- !
    INTEGER i, j, yj(nobs), yjm1(nobs), n
    INTEGER index(nresp-1), umin(nresp-1), vmax(nresp-1)
    DOUBLE PRECISION bigtheta(nresp+1), grad(nresp-1), avec(nresp-1)
    DOUBLE PRECISION bvec(nresp-2), fit0(nobs), fit1(nobs), mu(nobs)
    DOUBLE PRECISION Pi0(nobs), Pi1(nobs), u0(nobs), u1(nobs)
    DOUBLE PRECISION dtop(nresp), dbot(nresp), dtheta(nresp)
    DOUBLE PRECISION vj(nobs), va(nobs), vb(nobs), ivec(nresp-1)
    DOUBLE PRECISION u(nresp-1), v(nresp-1), d(nresp-1), delta(nresp-1)
! --------------- LOCAL DEFINITIONS --------------- !

! --------------- GRADIENT AND HESSIAN --------------- !
    ! initialize bigtheta
    bigtheta(1) = 100.0D0
    bigtheta(2:nresp) = theta
    bigtheta(nresp+1) = -100.0D0

    ! initialize grad, avec, and bvec
    grad = 0.0D0
    avec = 0.0D0
    bvec = 0.0D0

    ! define shift + linear predictor (for y and y+1)
    fit0 = eta + bigtheta(y)
    fit1 = eta + bigtheta(y+1)

    ! evaluate gradient / hessian components
    Pi0 = 1.0D0 / (1.0D0 + EXP(-fit0))
    Pi1 = 1.0D0 / (1.0D0 + EXP(-fit1))
    u0 = Pi0 * (1.0D0 - Pi0)
    u1 = Pi1 * (1.0D0 - Pi1)
    mu = Pi0 - Pi1

    ! update bvec (off-diagonals)
    dtop = EXP(bigtheta(1:nresp) + bigtheta(2:(nresp+1)))
    dbot = ( EXP(bigtheta(1:nresp)) - EXP(bigtheta(2:(nresp+1))) )**2
    dtheta = dtop / dbot
    bvec = (yfreq(2:(nresp-1)) * dtheta(2:(nresp-1))) / nobs
    
    ! update other shift gradient and hessian components
    DO i=1,nresp-1
        j = i + 1
        yj = MERGE(1, 0, y == j)
        yjm1 = MERGE(1, 0, y == (j-1))
        grad(i) = SUM( w * (u0 * yj - u1 * yjm1) / mu ) / nobs
        vj = 1.0D0 / (1.0D0 + EXP(-(eta + bigtheta(j))))
        vj = (1.0D0 - vj)**2 - (1.0D0 - vj)
        va = vj - dtheta(j-1)
        vb = vj - dtheta(j)
        avec(i) = -SUM( w * (yjm1 * va + yj * vb) ) / nobs
    END DO
! --------------- GRADIENT AND HESSIAN --------------- !

! --------------- INVSYMTRIDIAG --------------- !
    ! initializations
    n = nresp - 1
    u = 0.0D0
    v = 0.0D0
    d = 0.0D0
    delta = 0.0D0

    ! update d
    d(n) = avec(n)
    DO i=n-1,1,-1
        d(i) = avec(i) - bvec(i)**2 / d(i+1)
    END DO

    ! update v
    v(1) = 1 / d(1)
    DO i=2,n
        v(i) = PRODUCT(bvec(1:(i-1)) / d(1:(i-1))) / d(i)
    END DO

    ! update delta
    delta(1) = avec(1)
    DO i=1,n-1
        delta(i+1) = avec(i+1) - bvec(i)**2 / delta(i)
    END DO

    ! update u
    u(n) = 1 / (delta(n) * v(n))
    DO i=1,n-1
        u(i) = PRODUCT(bvec(i:(n-1)) / delta(i:(n-1))) / (delta(n) * v(n))
    END DO
! --------------- INVSYMTRIDIAG --------------- !

! --------------- NEWTON-RAPHSON UPDATE --------------- !
    ! calculate MATMUL(solve(hess), grad)
    index = (/ (i, i=1,n) /)
    ivec = 0.0D0
    DO i=1,n
        umin = index
        vmax = index
        umin(i:n) = i
        vmax(1:i) = i
        ivec(i) = SUM( (u(umin) * v(vmax)) * grad )
    END DO

    ! update maxdif and theta
    maxdif = MAXVAL( ABS(ivec) / (1.0D0 + ABS(theta)) )
    theta = theta + ivec
! --------------- NEWTON-RAPHSON UPDATE --------------- !
    
END SUBROUTINE
