module least_squares

       interface lsq_engine
          module procedure mp_lsq_engine
       end interface

contains

       subroutine mp_lsq_engine(xi,y,y_sd,yfit,x,x_sd,fj_)
              !
              !         Weighted Linear Least Squares Fit To An Arbitrary Function (JM Doster)
              !
              implicit none
              real, dimension(:), intent(in)    :: xi, y        ! x values, y values
              real, dimension(:), intent(in)    :: y_sd         ! standard deviation in y values
              real, dimension(:), intent(out)   :: yfit         ! values of lsq curve at xi
              real, dimension(:), intent(out)   :: x, x_sd      ! coefficient vector and standard deviation
              real, dimension(size(x),size(xi)) :: f, fw
              real, dimension(size(xi))         :: yw, w, sigma, rdev
              real, dimension(size(x),size(x))  :: a, Ainverse
              real, dimension(size(x))          :: s, var
              real                              :: chisqr, anorm
              integer                           :: nterms, npts ! number of terms, number of datapoints
              integer                           :: i,j,k

              interface fj
                 subroutine fj_(xi,f)
                        implicit none
                        real, intent(out), dimension(:,:) :: F  ! weight values (nterms,npts)
                        real, intent(in), dimension(:)    :: XI ! x values (npts)
                 end subroutine fj_
              end interface


              nterms = size(x)
              npts   = size(xi)

              yfit = (/ (0, i=1,npts) /)

              do i=1,nterms
                 s(i)=0.
                 var(i)=0.
                 do j=1,nterms
                    a(i,j)=0.
                    Ainverse(i,j)=0.
                 end do
              end do

              do i=1,npts
                 sigma(i)=y_sd(i)
              end do

              !         COMPUTE FUNCTION VALUES AT EACH DATA POINT
              !                 
              CALL FJ(XI,F)
              !                 
              !         COMPUTE WEIGHTING FUNCTION
              !                 
              DO I=1,NPTS
                 W(I)=1./SIGMA(I)
              end do

              DO J=1,NTERMS
                 DO I=1,NPTS
                    FW(J,I)=F(J,I)*W(I)
                 end do
              end do

              DO I=1,NPTS
                 YW(I)=Y(I)*W(I)
              end do
              !                 
              !         COMPUTE ELEMENTS OF COEFFICIENT MATRIX
              !         
              DO K=1,NTERMS
                 DO J=K,NTERMS
                    DO I=1,NPTS
                       A(K,J)=A(K,J)+FW(K,I)*FW(J,I)
                    end do
                 end do
              end do

              DO K=1,NTERMS
                 DO J=K+1,NTERMS
                    A(J,K)=A(K,J)
                 end do
              end do
              !                 
              !         COMPUTE ELEMENTS OF SOURCE VECTOR
              !                 
              DO K=1,NTERMS
                 DO I=1,NPTS
                    S(K)=S(K)+YW(I)*FW(K,I)
                 end do
              end do
              !
              !         Compute Matrix inverse
              !
              CALL mp_Inverse(A,Ainverse)
              !
              !         Compute Solution Vector
              !
              do k=1,nterms
                 x(k)=0.
                 do j=1,nterms
                    x(k)=x(k)+Ainverse(k,j)*S(j)
                 end do
              end do
              !                 
              !         COMPUTE VALUES OF FITTED FUNCTION AT EACH DATA POINT
              !                 
              DO J=1,NTERMS
                 DO I=1,NPTS
                    YFIT(I)=YFIT(I)+X(J)*F(J,I)
                 end do
              end do
              !                 
              !         COMPUTE AVERAGE CHI SQUARED VALUE
              !                 
              CHISQR=0.
              DO I=1,NPTS
                 CHISQR=CHISQR+((Y(I)-YFIT(I))*W(I))**2
              end do
              chisqr=chisqr/real(npts)
              !                 
              !         COMPUTE AVERAGE RELATIVE DEVIATION OF FIT AND DATA
              !                 
              ANORM=0.
              DO I=1,NPTS
                 RDEV(I)=(Y(I)-YFIT(I))/Y(I)
                 ANORM=ANORM+ABS(RDEV(I))
              end do

              ANORM=ANORM/real(NPTS)

              do i=1,nterms
                 x_sd(i)=sqrt(abs(Ainverse(i,i)))
              end do
       end subroutine mp_lsq_engine

       SUBROUTINE mp_GAUSS(A,B,X)
              ! 
              !         SUBROUTINE TO SOLVE LINEAR SVSTEMS OF EQUATIONS By
              !                 GAUSSIAN ELIMINATION WITH BACK SUBSTITUTION AND
              !
              DIMENSION A(:,:),B(:),X(:)
              N = size(A,1)
              !                 BEGIN LOOP ON PIVOT ELEMENT
              DO K=1,N

                 !                      SEARCH COLUMN K FOR MAXIMUM PIVOT ELEMENT
                 TEST=0.
                 DO I=K,N
                    IF (ABS(A(I,K)).GT.TEST)THEN
                       TEST=ABS(A(I,K))
                       ISTORE=I
                    END IF
                 end do
                 !                      INTERCHANGE ROW K AND ROW ISTORE
                 IF(ISTORE.NE.K)THEN
                    DO J=K,N
                       ASTORE=A(K,J)
                       A(K,J)=A(ISTORE,J)
                       A(ISTORE,J)=ASTORE
                    end do
                    BSTORE=B(K)
                    B(K)=B(ISTORE)
                    B(ISTORE)=BSTORE
                 END IF

                 PIVOT=A(K,K)

                 !                      commented out to comply with next change, apw
                 !                      A(K,K)=1.

                 !                      DIVIDE ROW K By THE PIVOT ELEMENT
                 B(K)=B(K)/PIVOT
                 !                      changed: apw formerly do j=k+1,n                        
                 DO J=K,N
                    A(K,J)=A(K,J)/PIVOT
                 end do

                 !                      ZERO COLUMN K

                 !                      changed: apw formerly do i=k+1,n                
                 if (k<n) then
                    DO I=K+1,N
                       AMULT=A(I,K)
                       A(I,K)=0.
                       IF(AMULT.ne.0.) then
                          B(I)=B(I)-AMULT*B(K)
                          DO J=K+1,N
                             A(I,J)=A(I,J)-AMULT*A(K,J)
                          end do
                       end if
                    end do
                 end if
              end do

              !         BACK SUBSTITUTION STEP
              X(N)=B(N)
              DO I=N-1,1,-1
                 SUM=0.
                 DO J=I+1,N
                    SUM=SUM+X(J)*A(I,J)
                 end do
                 X(I)=B(I)-SUM
              end do
       end subroutine mp_gauss

       !
       !        Subroutine to compute the matrix inverse
       !
       subroutine mp_Inverse(A,Ainverse)
              dimension A(:,:),Ainverse(:,:),Astore(size(A,1),size(A,2)),x(size(A,1))
              dimension b(size(a,1))

              nterms = size(a,1)

              do i=1,nterms
                 do j=1,nterms
                    Astore(i,j)=A(i,j)
                 end do
              end do

              do k=1,nterms
                 do i=1,nterms
                    do j=1,nterms
                       A(i,j)=Astore(i,j)
                    end do
                 end do

                 do i=1,nterms
                    if(i.eq.k)then
                       b(i)=1.
                    else
                       b(i)=0.
                    end if
                 end do
                 CALL mp_GAUSS(A,b,X)

                 do j=1,nterms
                    Ainverse(j,k)=x(j)
                 end do
              end do

       end subroutine mp_Inverse
end module least_squares