C SUBSOLVE  SOURCE    FD218221  25/09/03    21:15:06     12351          
      SUBROUTINE SUBSOLVE(M,N,MN,EPSIMIN,LOW,UPP,ALFA,BETA,P0,Q0,P,Q,
     &                    A0,A,B,C,D,
     &                    XMMA,YMMA,ZMMA,LAMMA,XSIMMA,ETAMMA,MUMMA,
     &                    ZETMMA,SMMA)

C     Typages implicites habituels
      IMPLICIT INTEGER(I-N)
      IMPLICIT REAL*8 (A-H,O-Z)

C ------------------------------------------------------------------------------------------------
C Ce code est une adaptation en FORTRAN de l'algorithme de la MMA (Methode des Asymptotes Mobiles)
C propose initialement en langage Matlab par K. Svarberg
C Les sources sont disponibles ici : https://www.smoptit.se/
C ------------------------------------------------------------------------------------------------
C    Cette subroutine resoud le sous probleme convexe suivant :
C
C    Minimiser :   SUM[ p0_j/(upp_j-x_j) + q0_j/(x_j-low_j) ] + a_0*z +
C                + SUM[ c_i*y_i + 0.5*d_i*(y_i)^2 ]
C
C     Soumis a :   SUM[ p_ij/(upp_j-x_j) + q_ij/(x_j-low_j) ] - a_i*z - y_i <= b_i,  i = 1,...,m
C                  alfa_j <= x_j <= beta_j,  j = 1,...,n
C                  z >= 0,   y_i >= 0,       i = 1,...,m
C
C  Entrees :
C  ---------
C   m     = Nombre de contraintes
C   n     = Nombre de variables x_j
C   mn    = Min (m,n) (pour dimensionner les matrices AA, BB et SOLUT)
C epsimin = Petit nombre positif afin de garantir la stabilite
C  low    = Vecteur colonne (n x 1) des bornes inferieures
C  upp    = Vecteur colonne (n x 1) des bornes superieures
C  alfa   = Vecteur colonne (n x 1) des asymptotes inferieures
C  beta   = Vecteur colonne (n x 1) des asymptotes superieures
C  p0     = Vecteur colonne (n x 1) des coefficients pour les bornes inferieures
C  q0     = Vecteur colonne (n x 1) des coefficients pour les bornes superieures
C  P      = Matrice (m x n) des coefficients pour les bornes inferieures des contraintes
C  Q      = Matrice (m x n) des coefficients pour les bornes superieures des contraintes
C  a0     = Constante a_0
C  a      = Vecteur colonne (m x 1) des constantes a_i
C  b      = Vecteur colonne (m x 1) des constantes b_i
C  c      = Vecteur colonne (m x 1) des constantes c_i
C  d      = Vecteur colonne (m x 1) des constantes d_i
C
C  Sorties :
C  ---------
C  xmma  = Vecteur colonne (n x 1) des variables x_j optimisees
C  ymma  = Vecteur colonne (m x 1) des variables y_j optimisees
C  zmma  = Valeur de la variable z optimisee
C  lamma = Vecteur colonne (m x 1) des multiplicateurs de Lagrange
C          des contraintes generales
C xsimma = Vecteur colonne (n x 1) des multiplicateurs de Lagrange
C          des contraintes alfa_j - x_j <= 0
C etamma = Vecteur colonne (n x 1) des multiplicateurs de Lagrange
C          des contraintes x_j - beta_j <= 0
C  mumma = Vecteur colonne (m x 1) des multiplicateurs de Lagrange
C          des contraintes de positivite -y_i <= 0
C zetmma = Multiplicateurs de Lagrange de la contrainte de positivite -z <= 0
C   smma = Vecteur colonne (m x 1) des variables d'ecart des contraintes generales
C ------------------------------------------------------------------------------------------------

C     Arguments de sortie
      REAL*8 XMMA(N,1),YMMA(M,1),ZMMA
      REAL*8 LAMMA(M,1),XSIMMA(N,1),ETAMMA(N,1),MUMMA(M,1),ZETMMA
      REAL*8 SMMA(M,1)
C     Arguments d'entree
      REAL*8 EPSIMIN,LOW(N,1),UPP(N,1),ALFA(N,1),BETA(N,1)
      REAL*8 P0(N,1),Q0(N,1),P(M,N),Q(M,N)
      REAL*8 A0,A(M,1),B(M,1),C(M,1),D(M,1)
C     Arguments internes
      INTEGER ITTT,ITTO
      INTEGER IPIV(MN+1)
      REAL*8 EPSI,RESIDUNORM,RESIDUMAX
      REAL*8 EEN(N,1),EEM(M,1),EPSVECM(M,1),EPSVECN(N,1)
      REAL*8 X(N,1),Y(M,1),Z,DX(N,1),DY(M,1),DZ
      REAL*8 XOLD(N,1),YOLD(M,1),ZOLD,DELX(N,1),DELY(M,1),DELZ
      REAL*8 LAM(M,1),MU(M,1),XSI(N,1),ETA(N,1),S(M,1)
      REAL*8 UX1(N,1),UX2(N,1),UX3(N,1)
      REAL*8 XL1(N,1),XL2(N,1),XL3(N,1)
      REAL*8 UXINV1(N,1),XLINV1(N,1),UXINV2(N,1),XLINV2(N,1)
      REAL*8 DELLAM(M,1),DIAGX(N,1),DIAGY(M,1)
      REAL*8 DIAGXINV(N,1),DIAGYINV(M,1)
      REAL*8 DIAGLAM(M,1),DIAGLAMYI(M,1),PLAM(N,1),QLAM(N,1),GVEC(M,1)
      REAL*8 DPSIDX(N,1),RES(M,1),REX(N,1),REY(M,1),REZ
      REAL*8 RELAM(M,1),REXSI(N,1),REETA(N,1),REMU(M,1),REZET,RESINEW
      REAL*8 BLAM(M,1)
      REAL*8 GG(M,N),BX(N,1),BZ,AXX(N,N),AXZ(N,1),AZZ
      REAL*8 TEMP(M,N),TEMP2(N,M)
      REAL*8 ALAM(M,M),DLAM(M,1),DIAGLAMYIINV(M,1),DELLAMYI(M,1)
      REAL*8 DXSI(N,1),DETA(N,1),DMU(M,1),ZET,DZET,DS(M,1)
      REAL*8 STMXX,STEPALFA(N,1),STMALFA,STEPBETA(N,1),STMBETA
      REAL*8 STMALBE,STMALBEXX,STMINV,STEG
      REAL*8 LAMOLD(M,1),XSIOLD(N,1),ETAOLD(N,1)
      REAL*8 MUOLD(M,1),ZETOLD,SOLD(M,1)
      REAL*8 AA(MN+1,MN+1),BB(MN+1,1),SOLUT(MN+1,1)
      REAL*8 XX(4*M+2*N+2,1),DXX(4*M+2*N+2,1)
      REAL*8 STEPXX(4*M+2*N+2,1)
      REAL*8 RESIDU1(N+M+1),RESIDU2(3*M+2*N+1),RESIDU(4*M+3*N+2)

C     Initialisation
      CALL UN(EEN,N,1)
      CALL UN(EEM,M,1)
      EPSI = 1.0D0
      EPSVECN = EPSI*EEN
      EPSVECM = EPSI*EEM
      X = 0.5D0*(ALFA+BETA)
      Y = EEM
      Z = 1
      LAM = EEM
      XSI = EEN/(X-ALFA)
      XSI = MAX(XSI,EEN)
      ETA = EEN/(BETA-X)
      ETA = MAX(ETA,EEN)
      MU  = MAX(EEM,0.5D0*C)
      ZET = 1
      S = EEM
      ITERA = 0
      
C     Boucle de stabilite numerique
      DO WHILE (EPSI.GT.EPSIMIN)
          EPSVECN = EPSI*EEN
          EPSVECM = EPSI*EEM
          UX1 = UPP-X
          XL1 = X-LOW
          UX2 = UX1*UX1
          XL2 = XL1*XL1
          UXINV1 = EEN/UX1
          XLINV1 = EEN/XL1
          PLAM = P0 + MATMUL(TRANSPOSE(P),LAM) 
          QLAM = Q0 + MATMUL(TRANSPOSE(Q),LAM) 
          GVEC = MATMUL(P,UXINV1) + MATMUL(Q,XLINV1)
          DPSIDX = PLAM/UX2 - QLAM/XL2
          REX = DPSIDX - XSI + ETA
          REY = C + D*Y - MU - LAM
          REZ = A0 - ZET - MAXVAL(MATMUL(TRANSPOSE(A),LAM))
          RELAM = GVEC - A*Z - Y + S - B
          REXSI = XSI*(X-ALFA) - EPSVECN
          REETA = ETA*(BETA-X) - EPSVECN
          REMU = MU*Y - EPSVECM
          REZET = ZET*Z - EPSI
          RES = LAM*S - EPSVECM
C         RESIDU1=[REX(N), REY(M), REZ]
          NR1=N+M+1
          RESIDU1(1:N)=REX(1:N,1)
          RESIDU1(N+1:N+M)=REY(1:M,1)
          RESIDU1(NR1)=REZ
C         RESIDU2=[RELAM(M), REXSI(N), REETA(N), REMU(M), REZET, RES(M)]
          NR2=2*N+3*M+1
          RESIDU2(1:M)=RELAM(1:M,1)
          RESIDU2(M+1:M+N)=REXSI(1:N,1)
          RESIDU2(M+N+1:M+2*N)=REETA(1:N,1)
          RESIDU2(M+2*N+1:2*M+2*N)=REMU(1:M,1)
          RESIDU2(2*M+2*N+1)=REZET
          RESIDU2(2*M+2*N+2:NR2)=RES(1:M,1)
C         RESIDU=[RESIDU1, RESIDU2]
          NR=NR1+NR2
          RESIDU(1:NR1)=RESIDU1
          RESIDU(NR1+1:NR)=RESIDU2
C         Normes 2 et infinie du residu
          RESIDUNORM = SQRT(DDOT(NR,RESIDU,1,RESIDU,1))
          RESIDUMAX = VECMAX(ABS(RESIDU),NR)
          ITTT = 0

C         Boucle d'optimisation
          DO WHILE ((RESIDUMAX.GT.0.9D0*EPSI).AND.(ITTT.LT.200))
              ITTT = ITTT + 1
              ITERA = ITERA + 1
              UX1 = UPP - X
              XL1 = X - LOW
              UX2 = UX1*UX1
              XL2 = XL1*XL1
              UX3 = UX1*UX2
              XL3 = XL1*XL2
              UXINV1 = EEN/UX1
              XLINV1 = EEN/XL1
              UXINV2 = EEN/UX2
              XLINV2 = EEN/XL2
              PLAM = P0 + MATMUL(TRANSPOSE(P),LAM)
              QLAM = Q0 + MATMUL(TRANSPOSE(Q),LAM)
              GVEC = MATMUL(P,UXINV1) + MATMUL(Q,XLINV1)
              CALL MULMV(TEMP,P,UXINV2,M,N)
              GG = TEMP
              CALL MULMV(TEMP,Q,XLINV2,M,N)
              GG = GG - TEMP
              DPSIDX = PLAM/UX2 - QLAM/XL2
              DELX = DPSIDX - EPSVECN/(X-ALFA) + EPSVECN/(BETA-X)
              DELY = C + D*Y - LAM - EPSVECM/Y
              DELZ = A0 - MAXVAL(MATMUL(TRANSPOSE(A),LAM)) - EPSI/Z
              DELLAM = GVEC - A*Z - Y - B + EPSVECM/LAM
              DIAGX = PLAM/UX3 + QLAM/XL3
              DIAGX = 2*DIAGX + XSI/(X-ALFA) + ETA/(BETA-X)
              DIAGXINV = EEN/DIAGX
              DIAGY = D + MU/Y
              DIAGYINV = EEM/DIAGY
              DIAGLAM = S/LAM
              DIAGLAMYI = DIAGLAM + DIAGYINV

C             Resolution du systeme lineaire : AA.SOLUT = BB

C             Cas ou il y moins de contraintes que de variables primales
C             AA est une matrice (m+1 x m+1)
C             BB est un vecteur  (m+1 x 1)
              IF (M.LT.N) THEN
C                 Assemblage de la matrice AA = [ALAM    A
C                                                A'   -ZET/Z]
                  CALL MULMV(TEMP,GG,DIAGXINV,M,N)
                  ALAM=MATMUL(TEMP,TRANSPOSE(GG))
                  DO I=1,M
                      ALAM(I,I)=ALAM(I,I)+DIAGLAMYI(I,1)
                  ENDDO
C                 Ajout du bloc ALAM
                  DO I=1,M
                      DO J=1,M
                          AA(I,J)=ALAM(I,J)
                      ENDDO
                  ENDDO
C                 Ajout du bloc A
                  DO I=1,M
                      AA(I,M+1)=A(I,1)
                  ENDDO
C                 Ajout du bloc A'
                  DO I=1,M
                      AA(M+1,I)=A(I,1)
                  ENDDO
C                 Ajout du bloc -ZET/Z
                  AA(M+1,M+1)=-ZET/Z
C                 Assemblage du vecteur BB = [BLAM
C                                             DELZ]
                  BLAM=DELLAM + DELY/DIAGY - MATMUL(GG,(DELX/DIAGX))
                  DO I=1,M
                      BB(I,1)=BLAM(I,1)
                  ENDDO
                  BB(M+1,1)=DELZ
C                 Appel au solveur de LAPACK
                  CALL DGESV(M+1,1,AA,M+1,IPIV,BB,M+1,INFO)
C                 Sortie en cas d'erreur
                  IF (INFO.NE.0) THEN
                      PRINT*,'ERROR IN DGESV SOLVER, INFO=',INFO
                      PRINT*,'CHECK THE LINEAR SYSTEM'
                      CALL ERREUR(26)
                      RETURN
                  ENDIF
                  SOLUT=BB
                  DLAM(1:M,1) = SOLUT(1:M,1)
                  DZ = SOLUT(M+1,1)
                  DX = -DELX/DIAGX - MATMUL(TRANSPOSE(GG),DLAM)/DIAGX

C             Cas ou il y plus de contraintes que de variables primales
C             AA est une matrice (n+1 x n+1)
C             BB est un vecteur  (n+1 x 1)
              ELSE
C                 Assemblage de la matrice AA = [AXX  AXZ
C                                                AXZ' AZZ]
                  DIAGLAMYIINV=EEM/DIAGLAMYI
                  CALL MULMV(TEMP2,TRANSPOSE(GG),DIAGLAMYIINV,N,M)
                  AXX=MATMUL(TEMP2,GG)
                  DO I=1,N
                      AXX(I,I)=AXX(I,I)+DIAGX(I,1)
                  ENDDO
                  AZZ=ZET/Z+MAXVAL(MATMUL(TRANSPOSE(A),(A/DIAGLAMYI)))
                  AXZ=MATMUL(TRANSPOSE(-GG),(A/DIAGLAMYI))
C                 Ajout du bloc AXX
                  DO I=1,N
                      DO J=1,N
                          AA(I,J)=AXX(I,J)
                      ENDDO
                  ENDDO
C                 Ajout du bloc AXZ
                  DO I=1,N
                      AA(I,N+1)=AXZ(I,1)
                  ENDDO
C                 Ajout du bloc AXZ'
                  DO I=1,N
                      AA(N+1,I)=AXZ(I,1)
                  ENDDO
C                 Ajout du bloc AZZ
                  AA(N+1,N+1)=AZZ
C                 Assemblage du vecteur BB = [-BX
C                                             -BZ]
                  DELLAMYI=DELLAM + DELY/DIAGY
                  BX=DELX+MATMUL(TRANSPOSE(GG),(DELLAMYI/DIAGLAMYI))
                  BZ=DELZ-MAXVAL(MATMUL(TRANSPOSE(A),
     &                                  (DELLAMYI/DIAGLAMYI)))
                  DO I=1,N
                      BB(I,1)=-BX(I,1)
                  ENDDO
                  BB(N+1,1)=-BZ
C                 Appel au solveur de LAPACK
                  CALL DGESV(N+1,1,AA,N+1,IPIV,BB,N+1,INFO)
C                 Sortie en cas d'erreur
                  IF (INFO.NE.0) THEN
                      PRINT*,'ERROR IN DGESV SOLVER, INFO=',INFO
                      PRINT*,'CHECK THE LINEAR SYSTEM'
                      CALL ERREUR(26)
                      RETURN
                  ENDIF
                  SOLUT = BB
                  DX(1:N,1) = SOLUT(1:N,1)
                  DZ = SOLUT(N+1,1)
                  DLAM =MATMUL(GG,DX)/DIAGLAMYI-DZ*(A/DIAGLAMYI)
     &                                         +DELLAMYI/DIAGLAMYI
              END IF

              DY = -DELY/DIAGY + DLAM/DIAGY
              DXSI = -XSI + EPSVECN/(X-ALFA) - (XSI*DX)/(X-ALFA)
              DETA = -ETA + EPSVECN/(BETA-X) + (ETA*DX)/(BETA-X)
              DMU  = -MU  + EPSVECM/Y        - (MU*DY)/Y
              DZET = -ZET + EPSI/Z           - ZET*DZ/Z
              DS   = -S   + EPSVECM/LAM      - (S*DLAM)/LAM

              NX=4*M+2*N+2
C             XX=[ Y, Z, LAM, XSI, ETA, MU, ZET, S]
              XX(1:M,1)=Y(1:M,1)
              XX(M+1,1)=Z
              XX(M+2:2*M+1,1)=LAM(1:M,1)
              XX(2*M+2:2*M+N+1,1)=XSI(1:N,1)
              XX(2*M+N+2:2*M+2*N+1,1)=ETA(1:N,1)
              XX(2*M+2*N+2:3*M+2*N+1,1)=MU(1:M,1)
              XX(3*M+2*N+2,1)=ZET
              XX(3*M+2*N+3:4*M+2*N+2,1)=S(1:M,1)
C             DXX=[DY,DZ,DLAM,DXSI,DETA,DMU,DZET,DS]
              DXX(1:M,1)=DY(1:M,1)
              DXX(M+1,1)=DZ
              DXX(M+2:2*M+1,1)=DLAM(1:M,1)
              DXX(2*M+2:2*M+N+1,1)=DXSI(1:N,1)
              DXX(2*M+N+2:2*M+2*N+1,1)=DETA(1:N,1)
              DXX(2*M+2*N+2:3*M+2*N+1,1)=DMU(1:M,1)
              DXX(3*M+2*N+2,1)=DZET
              DXX(3*M+2*N+3:4*M+2*N+2,1)=DS(1:M,1)

C             Calcul de la longueur du pas
              STEPXX = -1.01D0*DXX/XX
              STMXX  = MAXVAL(STEPXX)
              STEPALFA = -1.01D0*DX/(X-ALFA)
              STMALFA = MAXVAL(STEPALFA)
              STEPBETA = 1.01D0*DX/(BETA-X)
              STMBETA = MAXVAL(STEPBETA)
              STMALBE  = MAX(STMALFA,STMBETA)
              STMALBEXX = MAX(STMALBE,STMXX)
              STMINV = MAX(STMALBEXX,1.D0)
              STEG = 1.D0/STMINV

C             Mise a jour des variables
              XOLD = X
              YOLD = Y
              ZOLD = Z
              LAMOLD = LAM
              XSIOLD = XSI
              ETAOLD = ETA
              MUOLD  = MU
              ZETOLD = ZET
              SOLD = S

              ITTO = 0
              RESINEW = 2*RESIDUNORM

              DO WHILE ((RESINEW.GT.RESIDUNORM).AND.(ITTO.LT.50))
                  ITTO = ITTO + 1;
                  X = XOLD + STEG*DX
                  Y = YOLD + STEG*DY
                  Z = ZOLD + STEG*DZ
                  LAM = LAMOLD + STEG*DLAM
                  XSI = XSIOLD + STEG*DXSI
                  ETA = ETAOLD + STEG*DETA
                  MU = MUOLD  + STEG*DMU
                  ZET = ZETOLD + STEG*DZET
                  S = SOLD + STEG*DS
                  UX1 = UPP - X
                  XL1 = X - LOW
                  UX2 = UX1*UX1
                  XL2 = XL1*XL1
                  UXINV1 = EEN/UX1
                  XLINV1 = EEN/XL1
                  PLAM = P0 + MATMUL(TRANSPOSE(P),LAM) 
                  QLAM = Q0 + MATMUL(TRANSPOSE(Q),LAM) 
                  GVEC = MATMUL(P,UXINV1) + MATMUL(Q,XLINV1)
                  DPSIDX = PLAM/UX2 - QLAM/XL2 
                  REX = DPSIDX - XSI + ETA
                  REY = C + D*Y - MU - LAM
                  REZ = A0 - ZET - MAXVAL(MATMUL(TRANSPOSE(A),LAM))
                  RELAM = GVEC - A*Z - Y + S - B
                  REXSI = XSI*(X-ALFA) - EPSVECN
                  REETA = ETA*(BETA-X) - EPSVECN
                  REMU = MU*Y - EPSVECM
                  REZET = ZET*Z - EPSI
                  RES = LAM*S - EPSVECM
C                 RESIDU1=[REX(N), REY(M), REZ]
                  NR1=N+M+1
                  RESIDU1(1:N)=REX(1:N,1)
                  RESIDU1(N+1:N+M)=REY(1:M,1)
                  RESIDU1(NR1)=REZ
C                 RESIDU2=[RELAM(M), REXSI(N), REETA(N), REMU(M), REZET, RES(M)]
                  NR2=2*N+3*M+1
                  RESIDU2(1:M)=RELAM(1:M,1)
                  RESIDU2(M+1:M+N)=REXSI(1:N,1)
                  RESIDU2(M+N+1:M+2*N)=REETA(1:N,1)
                  RESIDU2(M+2*N+1:2*M+2*N)=REMU(1:M,1)
                  RESIDU2(2*M+2*N+1)=REZET
                  RESIDU2(2*M+2*N+2:NR2)=RES(1:M,1)
C                 RESIDU=[RESIDU1, RESIDU2]
                  NR=NR1+NR2
                  RESIDU(1:NR1)=RESIDU1
                  RESIDU(NR1+1:NR)=RESIDU2
C                 Normes 2 du residu
                  NR=4*M+3*N+2
                  RESINEW = SQRT(DDOT(NR,RESIDU,1,RESIDU,1))
                  STEG = STEG/2.0D0
              END DO
              RESIDUNORM = RESINEW
C             Norme infini du residu
              NR=4*M+3*N+2
              RESIDUMAX = VECMAX(ABS(RESIDU),NR)
              STEG = 2.0D0*STEG
          END DO
          EPSI = 0.1D0*EPSI
      END DO

C     Fin du programme
      XMMA = X
      YMMA = Y
      ZMMA = Z
      LAMMA = LAM
      XSIMMA = XSI
      ETAMMA = ETA
      MUMMA = MU
      ZETMMA = ZET
      SMMA = S
      RETURN
      END