c Bubblesoft Maths Library
c (extracts for demonstration package)

c Copyright Wessex Scientific and Technical Services 1996, all rights reserved.
c The following activities are forbidden with regards to this software:
c  1. Modification.
c  2. Incorporation in other software.
c  3. Use or transfer to any person for payment or consideration of any kind.

c The subroutines in this file are provided to demonstrate and illustrate
c the use of the library from which they are taken, on computers which cannot
c run the executable programs supplied.

c =====================================================================
c =====================================================================

c *** Least squares polynomial fit to 1D data       ***
c *** by solving overdetermined system of equations ***
c Polynomial is of form c1+c2.x+c3.x^2+...
      subroutine bm_polyfitg(x,w,npts,n,eps,yc,fail,res,a)
c Data in:
      real x(npts) ! x ordinate array
      real w(npts) ! weighting array
      integer npts ! no of data pts (x,y pairs)
      integer n ! order+1 of polynomial to fit i.e. n=number of coefficients
      real eps ! numerical bandwidth used in matrix inversion
c Data in/out:
      real yc(npts) ! y(x) data array. Coefficients returned in first n elements
c Data out:
      logical fail ! true if system of equations was singular
      real res ! residue from least squares fit
c Working space:
      real a(npts,n) ! overdetermined system of equations

c Internal variables:
      integer i,j
      real xpowerk

c     zero working space
      do i=1,npts
      do j=1,n
      a(i,j)=0.0
      enddo
      enddo

c     form overdetermined system of equations in A
c     (uses xpowerk to calculate powers efficiently)
      do i=1,npts
      yc(i)=yc(i)*w(i)
      do k=0,n-1
      if (k.eq.0) then
         xpowerk=1.0
      else
         xpowerk=xpowerk*x(i)
      endif
      a(i,k+1)=w(i)*xpowerk
      enddo
      enddo

c     solve to find least-squares fitting polynomial coefficients
      call bm_matsolg(a,npts,n,npts,eps,yc,fail,res,det)

      return
      end

c =====================================================================
c =====================================================================

c *** Solution of matrix equation Ax=y ***
c A:nr by nc, x:nc, y:nr; nr >= nc (i.e. can be overdetermined)
c uses Givens rotations to make upper triangular, then back substitution
      subroutine bm_matsolg(a,nr,nc,nrm,eps,yx,sing,res,det)
c Data in:
      real a(nrm,nc) ! matrix A (values corrupted in finding solution!)
      integer nr ! no of rows
      integer nc ! no of cols (less than or equal to nr)
      integer nrm ! dimensioned no of rows in A
      real eps ! numerical bandwidth used to declare a number insignificant
c Data in/out:
      real yx(nr) ! y values (nr) in; x values (nc) out
c Data out:
      logical sing ! true if A too close to singular so cannot solve
      real res ! residual error (in l2 solution of overdetermined system)
      real det ! determinant of A

c Internal variables:
      integer i,j,k,ii,nrow,j1
      real temp,beta,s,c,ai,aj,yi,yj
      real el,bigel

c     check for underdetermined system
      if (nr.lt.nc) stop 'BM_MATSOLG: underdetermined system ! '

c     set initial values
      sing=.false.
      det=1.0
      res=0.0

c     apply pivoting and Givens rotations to make A upper-triangular
      do j=1,nc ! for each column of A

c     swap rows below main diagonal to make diagonal element as large as poss
      if (j.lt.nr) then ! don't do it for last column if square matrix

c        find largest element in rows below and including main diagonal
         bigel=0.0
         nrow=0
         do ii=j,nr
         el=abs(a(ii,j))
         if (el.gt.bigel) then
            bigel=el
            nrow=ii
         endif
         enddo

c        if largest element is very small => near-singular so abort
         if (bigel.le.eps) then
            sing=.true.
            det=0.0
            return
         endif

c        otherwise swap rows (unless largest already on diagonal)
         if (nrow.ne.j) then

c           change sign of determinant
            det=-det

c           swap rows j and nrow of matrix
            do k=1,nc
            temp=a(j,k)
            a(j,k)=a(nrow,k)
            a(nrow,k)=temp
            enddo

c           swap rows j and nrow of rhs vector
            temp=yx(j)
            yx(j)=yx(nrow)
            yx(nrow)=temp
         endif
      endif

c     apply Givens rotations to zero elements below diagonal
      do i=nr,j+1,-1

c Calculate cos and sin of rotation angle
      beta=a(i,j)/a(j,j)
      c=1.0/sqrt(1.0+beta*beta)
      s=beta*c

c Apply to each col from diagonal to right and to rhs
      do j1=j,nc
      aj=a(j,j1)
      ai=a(i,j1)
      a(j,j1)=c*aj+s*ai
      a(i,j1)=c*ai-s*aj
      enddo
      yj=yx(j)
      yi=yx(i)
      yx(j)=c*yj+s*yi
      yx(i)=c*yi-s*yj
      enddo ! end of loop over rows to be zeroed by Givens rotations
      enddo ! end of loop over columns

c     calculate residue from nonzero part of y beyond nc rows
      res=0.0
      do i=nc+1,nr
      res=res+(abs(yx(i)))**2
      enddo

c     calculate determinant from product of elements on diagonal
      do i=1,nc
      det=det*a(i,i)
      enddo

c     solve upper-triangular system of equations for solution vector
      call bm_uptrisol(a,nc,nrm,eps,yx,sing)

      return
      end

c =====================================================================

c *** Upper triangular matrix solver ***
c N.B. Matrix values corrupted in solution.
      subroutine bm_uptrisol(a,nac,narmax,eps,yx,sing)
c Data in:
      real a(narmax,nac) ! upper triangular matrix
      integer nac ! number of columns used
      integer narmax ! dimensioned rows in A
      real eps ! numerical bandwidth used to declare a number insignificant
c Data in/out:
      real yx(*) ! rhs vector (nar values) in; lhs vector (nac values) out
      logical sing ! true if matrix too close to singular so cannot solve

c Internal variables:
      integer i,j

c     check for singular matrix
      sing=.false.
      do i=1,nac
      if (abs(a(i,i)).le.eps) then
         sing=.true.
         return
      endif
      enddo

c     reduce matrix to diagonal by back-substitution
      do i=nac,2,-1
      do j=1,i-1
      yx(j)=yx(j)-yx(i)*a(j,i)/a(i,i)
      a(j,i)=0.0
      enddo
      enddo

c     solve trivial problem for solution vector
      do i=1,nac
      yx(i)=yx(i)/a(i,i)
      a(i,i)=1.0
      enddo

      return
      end

c =====================================================================
c =====================================================================


c *** Simple display of matrix ***
c Prints to standard output
      subroutine bm_matshow(a,nr,nc,nrm)
c Data in:
      real a(nrm,*) ! matrix A
      integer nr,nc ! rows and columns of A
      integer nrm ! dimensioned rows of A

c     internal variables
      integer i,j ! array indices

      do i=1,nr
         print *,(a(i,j),j=1,nc)
      enddo

      return
      end

c =====================================================================
c =====================================================================

c *** Simple display of vector components ***
      subroutine bm_vecshow(n,a)

      integer n
      real a(*)

c Internal variables:
      integer i

      print *,(a(i),i=1,n)

      return
      end