# SNLLS algorithm
#
# YR -- 18 Jan 2022
# - both CFA and SEM, using RAM notation
# - this is in initial implementation merely to illustrate the method

lav_optim_snlss <- function(lavmodel, S = NULL, S.inv = NULL,
                            verbose = FALSE) {

    stopifnot(lavmodel@representation == "RAM",
              lavmodel@ngroups == 1L,
              lavmodel@conditional.x == FALSE,
              lavmodel@categorical == FALSE)

    # compute full V (for now)
    V <- 0.5 * lav_matrix_duplication_pre_post(S.inv %x% S.inv)

    # pre-compute Vs, sVs
    s <- lav_matrix_vech(S)
    Vs <- V %*% s
    sVs <- drop(crossprod(s, V) %*% s)

    # compute L.S
    L.S   <- lavaan:::lav_model_dmmdpar(lavmodel = fit.dummy@Model,
                                          target = "S")

    # starting values for A
    A.idx <- which(names(lavmodel@GLIST) == "A")
    A <- lavmodel@GLIST$A
    A.start <- lavmodel@GLIST$A[ lavmodel@m.free.idx[[A.idx]] ]
    A.m.free.idx <- lavmodel@m.free.idx[[A.idx]]

    # Filter matrix
    It <- diag(nrow(A))
    Fo <- It[lavmodel@GLIST$ov.idx, , drop = FALSE]

    # nlminb to get lambda elements
    optim.out <- nlminb(start = A.start,
                  objective = lav_optim_snlss_obj,
                  A = A, A.m.free.idx = A.m.free.idx,
                  V = V, Vs = Vs, sVs = sVs, L.S = L.S, Fo = Fo,
                  # mimic lavaan's control values
                  control = list(eval.max = 20000, iter.max = 10000,
                                 trace = 0, step.min = 1, step.max = 1,
                                 abs.tol = 2.220446e-15, rel.tol = 1e-10,
                                 x.tol = 1.5e-08, xf.tol = 2.2e-14),
                  verbose = verbose)

    # compute gradient at solution
    grad <- lav_optim_snlss_grad(x = optim.out$par,
                  A = A, A.m.free.idx = A.m.free.idx,
                  V = V, Vs = Vs, sVs = sVs, L.S = L.S, Fo = Fo)
    grad.ok <- all(abs(grad) < 0.001)

    # check for convergence
    if(optim.out$convergence != 0L || !grad.ok) {
        converged <- FALSE
        warning("lavaan WARNING: nlminb + SNLLS did not convergence.")
    } else {
        converged <- TRUE
    }

    # get (free) lambda elements
    A[ A.m.free.idx ] <- optim.out$par

    # use GLS to find S elements
    IA.inv <- lavaan:::lav_matrix_inverse_iminus(A)
    FIA.inv <- Fo %*% IA.inv
    G <- lav_matrix_duplication_ginv_pre(FIA.inv %x% FIA.inv) %*% L.S
    out <- solve(crossprod(G, V) %*% G, crossprod(G, Vs))

    # first elements belong to PSI
    S <- matrix(L.S %*% out, ncol(A), ncol(A))

    # free parameters only
    theta <- c(optim.out$par, out)

    # return MLIST (for now)
    MLIST <- list(ov.idx = lavmodel@GLIST$ov.idx, A = A, S = S,
                  converged = converged,
                  iterations = optim.out$iterations,
                  theta = theta)

    MLIST
}

lav_optim_snlss_obj <- function(x, A = NULL, A.m.free.idx,
                                V, Vs, sVs, L.S, Fo,
                                verbose = FALSE) {

    if(!all(is.finite(x))) {
        return(+Inf)
    }

    A[ A.m.free.idx ] <- x

    IA.inv <- lavaan:::lav_matrix_inverse_iminus(A)
    FIA.inv <- Fo %*% IA.inv

    # create G
    G <- lav_matrix_duplication_ginv_pre(FIA.inv %x% FIA.inv) %*% L.S

    # check rank of G
    G.rank <- qr(G)$rank
    if(G.rank < ncol(G)) {
        return(+Inf)
    }

    tGVG <- crossprod(G, V) %*% G
    tGVs <- crossprod(G, Vs)
    INV <- try(solve(tGVG, tGVs), silent = TRUE)
    if(inherits(INV, "try-error")) {
        return(+Inf)
    } else {
        tmp <- drop(crossprod(tGVs, INV))
    }

    Fobj <- sVs - tmp

    if(verbose) {
        cat("obj = ", Fobj, "\n")
    }

    Fobj
}