c      This program must link to real*8 versions of the following
c      Numerical Recipies (v1992) routines to work properly.

c      SUBROUTINE gaussj(a,n,np,b,m,mp)
c      SUBROUTINE svdcmp(a,m,n,mp,np,w,v)
c      SUBROUTINE svdvar(v,ma,np,w,cvm,ncvm)
c      SUBROUTINE qrdcmp(a,n,np,c,d,sing)
c      SUBROUTINE svbksb(u,w,v,m,n,mp,np,b,x)
c      REAL*8 FUNCTION pythag(a,b)

c***************************************************************

      program cull_points

c****************************************************************
c**
c**   FILE NAME: cull_points.f
c**
c**   DATE WRITTEN: 01/02/95
c**
c**   PROGRAMMER: Scott Hensley
c**
c**   FUNCTIONAL DESCRIPTION: This program will compute statics and a
c**   linear fit of offset data and then cull points based on thresholds
c**   for the SNR, match covariances, or residuals from fits.
c**
c**   ROUTINES CALLED: svdvecfit,QRdecomp
c**
c**   NOTES: none
c**
c**   UPDATE LOG:
c**
c**     Change to warn if too many points in input file
c**
c*****************************************************************

      IMPLICIT NONE

      integer I_NP,I_MP,I_RD,I_FP

c     PARAMETER STATEMENTS:
      parameter (I_MP=10000)
      parameter (I_RD=2)
      parameter (I_FP=4)
      parameter (I_NP=6)

c     INPUT VARIABLES:
      character*80 a_cmdfile,a_tpfile,a_outfile
      character*120 a_input

c     OUTPUT VARIABLES:

c     LOCAL VARIABLES:
      logical sing,l_chisq

      integer i,j
      integer i_ma,i_mm
      integer i_inarg,iargc

      real*8  r_w(I_NP)
      real*8  r_v(I_NP,I_NP)
      real*8  r_u(I_NP,I_NP)

      real*8  r_pi,r_dtor,r_rtod
      real*8  r_vecin(I_FP,I_MP)
      real*8  r_obs(I_RD,I_MP)
      real*8  r_cov(I_RD,I_RD,I_MP)
      real*8  r_chisq(I_RD,0:I_MP)

      real*8  r_a(I_NP)
      real*8  r_at2(I_NP)
      real*8  r_c(I_RD),r_d(I_RD)

      real*8  r_vec(I_RD)
      real*8  r_vec1(I_RD)
      real*8  r_vec2(I_RD)

      real*8  r_q1,r_q2
      real*8  r_c1t,r_c1t2
      real*8  r_c2t,r_c2t2
      real*8  r_uu(I_RD),r_uu2
      real*8  r_rot(I_RD,I_RD)
      real*8  r_aff(I_RD,I_RD)
      real*8  r_c1,r_c2
      real*8  r_skew
      real*8  r_rotang
      real*8  r_scale1,r_scale2
      real*8  r_h1,r_h2

      real*8 r_acloc(I_MP),r_dnloc(I_MP),r_acoff(I_MP),r_dnoff(I_MP)
      real*8 r_snrth,r_snrave,r_snrstd,r_snrmax,r_snrmin,r_snr(I_MP),r_delsnr
      real*8 r_covacth,r_covacave,r_covacstd,r_covacmax,r_covacmin,r_covac(I_MP),r_delcovac
      real*8 r_covdnth,r_covdnave,r_covdnstd,r_covdnmax,r_covdnmin,r_covdn(I_MP),r_delcovdn
      real*8 r_acres(I_MP),r_dnres(I_MP),r_acresth,r_dnresth,r_covx(I_MP)
      real*8 r_height(I_MP),r_height2(I_MP)
      real*8 r_acloc2(I_MP),r_dnloc2(I_MP)
      real*8 r_tmp1,r_tmp2
      integer i_binsnr(10),i_bincovac(10),i_bincovdn(10),i_npts,i_save(I_MP),nptsorg

      integer i_paramest(I_NP),i_usedata(I_RD)
      common/funcom/i_paramest,i_usedata

      integer length
      external length

c     DATA STATEMENTS:
      data r_pi /3.14159265359d0/

C     FUNCTION STATEMENTS:

c     PROCESSING STEPS:

      write(6,*) ' '
      write(6,*) '     << Cull Offset Points Program >>    '
      write(6,*) ' '

      i_inarg = iargc()
      if(i_inarg .eq. 0)then
         write(6,*) 'Usage 1 arguement  : cull_points tp_file '
         write(6,*) 'Usage 2 arguements : cull_points tp_file out_file'
c         stop
         write(6,*) ' '
         write(6,'(x,a,$)') 'Enter tiepoint filename: '
         read(5,'(a)') a_tpfile
         write(6,'(x,a,$)') 'Enter output   filename: '
         read(5,'(a)') a_outfile
      else
        if(i_inarg .eq. 1)then
           call getarg(1,a_tpfile)
           a_outfile=' '
        elseif(i_inarg .eq. 2)then
           call getarg(1,a_tpfile)
           call getarg(2,a_outfile)
        else
           stop 'Too Many Arguments on command line'
        endif
      endif

      r_dtor = r_pi/180.d0
      r_rtod = 180.d0/r_pi

      do i=1,I_NP
         i_paramest(i) = 1
      enddo
      i_ma=I_NP

      do i=1,I_RD
         i_usedata(i)=1
      enddo

      open(10,file=a_tpfile,status='old')

c      open(12,file='out.cul')
      if(a_outfile .ne. ' ' ) open(13,file=a_outfile)

c                                                File I               File II
c     read in two dimensional tie points  (across across offset    down  down offset)

      i_npts = 0
      do while(1 .eq. 1)
         read(10,'(a)',end=999) a_input
         if (a_input .ne. ' ' .and. index(a_input,';') .eq. 0) then
           i_npts = i_npts + 1
           if (i_npts .le. I_MP) then
             read(a_input,*,end=999) r_vecin(2,i_npts),r_vecin(1,i_npts),r_h1,r_tmp1,r_tmp2,r_h2,
     +       r_vecin(3,i_npts),r_vecin(4,i_npts),r_snr(i_npts),r_covac(i_npts),r_covdn(i_npts),r_covx(i_npts)

             r_dnloc(i_npts) = r_vecin(2,i_npts)
             r_acloc(i_npts) = r_vecin(1,i_npts)
             r_height(i_npts) = r_h1
             r_dnloc2(i_npts) = r_tmp1
             r_acloc2(i_npts) = r_tmp2
             r_height2(i_npts) = r_h2
             r_acoff(i_npts) = r_vecin(3,i_npts)
             r_dnoff(i_npts) = r_vecin(4,i_npts)
             r_covac(i_npts) = sqrt(r_covac(i_npts))
             r_covdn(i_npts) = sqrt(r_covdn(i_npts))
             if (r_h1 .eq. 0. .or. r_h2 .eq. 0.) i_npts = i_npts - 1
           end if
         else
           write(13,'(a)') a_input(:max(length(a_input),1))
         endif
      enddo

 999  i_npts = i_npts - 1
      if (i_npts .gt. I_MP-3) then
        write(6,*) '*** Error ***  To many points in input file: ',i_npts, I_MP
        i_npts = I_MP
      end if
      nptsorg = i_npts

      write(6,*) 'Number of tie points = ',i_npts
      write(6,*) ' '

c     compute stats on snr and cov measurements

      r_snrmin = r_snr(1)
      r_snrmax = r_snr(1)
      r_snrave = 0.
      r_snrstd = 0.

      r_covacmin = r_covac(1)
      r_covacmax = r_covac(1)
      r_covacave = 0.
      r_covacstd = 0.

      r_covdnmin = r_covdn(1)
      r_covdnmax = r_covdn(1)
      r_covdnave = 0.
      r_covdnstd = 0.

      do i=1,i_npts

         r_snrmin = min(r_snr(i),r_snrmin)
         r_snrmax = max(r_snr(i),r_snrmax)
         r_snrave = r_snrave + r_snr(i)
         r_snrstd = r_snrstd + r_snr(i)**2

         r_covacmin = min(r_covac(i),r_covacmin)
         r_covacmax = max(r_covac(i),r_covacmax)
         r_covacave = r_covacave + r_covac(i)
         r_covacstd = r_covacstd + r_covac(i)**2

         r_covdnmin = min(r_covdn(i),r_covdnmin)
         r_covdnmax = max(r_covdn(i),r_covdnmax)
         r_covdnave = r_covdnave + r_covdn(i)
         r_covdnstd = r_covdnstd + r_covdn(i)**2

      enddo

      r_snrave = r_snrave/i_npts
      r_snrstd = sqrt(r_snrstd/i_npts - r_snrave**2)
      r_covacave = r_covacave/i_npts
      r_covacstd = sqrt(r_covacstd/i_npts - r_covacave**2)
      r_covdnave = r_covdnave/i_npts
      r_covdnstd = sqrt(r_covdnstd/i_npts - r_covdnave**2)

c     compute histogram of snr's and covariance measurements

      do i=1,10
         i_binsnr(i) = 0
         i_bincovac(i) = 0
         i_bincovdn(i) = 0
      enddo
      i_binsnr(10) = 1
      i_bincovac(10) = 1
      i_bincovdn(10) = 1

      r_delsnr = (r_snrmax - r_snrmin)/10.
      r_delcovac = (r_covacmax - r_covacmin)/10.
      r_delcovdn = (r_covdnmax - r_covdnmin)/10.

      do i=1,i_npts

         do j=1,10

            if(r_snr(i) .ge. r_snrmin + r_delsnr*(j-1) .and.
     +         r_snr(i) .lt. r_snrmin + r_delsnr*j)then
               i_binsnr(j) = i_binsnr(j) + 1
            endif

            if(r_covac(i) .ge. r_covacmin + r_delcovac*(j-1) .and.
     +         r_covac(i) .lt. r_covacmin + r_delcovac*j)then
               i_bincovac(j) = i_bincovac(j) + 1
            endif

            if(r_covdn(i) .ge. r_covdnmin + r_delcovdn*(j-1) .and.
     +         r_covdn(i) .lt. r_covdnmin + r_delcovdn*j)then
               i_bincovdn(j) = i_bincovdn(j) + 1
            endif

         enddo

      enddo

c     write out information to screen and prompt for thresholds

      write(6,*) ' '
      write(6,*) '  Stats Summary '
      write(6,*) ' '

      write(6,260) 'SNR Min,Max Ave ,Std = ',r_snrmin,r_snrmax,r_snrave,r_snrstd
 260  format(x,a,4(f10.5))
      do j=1,10
         write(6,261) r_snrmin + r_delsnr*(j-1),r_snrmin + r_delsnr*j,i_binsnr(j)
 261     format(x,2(f10.5,x),i5)
      enddo

      write(6,*) ' '

      write(6,260) 'Cov Ac  Min,Max Ave ,Std = ',r_covacmin,r_covacmax,r_covacave,r_covacstd
      do j=1,10
         write(6,261) r_covacmin + r_delcovac*(j-1),r_covacmin + r_delcovac*j,i_bincovac(j)
      enddo

      write(6,*) ' '

      write(6,260) 'Cov Dn  Min,Max Ave ,Std = ',r_covdnmin,r_covdnmax,r_covdnave,r_covdnstd
      do j=1,10
         write(6,261) r_covdnmin + r_delcovdn*(j-1),r_covdnmin + r_delcovdn*j,i_bincovdn(j)
      enddo

      write(6,*) ' '
      write(6,263) 'Enter SNR,Cov AC, and Cov DN thresholds ==>'
 263  format(x,a,$)
      read(5,*) r_snrth,r_covacth,r_covdnth

c     Cull data based on thresholds and then due a 2d linear fit - a final culling based
c     on residuals of the fit will then be done

      i_mm = 0
      do i=1,i_npts
         if(r_snr(i) .ge. r_snrth .and. r_covac(i) .le. r_covacth .and.
     +        r_covdn(i) .le. r_covdnth)then
            i_mm = i_mm + 1
            i_save(i_mm) = i
         endif
      enddo

      write(6,*) ' '
      write(6,*) ' Number of points after culling = ',i_mm

c     use the culled points to generate the fit

      do i_npts=1,i_mm

         r_vecin(1,i_npts) = r_acloc(i_save(i_npts))
         r_vecin(2,i_npts) = r_dnloc(i_save(i_npts))
         r_vecin(3,i_npts) = r_acloc(i_save(i_npts))+r_acoff(i_save(i_npts))
         r_vecin(4,i_npts) = r_dnloc(i_save(i_npts))+r_dnoff(i_save(i_npts))

         r_obs(1,i_npts) = -r_acoff(i_save(i_npts))
         r_obs(2,i_npts) = -r_dnoff(i_save(i_npts))

         r_cov(1,1,i_npts) = 1.0
         r_cov(2,2,i_npts) = 1.0
         r_cov(1,2,i_npts) = 0.0
         r_cov(2,1,i_npts) = 0.0

      enddo

c     init affine transformation

      r_a(1) = 1.0
      r_a(2) = 0.0
      r_a(3) = 0.0
      r_a(4) = 1.0
      r_a(5) = 0.0
      r_a(6) = 0.0

c     call svdvecfit to get the estimate for this iteration

      l_chisq = .false.
      call svdvecfit(i_mm,I_RD,I_FP,r_vecin,r_obs,r_cov,
     +                I_NP,r_a,r_at2,r_u,r_v,r_w,r_chisq,l_chisq)

c     Decompose matrix and examine residuals

      r_aff(1,1) = r_a(1)
      r_aff(1,2) = r_a(2)
      r_aff(2,1) = r_a(3)
      r_aff(2,2) = r_a(4)

      write(6,*) ' '
      write(6,*) '      Matrix  Analysis       '
      write(6,*) ' '

      write(6,*) ' Affine Matrix '
      write(6,*) ' '
      write(6,70) r_aff(1,1), r_aff(1,2)
      write(6,70) r_aff(2,1), r_aff(2,2)
 70   format(x,f15.10,x,f15.10)
      write(6,*) ' '
      write(6,*) 'Translation Vector'
      write(6,*) ' '
      write(6,71) r_a(5),r_a(6)
 71   format(x,f11.3,x,f11.3,x)

c     decompose matrix

      call qrdcmp(r_aff,2,2,r_c,r_d,sing)

      r_uu(1) = r_aff(1,1)
      r_uu(2) = r_aff(2,1)

      r_uu2 = .5d0*(r_uu(1)**2  + r_uu(2)**2)

      r_rot(1,1) = (1.d0 - (r_uu(1)**2/r_uu2))
      r_rot(1,2) = -(r_uu(1)*r_uu(2))/r_uu2
      r_rot(2,1) = -(r_uu(1)*r_uu(2))/r_uu2
      r_rot(2,2) = (1.d0 - (r_uu(2)**2/r_uu2))

      if(r_d(1) .lt. 0)then
         r_rot(1,1) = -r_rot(1,1)
         r_rot(2,1) = -r_rot(2,1)
         r_d(1) = -r_d(1)
         r_aff(1,2) = -r_aff(1,2)
      elseif(r_d(2) .lt. 0)then
         r_rot(1,2) = -r_rot(1,2)
         r_rot(2,2) = -r_rot(2,2)
         r_d(2) = -r_d(2)
      endif

      r_scale1 = abs(r_d(1))
      r_scale2 = abs(r_d(2))

      r_skew = r_aff(1,2)/r_d(1)

      r_rotang = atan2(r_rot(2,1),r_rot(1,1))

      write(6,*) ' '
      write(6,*) ' Rotation Matrix '
      write(6,*) ' '
      write(6,70) r_rot(1,1),r_rot(1,2)
      write(6,70) r_rot(2,1),r_rot(2,2)
      write(6,*) ' '
      write(6,*) 'Rotation Angle (deg) = ',r_rotang*r_rtod
      write(6,*) ' '
      write(6,*) ' Axis Scale Factors'
      write(6,*) ' '
      write(6,72) r_scale1,r_scale2
 72   format(x,f11.7,x,f11.7)
      write(6,*) ' '
      write(6,*) ' Skew Term'
      write(6,*) ' '
      write(6,73) r_skew
 73   format(x,f11.7)

c     residual analysis

      write(6,*) ' '
      write(6,*) '      Residual Analysis       '
      write(6,*) ' '

      r_aff(1,1) = r_a(1)
      r_aff(1,2) = r_a(2)
      r_aff(2,1) = r_a(3)
      r_aff(2,2) = r_a(4)

      r_c1t = 0.
      r_c2t = 0.
      r_c1t2 = 0.
      r_c2t2 = 0.
      do i=1,i_mm

         r_c1 = r_vecin(1,i)*r_aff(1,1) + r_vecin(2,i)*r_aff(1,2) + r_a(5)
         r_c2 = r_vecin(1,i)*r_aff(2,1) + r_vecin(2,i)*r_aff(2,2) + r_a(6)
         r_q1 = r_c1 - r_vecin(3,i)
         r_q2 = r_c2 - r_vecin(4,i)
         r_c1t = r_c1t + r_q1
         r_c2t = r_c2t + r_q2
         r_c1t2 = r_c1t2 + r_q1**2
         r_c2t2 = r_c2t2 + r_q2**2
         r_dnres(i_save(i)) = abs(r_q2)
         r_acres(i_save(i)) = abs(r_q1)
c         write(6,*) ' '
c         write(6,*) 'Point = ',i
c         write(6,101) 'Input Points Img1  = ',r_vecin(1,i),r_vecin(2,i)
c         write(6,101) 'Input Points Img2  = ',r_vecin(3,i),r_vecin(4,i)
c         write(6,101) 'Transformed Points = ',r_c1,r_c2
c         write(6,101) 'Residuals          = ',r_q1,r_q2
c 101     format(x,a,x,2(f12.2,x))
         write(6,101)  i, r_vecin(1,i),r_vecin(2,i),r_q1,r_q2
 101     format(x,i5,x,4(f12.2,x))
      enddo

      write(6,*) ' '
      write(6,81) 'Mean error (across,down) = ',r_c1t/i_mm,r_c2t/i_mm
      write(6,81) ' STD  DEV  (across,down) = ',sqrt(r_c1t2/i_mm - (r_c1t/i_mm)**2),
     +                                          sqrt(r_c2t2/i_mm - (r_c2t/i_mm)**2)

 81   format(x,a,x,2(f15.7,x))
c     get thresholds on fit residuals

      write(6,*) ' '
      write(6,263) 'Enter AC and DN residual thresholds ==>'
      read(5,*) r_acresth,r_dnresth

c     Cull data based on thresholds and then due a 2d linear fit - a final culling based
c     on residuals of the fit will then be done

      do i=1,nptsorg
         i_save(i) = 0
      enddo

      i_npts = 0
      do i=1,nptsorg
         if(r_snr(i) .ge. r_snrth .and. r_covac(i) .le. r_covacth .and.
     +        r_covdn(i) .le. r_covdnth .and. r_dnres(i) .le. r_dnresth .and.
     +        r_acres(i) .le. r_acresth)then
            i_npts = i_npts + 1
            i_save(i_npts) = i
         endif
      enddo

      write(6,*) ' '
      write(6,*) ' Number of points after all culling = ',i_npts

c     cull data based on residuals

c        do i=1,i_npts
c          j = i_save(i)
c          write(12,270) r_acloc(j),r_dnloc(j),r_height(j),
c     +                  r_acloc(j)+r_acoff(j),r_dnloc(j)+r_dnoff(j),r_height2(j)
c 270      format(x,6f13.3)
c        enddo
c        close(12)

      if(i_inarg .ge. 2)then

         do i=1,i_npts
            j = i_save(i)
            write(13,269) r_dnloc(j),r_acloc(j),r_height(j),
     &                    r_dnloc2(j),r_acloc2(j),r_height2(j),
     &                    r_acoff(j),r_dnoff(j),r_snr(j),
     &                    r_covac(j)**2,r_covdn(j)**2,r_covx(j)
 269        format(x,2(2f10.2,f10.3),2f8.2,4f8.4)
        enddo

      close(13)
      endif

      end

c ======================================================================
c
        integer function length(a_string)
c
c  This function returns the position of the last none blank character
c                   in the string
c
c  Date  :      6/29/93
c  Author:      Scott Shaffer
c
c  Input:  a_string
c
c  Output: length
c
c ======================================================================
        implicit none

        character*(*) a_string

        integer i_len

          i_len=len(a_string)
          do while(i_len .gt. 0 .and. a_string(i_len:i_len) .eq. ' ')
            i_len=i_len-1
          enddo

          length=i_len
          return
        end

c*****************************************************************************

      subroutine funcs(i_ip,i_rd,i_fp,r_vecin,i_np,r_a,r_amat)

c     funcs is the partial derivatives of a affine matrix fit wrt
c     where the affine matrix is of the form

c             _            _    _    _
c      mat = |   a11  a12   |  |  T1  |     =
c            |_  a21  a22  _|, |_ T2 _|
c

      implicit none

      integer I_RDE
      integer I_NPE
      parameter(I_RDE=2)
      parameter(I_NPE=6)

      integer i,j

      integer i_ip
      integer i_rd
      integer i_fp
      integer i_np

      real*8 r_a(i_np)
      real*8 r_vecin(i_fp)
      real*8 r_amat(i_rd,i_np)

      integer i_paramest(I_NPE),i_usedata(I_RDE)
      common/funcom/i_paramest,i_usedata

      if (i_fp .ne.  4)    stop 'ERROR - i_fp not equal to     4 in funcs'
      if (i_rd .ne. I_RDE) stop 'ERROR - i_rd not equal to I_RDE in funcs'
      if (i_np .ne. I_NPE) stop 'ERROR - i_np not equal to I_NPE in funcs'

      r_amat(1,1) = r_vecin(1)
      r_amat(1,2) = r_vecin(2)
      r_amat(1,3) = 0.
      r_amat(1,4) = 0.
      r_amat(1,5) = 1.
      r_amat(1,6) = 0.

      r_amat(2,1) = 0.
      r_amat(2,2) = 0.
      r_amat(2,3) = r_vecin(1)
      r_amat(2,4) = r_vecin(2)
      r_amat(2,5) = 0.
      r_amat(2,6) = 1.0

c     set to zero the derivatives all parameters not being estimated

      do i=1,i_np
         do j=1,i_rd
            r_amat(j,i) = r_amat(j,i)*i_paramest(i)*i_usedata(j)
         enddo
      enddo

      return

      end

c****************************************************************
      subroutine svdvecfit(i_mp,i_rd,i_fp,r_vecin,r_vobs,r_cov,
     +     i_np,r_a,r_at2,r_u,r_v,r_w,r_chisq,l_chisq)

c****************************************************************
c**
c**   FILE NAME: svdvecfit.f
c**
c**   DATE WRITTEN: 01/02/95
c**
c**   PROGRAMMER: Scott Hensley
c**
c**   FUNCTIONAL DESCRIPTION: This routine does a least squares fit
c**   to a vector valued observation least squares problem.
c**
c**   ROUTINES CALLED: gaussj,svbksb,svdcmp,funcs
c**
c**   NOTES: funcs is a user supplied function giving the jacobian
c**   of the observation parameters wrt to fit parameters. This routine
c**   is a generalization of Numerical Recipes svdfit. Note that this
c**   routine can also be used in a nonlinear least squares procedure
c**   by iterating properly.
c**
c**   Solves the least problem
c**
c**             T   -1     -1     T   -1
c**    A = (AMAT COV  AMAT)  (AMAT COV  )VOBS
c**
c**    where AMAT is the jacobain of the observations vs parameters,
c**    COV is the covriance matrix of observations
c**    and VOBS is the vector of observations.
c**
c**    r_a should be passed in with current best estimate of values
c**
c**   UPDATE LOG:
c**
c**      4/17/95 - Reversed order of r_vecin, r_vobs, and r_cov    SJS
c**                   revmoved r_vt, cleaned up parameter list
c**
c*****************************************************************

      implicit none

c     PARAMETERS:
      integer I_NPE                  !number of parameters to estimate = i_np
      integer I_RDE                  !number of observations per point = i_rd
      real*8 R_TOL,R_LAMBDA
      parameter(I_NPE=6)
      parameter(I_RDE=2)
      parameter(R_TOL=1.0d-20)
      parameter (R_LAMBDA=1.d0)

c     INPUT VARIABLES:
      integer i_mp                   !number of input points
      integer i_rd                   !number of observations each point
      integer i_fp                   !number of input parameters to func
      integer i_np                   !number of parameters to solve for

      real*8  r_vecin(i_fp,i_mp) 	 !vector values for func
      real*8  r_vobs(i_rd,i_mp) 	 !vector of observations
      real*8  r_cov(i_rd,i_rd,i_mp)  !covariance matrix of observation
      real*8  r_chisq(i_rd,0:i_mp) 	 !chisq for solution and fit vs observation
      real*8  r_a(i_np)   			 !solution to least squares
                                     !for each point
      logical l_chisq                !evaluate the chisq for this fit

c     OUTPUT VARIABLES:
      real*8 r_at2(i_np)             !delta to add to previous solution
      real*8 r_u(i_np,i_np)          !svd matrix, orthogonal matrix
      real*8 r_v(i_np,i_np)          !svd matrix, orthogonal matrix
      real*8 r_w(i_np)               !svd matrix, diagonal matrix

c     LOCAL VARIABLES:
      integer i,j,k,i_pts
      real*8  r_covtemp(I_RDE,I_RDE)
      real*8  r_am(I_NPE,I_RDE)
      real*8  r_amat(I_RDE,I_NPE)
      real*8  r_ptot(I_NPE)
      real*8  r_wmax,r_thres,r_b(I_RDE,1),r_chird(I_RDE)

      integer i_paramest(I_NPE),i_usedata(I_RDE)
      common/funcom/i_paramest,i_usedata

c     DATA STATEMENTS:

C     FUNCTION STATEMENTS:

c     PROCESSING STEPS:

c     init some arrays

c      type*, ' '
c      type*, 'Inside SVDVECFIT'
c      type*, ' '

      if (i_rd .ne. I_RDE) stop 'ERROR - i_rd not equal to I_RDE in SVDVECFIT'
      if (i_np .ne. I_NPE) stop 'ERROR - i_np not equal to I_NPE in SVDVECFIT'

      do i=1,i_np
         do j=1,i_np
            r_u(i,j) = 0.0
         enddo
         r_ptot(i) = 0.0
      enddo

c     loop over the input points

      do i_pts=1,i_mp

c         type*, 'i_pts = ',i_pts

c     invert the covariance matrix of the observation

         do i=1,i_rd
            do j=1,i_rd
               r_covtemp(i,j) = r_cov(i,j,i_pts)
            enddo
         enddo

         call gaussj(r_covtemp,i_rd,i_rd,r_b,1,1)

c     get the required jacobian matrix

         call funcs(i_pts,i_rd,i_fp,r_vecin(1,i_pts),i_np,r_a,r_amat)

c         do i=1,i_rd
c            do j=1,i_np
c               type*, 'i,j,r_amat = ',i,j,r_amat(i,j)
c            enddo
c         enddo

c     multiply amat transpose by the inverse cov matrix

         do i=1,i_np
            do j=1,i_rd
               r_am(i,j) = 0.0
               do k=1,i_rd
                  r_am(i,j) = r_am(i,j) + r_amat(k,i)*r_covtemp(k,j)
               enddo
            enddo
         enddo

c         do i=1,i_np
c            do j=1,i_rd
c               type*, 'i,j,r_am = ',i,j,r_am(i,j)
c            enddo
c         enddo

c     multiply am by amat

         do i=1,i_np
            do j=1,i_np
               do k=1,i_rd
                  r_u(i,j) = r_u(i,j) + r_am(i,k)*r_amat(k,j)
               enddo
            enddo
         enddo

c     multilpy am by vobs

c         type*, 'r_vobs,i_pts = ',i_pts,r_vobs(1,i_pts),r_vobs(2,i_pts)
         do i=1,i_np
            do k=1,i_rd
               r_ptot(i) = r_ptot(i) + r_am(i,k)*r_vobs(k,i_pts)
            enddo
         enddo

      enddo   !i_pts

c     find the SVD of the r_u matrix

c         do i=1,i_np
c            do j=1,i_np
c               type*, 'i,j,r_u = ',i,j,r_u(i,j)
c            enddo
c         enddo

      call svdcmp(r_u,i_np,i_np,i_np,i_np,r_w,r_v)

c         do i=1,i_np
c            do j=1,i_np
c               type*, 'i,j,r_u,r_v = ',i,j,r_u(i,j),r_v(i,j)
c            enddo
c         enddo

c         do i=1,i_np
c            type*, 'w = ',i,r_w(i)
c         enddo

c     kill off all the singular values

      r_wmax = 0.0
      do i=1,i_np
         if(r_w(i) .gt. r_wmax)then
            r_wmax = r_w(i)
         endif
      enddo
      r_thres = r_wmax*R_TOL
c      type*, 'r_thres = ',r_thres

      do i=1,i_np
         if(r_w(i) .lt. r_thres)then
            r_w(i) = 0.0
         endif
      enddo

c      do i=1,i_np
c         type*, 'w = ',i,r_w(i)
c      enddo

c     use the svbksb routine to solve for the desired parameters

      call svbksb(r_u,r_w,r_v,i_np,i_np,i_np,i_np,r_ptot,r_at2)

c     update the r_a vector

      do i=1,i_np
         r_at2(i) = -r_at2(i)*i_paramest(i)
         r_a(i) = r_at2(i)/R_LAMBDA + r_a(i)
c         type *,'a=',i,r_a(i),r_at2(i)
      enddo

c     evaluate the chisq array (linearized version)

      if(l_chisq)then

c     loop over data points

         do i=1,i_rd
            r_chird(i) = 0.
         enddo
         r_chisq(1,0) = 0.0
         do i=1,i_mp

            call funcs(i,i_rd,i_fp,r_vecin(1,i),i_np,r_a,r_amat)

            do j=1,i_rd
               r_chisq(j,i) = 0.0
               do k=1,i_np
                  r_chisq(j,i) = r_chisq(j,i) + r_amat(j,k)*r_at2(k)
               enddo
c               type*, 'r_chisq = ',i,j,r_chisq(j,i),r_vobs(j,i)
               r_chisq(j,i) = r_covtemp(j,j)*(r_chisq(j,i) -
     +              r_vobs(j,i))**2
               r_chisq(1,0) = r_chisq(1,0) + r_chisq(j,i)
               r_chird(j) = r_chird(j) + r_chisq(j,i)
            enddo

         enddo                  !i_pts loop for chisq

         r_chisq(1,0) = sqrt(r_chisq(1,0)/(2.*i_mp))
         print*, 'r_chisq = ',r_chisq(1,0),sqrt(r_chird(1)/i_mp),sqrt(r_chird(2)/i_mp)

      endif

      end

c      SUBROUTINE gaussj(a,n,np,b,m,mp)
c      SUBROUTINE svdcmp(a,m,n,mp,np,w,v)
c      SUBROUTINE svdvar(v,ma,np,w,cvm,ncvm)
c      SUBROUTINE qrdcmp(a,n,np,c,d,sing)
c      SUBROUTINE svbksb(u,w,v,m,n,mp,np,b,x)
c      REAL*8 FUNCTION pythag(a,b)


C++**************************************************************
C	PLAGAL
C
C Removes the galaxy in a CCD image
C Simple geometry : concentric ellipses with
C fixed or variable ellipticity and a fixed orientation.
C Removes a profile, a smoothed profile or an r**1/4 law
C
C IMAGE(NX,NY) declared as (IDIM,IDIM)
C
C JLP
C Version of 27/08/87
C--**************************************************************
	PROGRAM PLAGAL
	PARAMETER (IDIM=600,IDIM1=2000)
	REAL*4 INPUT(IDIM,IDIM),OUTPUT(IDIM,IDIM)
	REAL*8 VRAD,VPRO,WORK
	REAL*4 RAD2,AXRATIO2
	CHARACTER FICH3*40,NAMEIN*40,NAMEOUT*40,ANS*1,
     1	BUFFER*80,COMMENTS*80
	LOGICAL FIXED_ELLI
	COMMON/PLG1_PARAM/RADMIN,RADMAX,X0,Y0,SCALE,SKY,VPRO(IDIM1),
     1	VRAD(IDIM1),NPTS,RAD2(100),AXRATIO2(100),NPOINT2
	COMMON/FIXELLIP/UU,VV,WW,AXRATIO,CO,SI

10	FORMAT(A)

C Open 10 which is used in POLYFIT
	OPEN(10,FILE='plagal.tmp',STATUS='unknown')
	OPEN(9,FILE='plagal.log',STATUS='unknown',
     1	ACCESS='SEQUENTIAL')
	WRITE(9,81)
	PRINT 81
81	FORMAT(' PROGRAM PLAGAL - Version of 27/08/87-',/)

C Inquires about the format of the files :
	CALL JLP_INQUIFMT

C Input of the image :
	NAMEIN=' '
	CALL JLP_READIMAG(INPUT,NX,NY,IDIM,NAMEIN,COMMENTS)
	WRITE(9,82)NAMEIN
82	FORMAT(' INPUT IMAGE : ',A)

	PRINT *,' OUTPUT FILE ?'
	READ(5,10) NAMEOUT

	PRINT 89
89	FORMAT(' Program PLAGAL to remove the galaxy background',/,
     1	' Possibility of subtracting:',/,
     1	' 1: a profile smoothed in this program',/,
     1	' 2: a profile not smoothed',/,
     1	' 3: an r 1/4 law',/,
     1	' Enter the option: ',$)
	READ(5,*) IOP
	WRITE (9,89)
	WRITE (9,83) IOP
83	FORMAT(' OPTION = ',I2)

C Parameters of the ellipses :
	PRINT *,'Scale in arcseconds/pixel ?'
	READ(5,*) SCALE
	PRINT *,' Position angle of the major axis in degrees'
	READ(5,*) XPHI
    	CO=COS(XPHI*3.14159/180.)
	SI=SIN(XPHI*3.14159/180.)
	PRINT *,' Centre of the ellipses: X0,Y0 '
	READ(5,*) X0,Y0
	WRITE(9,84)SCALE,XPHI,X0,Y0
84	FORMAT(' Scale : ',F12.4,' Position angle :',F10.4,/,
     1	' CENTRE : OX,OY ',F10.3,2X,F10.3)

C---------------------------------------------------------------------
C Possibility of variable ellipticity

	PRINT 86
	WRITE(9,86)
86	FORMAT(' Do you want a fixed ellipticity? (Y) ',$)
	READ(5,10) ANS
	WRITE(9,10)ANS
	IF(ANS.NE.'N'.AND.ANS.NE.'n')THEN
	  FIXED_ELLI=.TRUE.
	  PRINT *,' Axis ratio b/a :'
	  READ(5,*) AXRATIO
	  WRITE(9,70)AXRATIO
70	  FORMAT(' Axis ratio :',F12.3)
	ELSE
	  FIXED_ELLI=.FALSE.
	  PRINT 71
71	  FORMAT(' Enter upper equiv. radius in arcsec and',
     1	' axis ratio b/a :',/,
     1	' (To exit type 0.,0.)')
	  I=1
	  READ(5,*) RAD2(I),AXRATIO2(I)
94	    IF(RAD2(I).EQ.0..AND.AXRATIO2(I).EQ.0.) GOTO 95
	      I=I+1
	      READ(5,*) RAD2(I),AXRATIO2(I)
            GOTO 94
95	  NPOINT2=I-1

C Getting the first guess for AXRATIO : (Mean of the axis ratios)
	  AXRATIO=0.
	    DO I=1,NPOINT2
	    WRITE(9,72)I,RAD2(I),AXRATIO2(I)
72	    FORMAT(' Point number : ',I2,' Radius and axis ratio :',
     1	F9.2,1X,F8.4)
	    AXRATIO=AXRATIO2(I)+AXRATIO
	    END DO
	  AXRATIO=AXRATIO/NPOINT2
	ENDIF

C Computing the first guess for UU,VV,WW : (Passed through COMMON/FIXELLIP)
	UU=CO*CO+(SI/AXRATIO)**2
	VV=SI*SI+(CO/AXRATIO)**2
	WW=2.*SI*CO*(1.-1./(AXRATIO*AXRATIO))
C-----------------------------------------------------------------

	IF (IOP.EQ.1.OR.IOP.EQ.2) THEN
8	  PRINT *,' Input profile :'
	  READ(5,10) FICH3
	  OPEN(3,FILE=FICH3,STATUS='OLD',ERR=8)
	  PRINT *,' Is it a profile produced by "PROFILE1" ?(Y)'
	  READ(5,10) ANS
C If new format reads 32 lines of comments first:
	  IF(ANS.NE.'N'.AND.ANS.NE.'n')THEN
	    DO I=1,32
	    READ(3,10)BUFFER
	   END DO
	  ENDIF
	  READ(3,*)NPTS
	  PRINT *,' NPTS =',NPTS
	    DO I=1,NPTS
	    READ(3,*) VRAD(I),VPRO(I),WORK
	    END DO
	  WRITE(9,73)FICH3,NPTS
73	  FORMAT(' Input profile :',A,' NPTS =',I6)
	CLOSE(3)
	ENDIF

C*************************************************************************
C Option 1 :
C*************************************************************************
	IF(IOP.EQ.1)THEN

101	  PRINT *,' Minimum and maximum radii for the fit ? (arcsec)'
	  READ(5,*) RADMIN,RADMAX
	  PRINT *,' Order of the polynomial ?'
	  READ(5,*) KORDER
	  PRINT *,' Sky level (approximative lower value',
     1	' for the fit) :'
	  READ(5,*) SKY

	  CALL PLAGAL1(INPUT,OUTPUT,NX,NY,IDIM,FIXED_ELLI,KORDER)

	ENDIF

C**************************************************************************
C Option 2 :

	IF(IOP.EQ.2)THEN

	RADMIN=VRAD(1)
	RADMAX=VRAD(NPTS-1)

	DO J=1,NY
	  Y1=FLOAT(J)-Y0
	 DO I=1,NX
	  X1=FLOAT(I)-X0
C F1: equivalent radius in arcsec of the ellipse containing the pixel (x,y)
	  CALL EQUIV_RADIUS(X1,Y1,F1,SCALE,
     1	RAD2,AXRATIO2,NPOINT2,FIXED_ELLI)
	    IF(F1.GT.RADMIN.AND.F1.LT.RADMAX
     1	.AND.INPUT(I,J).NE.0.)THEN
	      WORK=F1
	      CALL INDEX_MAX8(VRAD,NPTS,WORK,INDEX1)
	      WORK=(VPRO(INDEX1+1)-VPRO(INDEX1))/
     1	(VRAD(INDEX1+1)-VRAD(INDEX1))
	      VALUE=VPRO(INDEX1)+(F1-VRAD(INDEX1))*WORK
	      OUTPUT(I,J)=INPUT(I,J)-VALUE
	    ELSE
	      OUTPUT(I,J)=0.
	    ENDIF
	 END DO
	END DO
	ENDIF

C**************************************************************************
C Option 3 :
C R 1/4 law :
C**************************************************************************

	IF(IOP.EQ.3)THEN

C Entry of the parameters :
	  PRINT *,' Sky level:'
	  READ(5,*) SKY

	  PRINT *,' Zero point for the magnitudes:'
	  READ(5,*) CMAG0
	  PRINT 309
309	  FORMAT(' -2.5*LOG10(I-SKY)= Be - CMAG0 +',
     1	' 8.325 ((R/Re)**1/4 - 1)')

	  PRINT *,' Parameters for r 1/4 law:',
     1	' Be (mag), Re (arcsec)'
	  READ(5,*) VM0,REQUI
	  ALPHA=-3.33/(REQUI**0.25)
	  BETA=((VM0-CMAG0)/-2.5)+3.33

	  PRINT *,' Radmin (arcsec)?'
	  READ(5,*) RADMIN

C Main loop :

	DO J=1,NY
	  Y1=FLOAT(J)-Y0
	 DO I=1,NX
	  X1=FLOAT(I)-X0
C F1: equivalent radius in arcsec of the ellipse containing the pixel (x,y)
	  IF(INPUT(I,J).NE.0.)THEN
	   CALL EQUIV_RADIUS(X1,Y1,F1,SCALE,
     1	RAD2,AXRATIO2,NPOINT2,FIXED_ELLI)
	    IF(F1.GT.RADMIN)THEN
	      F2=SQRT(SQRT(F1))
	      F3=10**(ALPHA*F2+BETA)
	      OUTPUT(I,J)=INPUT(I,J)-F3-SKY
	    ELSE
	      OUTPUT(I,J)=0.
	    ENDIF
	  ELSE
	   OUTPUT(I,J)=0.
	  ENDIF
	 END DO
	END DO

	ENDIF

C**** Writing the output file with comments **************
	LNAMEIN=MIN(20,INDEX(NAMEIN,' '))
	LNAMEIN=MAX(1,LNAMEIN)
	LFICH3=MIN(20,INDEX(FICH3,' '))
	LFICH3=MAX(1,LFICH3)
	IF(IOP.EQ.1)
     1	WRITE(COMMENTS,303) NAMEIN(1:LNAMEIN),FICH3(1:LFICH3)
303	FORMAT(' PLAGAL/',A,' with a smoothed profile: ',A,'//')
	IF(IOP.EQ.2)
     1	WRITE(COMMENTS,304) NAMEIN(1:LNAMEIN),FICH3(1:LFICH3)
304	FORMAT(' PLAGAL/',A,' with a profile not smoothed: ',A,'//')
	IF(IOP.EQ.3)
     1	WRITE(COMMENTS,305) NAMEIN(1:LNAMEIN)
305	FORMAT(' PLAGAL/',A,' with an r 1/4 law//')
	CALL JLP_WRITEIMAG(OUTPUT,NX,NY,IDIM,NAMEOUT,COMMENTS)

	CLOSE(9)
	CLOSE(10)
	PRINT *,' Logfile in "plagal.log"'
	STOP
	END
C*************************************************************
	include 'jlpsub:plagal_set.for'
	include 'jlpsub:polyfit.for'


      SUBROUTINE MB01RW( UPLO, TRANS, M, N, A, LDA, Z, LDZ, DWORK,
     $                   INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the transformation of the symmetric matrix A by the
C     matrix Z in the form
C
C        A := op(Z)*A*op(Z)',
C
C     where op(Z) is either Z or its transpose, Z'.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     UPLO    CHARACTER*1
C             Specifies whether the upper or lower triangle of A
C             is stored:
C             = 'U':  Upper triangle of A is stored;
C             = 'L':  Lower triangle of A is stored.
C
C     TRANS   CHARACTER*1
C             Specifies whether op(Z) is Z or its transpose Z':
C             = 'N':  op(Z) = Z;
C             = 'T':  op(Z) = Z'.
C
C     Input/Output Parameters
C
C     M       (input) INTEGER
C             The order of the resulting symmetric matrix op(Z)*A*op(Z)'
C             and the number of rows of the matrix Z, if TRANS = 'N',
C             or the number of columns of the matrix Z, if TRANS = 'T'.
C             M >= 0.
C
C     N       (input) INTEGER
C             The order of the symmetric matrix A and the number of
C             columns of the matrix Z, if TRANS = 'N', or the number of
C             rows of the matrix Z, if TRANS = 'T'.  N >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension
C             (LDA,MAX(M,N))
C             On entry, the leading N-by-N upper or lower triangular
C             part of this array must contain the upper (UPLO = 'U')
C             or lower (UPLO = 'L') triangular part of the symmetric
C             matrix A.
C             On exit, the leading M-by-M upper or lower triangular
C             part of this array contains the upper (UPLO = 'U') or
C             lower (UPLO = 'L') triangular part of the symmetric
C             matrix op(Z)*A*op(Z)'.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,M,N).
C
C     Z       (input) DOUBLE PRECISION array, dimension (LDQ,K)
C             where K = N if TRANS = 'N' and K = M if TRANS = 'T'.
C             The leading M-by-N part, if TRANS = 'N', or N-by-M part,
C             if TRANS = 'T', of this array contains the matrix Z.
C
C     LDZ     INTEGER
C             The leading dimension of the array Z.
C             LDZ >= MAX(1,M) if TRANS = 'N' and
C             LDZ >= MAX(1,N) if TRANS = 'T'.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     FURTHER COMMENTS
C
C     This is a simpler, BLAS 2 version for MB01RD.
C
C     CONTRIBUTOR
C
C     A. Varga, DLR, Feb. 1995.
C
C     REVISIONS
C
C     April 1998 (T. Penzl).
C     Sep. 1998 (V. Sima).
C
C    ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         TRANS, UPLO
      INTEGER           INFO, LDA, LDZ, M, N
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), DWORK(*), Z(LDZ,*)
C     .. Local Scalars ..
      LOGICAL           NOTTRA, UPPER
      INTEGER           I, J
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DGEMV, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX
C
C     .. Executable Statements
C
      NOTTRA = LSAME( TRANS, 'N' )
      UPPER  = LSAME( UPLO,  'U' )
C
      INFO = 0
      IF( .NOT.( UPPER .OR. LSAME( UPLO, 'L') ) ) THEN
         INFO = -1
      ELSE IF( .NOT.( NOTTRA .OR. LSAME( TRANS, 'T') ) ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, M, N ) ) THEN
         INFO = -6
      ELSE IF( (      NOTTRA .AND. LDZ.LT.MAX( 1, M ) )   .OR.
     $         ( .NOT.NOTTRA .AND. LDZ.LT.MAX( 1, N ) ) ) THEN
         INFO = -8
      END IF
C
      IF ( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB01RW', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 )
     $   RETURN
C
      IF ( NOTTRA ) THEN
C
C        Compute Z*A*Z'.
C
         IF ( UPPER ) THEN
C
C           Compute Z*A in A (M-by-N).
C
            DO 10 J = 1, N
               CALL DCOPY( J-1, A(1,J), 1, DWORK, 1 )
               CALL DCOPY( N-J+1, A(J,J), LDA, DWORK(J), 1 )
               CALL DGEMV( TRANS, M, N, ONE, Z, LDZ, DWORK, 1, ZERO,
     $                     A(1,J), 1 )
   10       CONTINUE
C
C           Compute A*Z' in the upper triangular part of A.
C
            DO 20 I = 1, M
               CALL DCOPY( N, A(I,1), LDA, DWORK, 1 )
               CALL DGEMV( TRANS, M-I+1, N, ONE, Z(I,1), LDZ, DWORK, 1,
     $                     ZERO, A(I,I), LDA )
   20       CONTINUE
C
         ELSE
C
C           Compute A*Z' in A (N-by-M).
C
            DO 30 I = 1, N
               CALL DCOPY( I-1, A(I,1), LDA, DWORK, 1 )
               CALL DCOPY( N-I+1, A(I,I), 1, DWORK(I), 1 )
               CALL DGEMV( TRANS, M, N, ONE, Z, LDZ, DWORK, 1, ZERO,
     $                     A(I,1), LDA )
   30       CONTINUE
C
C           Compute Z*A in the lower triangular part of A.
C
            DO 40 J = 1, M
               CALL DCOPY( N, A(1,J), 1, DWORK, 1 )
               CALL DGEMV( TRANS, M-J+1, N, ONE, Z(J,1), LDZ, DWORK, 1,
     $                     ZERO, A(J,J), 1 )
   40       CONTINUE
C
         END IF
      ELSE
C
C        Compute Z'*A*Z.
C
         IF ( UPPER ) THEN
C
C           Compute Z'*A in A (M-by-N).
C
            DO 50 J = 1, N
               CALL DCOPY( J-1, A(1,J), 1, DWORK, 1 )
               CALL DCOPY( N-J+1, A(J,J), LDA, DWORK(J), 1 )
               CALL DGEMV( TRANS, N, M, ONE, Z, LDZ, DWORK, 1, ZERO,
     $                     A(1,J), 1 )
   50       CONTINUE
C
C           Compute A*Z in the upper triangular part of A.
C
            DO 60 I = 1, M
               CALL DCOPY( N, A(I,1), LDA, DWORK, 1 )
               CALL DGEMV( TRANS, N, M-I+1, ONE, Z(1,I), LDZ, DWORK, 1,
     $                     ZERO, A(I,I), LDA )
   60       CONTINUE
C
         ELSE
C
C           Compute A*Z in A (N-by-M).
C
            DO 70 I = 1, N
               CALL DCOPY( I-1, A(I,1), LDA, DWORK, 1 )
               CALL DCOPY( N-I+1, A(I,I), 1, DWORK(I), 1 )
               CALL DGEMV( TRANS, N, M, ONE, Z, LDZ, DWORK, 1, ZERO,
     $                     A(I,1), LDA )
   70       CONTINUE
C
C           Compute Z'*A in the lower triangular part of A.
C
            DO 80 J = 1, M
               CALL DCOPY( N, A(1,J), 1, DWORK, 1 )
               CALL DGEMV( TRANS, N, M-J+1, ONE, Z(1,J), LDZ, DWORK, 1,
     $                     ZERO, A(J,J), 1 )
   80       CONTINUE
C
         END IF
      END IF
C
      RETURN
C *** Last line of MB01RW ***
      END


c> @file
c> Clawpatch utilities

c     ---------------------------------------------------------------
c>    @brief Determines the tranformation from a coarse patch to it's
c>    half-sized neighbor's coordinate system
c>
c>    @param[in]  the pointer to the fclaw2d_patch_transform_data struct
c>    @param[out] a the 2x2 tranform matrix for the i,j indexes of a patch
c>    @param[out] f the transform vector
c     ---------------------------------------------------------------
      subroutine fclaw3dx_clawpatch_build_transform(transform_ptr,a,f)
      implicit none

      integer a(2,2)
      integer*8 transform_ptr
      integer f(2)
      integer mi(4),mj(4)
      integer i1,j1

c     # Assume index mapping fclaw2d_transform_face_half has the
c     # the form
c     #
c     #       T(ic,jc) = A*(ic,jc) + F = (iff,jff)
c     #
c     # where (ic,jc) is the coarse grid index, and (iff,jff)
c     # is the first fine grid index.
c     #
c     # We can recover coefficients A(2,2) with the following
c     # calls to T.

      i1 = 0
      j1 = 0
      call fclaw3dx_clawpatch_transform_face_half(i1,j1,mi,mj,
     &      transform_ptr)
      f(1) = mi(1)
      f(2) = mj(1)

      i1 = 1
      j1 = 0
      call fclaw3dx_clawpatch_transform_face_half(i1,j1,mi,mj,
     &      transform_ptr)
      a(1,1) = mi(1) - f(1)
      a(2,1) = mj(1) - f(2)

      i1 = 0
      j1 = 1
      call fclaw3dx_clawpatch_transform_face_half(i1,j1,mi,mj,
     &      transform_ptr)
      a(1,2) = mi(1) - f(1)
      a(2,2) = mj(1) - f(2)

      end


C  Copyright (C) 2007: Leslie Greengard
C  Contact: greengard@cims.nyu.edu
C
C  All rights reserved.
C  This copy is not for distribution without permission of the author.
C
C**********************************************************************C
C     Miscelllaneous utility functions for spherical harmonic
C     expansions.
C
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      subroutine unrolltoshxp(nterms,sol,sh1)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c     This subroutine converts an "unrolled" spherical harmonic
c     expansion to a doubly indexed array where (i,j) refers to
c     the Y_{i,j} coefficient.
c
c     INPUT:
c             sol  (unrolled)
c     OUTPUT:
c             sh1  (matrix format)
c
      implicit real *8 (a-h,o-z)
      complex *16 sol(1)
      complex *16 sh1(0:nterms,-nterms:nterms)
c
      next = 1
      do i = 0,nterms
         do j = -i,i
            sh1(i,j) = sol(next)
	    next = next+1
         enddo
      enddo
      return
      end
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      subroutine shxptounroll(nterms,sol,sh1)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c     This subroutine converts a double indexed spherical harmonic
c     expansion to an "unrolled" one.
c
c     INPUT:
c              sh1  (matrix format)
c     OUTPUT:
c              sol  (unrolled)
c
      implicit real *8 (a-h,o-z)
      complex *16 sol(1)
      complex *16 sh1(0:nterms,-nterms:nterms)
c
      next = 1
      do i = 0,nterms
         do j = -i,i
            sol(next) = sh1(i,j)
	    next = next+1
         enddo
      enddo
      return
      end
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      subroutine sourcetoshxp(nterms,x0y0z0,source,
     1          str,zk0,sphererad,scale,pot,dpotdn,wrk,lw,ier)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit real *8 (a-h,o-z)
c
c------------------------------------------------------------------
c     WARNING!!!!  f77 static arrays set to length 1000. Thus,
c     expansion lengths limited. => nterms < 900 or so.
c
c------------------------------------------------------------------
c     utility function:
c
c     takes source at (source(1), source(2), source(3)) of strength str
c     and frequency zk0
c     and computes induced potential and outward normal derivative
c     of induced potential ON sphere centered at x0y0z0 of
c     radius sphererad. These functions are tabulated as spherical
c     harmonic expansions.
c
c     zk0 is Helmholtz parameter.
c
c     Method:
c
c     If source is inside sphere, H expansion is generated.
c     If source is outside sphere, J expansion is generated.
c
c
c     INPUT:
c
c     nterms        length of expansion
c     x0y0z0        sphere center
c     source        source location
c     str           source strength
c     zk0           Helmholtz parameter
c     sphererad     sphere radius
c     scale         expansion scaling parameter
c     wrk           workspace
c     lw            length of workspace
c                   at least (nterms+1)**2 + 6*nterms + 10000
c
c     OUTPUT:
c
c     pot           spherical harmonic expansion of potential phi
c     dpotdn        spherical harmonic expansion of dphi/dn
c     ier           error rreturn code
c
c
      integer *4 iw(0:1000)
      integer *4 lw
      dimension source(3)
      dimension x0y0z0(3)
      complex *16 pot(0:nterms,-nterms:nterms)
      complex *16 dpotdn(0:nterms,-nterms:nterms)
      complex *16 z,zk0,wronsk,str
      complex *16 htemp(0:1000),hptemp(0:1000)
      complex *16 wrk(1)
c
      pi = 4*datan(1.0d0)
cc      call prin2(' in sourcetoshxp source is *',source,3)
cc      call prin2(' in sourcetoshxp x0y0z0 is *',x0y0z0,3)
      dx = source(1)-x0y0z0(1)
      dy = source(2)-x0y0z0(2)
      dz = source(3)-x0y0z0(3)
      r = dsqrt(dx*dx+dy*dy+dz*dz)
ccc      call prin2(' in sourcetoshxp r is *',r,1)
ccc      call prin2(' in sourcetoshxp sphererad is *',sphererad,1)
      ns = 1
      ifder = 1
      if (r.lt.sphererad) then
	 call h3dformmp(ier,zk0,scale,source,str,ns,x0y0z0,nterms,
     1        pot,wrk,lw,lused)
         z = zk0*sphererad
         call h3dall(nterms,z,scale,htemp,ifder,hptemp)
ccc	 call prin2(' in sourcetoshxp htemp is *',htemp,2*nterms+2)
         do n = 0,nterms
            do m = -n,n
	       dpotdn(n,m) = pot(n,m)*zk0*hptemp(n)
	       pot(n,m) = pot(n,m)*htemp(n)
            enddo
         enddo
      else
	 call h3dformta(ier,zk0,scale,source,str,ns,x0y0z0,nterms,
     1        pot,wrk,lw,lused)
         z = zk0*sphererad
	 ljtemp = 1000
         call jfuns3d(ier,nterms,z,scale,htemp,ifder,hptemp,
     1        ljtemp,iw,ntop)
         do n = 0,nterms
            do m = -n,n
	       dpotdn(n,m) = pot(n,m)*zk0*hptemp(n)
	       pot(n,m) = pot(n,m)*htemp(n)
            enddo
         enddo
      endif
      return
      end
c


! ===================================================================
!
!                      Saturation_Curve
!
! ===================================================================
! This subroutine create a grid in the 2-phase region by discretizing P_sat
! from P_tri to P_crit to obtain a table of the saturation properties:
!
!                 P_sat, V_v, V_l, e_v, e_l, T_sat
!
! The spline coefficients are also computed, so for given values of
! saturation pressure, coefficients of spline are ble to express
!                   u(p_sat), v(p_sat) , T(p_sat)
!
! ===================================================================
MODULE saturation
!
     USE def_constants, ONLY: pr, ord_spline, P_cr,P_tri, NNN_TP, NNN_sat_TP,&
&                             rho_tri_L, rho_tri_R, T_tri,e_cr,NNN_TP2, NNN_sat_TP2,&
&                             rho_cr, T_cr,e_tri_L,e_tri_R
     USE def_variables, ONLY: saturP, saturP_sat, vL_psat_spline, vV_psat_spline,&
&                             uL_psat_spline, uV_psat_spline, Tsat_psat_spline,  &
&                             saturP2, saturP_sat2, vL_psat_spline2, vV_psat_spline2,&
&                             uL_psat_spline2, uV_psat_spline2, Tsat_psat_spline2
     USE non_linear_solvers, ONLY: New_Rap3D
     USE properties, ONLY: inter_energy
     USE grid_functions, ONLY: polyfit

     PRIVATE
     PUBLIC  saturation_curve, sat_curve
     CONTAINS
!========================================================================
!
     SUBROUTINE saturation_curve()
!
!========================================================================
        IMPLICIT NONE
!
        INTEGER  :: i, j, exitflag,Niter
!
        REAL(pr)  :: delta, delta_sat,v_v,v_l,Tsat,u_l,u_v,&
 &                   resnorm, guess1,guess2,guess3,in_2
!
        REAL(pr), DIMENSION (NNN_sat_TP) :: vL, vV, uL, uV,saturT
!
!
!
        delta      = (P_cr - P_tri) / (NNN_TP-1)
        saturP     = P_tri + (/(i*delta, i=0, NNN_TP-1)/)
        delta_sat  = (P_cr - P_tri) / (NNN_sat_TP-1)
        saturP_sat = P_tri + (/(i*delta_sat, i=0,NNN_sat_TP-1)/)
!
! points on saturation curve
!
        guess1 = 1_pr/rho_tri_L
        guess2 = 1_pr/rho_tri_R
        guess3 = T_tri
!
        DO i = 1, NNN_sat_TP-1

         CALL New_Rap3D(3,v_l , v_v, Tsat, &
     &   resnorm, Niter, exitflag, saturP_sat(i),in_2, guess1, guess2,guess3)

        IF (resnorm > 1e-7_pr) THEN
        print*, "saturation curve", resnorm, i
        STOP
        ENDIF
!
!        print*, v_l,v_v
!        print*, u_l,u_v
!        print*," "
         CALL inter_energy(Tsat,v_v,u_v)
         CALL inter_energy(Tsat,v_l,u_l)
           vL(i)     = v_l
           vV(i)     = v_v
           uL(i)     = u_l
           uV(i)     = u_v
           saturT(i) = Tsat
!
           guess1 = v_l
           guess2 = v_v
           guess3 = Tsat
        ENDDO
!
        vL(NNN_sat_TP)     = 1_pr/rho_cr
        vV(NNN_sat_TP)     = 1_pr/rho_cr
        uL(NNN_sat_TP)     = e_cr
        uV(NNN_sat_TP)     = e_cr
        saturT(NNN_sat_TP) = T_cr
! intervals on saturation curve
!
        DO i = 1, NNN_TP-1
           j = 3 * (i-1) + 1
             CALL polyfit(vL_psat_spline(:,i), saturP_sat (j:j+3), vL(j:j+3),ord_spline)
             CALL polyfit(vV_psat_spline(:,i), saturP_sat (j:j+3), vV(j:j+3),ord_spline)
             CALL polyfit(uL_psat_spline(:,i), saturP_sat (j:j+3), uL(j:j+3),ord_spline)
             CALL polyfit(Tsat_psat_spline(:,i), saturP_sat(j:j+3),saturT(j:j+3), ord_spline)
             CALL polyfit(uV_psat_spline(:,i), saturP_sat (j:j+3), uV(j:j+3),ord_spline)
        ENDDO
!print*,'-------------------------------------------------------------------------'
!print*,'           construction for saturation curve with 3e order spline        '
!print*,'-------------------------------------------------------------------------'
!
!
     END SUBROUTINE saturation_curve
!
!===================================================================================
!
     SUBROUTINE sat_curve()
!
!===================================================================================
        IMPLICIT NONE
!
        INTEGER  :: i, j, exitflag,Niter
!
        REAL(pr)  :: delta, delta_sat,v_v,v_l,Tsat,u_l,u_v,&
 &                   resnorm, guess1,guess2,guess3,in_2
!
        REAL(pr), DIMENSION (NNN_sat_TP2) :: vL, vV, uL, uV,saturT
!
!
!
        delta      = (P_cr - P_tri) / (NNN_TP2-1)
        saturP2     = P_tri + (/(i*delta, i=0, NNN_TP2-1)/)
        delta_sat  = (P_cr - P_tri) / (NNN_sat_TP2-1)
        saturP_sat2 = P_tri + (/(i*delta_sat, i=0,NNN_sat_TP2-1)/)
!
! points on saturation curve
!
        guess1 = 1_pr/rho_tri_L
        guess2 = 1_pr/rho_tri_R
        guess3 = T_tri
!
        DO i = 2, NNN_sat_TP2-1

         CALL New_Rap3D(3,v_l , v_v, Tsat, &
     &   resnorm, Niter, exitflag, saturP_sat2(i),in_2, guess1, guess2,guess3)

        IF (resnorm > 1e-12_pr) THEN
        print*, "sat  curve", resnorm, i, Niter
        STOP
        ENDIF
!
!        print*, v_l,v_v
!        print*, u_l,u_v
!        print*," "
         CALL inter_energy(Tsat,v_v,u_v)
         CALL inter_energy(Tsat,v_l,u_l)
           vL(i)     = v_l
           vV(i)     = v_v
           uL(i)     = u_l
           uV(i)     = u_v
           saturT(i) = Tsat
!
           guess1 = v_l
           guess2 = v_v
           guess3 = Tsat
        ENDDO
!
        vL(NNN_sat_TP2)     = 1_pr/rho_cr
        vV(NNN_sat_TP2)     = 1_pr/rho_cr
        uL(NNN_sat_TP2)     = e_cr
        uV(NNN_sat_TP2)     = e_cr
        saturT(NNN_sat_TP2) = T_cr
!
        vL(1)     = 1_pr/rho_tri_L
        vV(1)     = 1_pr/rho_tri_R
        uL(1)     = e_tri_L
        uV(1)     = e_tri_R
        saturT(1) = T_tri

! intervals on saturation curve
!
        DO i = 1, NNN_TP2-1
           j = 5 * (i-1) + 1
             CALL polyfit(vL_psat_spline2(:,i),   saturP_sat2 (j:j+5), vL(j:j+5),     5)
             CALL polyfit(vV_psat_spline2(:,i),   saturP_sat2 (j:j+5), vV(j:j+5),     5)
             CALL polyfit(uL_psat_spline2(:,i),   saturP_sat2 (j:j+5), uL(j:j+5),     5)
             CALL polyfit(Tsat_psat_spline2(:,i), saturP_sat2 (j:j+5), saturT(j:j+5), 5)
             CALL polyfit(uV_psat_spline2(:,i),   saturP_sat2 (j:j+5), uV(j:j+5),     5)
        ENDDO
!print*,'-------------------------------------------------------------------------'
!print*,'construction for saturation curve with 5e order spline                   '
!print*,'-------------------------------------------------------------------------'
!
!
     END SUBROUTINE sat_curve
!
!=================================================================================
!
!     SUBROUTINE satprop(mode, psat, Tsat, vvsat, vlsat, uvsat, ulsat)
!
!=================================================================================
!      IMPLICIT NONE
!!
!      INTEGER  :: i, j, j_sat
!!
!      REAL(pr), INTENT(in)  :: psat
!      INTEGER , INTENT(in)  :: mode
!!
!      REAL(pr), INTENT(out) :: Tsat, vvsat,vlsat,uvsat,ulsat
!!
!      REAL(pr) :: delta, pp, ratio, c_out, a_out, x_out, v_in, du_dp_x, dv_dp_x, temp
!      REAL(pr) :: duL_dp, duV_dp, dvL_dp, dvV_dp
!      REAL(pr) :: vL, uL, vV, uV
!
!
!use 3e order spline
!
!      IF (mode == 3) THEN
!!
!       delta = saturP(2) - saturP(1)
!       j_sat = INT((psat  - saturP(1))/delta) + 1
!!
!!##computing derivatives
!!
!       duL_dp = 0_pr;  duV_dp = 0_pr;  dvL_dp = 0_pr;  dvV_dp = 0_pr
!       DO i = 1, ord_spline
!          pp      = psat**(ord_spline - i)
!          duL_dp  = duL_dp + (ord_spline+1-i) * uL_psat_spline(i,j_sat) *pp
!          duV_dp  = duV_dp + (ord_spline+1-i) * uV_psat_spline(i,j_sat) *pp
!          dvL_dp  = dvL_dp + (ord_spline+1-i) * vL_psat_spline(i,j_sat) *pp
!          dvV_dp  = dvV_dp + (ord_spline+1-i) * vV_psat_spline(i,j_sat) *pp
!       ENDDO
!
!##computing saturation quantities
!
!       vL = 0_pr; uL = 0_pr;  vV = 0_pr;  uV = 0_pr; temp = 0_pr
!       DO i = 1, ord_spline+1
!          pp = psat**(i-1)
!          vL   = vL  + vL_psat_spline  (ord_spline+2-i, j_sat) * pp
!          uL   = uL  + uL_psat_spline  (ord_spline+2-i, j_sat) * pp
!          vV   = vV  + vV_psat_spline  (ord_spline+2-i, j_sat) * pp
!          uV   = uV  + uV_psat_spline  (ord_spline+2-i, j_sat) * pp
!          temp = temp+ Tsat_psat_spline(ord_spline+2-i, j_sat) * pp
!       ENDDO
!
!       ratio = ((uV - uL)/(vV - vL))   ! (J/kg)/(m3/kg)
!       du_dp_x = x_out * duV_dp + (1_pr - x_out) * duL_dp
!       dv_dp_x = x_out * dvV_dp + (1_pr - x_out) * dvL_dp
!
!       c_out = SQRT((psat  + ratio)/(du_dp_x - ratio * dv_dp_x)) * v_in ! (m/s)
!
!       a_out = x_out*vV/v_in
!
!##for output saturation quantities
!
!       Tsat  = temp
!       vvsat = vV
!       vlsat = vL
!       uvsat = uV
!       ulsat = uL
!
!use 5e order spline
!
!      ELSEIF (mode == 5) THEN
!
!       delta = saturP2(2) - saturP2(1)
!       j_sat = INT((psat  - saturP2(1))/delta) + 1
!
!##computing derivatives
!
!       duL_dp = 0_pr;  duV_dp = 0_pr;  dvL_dp = 0_pr;  dvV_dp = 0_pr; temp=0_pr
!       DO i = 1, 5
!          pp      = psat**(5 - i)
!          duL_dp  = duL_dp + (5+1-i) * uL_psat_spline2(i,j_sat) *pp
!          duV_dp  = duV_dp + (5+1-i) * uV_psat_spline2(i,j_sat) *pp
!          dvL_dp  = dvL_dp + (5+1-i) * vL_psat_spline2(i,j_sat) *pp
!          dvV_dp  = dvV_dp + (5+1-i) * vV_psat_spline2(i,j_sat) *pp
!       ENDDO
!
!##computing saturation quantities
!
!       vL = 0_pr; uL = 0_pr;  vV = 0_pr;  uV = 0_pr
!       DO i = 1, 6
!          pp = psat**(i-1)
!          vL   = vL  + vL_psat_spline2  (5+2-i, j_sat) * pp
!          uL   = uL  + uL_psat_spline2  (5+2-i, j_sat) * pp
!          vV   = vV  + vV_psat_spline2  (5+2-i, j_sat) * pp
!          uV   = uV  + uV_psat_spline2  (5+2-i, j_sat) * pp
!          temp = temp+ Tsat_psat_spline2(5+2-i, j_sat) * pp
!       ENDDO
!
!       ratio = ((uV - uL)/(vV - vL))   ! (J/kg)/(m3/kg)
!       du_dp_x = x_out * duV_dp + (1_pr - x_out) * duL_dp
!       dv_dp_x = x_out * dvV_dp + (1_pr - x_out) * dvL_dp
!
!       c_out = SQRT((psat  + ratio)/(du_dp_x - ratio * dv_dp_x)) * v_in ! (m/s)
!
!       a_out = x_out*vV/v_in
!
!##for output saturation quantities
!
!       Tsat  = temp
!       vvsat = vV
!       vlsat = vL
!       uvsat = uV
!       ulsat = uL
!
!      ENDIF
!     END SUBROUTINE satprop
!
!
!
END MODULE saturation


MODULE sw
  IMPLICIT NONE
  INTEGER,PARAMETER :: r8 = SELECTED_REAL_KIND(12) ! 8 byte real

  REAL(r8), PARAMETER ::       &
       alpha      = 0.        ,&! angle of axis rotation about the poles
       !
       !
       g          = 9.80616d0 ,&! gravitational acceleration (m/s^2)
       a          = 6371229.d0,&! Earth's radius in m
       omega      = 7.29212d-5,&! angular velocity 1/s
       pi         = 3.14159265358979323846_R8,&  ! pi
       deg2rad    = pi/180.d0

CONTAINS
  !
  !**************************************************************************
  !
  ! Surface geopotential
  !
  !**************************************************************************
  !
  REAL(r8) FUNCTION surface_geopotential(lon,lat,test_case)
    IMPLICIT NONE
    REAL(r8), INTENT(IN) :: lon, lat
    INTEGER , INTENT(IN) :: test_case
    REAL(r8) :: r, r0, p1(2), p2(2)

      select case (test_case)

      case(1)

         surface_geopotential = 0.0

      case(2)

         surface_geopotential = 0.0

      case(5)

         r0 = pi/9.
         p1(1) = pi/2.
         p1(2) = pi/6.
         p2(1) = lon
         p2(2) = lat
         r = SQRT(MIN(r0*r0, (p2(1)-p1(1))*(p2(1)-p1(1)) + (p2(2)-p1(2))*(p2(2)-p1(2))))
         surface_geopotential = 2000.0*g*(1.0-(r/r0))

      end select

  END FUNCTION surface_geopotential
  !
  !**************************************************************************
  !
  ! heights
  !
  !**************************************************************************
  !
  REAL(r8) FUNCTION height(lon,lat,test_case)
    IMPLICIT NONE
    REAL(r8), INTENT(IN) :: lon, lat
    INTEGER , INTENT(IN) :: test_case
    REAL(r8) :: Ubar
    REAL(r8) :: r, r0, p1(2), p2(2)

      select case (test_case)

      case(1)

         r0 = a/3.
         p1(1) = pi/2.
         p1(2) = 0.0
         p2(1) = lon
         p2(2) = lat
         r = a*acos( sin(p1(2))*sin(p2(2)) + cos(p1(2))*cos(p2(2))*cos(p2(1)-p1(1)) )
         if (r < r0) then
            height = (1000.0/2.0)*(1.0+cos(pi*r/r0))
         else
            height = 0.0
         endif

      case(2)

         Ubar = (2.0*pi*a)/(12.0*86400.0)
         height = 2.94e4 - (a*omega*Ubar + (Ubar*Ubar)/2.) * &
                   ( -1.*cos(lon)*cos(lat)*sin(alpha) + &
                                  sin(lat)*cos(alpha) ) ** 2

      case(5)

         Ubar = (2.0*pi*a)/(12.0*86400.0)
         height = 5400.*g - (a*omega*Ubar + (Ubar*Ubar)/2.) * &
                   ( -1.*cos(lon)*cos(lat)*sin(alpha) + &
                                  sin(lat)*cos(alpha) ) ** 2

      end select

  END FUNCTION height
  !
  !********************************************************************
  !
  ! wind components
  !
  !********************************************************************
  !
  REAL(r8) FUNCTION u_wind(lon,lat,test_case)
    IMPLICIT NONE
    REAL(r8), INTENT(IN) :: lon,lat
    INTEGER, INTENT(IN)  :: test_case
    REAL(r8) :: Ubar

         Ubar = (2.0*pi*a)/(12.0*86400.0)
         u_wind = Ubar*( cos(lat)*cos(alpha) + cos(lon)*sin(lat)*sin(alpha))

  END FUNCTION u_wind

  REAL(r8) FUNCTION v_wind(lon,lat,test_case)
    IMPLICIT NONE
    REAL(r8), INTENT(IN) :: lon,lat
    INTEGER, INTENT(IN)  :: test_case
    REAL(r8) :: Ubar

         Ubar = (2.0*pi*a)/(12.0*86400.0)
         v_wind = -Ubar*sin(lon)*sin(alpha)

  END FUNCTION v_wind

END MODULE sw


        SUBROUTINE FCN2(X2,f2,n2,par2)
        common/density/denim(2),denm(2),gdot(2)
        DIMENSION x2(n2),f2(n2),par2(3),yprime2(2)
        b=3.0e-10
        ak1=0.0127/b
        ak2=6.67
        ak3=5.53
        ak4=0.0000276/(b*b)
        yprime2(1)=abs(gdot(2))*(ak1*(x2(1)**0.5)+ak3*x2(2)-ak2*x2(1))
        yprime2(2)=abs(gdot(2))*(ak4*x2(1)/x2(2)-ak1*(x2(1)**0.5)
     1-ak3*x2(2))
        f2(1)=x2(1)-par2(1)-par2(3)*yprime2(1)
        f2(2)=x2(2)-par2(2)-par2(3)*yprime2(2)
             RETURN
             END


!==========================================================================
elemental function gsw_sstar_from_sa (sa, p, long, lat)
!==========================================================================
!
! Calculates Preformed Salinity, Sstar, from Absolute Salinity, SA.
!
! sa     : Absolute Salinity                               [g/kg]
! p      : sea pressure                                    [dbar]
! long   : longitude                                       [deg E]
! lat    : latitude                                        [deg N]
!
! gsw_sstar_from_sa : Preformed Salinity                   [g/kg]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_saar

use gsw_mod_error_functions, only : gsw_error_code, gsw_error_limit

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, p, long, lat

real (r8) :: gsw_sstar_from_sa

real (r8) :: saar

character (*), parameter :: func_name = "gsw_sstar_from_sa"

! In the Baltic Sea, Sstar = sa, and note that gsw_saar returns zero
! for saar in the Baltic.

saar = gsw_saar(p,long,lat)

if (saar.gt.gsw_error_limit) then
   gsw_sstar_from_sa = gsw_error_code(1,func_name,saar)
else
   gsw_sstar_from_sa = sa*(1.0_r8 - 0.35_r8*saar)/(1.0_r8 + saar)
end if

return
end function

!--------------------------------------------------------------------------


!
! -- Mathematical core of the XT3D method.
!
module Xt3dAlgorithmModule

  use KindModule, only: DP, I4B
  implicit none

  contains

  subroutine qconds(nnbrmx,nnbr0,inbr0,il01,vc0,vn0,dl0,dl0n,ck0,              &
                    nnbr1,inbr1,il10,vc1,vn1,dl1,dl1n,ck1,ar01,ar10,           &
                    vcthresh,allhc0,allhc1,chat01,chati0,chat1j)
! ******************************************************************************
!
!.....Compute the "conductances" in the normal-flux expression for an
!        interface (modflow-usg version).  The cell on one side of
!        the interface is "cell 0", and the one on the other side is
!        "cell 1".
!
!        nnbrmx = maximum number of neighbors allowed for a cell.
!        nnbr0 = number of neighbors (local connections) for cell 0.
!        inbr0 = array with the list of neighbors for cell 0.
!        il01 = local node number of cell 1 with respect to cell 0.
!        vc0 = array of connection unit-vectors for cell 0 with its
!           neighbors.
!        vn0 = array of unit normal vectors for cell 0's interfaces.
!        dl0 = array of lengths contributed by cell 0 to its
!           connections with its neighbors.
!        dl0n = array of lengths contributed by cell 0's neighbors to
!           their connections with cell 0.
!        ck0 = conductivity tensor for cell 0.
!        nnbr1 = number of neighbors (local connections) for cell 1.
!        inbr1 = array with the list of neighbors for cell 1.
!        il10 = local node number of cell 0 with respect to cell 1.
!        vc1 = array of connection unit-vectors for cell 1 with its
!           neighbors.
!        vn1 = array of unit normal vectors for cell 1's interfaces.
!        dl1 = array of lengths contributed by cell 1 to its
!           connections with its neighbors.
!        dl1n = array of lengths contributed by cell 1's neighbors to
!           their connections with cell 1.
!        ck1 = conductivity tensor for cell1.
!        ar01 = area of interface (0,1).
!        ar10 = area of interface (1,0).
!        chat01 = "conductance" for connection (0,1).
!        chati0 = array of "conductances" for connections of cell 0.
!           (zero for connection with cell 1, as this connection is
!           already covered by chat01.)
!        chat1j = array of "conductances" for connections of cell 1.
!           (zero for connection with cell 0., as this connection is
!           already covered by chat01.)
! ******************************************************************************
!
!    SPECIFICATIONS:
! ------------------------------------------------------------------------------
    ! -- dummy
    integer(I4B) :: nnbrmx
    integer(I4B) :: nnbr0
    integer(I4B), dimension(nnbrmx) :: inbr0
    integer(I4B) :: il01
    real(DP), dimension(nnbrmx, 3) :: vc0
    real(DP), dimension(nnbrmx, 3) :: vn0
    real(DP), dimension(nnbrmx) :: dl0
    real(DP), dimension(nnbrmx) :: dl0n
    real(DP), dimension(3, 3) :: ck0
    integer(I4B) :: nnbr1
    integer(I4B), dimension(nnbrmx) :: inbr1
    integer(I4B) :: il10
    real(DP), dimension(nnbrmx) :: vc1
    real(DP), dimension(nnbrmx) :: vn1
    real(DP), dimension(nnbrmx) :: dl1
    real(DP), dimension(nnbrmx) :: dl1n
    real(DP), dimension(3, 3) :: ck1
    real(DP) :: ar01
    real(DP) :: ar10
    real(DP) :: vcthresh
    logical :: allhc0
    logical :: allhc1
    real(DP) :: chat01
    real(DP), dimension(nnbrmx) :: chati0
    real(DP), dimension(nnbrmx) :: chat1j
    ! -- local
    integer(I4B) :: i1
    integer(I4B) :: i
    real(DP) :: ahat0
    real(DP) :: ahat1
    real(DP) :: wght1
    real(DP) :: wght0
    real(DP), dimension(nnbrmx) :: bhat0
    real(DP), dimension(nnbrmx) :: bhat1
! ------------------------------------------------------------------------------
!
!.....Set the global cell number for cell 1, as found in the neighbor
!        list for cell 0.  It is assumed that cells 0 and 1 are both
!        active, or else this subroutine would not have been called.
    i1 = inbr0(il01)
!
!.....If area ar01 is zero (in which case ar10 is also zero, since
!        this can only happen here in the case of Newton), then the
!        "conductances" are all zero.
    if (ar01.eq.0d0) then
      chat01 = 0d0
      do i=1,nnbrmx
      chati0(i) = 0d0
      chat1j(i) = 0d0
      enddo
!.....Else compute "conductances."
    else
!........Compute contributions from cell 0.
      call abhats(nnbrmx,nnbr0,inbr0,il01,vc0,vn0,dl0,dl0n,ck0,        &
                  vcthresh,allhc0,ar01,ahat0,bhat0)
!........Compute contributions from cell 1.
      call abhats(nnbrmx,nnbr1,inbr1,il10,vc1,vn1,dl1,dl1n,ck1,        &
                  vcthresh,allhc1,ar10,ahat1,bhat1)
!........Compute "conductances" based on the two flux estimates.
      wght1 = ahat0/(ahat0 + ahat1)
      wght0 = 1d0 - wght1
      chat01 = wght1*ahat1
      do i=1,nnbrmx
        chati0(i) = wght0*bhat0(i)
        chat1j(i) = wght1*bhat1(i)
      enddo
    end if
!
    return
    end subroutine qconds

  subroutine abhats(nnbrmx,nnbr,inbr,il01,vc,vn,dl0,dln,ck,                    &
                    vcthresh,allhc,ar01,ahat,bhat)
! ******************************************************************************
!.....Compute "ahat" and "bhat" coefficients for one side of an
!        interface.
! ******************************************************************************
!
!    SPECIFICATIONS:
! ------------------------------------------------------------------------------
    ! -- dummy
    integer(I4B) :: nnbrmx
    integer(I4B) :: nnbr
    integer(I4B), dimension(nnbrmx) :: inbr
    integer(I4B) :: il01
    real(DP), dimension(nnbrmx, 3) :: vc
    real(DP), dimension(nnbrmx, 3) :: vn
    real(DP), dimension(nnbrmx) :: dl0
    real(DP), dimension(nnbrmx) :: dln
    real(DP), dimension(3, 3) :: ck
    real(DP) :: vcthresh
    logical :: allhc
    real(DP) :: ar01
    real(DP) :: ahat
    real(DP), dimension(nnbrmx) :: bhat
    ! -- local
    logical :: iscomp
    real(DP), dimension(nnbrmx, 3) :: vccde
    real(DP), dimension(3, 3) :: rmat
    real(DP), dimension(3) :: sigma
    real(DP), dimension(nnbrmx) :: bd
    real(DP), dimension(nnbrmx) :: be
    real(DP), dimension(nnbrmx) :: betad
    real(DP), dimension(nnbrmx) :: betae
    integer(I4B) :: iml0
    integer(I4B) :: il
    real(DP) :: acd
    real(DP) :: add
    real(DP) :: aed
    real(DP) :: ace
    real(DP) :: aee
    real(DP) :: ade
    real(DP) :: determ
    real(DP) :: oodet
    real(DP) :: alphad
    real(DP) :: alphae
    real(DP) :: dl0il
! ------------------------------------------------------------------------------
!
!.....Determine the basis vectors for local "(c, d, e)" coordinates
!        associated with the connection between cells 0 and 1, and
!        set the rotation matrix that transforms vectors from model
!        coordinates to (c, d, e) coordinates.  (If no active
!        connection is found that has a non-negligible component
!        perpendicular to the primary connection, ilmo=0 is returned.)
    call getrot(nnbrmx,nnbr,inbr,vc,il01,rmat,iml0)
!
!.....If no active connection with a non-negligible perpendicular
!        component, assume no perpendicular gradient and base gradient
!        solely on the primary connection.  Otherwise, proceed with
!        basing weights on information from neighboring connections.
    if (iml0.eq.0) then
!
!........Compute ahat and bhat coefficients assuming perpendicular
!           components of gradient are zero.
      sigma(1) = dot_product(vn(il01, :), matmul(ck, rmat(:, 1)))
      ahat = sigma(1) / dl0(il01)
      bhat = 0d0
!
    else
!
!........Transform local connection unit-vectors from model coordinates
!           to "(c, d, e)" coordinates associated with the connection
!           between cells 0 and 1.
      call tranvc(nnbrmx,nnbr,rmat,vc,vccde)
!
!........Get "a" and "b" weights for first perpendicular direction.
      call abwts(nnbrmx,nnbr,inbr,il01,2,vccde,           &
                 vcthresh,dl0,dln,acd,add,aed,bd)
!
!........If all neighboring connections are user-designated as
!           horizontal, or if none have a non-negligible component in
!           the second perpendicular direction, assume zero gradient in
!           the second perpendicular direction.  Otherwise, get "a" and
!           "b" weights for second perpendicular direction based on
!           neighboring connections.
      if (allhc) then
        ace = 0d0
        aee = 1d0
        ade = 0d0
        be = 0d0
      else
        iscomp = .false.
        do il = 1, nnbr
          if ((il == il01) .or. (inbr(il) == 0)) then
            cycle
          else if (dabs(vccde(il, 3)) > 1d-10) then
            iscomp = .true.
            exit
          end if
        end do
        if (iscomp) then
          call abwts(nnbrmx,nnbr,inbr,il01,3,vccde,    &
            vcthresh,dl0,dln,ace,aee,ade,be)
        else
          ace = 0d0
          aee = 1d0
          ade = 0d0
          be = 0d0
        end if
      end if
!
!........Compute alpha and beta coefficients.
        determ = add * aee - ade * aed
        oodet = 1d0 / determ
        alphad = (acd * aee - ace * aed) * oodet
        alphae = (ace * add - acd * ade) * oodet
        betad = 0d0
        betae = 0d0
        do il = 1, nnbr
!...........If this is connection (0,1) or inactive, skip.
          if ((il == il01) .or. (inbr(il) == 0)) cycle
          betad(il) = (bd(il) * aee - be(il) * aed) * oodet
          betae(il) = (be(il) * add - bd(il) * ade) * oodet
       end do
!
!........Compute sigma coefficients.
        sigma = matmul(vn(il01, :), matmul(ck, rmat))
!
!........Compute ahat and bhat coefficients.
        ahat = (sigma(1) - sigma(2) * alphad - sigma(3) * alphae) / dl0(il01)
        bhat = 0d0
        do il = 1, nnbr
!...........If this is connection (0,1) or inactive, skip.
          if ((il == il01) .or. (inbr(il) == 0)) cycle
          dl0il = dl0(il) + dln(il)
          bhat(il) = (sigma(2) * betad(il) + sigma(3) * betae(il)) / dl0il
       end do
!........Set the bhat for connection (0,1) to zero here, since we have
!           been skipping it in our do loops to avoiding explicitly
!           computing it.  This will carry through to the corresponding
!           chati0 and chat1j value, so that they too are zero.
        bhat(il01) = 0d0
!
    end if
!
!.....Multiply by area.
    ahat = ahat * ar01
    bhat = bhat * ar01
!
    return
  end subroutine abhats

  subroutine getrot(nnbrmx,nnbr,inbr,vc,il01,rmat,iml0)
! ******************************************************************************
!.....Compute the matrix that rotates the model-coordinate axes to
!     the "(c, d, e)-coordinate" axes associated with a connection.
!     This is also the matrix that transforms the components of a vector
!     from (c, d, e) coordinates to model coordinates.  [Its transpose
!     transforms from model to (c, d, e) coordinates.]
!
!        vcc = unit vector for the primary connection, in model
!           coordinates.
!        vcd = unit vector for the first perpendicular direction,
!           in model coordinates.
!        vce = unit vector for the second perpendicular direction,
!           in model coordinates.
!        vcmax = unit vector for the connection with the maximum
!           component perpendicular to the primary connection,
!           in model coordinates.
!        rmat = rotation matrix from model to (c, d, e) coordinates.
!
! ******************************************************************************
!
!    SPECIFICATIONS:
! ------------------------------------------------------------------------------
    ! -- dummy
    integer(I4B) :: nnbrmx
    integer(I4B) :: nnbr
    integer(I4B), dimension(nnbrmx) :: inbr
    real(DP), dimension(nnbrmx, 3) :: vc
    integer(I4B) :: il01
    real(DP), dimension(3, 3) :: rmat
    integer(I4B) :: iml0
    ! -- local
    real(DP), dimension(3) :: vcc
    real(DP), dimension(3) :: vcd
    real(DP), dimension(3) :: vce
    real(DP), dimension(3) :: vcmax
    integer(I4B) :: il
    real(DP) :: acmpmn
    real(DP) :: cmp
    real(DP) :: acmp
    real(DP) :: cmpmn
! ------------------------------------------------------------------------------
!
!.....set vcc.
    vcc(:) = vc(il01,:)
!
!.....Set vcmax.  (If no connection has a perpendicular component
!        greater than some tiny threshold, return with iml0=0 and
!        the first column of rmat set to vcc -- the other columns
!        are not needed.)
    acmpmn = 1d0 - 1d-10
    iml0 = 0
    do il = 1, nnbr
      if ((il.eq.il01).or.(inbr(il).eq.0)) then
        cycle
      else
        cmp = dot_product(vc(il,:), vcc)
        acmp = dabs(cmp)
        if (acmp.lt.acmpmn) then
          cmpmn = cmp
          acmpmn = acmp
          iml0 = il
        end if
      end if
    enddo
    if (iml0 == 0) then
        rmat(:,1) = vcc(:)
        goto 999
    else
        vcmax(:) = vc(iml0,:)
    end if
!
!.....Set the first perpendicular direction as the direction that is
!        coplanar with vcc and vcmax and perpendicular to vcc.
    vcd = vcmax - cmpmn * vcc
    vcd = vcd / dsqrt(1d0 - cmpmn * cmpmn)
!
!.....Set the second perpendicular direction as the cross product of
!        the primary and first-perpendicular directions.
    vce(1) = vcc(2)*vcd(3) - vcc(3)*vcd(2)
    vce(2) = vcc(3)*vcd(1) - vcc(1)*vcd(3)
    vce(3) = vcc(1)*vcd(2) - vcc(2)*vcd(1)
!
!.....Set the rotation matrix as the matrix with vcc, vcd, and vce
!        as its columns.
    rmat(:,1) = vcc(:)
    rmat(:,2) = vcd(:)
    rmat(:,3) = vce(:)
!
999 return
  end subroutine getrot

  subroutine tranvc(nnbrmx,nnbrs,rmat,vc,vccde)
! ******************************************************************************
!.....Transform local connection unit-vectors from model coordinates
!        to "(c, d, e)" coordinates associated with a connection.
!
!        nnbrs = number of neighbors (local connections)
!        rmat = rotation matrix from (c, d, e) to model coordinates.
!        vc = array of connection unit-vectors with respect to model
!           coordinates.
!        vccde = array of connection unit-vectors with respect to
!           (c, d, e) coordinates.
!
! ******************************************************************************
!
!    SPECIFICATIONS:
! ------------------------------------------------------------------------------
    ! -- dummy
    integer(I4B) :: nnbrmx
    integer(I4B) :: nnbrs
    real(DP), dimension(3, 3) :: rmat
    real(DP), dimension(nnbrmx, 3) :: vc
    real(DP), dimension(nnbrmx, 3) :: vccde
    ! -- local
    integer(I4B) :: il
! ------------------------------------------------------------------------------
!
!.....Loop over the local connections, transforming the unit vectors.
!        Note that we are multiplying by the transpose of the
!        rotation matrix so that the transformation is from model
!        to (c, d, e) coordinates.
    do il = 1, nnbrs
      vccde(il,:) = matmul(transpose(rmat), vc(il,:))
    enddo
!
    return
  end subroutine tranvc

  subroutine abwts(nnbrmx,nnbr,inbr,il01,nde1,vccde,                           &
                   vcthresh,dl0,dln,acd,add,aed,bd)
! ******************************************************************************
!.....Compute "a" and "b" weights for the local connections with respect
!        to the perpendicular direction of primary interest.
!
!        nde1 = number that indicates the perpendicular direction of
!           primary interest on this call: "d" (2) or "e" (3).
!        vccde = array of connection unit-vectors with respect to
!           (c, d, e) coordinates.
!        bd = array of "b" weights.
!        aed = "a" weight that goes on the matrix side of the 2x2
!           problem.
!        acd = "a" weight that goes on the right-hand side of the
!           2x2 problem.
!
! ******************************************************************************
!
!    SPECIFICATIONS:
! ------------------------------------------------------------------------------
    ! -- dummy
    integer(I4B) :: nnbrmx
    integer(I4B) :: nnbr
    integer(I4B), dimension(nnbrmx) :: inbr
    integer(I4B) :: il01
    integer(I4B) :: nde1
    real(DP), dimension(nnbrmx, 3) :: vccde
    real(DP) :: vcthresh
    real(DP), dimension(nnbrmx) :: dl0
    real(DP), dimension(nnbrmx) :: dln
    real(DP) :: acd
    real(DP) :: add
    real(DP) :: aed
    real(DP), dimension(nnbrmx) :: bd
    ! -- local
    integer(I4B) :: nde2
    integer(I4B) :: il
    real(DP) :: vcmx
    real(DP) :: dlm
    real(DP) :: cosang
    real(DP) :: dl4wt
    real(DP) :: fact
    real(DP) :: dsum
    real(DP) :: oodsum
    real(DP) :: fatten
    real(DP), dimension(nnbrmx) :: omwt
! ------------------------------------------------------------------------------
!
!.....Set the perpendicular direction of secondary interest.
      nde2 = 5 - nde1
!
!.....Begin computing "omega" weights.
      omwt = 0d0
      dsum = 0d0
      vcmx = 0d0
      do il = 1, nnbr
!........if this is connection (0,1) or inactive, skip.
         if ((il.eq.il01).or.(inbr(il).eq.0)) cycle
         vcmx = max(dabs(vccde(il,nde1)), vcmx)
         dlm = 5d-1*(dl0(il) + dln(il))
!...........Distance-based weighting.  dl4wt is the distance between
!              the point supplying the gradient information and the
!              point at which the flux is being estimated.  Could be
!              coded as a special case of resistance-based weighting
!              (by setting the conductivity matrix to be the identity
!              matrix), but this is more efficient.
          cosang = vccde(il,1)
          dl4wt = dsqrt(dlm*dlm + dl0(il01)*dl0(il01)                          &
                  - 2d0*dlm*dl0(il01)*cosang)
         omwt(il) = dabs(vccde(il,nde1))*dl4wt
         dsum = dsum + omwt(il)
      end do
!
!.....Finish computing non-normalized "omega" weights.  [Add a
!        tiny bit to dsum so that the normalized omega weight later
!        evaluates to (essentially) 1 in the case of a single relevant
!        connection, avoiding 0/0.]
      dsum = dsum + 1d-10*dsum
      do il = 1, nnbr
!........If this is connection (0,1) or inactive, skip.
         if ((il.eq.il01).or.(inbr(il).eq.0)) cycle
         fact = dsum - omwt(il)
         omwt(il) = fact*dabs(vccde(il,nde1))
      end do
!
!.....Compute "b" weights.
      bd = 0d0
      dsum = 0d0
      do il = 1, nnbr
!........If this is connection (0,1) or inactive, skip.
         if ((il.eq.il01).or.(inbr(il).eq.0)) cycle
         bd(il) = omwt(il)*sign(1d0,vccde(il,nde1))
         dsum = dsum + omwt(il)*dabs(vccde(il,nde1))
      end do
      oodsum = 1d0/dsum
      do il = 1, nnbr
!........If this is connection (0,1) or inactive, skip.
         if ((il.eq.il01).or.(inbr(il).eq.0)) cycle
         bd(il) = bd(il)*oodsum
      end do
!
!.....Compute "a" weights.
      add = 1d0
      acd = 0d0
      aed = 0d0
      do il = 1, nnbr
!........If this is connection (0,1) or inactive, skip.
         if ((il.eq.il01).or.(inbr(il).eq.0)) cycle
         acd = acd + bd(il)*vccde(il,1)
         aed = aed + bd(il)*vccde(il,nde2)
      end do
!
!.....Apply attenuation function to acd, aed, and bd.
      if (vcmx.lt.vcthresh) then
         fatten = vcmx/vcthresh
         acd = acd*fatten
         aed = aed*fatten
         bd = bd*fatten
      end if
!
  999 return
  end subroutine abwts
!
end module Xt3dAlgorithmModule


c======================================================================
      subroutine fc2d_geoclaw_rpn2(ixy,maxm,meqn,mwaves,maux,mbc,mx,
     &      ql,qr,auxl,auxr,fwave,s,amdq,apdq)
c======================================================================
c
c Solves normal Riemann problems for the 2D SHALLOW WATER equations
c     with topography:
c     #        h_t + (hu)_x + (hv)_y = 0                           #
c     #        (hu)_t + (hu^2 + 0.5gh^2)_x + (huv)_y = -ghb_x      #
c     #        (hv)_t + (huv)_x + (hv^2 + 0.5gh^2)_y = -ghb_y      #

c On input, ql contains the state vector at the left edge of each cell
c     qr contains the state vector at the right edge of each cell
c
c This data is along a slice in the x-direction if ixy=1
c     or the y-direction if ixy=2.

c  Note that the i'th Riemann problem has left state qr(i-1,:)
c     and right state ql(i,:)
c  From the basic clawpack routines, this routine is called with
c     ql = qr
c
c
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!                                                                           !
!      # This Riemann solver is for the shallow water equations.            !
!                                                                           !
!       It allows the user to easily select a Riemann solver in             !
!       riemannsolvers_geo.f. this routine initializes all the variables    !
!       for the shallow water equations, accounting for wet dry boundary    !
!       dry cells, wave speeds etc.                                         !
!                                                                           !
!           David George, Vancouver WA, Feb. 2009                           !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      use geoclaw_module, only: g => grav, drytol => dry_tolerance
      use geoclaw_module, only: earth_radius, deg2rad
      use amr_module, only: mcapa

      implicit none

      !input
      integer maxm,meqn,maux,mwaves,mbc,mx,ixy

      double precision  fwave(meqn, mwaves, 1-mbc:maxm+mbc)
      double precision  s(mwaves, 1-mbc:maxm+mbc)
      double precision  ql(meqn, 1-mbc:maxm+mbc)
      double precision  qr(meqn, 1-mbc:maxm+mbc)
      double precision  apdq(meqn,1-mbc:maxm+mbc)
      double precision  amdq(meqn,1-mbc:maxm+mbc)
      double precision  auxl(maux,1-mbc:maxm+mbc)
      double precision  auxr(maux,1-mbc:maxm+mbc)

      !local only
      integer m,i,mw,maxiter,mu,nv
      double precision wall(3)
      double precision fw(3,3)
      double precision sw(3)

      double precision hR,hL,huR,huL,uR,uL,hvR,hvL,vR,vL,phiR,phiL
      double precision bR,bL,sL,sR,sRoe1,sRoe2,sE1,sE2,uhat,chat
      double precision s1m,s2m
      double precision hstar,hstartest,hstarHLL,sLtest,sRtest
      double precision tw,dxdc

      logical rare1,rare2

      !loop through Riemann problems at each grid cell
      do i=2-mbc,mx+mbc

!-----------------------Initializing-----------------------------------
         !inform of a bad riemann problem from the start
         if((qr(1,i-1).lt.0.d0).or.(ql(1,i) .lt. 0.d0)) then
            write(*,*) 'Negative input: hl,hr,i=',qr(1,i-1),ql(1,i),i
         endif

         !Initialize Riemann problem for grid interface
         do mw=1,mwaves
              s(mw,i)=0.d0
                 fwave(1,mw,i)=0.d0
                 fwave(2,mw,i)=0.d0
                 fwave(3,mw,i)=0.d0
         enddo

c        !set normal direction
         if (ixy.eq.1) then
            mu=2
            nv=3
         else
            mu=3
            nv=2
         endif

         !zero (small) negative values if they exist
         if (qr(1,i-1).lt.0.d0) then
               qr(1,i-1)=0.d0
               qr(2,i-1)=0.d0
               qr(3,i-1)=0.d0
         endif

         if (ql(1,i).lt.0.d0) then
               ql(1,i)=0.d0
               ql(2,i)=0.d0
               ql(3,i)=0.d0
         endif

         !skip problem if in a completely dry area
         if (qr(1,i-1) <= drytol .and. ql(1,i) <= drytol) then
            go to 30
         endif

         !Riemann problem variables
         hL = qr(1,i-1)
         hR = ql(1,i)
         huL = qr(mu,i-1)
         huR = ql(mu,i)
         bL = auxr(1,i-1)
         bR = auxl(1,i)

         hvL=qr(nv,i-1)
         hvR=ql(nv,i)

         !check for wet/dry boundary
         if (hR.gt.drytol) then
            uR=huR/hR
            vR=hvR/hR
            phiR = 0.5d0*g*hR**2 + huR**2/hR
         else
            hR = 0.d0
            huR = 0.d0
            hvR = 0.d0
            uR = 0.d0
            vR = 0.d0
            phiR = 0.d0
         endif

         if (hL.gt.drytol) then
            uL=huL/hL
            vL=hvL/hL
            phiL = 0.5d0*g*hL**2 + huL**2/hL
         else
            hL=0.d0
            huL=0.d0
            hvL=0.d0
            uL=0.d0
            vL=0.d0
            phiL = 0.d0
         endif

         wall(1) = 1.d0
         wall(2) = 1.d0
         wall(3) = 1.d0
         if (hR.le.drytol) then
            call riemanntype(hL,hL,uL,-uL,hstar,s1m,s2m,
     &                                  rare1,rare2,1,drytol,g)
            hstartest=max(hL,hstar)
            if (hstartest+bL.lt.bR) then !right state should become ghost values that mirror left for wall problem
c                bR=hstartest+bL
               wall(2)=0.d0
               wall(3)=0.d0
               hR=hL
               huR=-huL
               bR=bL
               phiR=phiL
               uR=-uL
               vR=vL
            elseif (hL+bL.lt.bR) then
               bR=hL+bL
            endif
         elseif (hL.le.drytol) then ! right surface is lower than left topo
            call riemanntype(hR,hR,-uR,uR,hstar,s1m,s2m,
     &                                  rare1,rare2,1,drytol,g)
            hstartest=max(hR,hstar)
            if (hstartest+bR.lt.bL) then  !left state should become ghost values that mirror right
c               bL=hstartest+bR
               wall(1)=0.d0
               wall(2)=0.d0
               hL=hR
               huL=-huR
               bL=bR
               phiL=phiR
               uL=-uR
               vL=vR
            elseif (hR+bR.lt.bL) then
               bL=hR+bR
            endif
         endif

         !determine wave speeds
         sL=uL-sqrt(g*hL) ! 1 wave speed of left state
         sR=uR+sqrt(g*hR) ! 2 wave speed of right state

         uhat=(sqrt(g*hL)*uL + sqrt(g*hR)*uR)/(sqrt(g*hR)+sqrt(g*hL)) ! Roe average
         chat=sqrt(g*0.5d0*(hR+hL)) ! Roe average
         sRoe1=uhat-chat ! Roe wave speed 1 wave
         sRoe2=uhat+chat ! Roe wave speed 2 wave

         sE1 = min(sL,sRoe1) ! Eindfeldt speed 1 wave
         sE2 = max(sR,sRoe2) ! Eindfeldt speed 2 wave

         !--------------------end initializing...finally----------
         !solve Riemann problem.

         maxiter = 1

         call riemann_aug_JCP(maxiter,3,3,hL,hR,huL,
     &        huR,hvL,hvR,bL,bR,uL,uR,vL,vR,phiL,phiR,sE1,sE2,
     &                                    drytol,g,sw,fw)

c         call riemann_ssqfwave(maxiter,meqn,mwaves,hL,hR,huL,huR,
c     &     hvL,hvR,bL,bR,uL,uR,vL,vR,phiL,phiR,sE1,sE2,drytol,g,sw,fw)

c          call riemann_fwave(meqn,mwaves,hL,hR,huL,huR,hvL,hvR,
c     &      bL,bR,uL,uR,vL,vR,phiL,phiR,sE1,sE2,drytol,g,sw,fw)

c        !eliminate ghost fluxes for wall
         do mw=1,3
            sw(mw)=sw(mw)*wall(mw)

               fw(1,mw)=fw(1,mw)*wall(mw)
               fw(2,mw)=fw(2,mw)*wall(mw)
               fw(3,mw)=fw(3,mw)*wall(mw)
         enddo

         do mw=1,mwaves
            s(mw,i)=sw(mw)
            fwave(1,mw,i)=fw(1,mw)
            fwave(mu,mw,i)=fw(2,mw)
            fwave(nv,mw,i)=fw(3,mw)
!            write(51,515) sw(mw),fw(1,mw),fw(2,mw),fw(3,mw)
!515         format("++sw",4e25.16)
         enddo

 30      continue
      enddo

c==========Capacity for mapping from latitude longitude to physical space====
        if (mcapa.gt.0) then
         do i=2-mbc,mx+mbc
          if (ixy.eq.1) then
             dxdc=(earth_radius*deg2rad)
          else
             dxdc=earth_radius*cos(auxl(3,i))*deg2rad
          endif

          do mw=1,mwaves
c             if (s(mw,i) .gt. 316.d0) then
c               # shouldn't happen unless h > 10 km!
c                write(6,*) 'speed > 316: i,mw,s(mw,i): ',i,mw,s(mw,i)
c                endif
	           s(mw,i)=dxdc*s(mw,i)
               fwave(1,mw,i)=dxdc*fwave(1,mw,i)
               fwave(2,mw,i)=dxdc*fwave(2,mw,i)
               fwave(3,mw,i)=dxdc*fwave(3,mw,i)
          enddo
         enddo
        endif

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

c============= compute fluctuations=============================================
         amdq(1:3,:) = 0.d0
         apdq(1:3,:) = 0.d0
         do i=2-mbc,mx+mbc
            do  mw=1,mwaves
               if (s(mw,i) < 0.d0) then
                     amdq(1:3,i) = amdq(1:3,i) + fwave(1:3,mw,i)
               else if (s(mw,i) > 0.d0) then
                  apdq(1:3,i)  = apdq(1:3,i) + fwave(1:3,mw,i)
               else
                 amdq(1:3,i) = amdq(1:3,i) + 0.5d0 * fwave(1:3,mw,i)
                 apdq(1:3,i) = apdq(1:3,i) + 0.5d0 * fwave(1:3,mw,i)
               endif
            enddo
         enddo
!--       do i=2-mbc,mx+mbc
!--            do m=1,meqn
!--                write(51,151) m,i,amdq(m,i),apdq(m,i)
!--                write(51,152) fwave(m,1,i),fwave(m,2,i),fwave(m,3,i)
!--151             format("++3 ampdq ",2i4,2e25.15)
!--152             format("++3 fwave ",8x,3e25.15)
!--            enddo
!--        enddo

      return
      end subroutine


!##############################################################################
!# Tutorial 006g: Discretise with Q1, access entries in a matrix and a vector.
!##############################################################################

module tutorial006g

  ! Include basic Feat-2 modules
  use fsystem
  use genoutput

  use triangulation
  use meshgeneration

  use element
  use spatialdiscretisation
  use linearsystemscalar
  use bilinearformevaluation

  use matrixio
  use vectorio

  implicit none
  private

  public :: start_tutorial006g

contains

  ! ***************************************************************************

  subroutine start_tutorial006g

    ! Declare some variables.
    type(t_triangulation) :: rtriangulation
    type(t_spatialdiscretisation) :: rdiscretisation
    type(t_vectorScalar) :: rx
    type(t_matrixScalar) :: rmatrix
    character(LEN=SYS_STRLEN) :: spostdir

    integer :: i
    real(DP), dimension(:), pointer :: p_Ddata
    integer, dimension(:), pointer :: p_Kcol, p_Kld

    ! Print a message
    call output_lbrk()
    call output_separator (OU_SEP_STAR)
    call output_line ("This is FEAT-2. Tutorial 006g")
    call output_separator (OU_SEP_MINUS)

    ! =================================
    ! Create a brick mesh
    ! =================================

    ! The mesh must always be in "standard" format.
    ! First create a 5x5-mesh on [0,1]x[0,1], then convert to standard.
    call meshgen_rectangular2DQuadMesh (rtriangulation, 0.0_DP, 1.0_DP, 0.0_DP, 1.0_DP, 4, 4)
    call tria_initStandardMeshFromRaw (rtriangulation)

    ! =================================
    ! Discretise with Q1.
    !
    ! Create a structure rdiscretisation
    ! which describes the discretisation.
    ! =================================

    call spdiscr_initDiscr_simple (rdiscretisation,EL_Q1_2D,rtriangulation)

    ! =================================
    ! Create a scalar vector.
    ! Create a CSR matrix corresponding
    ! to the Q1 space.
    ! =================================

    ! Create a vector.
    call lsyssc_createVector (rdiscretisation,rx)

    ! Create the matrix structure.
    call bilf_createMatrixStructure (rdiscretisation,LSYSSC_MATRIX9,rmatrix)

    ! Allocate memory for the matrix entries.
    call lsyssc_allocEmptyMatrix (rmatrix)

    ! =================================
    ! Fill the vector with data.
    ! =================================

    ! Get a pointer to the data
    call lsyssc_getbase_double (rx,p_Ddata)

    ! Initialise the data: 1,2,3,...
    do i=1,rx%NEQ
      p_Ddata(i) = real(i,DP)
    end do

    ! =================================
    ! Fill the matrix with data.
    ! =================================

    ! Get a pointer to the matrix data, the column numbers
    ! and the row indices that are usesd in the CSR format.
    call lsyssc_getbase_double (rmatrix,p_Ddata)
    call lsyssc_getbase_Kcol (rmatrix,p_Kcol)
    call lsyssc_getbase_Kld (rmatrix,p_Kld)

    ! Initialise the data: Column number.
    do i=1,rmatrix%NA
      p_Ddata(i) = real( p_Kcol(i) ,DP )
    end do

    ! Fill the first row with 1.0.
    do i=p_Kld(1), p_Kld(2)-1
      p_Ddata(i) = 1.0_DP
    end do

    ! Fill the last row with "NEQ".
    do i=p_Kld(rmatrix%NEQ), p_Kld(rmatrix%NEQ+1)-1
      p_Ddata(i) = real( rmatrix%NEQ, DP )
    end do

    ! =================================
    ! Output to files.
    ! =================================
    call output_line ("Writing to text files...")

    if (sys_getenv_string("POSTDIR",spostdir)) then
      ! Write the vector to a text file.
      call vecio_writeVectorHR (rx, "vector", .true., 0, &
          trim(spostdir)//"/tutorial006g_vector.txt", "(E11.2)")

      ! Write the matrix to a text file, omit nonexisting entries in the matrix.
      call matio_writeMatrixHR (rmatrix, "matrix", .true., 0, &
          trim(spostdir)//"/tutorial006g_matrix.txt", "(E11.2)")
    else
      ! Write the vector to a text file.
      call vecio_writeVectorHR (rx, "vector", .true., 0, &
          "./post/tutorial006g_vector.txt", "(E11.2)")

      ! Write the matrix to a text file, omit nonexisting entries in the matrix.
      call matio_writeMatrixHR (rmatrix, "matrix", .true., 0, &
          "./post/tutorial006g_matrix.txt", "(E11.2)")
    end if

    ! =================================
    ! Cleanup
    ! =================================

    ! Release the vector
    call lsyssc_releaseVector (rx)

    ! Release the matrix
    call lsyssc_releaseMatrix (rmatrix)

    ! Release the Q1-discretisation
    call spdiscr_releaseDiscr (rdiscretisation)

    ! Release the triangulation
    call tria_done (rtriangulation)

  end subroutine

end module


      SUBROUTINE slREFV (VU, REFA, REFB, VR)
*+
*     - - - - -
*      R E F V
*     - - - - -
*
*  Adjust an unrefracted Cartesian vector to include the effect of
*  atmospheric refraction, using the simple A tan Z + B tan**3 Z
*  model.
*
*  Given:
*    VU    dp    unrefracted position of the source (Az/El 3-vector)
*    REFA  dp    tan Z coefficient (radian)
*    REFB  dp    tan**3 Z coefficient (radian)
*
*  Returned:
*    VR    dp    refracted position of the source (Az/El 3-vector)
*
*  Notes:
*
*  1  This routine applies the adjustment for refraction in the
*     opposite sense to the usual one - it takes an unrefracted
*     (in vacuo) position and produces an observed (refracted)
*     position, whereas the A tan Z + B tan**3 Z model strictly
*     applies to the case where an observed position is to have the
*     refraction removed.  The unrefracted to refracted case is
*     harder, and requires an inverted form of the text-book
*     refraction models;  the algorithm used here is equivalent to
*     one iteration of the Newton-Raphson method applied to the above
*     formula.
*
*  2  Though optimized for speed rather than precision, the present
*     routine achieves consistency with the refracted-to-unrefracted
*     A tan Z + B tan**3 Z model at better than 1 microarcsecond within
*     30 degrees of the zenith and remains within 1 milliarcsecond to
*     beyond ZD 70 degrees.  The inherent accuracy of the model is, of
*     course, far worse than this - see the documentation for slRFCO
*     for more information.
*
*  3  At low elevations (below about 3 degrees) the refraction
*     correction is held back to prevent arithmetic problems and
*     wildly wrong results.  For optical/IR wavelengths, over a wide
*     range of observer heights and corresponding temperatures and
*     pressures, the following levels of accuracy (arcsec, worst case)
*     are achieved, relative to numerical integration through a model
*     atmosphere:
*
*              ZD    error
*
*              80      0.7
*              81      1.3
*              82      2.5
*              83      5
*              84     10
*              85     20
*              86     55
*              87    160
*              88    360
*              89    640
*              90   1100
*              91   1700         } relevant only to
*              92   2600         } high-elevation sites
*
*     The results for radio are slightly worse over most of the range,
*     becoming significantly worse below ZD=88 and unusable beyond
*     ZD=90.
*
*  4  See also the routine slREFZ, which performs the adjustment to
*     the zenith distance rather than in Cartesian Az/El coordinates.
*     The present routine is faster than slREFZ and, except very low down,
*     is equally accurate for all practical purposes.  However, beyond
*     about ZD 84 degrees slREFZ should be used, and for the utmost
*     accuracy iterative use of slRFRO should be considered.
*
*  P.T.Wallace   Starlink   10 April 2004
*
*  Copyright (C) 2004 Rutherford Appleton Laboratory
*
*  License:
*    This program 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 2 of the License, or
*    (at your option) any later version.
*
*    This program 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
*    along with this program (see SLA_CONDITIONS); if not, write to the
*    Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
*    Boston, MA  02110-1301  USA
*
*  Copyright (C) 1995 Association of Universities for Research in Astronomy Inc.
*-

      IMPLICIT NONE

      DOUBLE PRECISION VU(3),REFA,REFB,VR(3)

      DOUBLE PRECISION X,Y,Z1,Z,ZSQ,RSQ,R,WB,WT,D,CD,F

*  Initial estimate = unrefracted vector
      X = VU(1)
      Y = VU(2)
      Z1 = VU(3)

*  Keep correction approximately constant below about 3 deg elevation
      Z = MAX(Z1,0.05D0)

*  One Newton-Raphson iteration
      ZSQ = Z*Z
      RSQ = X*X+Y*Y
      R = SQRT(RSQ)
      WB = REFB*RSQ/ZSQ
      WT = (REFA+WB)/(1D0+(REFA+3D0*WB)*(ZSQ+RSQ)/ZSQ)
      D = WT*R/Z
      CD = 1D0-D*D/2D0
      F = CD*(1D0-WT)

*  Post-refraction x,y,z
      VR(1) = X*F
      VR(2) = Y*F
      VR(3) = CD*(Z+D*R)+(Z1-Z)

      END