*-----------------------------------------------------------------------
* osolvd - solve overconstrained systems of linear equations
* algorithm:  complete orthogonal factorization
* by Andy Allinger, 2023, released to the public domain
*
*    Permission  to  use, copy, modify, and distribute this software and
*    its documentation  for  any  purpose  and  without  fee  is  hereby
*    granted,  without any conditions or restrictions.  This software is
*    provided "as is" without express or implied warranty.
*-----------------------------------------------------------------------

*-----------------------------------------------------------------------
* osolvd - linear least squares problem
*
* Solve Ax = b in the least-squares sense by complete orthogonal 
* factorization.  Factor A:
*      
*      PRx = b
*
* Factor R:
*
*      P S^T Q^T x = b
*   
* where P and Q have orthogonal columns, and R and S are right 
* triangular.
*
* The subroutine is intended for the case M .GE. N
* It may be used in the case M < N if A is dimensioned A[N,N]
*
*___Name_______Type_______________In/Out___Description__________________
*   M          Integer            In       Rows in A
*   N          Integer            In       Columns in A
*   A(LDA,N)   Double precision   In       Matrix of size [M,N]
*                                            A is destroyed.
*   LDA        Integer            In       Row bound of A
*                                            LDA .GE. M if M .GE. N
*                                            LDA .GE. N if M < N
*   B(M)       Double precision   In       Right-hand vector B[M]
*   RELTOL     Double precision   In       Relative tolerance
*   KRANK      Integer            Out      Rank of A
*   X(N)       Double precision   Out      Solution vector
*   W(N)       Double precision   None     Work array
*   C(N)       Double precision   None     Squared column lengths
*   IND(N)     Integer            None     Which columns to use
*   IFAULT     Integer            Out      Error code:
*                                             0 = no errors
*                                             1 = M out of bounds
*                                             2 = N out of bounds
*                                             3 = not enough memory
*                                             4 = RELTOL out of bounds
*                                             5 = Can't determine rank
*-----------------------------------------------------------------------
      SUBROUTINE OSOLVD (M, N, A, LDA, B, RELTOL, KRANK, X, 
     $                    W, C, IND, IFAULT)
       IMPLICIT NONE
       INTEGER LDA, M, N
       INTEGER KRANK, IND(N), IFAULT
       DOUBLE PRECISION A(LDA,N), B(M), RELTOL, X(N), W(N), C(N)

       DOUBLE PRECISION ZERO, ONE, PAD, BIGSR, BIG
       PARAMETER (ZERO = 0.D0,
     $            ONE = 1.D0,
     $            PAD = 1.D1,
     $            BIGSR = 1.D18,
     $            BIG = 1.D36)
     
       INTEGER 
     $         I, J, JP, K, KP,   ! indices, counters
     $         JMAX,              ! index of largest column
     $         ISWAP,             ! temporary storage
     $         L, LL              ! estimates of rank
     
       DOUBLE PRECISION 
     $                  D,        ! a scalar
     $                  TAU,      ! minimum pivot value
     $                  ULEN      ! vector length
       
*-----------------------------------------------------------------------
*             Begin.
*-----------------------------------------------------------------------
       IF (M .LE. 0 .OR. M > LDA) THEN
         IFAULT = 1
         RETURN
       ELSE IF (N .LE. 0) THEN
         IFAULT = 2
         RETURN
       ELSE IF (M < N .AND. N > LDA) THEN
         IFAULT = 3
         RETURN
       ELSE IF (RELTOL .LE. ZERO .OR. RELTOL .GE. ONE) THEN
         IFAULT = 4
         RETURN
       ELSE
         IFAULT = 0
       END IF

*-----------------------------------------------------------------------
*             Matrix norm.
*-----------------------------------------------------------------------
       TAU = ZERO
       DO J = 1, N
         C(J) = ZERO
         X(J) = ZERO
         DO I = 1, M
           C(J) = C(J) + A(I,J)**2
         END DO
         TAU = TAU + C(J)
       END DO
       IF (TAU .EQ. ZERO) RETURN
       TAU = SQRT(TAU) * RELTOL                  ! critical magnitude

*-----------------------------------------------------------------------
*             Factor A -> P R
*
* Matrix P is formed in columns of array A.  Rows of R are stored in X. 
* As soon as each column of P is complete, compute the next element of 
* P^T b and store in array W.  Then overwrite the column of P in array A
* with the row of R in array X.  Array A now holds a permutation of R^T
*-----------------------------------------------------------------------
       L = 0
       DO I = 1, N                               ! identity permutation
         IND(I) = I
       END DO

       DO 10 KP = 1, N
         DO J = 1, N                             ! Clear X.
           X(J) = ZERO
         END DO
         
         ULEN = -BIG                             ! choose largest column
         K = KP                                  ! initialization
         JMAX = KP
         DO JP = L+1, N
           J = IND(JP)
           IF (C(J) > ULEN) THEN
             ULEN = C(J)
             K = J
             JMAX = JP
           END IF
         END DO

         ULEN = ZERO                             ! Recompute magnitude.
         DO I = 1, M
           ULEN = ULEN + A(I,K)**2
         END DO
         ULEN = SQRT(ULEN)
         IF (ULEN < TAU) THEN
           C(K) = -BIGSR
           GO TO 10
         END IF

         L = L + 1                               ! Increment rank.
         ISWAP = IND(JMAX)                       ! Swap indices.
         IND(JMAX) = IND(L)
         IND(L) = ISWAP

         D = ONE / ULEN                          ! Normalize.
         DO I = 1, M
           A(I,K) = A(I,K) * D
         END DO
         X(K) = ULEN

         DO JP = L+1, N
           J = IND(JP)
           D = ZERO                              ! dot product
           DO I = 1, M
             D = D + A(I,J) * A(I,K)
           END DO
           DO I = 1, M                           ! Subtract.
             A(I,J) = A(I,J) - D * A(I,K)
           END DO
           X(J) = D
           C(J) = C(J) - D**2                    ! Update magnitude.
         END DO
           
         W(L) = ZERO                             ! Multiply P^T b
         DO I = 1, M
           W(L) = W(L) + A(I,K) * B(I)
         END DO

         DO I = 1, N                             ! Replace P with R^T
           A(I,K) = X(I)
         END DO
  10   CONTINUE

*-----------------------------------------------------------------------
*             Factor R -> S^T Q^T
*
* Columns of Q are formed in in columns of A.  Rows of S^T are stored in
* array X.  As soon as each row of S^T is complete, solve an equation of 
* the system
*
*             S^T y = P^T b
*
* On completion, array A holds a permutation of matrix Q, and array W 
* holds vector y.
*-----------------------------------------------------------------------
       TAU = TAU / PAD
       LL = 0
       DO KP = 1, L
         K = IND(KP)
         DO JP = 1, LL
           J = IND(JP)
           D = ZERO                              ! dot product
           DO I = 1, N
             D = D + A(I,J) * A(I,K)
           END DO
           DO I = 1, N                           ! Subtract.
             A(I,K) = A(I,K) - D * A(I,J)
           END DO
           X(JP) = D
         END DO
           
         ULEN = ZERO                             ! vector magnitude
         DO I = 1, N
           ULEN = ULEN + A(I,K)**2
         END DO
         ULEN = SQRT(ULEN)
         IF (ULEN < TAU) THEN                    ! rank slip
           IFAULT = 5
           RETURN
         END IF

         LL = LL + 1
         D = ONE / ULEN                          ! Normalize.
         DO I = 1, N
           A(I,K) = A(I,K) * D
         END DO
         X(LL) = ULEN

         DO JP = 1, LL-1                         ! Solve.
           W(LL) = W(LL) - X(JP) * W(JP)         ! Subtract mid terms.
         END DO
         W(LL) = W(LL) / X(LL)                   ! Divide by pivot.
       END DO

*-----------------------------------------------------------------------
*             Multiply x = Qy
*-----------------------------------------------------------------------
       DO I = 1, N
         X(I) = ZERO
       END DO
       DO JP = 1, L
         J = IND(JP)
         DO I = 1, N
           X(I) = X(I) + A(I,J) * W(JP)
         END DO
       END DO

       KRANK = L
       RETURN        
      END  ! of osolvd
