########################################################################################################
### GCMMA-MMA-Julia                                                                                  ### 
###                                                                                                  ###
### This file is part of GCMMA-MMA-Julia. GCMMA-MMA-Julia is licensed under the terms of GNU         ###
### General Public License as published by the Free Software Foundation. For more information and    ###
### the LICENSE file, see <https://github.com/pollinico/TopOpt_jl/blob/main/LICENSE>.                ###
###                                                                                                  ###
### The orginal work is written by Krister Svanberg in MATLAB.                                       ###
### This is the Julia version of the code written by Nicol√≤ Pollini.                                 ###
### version 18-05-2023                                                                               ###
########################################################################################################

#-------------------------------------------------------------
#
#    Copyright (C) 2007, 2008 Krister Svanberg
#
#    This file, mmasub.m, is part of GCMMA-MMA-code.
#    
#    GCMMA-MMA-code is free software; you can redistribute it and/or
#    modify it under the terms of the GNU General Public License as 
#    published by the Free Software Foundation; either version 3 of 
#    the License, or (at your option) any later version.
#    
#    This code is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#    
#    You should have received a copy of the GNU General Public License
#    (file COPYING) along with this file.  If not, see 
#    <http://www.gnu.org/licenses/>.
#    
#    You should have received a file README along with this file,
#    containing contact information.  If not, see
#    <http://www.smoptit.se/> or e-mail mmainfo@smoptit.se or krille@math.kth.se.
#
#    Version September 2007 (and a small change August 2008)
#
#
using SparseArrays
function mmasub(m,n,iter,xval,xmin,xmax,xold1,xold2,f0val,df0dx,fval,dfdx,low,upp,a0,a,c,d)
    #
    #    This function mmasub performs one MMA-iteration, aimed at
    #    solving the nonlinear programming problem:
    #         
    #      Minimize  f_0(x) + a_0*z + sum( c_i*y_i + 0.5*d_i*(y_i)^2 )
    #    subject to  f_i(x) - a_i*z - y_i <= 0,  i = 1,...,m
    #                xmin_j <= x_j <= xmax_j,    j = 1,...,n
    #                z >= 0,   y_i >= 0,         i = 1,...,m
    #*** INPUT:
    #
    #   m    = The number of general constraints.
    #   n    = The number of variables x_j.
    #  iter  = Current iteration number ( =1 the first time mmasub is called).
    #  xval  = Column vector with the current values of the variables x_j.
    #  xmin  = Column vector with the lower bounds for the variables x_j.
    #  xmax  = Column vector with the upper bounds for the variables x_j.
    #  xold1 = xval, one iteration ago (provided that iter>1).
    #  xold2 = xval, two iterations ago (provided that iter>2).
    #  f0val = The value of the objective function f_0 at xval.
    #  df0dx = Column vector with the derivatives of the objective function
    #          f_0 with respect to the variables x_j, calculated at xval.
    #  fval  = Column vector with the values of the constraint functions f_i,
    #          calculated at xval.
    #  dfdx  = (m x n)-matrix with the derivatives of the constraint functions
    #          f_i with respect to the variables x_j, calculated at xval.
    #          dfdx(i,j) = the derivative of f_i with respect to x_j.
    #  low   = Column vector with the lower asymptotes from the previous
    #          iteration (provided that iter>1).
    #  upp   = Column vector with the upper asymptotes from the previous
    #          iteration (provided that iter>1).
    #  a0    = The constants a_0 in the term a_0*z.
    #  a     = Column vector with the constants a_i in the terms a_i*z.
    #  c     = Column vector with the constants c_i in the terms c_i*y_i.
    #  d     = Column vector with the constants d_i in the terms 0.5*d_i*(y_i)^2.
    #     
    #*** OUTPUT:
    #
    #  xmma  = Column vector with the optimal values of the variables x_j
    #          in the current MMA subproblem.
    #  ymma  = Column vector with the optimal values of the variables y_i
    #          in the current MMA subproblem.
    #  zmma  = Scalar with the optimal value of the variable z
    #          in the current MMA subproblem.
    #  lam   = Lagrange multipliers for the m general MMA constraints.
    #  xsi   = Lagrange multipliers for the n constraints alfa_j - x_j <= 0.
    #  eta   = Lagrange multipliers for the n constraints x_j - beta_j <= 0.
    #   mu   = Lagrange multipliers for the m constraints -y_i <= 0.
    #  zet   = Lagrange multiplier for the single constraint -z <= 0.
    #   s    = Slack variables for the m general MMA constraints.
    #  low   = Column vector with the lower asymptotes, calculated and used
    #          in the current MMA subproblem.
    #  upp   = Column vector with the upper asymptotes, calculated and used
    #          in the current MMA subproblem.
    #
    #epsimin = sqrt(m+n)*10^(-9);
    epsimin = 10^(-7)
    raa0 = 0.00001
    move = 0.5
    albefa = 0.1
    asyinit = 0.5
    asyincr = 1.2
    asydecr = 0.7
    eeen = ones(n)
    eeem = ones(m)
    zeron = zeros(n)

    # Calculation of the asymptotes low and upp :
    if iter < 2.5
        low = xval - asyinit*(xmax-xmin)
        upp = xval + asyinit*(xmax-xmin)
    else
        zzz = (xval-xold1).*(xold1-xold2)
        factor = copy(eeen)
        factor[findall(x -> x>0, zzz)] .= asyincr
        factor[findall(x -> x<0, zzz)] .= asydecr
        low = xval - factor.*(xold1 - low)
        upp = xval + factor.*(upp - xold1)
        lowmin = xval - 10*(xmax-xmin)
        lowmax = xval - 0.01*(xmax-xmin)
        uppmin = xval + 0.01*(xmax-xmin)
        uppmax = xval + 10*(xmax-xmin)
        low = max.(low,lowmin)
        low = min.(low,lowmax)
        upp = min.(upp,uppmax)
        upp = max.(upp,uppmin)
    end

    # Calculation of the bounds alfa and beta :

    zzz1 = low + albefa*(xval-low)
    zzz2 = xval - move*(xmax-xmin)
    zzz  = max.(zzz1,zzz2)
    alfa = max.(zzz,xmin)
    zzz1 = upp - albefa*(upp-xval)
    zzz2 = xval + move*(xmax-xmin)
    zzz  = min.(zzz1,zzz2)
    beta = min.(zzz,xmax)

    # Calculations of p0, q0, P, Q and b.

    xmami = xmax-xmin
    xmamieps = 0.00001*eeen
    xmami = max.(xmami,xmamieps)
    xmamiinv = eeen./xmami
    ux1 = upp-xval
    ux2 = ux1.*ux1
    xl1 = xval-low
    xl2 = xl1.*xl1
    uxinv = eeen./ux1
    xlinv = eeen./xl1
    #
    p0 = copy(zeron)
    q0 = copy(zeron)
    p0 = max.(df0dx,0)
    q0 = max.(-df0dx,0)
    pq0 = 0.001*(p0 + q0) + raa0*xmamiinv
    p0 = p0 + pq0
    q0 = q0 + pq0
    p0 = p0.*ux2
    q0 = q0.*xl2
    #
    P = spzeros(m,n)
    Q = spzeros(m,n)
    P = max.(dfdx,0)
    Q = max.(-dfdx,0)
    PQ = 0.001*(P + Q) + raa0*eeem*xmamiinv'
    P = P + PQ
    Q = Q + PQ
    P = P * spdiagm(n,n,vec(ux2))
    Q = Q * spdiagm(n,n,vec(xl2))
    b = P*uxinv + Q*xlinv - fval 
    #
    ### Solving the subproblem by a primal-dual Newton method
    xmma,ymma,zmma,lam,xsi,eta,mu,zet,s = subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d)
    #
    return xmma, ymma, zmma, lam, xsi, eta, mu, zet, s, low, upp
end


