subroutine rutope(ndime,ngaus,posgp,weigp)

!-----------------------------------------------------------------------
!
!     This routine sets up the integration constants of open rules for
!     triangles and tetrahedra
!
!             NDIME = 2             NDIME = 3
!
!          NGAUS  EXACT POL.     NGAUS  EXACT POL.
!          -----  ----------     -----  ----------
!            1       p1            1       p1
!            3       p2            4       p2
!            4       p3            5       p3
!            6       p4           11       p4
!            7       p5           14       p5
!           13       p9
!
!-----------------------------------------------------------------------

  use      typre
  implicit none

  integer(ip), intent(in)  :: ndime,ngaus
  real(rp),    intent(out) :: posgp(ndime,ngaus),weigp(ngaus)
  real(rp)                 :: a,b,c,d,e,f,g,h,p,q,r,w1,w2,w3,w4
  real(rp)                 :: ex1,et1,ez1,ex2,et2,ez2
  integer(ip)              :: istop

  istop=0
!
! Line integral (the same as for brick elements)
!
  if(ndime==1) then
     if(ngaus==1) then
        posgp(1,1)=0.0_rp
        weigp(  1)=2.0_rp
     else if(ngaus==2) then
        posgp(1,1)=-0.577350269189626_rp
        posgp(1,2)= 0.577350269189626_rp
        weigp(  1)= 1.0_rp
        weigp(  2)= 1.0_rp
     else if(ngaus==3) then
        posgp(1,1)=-0.774596669241483_rp
        posgp(1,2)= 0.0_rp
        posgp(1,3)= 0.774596669241483_rp
        weigp(  1)= 0.555555555555556_rp
        weigp(  2)= 0.888888888888889_rp
        weigp(  3)= 0.555555555555556_rp
     else if(ngaus==4) then
        posgp(1,1)=-0.861136311594053_rp
        posgp(1,2)=-0.339981043584856_rp
        posgp(1,3)= 0.339981043584856_rp
        posgp(1,4)= 0.861136311594053_rp
        weigp(1)= 0.347854845137454_rp
        weigp(2)= 0.652145154862546_rp
        weigp(3)= 0.652145154862546_rp
        weigp(4)= 0.347854845137454_rp
!* * * * * * * * * * * * * * * * * * * * * * * * * *
!*
!* Valores añadidos para ngaus==5 tomados de ruqope
!*
!* * * * * * * * * * * * * * * * * * * * * * * * * *
     else if(ngaus==5) then
            posgp(1,1) = -0.906179845938664_rp
            posgp(1,2) = -0.538469310105683_rp
            posgp(1,3) =  0.0_rp
            posgp(1,4) =  0.538469310105683_rp
            posgp(1,5) =  0.906179845938664_rp
            weigp(1) =  0.236926885056189_rp
            weigp(2) =  0.478628670499366_rp
            weigp(3) =  0.568888888888889_rp
            weigp(4) =  0.478628670499366_rp
            weigp(5) =  0.236926885056189_rp
     else
        istop=1
     end if

!
! Area integral (triangles)
!
  else if(ndime==2) then
     if(ngaus==1) then
        posgp(1,1)= 1.0_rp/3.0_rp
        posgp(2,1)= 1.0_rp/3.0_rp
        weigp(  1)= 1.0_rp/2.0_rp
       else if(ngaus==3) then   ! abierta para p2
        posgp(1,1)= 2.0_rp/3.0_rp
        posgp(2,1)= 1.0_rp/6.0_rp
        posgp(1,2)= 1.0_rp/6.0_rp
        posgp(2,2)= 2.0_rp/3.0_rp
        posgp(1,3)= 1.0_rp/6.0_rp
        posgp(2,3)= 1.0_rp/6.0_rp
        weigp(  1)= 1.0_rp/6.0_rp
        weigp(  2)= 1.0_rp/6.0_rp
        weigp(  3)= 1.0_rp/6.0_rp
      else if(ngaus==4) then   !abierta para p3
        posgp(1,1)= 1.0_rp/3.0_rp
        posgp(2,1)= 1.0_rp/3.0_rp
        posgp(1,2)= 1.0_rp/5.0_rp
        posgp(2,2)= 1.0_rp/5.0_rp
        posgp(1,3)= 3.0_rp/5.0_rp
        posgp(2,3)= 1.0_rp/5.0_rp
        posgp(1,4)= 1.0_rp/5.0_rp
        posgp(2,4)= 3.0_rp/5.0_rp
        weigp(  1)=-27.0_rp/96.0_rp
        weigp(  2)= 25.0_rp/96.0_rp
        weigp(  3)= 25.0_rp/96.0_rp
        weigp(  4)= 25.0_rp/96.0_rp
     else if(ngaus==6) then   !abierta para p4
        ex1 = 0.816847572980459_rp
        et1 = 0.091576213509771_rp
        ez1 = 0.091576213509771_rp
        ex2 = 0.108103018168070_rp
        et2 = 0.445948490915965_rp
        ez2 = 0.445948490915965_rp
        posgp(1,1)= ex1
        posgp(2,1)= et1
        posgp(1,2)= et1
        posgp(2,2)= ez1
        posgp(1,3)= ez1
        posgp(2,3)= ex1
        posgp(1,4)= ex2
        posgp(2,4)= et2
        posgp(1,5)= et2
        posgp(2,5)= ez2
        posgp(1,6)= ez2
        posgp(2,6)= ex2
        a = 0.054975870996713638_rp
        b = 0.1116907969117165_rp
        weigp(1)  = a
        weigp(2)  = a
        weigp(3)  = a
        weigp(4)  = b
        weigp(5)  = b
        weigp(6)  = b
     else if(ngaus==7) then !abierta para p5
        e = 1.0_rp / 3.0_rp
        a = ( 6.0_rp +          sqrt ( 15.0_rp ) ) / 21.0_rp
        b = ( 6.0_rp -          sqrt ( 15.0_rp ) ) / 21.0_rp
        c = ( 9.0_rp + 2.0_rp * sqrt ( 15.0_rp ) ) / 21.0_rp
        d = ( 9.0_rp - 2.0_rp * sqrt ( 15.0_rp ) ) / 21.0_rp
        w1 = 0.1125_rp
        w2 = ( 155.0_rp + sqrt ( 15.0_rp ) ) / 2400.0_rp
        w3 = ( 155.0_rp - sqrt ( 15.0_rp ) ) / 2400.0_rp
        posgp(1,1)= e
        posgp(2,1)= e
        posgp(1,2)= a
        posgp(2,2)= a
        posgp(1,3)= d
        posgp(2,3)= a
        posgp(1,4)= a
        posgp(2,4)= d
        posgp(1,5)= b
        posgp(2,5)= b
        posgp(1,6)= c
        posgp(2,6)= b
        posgp(1,7)= b
        posgp(2,7)= c
        weigp(  1)= w1
        weigp(  2)= w2
        weigp(  3)= w2
        weigp(  4)= w2
        weigp(  5)= w3
        weigp(  6)= w3
        weigp(  7)= w3
     else if(ngaus==12) then   !abierta para p6
        posgp(1,1) =  -0.501426509658180_rp
        posgp(1,2) =  -0.501426509658180_rp
        posgp(1,3) =   0.002853019316358_rp
        posgp(1,4) =  -0.873821971016996_rp
        posgp(1,5) =  -0.873821971016996_rp
        posgp(1,6) =   0.747643942033992_rp
        posgp(1,7) =  -0.379295097932432_rp
        posgp(1,8) =   0.273004998242798_rp
        posgp(1,9) =  -0.893709900310366_rp
        posgp(1,10)=  -0.379295097932432_rp
        posgp(1,11)=   0.273004998242798_rp
        posgp(1,12)=  -0.893709900310366_rp
        posgp(2,1) =  -0.501426509658180_rp
        posgp(2,2) =   0.002853019316358_rp
        posgp(2,3) =  -0.501426509658180_rp
        posgp(2,4) =  -0.873821971016996_rp
        posgp(2,5) =   0.747643942033992_rp
        posgp(2,6) =  -0.873821971016996_rp
        posgp(2,7) =   0.273004998242798_rp
        posgp(2,8) =  -0.893709900310366_rp
        posgp(2,9) =  -0.379295097932432_rp
        posgp(2,10)=  -0.893709900310366_rp
        posgp(2,11)=  -0.379295097932432_rp
        posgp(2,12)=   0.273004998242798_rp
        weigp( 1) = 0.233572551452758*0.25_rp
        weigp( 2) = 0.233572551452758*0.25_rp
        weigp( 3) = 0.233572551452758*0.25_rp
        weigp( 4) = 0.101689812740414*0.25_rp
        weigp( 5) = 0.101689812740414*0.25_rp
        weigp( 6) = 0.101689812740414*0.25_rp
        weigp( 7) = 0.165702151236748*0.25_rp
        weigp( 8) = 0.165702151236748*0.25_rp
        weigp( 9) = 0.165702151236748*0.25_rp
        weigp(10) = 0.165702151236748*0.25_rp
        weigp(11) = 0.165702151236748*0.25_rp
        weigp(12) = 0.165702151236748*0.25_rp
!        do igaus =1, ngaus
        posgp = 0.5_rp*(posgp + 1.0_rp)
!        end do
     else if(ngaus==13) then  ! abierta para p7
        a = 0.333333333333333_rp
        b = 0.479308067841920_rp
        c = 0.869739794195568_rp
        d = 0.638444188569810_rp
        e = 0.260345966079040_rp
        f = 0.065130102902216_rp
        g = 0.312865496004874_rp
        h = 0.048690315425316_rp
        w1=-0.149570044467670_rp/2.0_rp
        w2= 0.175615257433204_rp/2.0_rp
        w3= 0.053347235608839_rp/2.0_rp
        w4= 0.077113760890257_rp/2.0_rp
        posgp(1, 1)= a
        posgp(2, 1)= a
        posgp(1, 2)= e
        posgp(2, 2)= e
        posgp(1, 3)= b
        posgp(2, 3)= e
        posgp(1, 4)= e
        posgp(2, 4)= b
        posgp(1, 5)= f
        posgp(2, 5)= f
        posgp(1, 6)= c
        posgp(2, 6)= f
        posgp(1, 7)= f
        posgp(2, 7)= c
        posgp(1, 8)= d
        posgp(2, 8)= g
        posgp(1, 9)= d
        posgp(2, 9)= h
        posgp(1,10)= g
        posgp(2,10)= d
        posgp(1,11)= g
        posgp(2,11)= h
        posgp(1,12)= h
        posgp(2,12)= d
        posgp(1,13)= h
        posgp(2,13)= g
        weigp( 1) = w1
        weigp( 2) = w2
        weigp( 3) = w2
        weigp( 4) = w2
        weigp( 5) = w3
        weigp( 6) = w3
        weigp( 7) = w3
        weigp( 8) = w4
        weigp( 9) = w4
        weigp(10) = w4
        weigp(11) = w4
        weigp(12) = w4
        weigp(13) = w4
! * * * * * * * * * * * * * * * * * * * * * * * * * * *
! *
! *  Regla cerrada para ngaus==15, p5 exacto
! *
! * * * * * * * * * * * * * * * * * * * * * * * * * * *
     else if(ngaus==15) then
        posgp(1, 1)= 0.0_rp
        posgp(2, 1)= 0.0_rp
        posgp(1, 2)= 1.0_rp
        posgp(2, 2)= 0.0_rp
        posgp(1, 3)= 0.0_rp
        posgp(2, 3)= 1.0_rp
        posgp(1, 4)= 1.0_rp/4.0_rp
        posgp(2, 4)= 0.0_rp
        posgp(1, 5)= 1.0_rp/2.0_rp
        posgp(2, 5)= 0.0_rp
        posgp(1, 6)= 3.0_rp/4.0_rp
        posgp(2, 6)= 0.0_rp
        posgp(1, 7)= 3.0_rp/4.0_rp
        posgp(2, 7)= 1.0_rp/4.0_rp
        posgp(1, 8)= 1.0_rp/2.0_rp
        posgp(2, 8)= 1.0_rp/2.0_rp
        posgp(1, 9)= 1.0_rp/4.0_rp
        posgp(2, 9)= 3.0_rp/4.0_rp
        posgp(1,10)= 0.0_rp
        posgp(2,10)= 3.0_rp/4.0_rp
        posgp(1,11)= 0.0_rp
        posgp(2,11)= 1.0_rp/2.0_rp
        posgp(1,12)= 0.0_rp
        posgp(2,12)= 1.0_rp/4.0_rp
        posgp(1,13)= (7.0_rp-sqrt(7.0_rp))/21.0_rp
        posgp(2,13)= (7.0_rp-sqrt(7.0_rp))/21.0_rp
        posgp(1,14)= (7.0_rp+2.0_rp*sqrt(7.0_rp))/21.0_rp
        posgp(2,14)= (7.0_rp-sqrt(7.0_rp))/21.0_rp
        posgp(1,15)= (7.0_rp-sqrt(7.0_rp))/21.0_rp
        posgp(2,15)= (7.0_rp+2.0_rp*sqrt(7.0_rp))/21.0_rp
        weigp(   1)= 11.0_rp*sqrt(7.0_rp)/15120.0_rp+1.0_rp/216.0_rp
        weigp(   2)= 11.0_rp*sqrt(7.0_rp)/15120.0_rp+1.0_rp/216.0_rp
        weigp(   3)= 11.0_rp*sqrt(7.0_rp)/15120.0_rp+1.0_rp/216.0_rp
        weigp(   4)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(   5)= 11.0_rp*sqrt(7.0_rp)/630.0_rp-1.0_rp/30.0_rp
        weigp(   6)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(   7)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(   8)= 11.0_rp*sqrt(7.0_rp)/630.0_rp-1.0_rp/30.0_rp
        weigp(   9)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(  10)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(  11)= 11.0_rp*sqrt(7.0_rp)/630.0_rp-1.0_rp/30.0_rp
        weigp(  12)= 4.0_rp/135.0_rp-4.0_rp*sqrt(7.0_rp)/945.0_rp
        weigp(  13)= 49.0_rp/360.0_rp-7.0_rp*sqrt(7.0_rp)/720.0_rp
        weigp(  14)= 49.0_rp/360.0_rp-7.0_rp*sqrt(7.0_rp)/720.0_rp
        weigp(  15)= 49.0_rp/360.0_rp-7.0_rp*sqrt(7.0_rp)/720.0_rp
     else
        istop=1
     end if
!
! Volume integral ( tetrahedra )
!
  else if(ndime==3) then
     if(ngaus==1) then
        posgp(1,1)= 1.0_rp/4.0_rp
        posgp(2,1)= 1.0_rp/4.0_rp
        posgp(3,1)= 1.0_rp/4.0_rp
        weigp(1)  = 1.0_rp/6.0_rp
     else if(ngaus==4) then
        a=0.5854101966249685_rp
        b=0.1381966011250105_rp
        posgp(1,1)= b
        posgp(2,1)= b
        posgp(3,1)= b
        posgp(1,2)= a
        posgp(2,2)= b
        posgp(3,2)= b
        posgp(1,3)= b
        posgp(2,3)= a
        posgp(3,3)= b
        posgp(1,4)= b
        posgp(2,4)= b
        posgp(3,4)= a
        weigp(  1)= 1.0_rp/24.0_rp
        weigp(  2)= 1.0_rp/24.0_rp
        weigp(  3)= 1.0_rp/24.0_rp
        weigp(  4)= 1.0_rp/24.0_rp
     else if(ngaus==5) then
        posgp(1,1)= 1.0_rp/4.0_rp
        posgp(2,1)= 1.0_rp/4.0_rp
        posgp(3,1)= 1.0_rp/4.0_rp
        posgp(1,2)= 1.0_rp/6.0_rp
        posgp(2,2)= 1.0_rp/6.0_rp
        posgp(3,2)= 1.0_rp/6.0_rp
        posgp(1,3)= 1.0_rp/2.0_rp
        posgp(2,3)= 1.0_rp/6.0_rp
        posgp(3,3)= 1.0_rp/6.0_rp
        posgp(1,4)= 1.0_rp/6.0_rp
        posgp(2,4)= 1.0_rp/2.0_rp
        posgp(3,4)= 1.0_rp/6.0_rp
        posgp(1,5)= 1.0_rp/6.0_rp
        posgp(2,5)= 1.0_rp/6.0_rp
        posgp(3,5)= 1.0_rp/2.0_rp
        weigp(  1)=-2.0_rp/15.0_rp
        weigp(  2)= 1.5_rp/20.0_rp
        weigp(  3)= 1.5_rp/20.0_rp
        weigp(  4)= 1.5_rp/20.0_rp
        weigp(  5)= 1.5_rp/20.0_rp
     else if(ngaus==11) then
        a=0.3994035761667992_rp
        b=0.1005964238332008_rp
        c=343.0_rp/7500.0_rp/6.0_rp
        d=56.0_rp/375.0_rp/6.0_rp
        posgp(1,1) = 1.0_rp/4.0_rp
        posgp(2,1) = 1.0_rp/4.0_rp
        posgp(3,1) = 1.0_rp/4.0_rp
        posgp(1,2) = 11.0_rp/14.0_rp
        posgp(2,2) = 1.0_rp/14.0_rp
        posgp(3,2) = 1.0_rp/14.0_rp
        posgp(1,3) = 1.0_rp/14.0_rp
        posgp(2,3) = 11.0_rp/14.0_rp
        posgp(3,3) = 1.0_rp/14.0_rp
        posgp(1,4) = 1.0_rp/14.0_rp
        posgp(2,4) = 1.0_rp/14.0_rp
        posgp(3,4) = 11.0_rp/14.0_rp
        posgp(1,5) = 1.0_rp/14.0_rp
        posgp(2,5) = 1.0_rp/14.0_rp
        posgp(3,5) = 1.0_rp/14.0_rp
        posgp(1,6) = a
        posgp(2,6) = a
        posgp(3,6) = b
        posgp(1,7) = a
        posgp(2,7) = b
        posgp(3,7) = a
        posgp(1,8) = a
        posgp(2,8) = b
        posgp(3,8) = b
        posgp(1,9) = b
        posgp(2,9) = a
        posgp(3,9) = a
        posgp(1,10)= b
        posgp(2,10)= a
        posgp(3,10)= b
        posgp(1,11)= b
        posgp(2,11)= b
        posgp(3,11)= a
        weigp(1)   =-148.0_rp/1875.0_rp/6.0_rp
        weigp(2)   = c
        weigp(3)   = c
        weigp(4)   = c
        weigp(5)   = c
        weigp(6)   = d
        weigp(7)   = d
        weigp(8)   = d
        weigp(9)   = d
        weigp(10)  = d
        weigp(11)  = d
     else if(ngaus==14) then
        a=0.0673422422100983_rp
        b=0.3108859192633005_rp
        c=0.7217942490673264_rp
        d=0.0927352503108912_rp
        e=0.4544962958743506_rp
        f=0.0455037041256494_rp
        p=0.1126879257180162_rp/6.0_rp
        q=0.0734930431163619_rp/6.0_rp
        r=0.0425460207770812_rp/6.0_rp
        posgp(1,1) = a
        posgp(2,1) = b
        posgp(3,1) = b
        posgp(1,2) = b
        posgp(2,2) = a
        posgp(3,2) = b
        posgp(1,3) = b
        posgp(2,3) = b
        posgp(3,3) = a
        posgp(1,4) = b
        posgp(2,4) = b
        posgp(3,4) = b
        posgp(1,5) = c
        posgp(2,5) = d
        posgp(3,5) = d
        posgp(1,6) = d
        posgp(2,6) = c
        posgp(3,6) = d
        posgp(1,7) = d
        posgp(2,7) = d
        posgp(3,7) = c
        posgp(1,8) = d
        posgp(2,8) = d
        posgp(3,8) = d
        posgp(1,9) = e
        posgp(2,9) = e
        posgp(3,9) = f
        posgp(1,10)= e
        posgp(2,10)= f
        posgp(3,10)= e
        posgp(1,11)= e
        posgp(2,11)= f
        posgp(3,11)= f
        posgp(1,12)= f
        posgp(2,12)= e
        posgp(3,12)= e
        posgp(1,13)= f
        posgp(2,13)= e
        posgp(3,13)= f
        posgp(1,14)= f
        posgp(2,14)= f
        posgp(3,14)= e
        weigp(1)   = p
        weigp(2)   = p
        weigp(3)   = p
        weigp(4)   = p
        weigp(5)   = q
        weigp(6)   = q
        weigp(7)   = q
        weigp(8)   = q
        weigp(9)   = r
        weigp(10)  = r
        weigp(11)  = r
        weigp(12)  = r
        weigp(13)  = r
        weigp(14)  = r
     else
        istop=1
     end if
  end if
!
! Errors
!
  if(istop==1) call runend('RUTOPE: NOT AVAILABLE QUADRATURE')

end subroutine rutope


program CoulombOptimizer

    use iso_fortran_env, only : real64
    implicit none

    integer, parameter :: dp = real64
    integer :: arglen, num_points, dim, num_steps, step, i, u
    character(len=:), allocatable :: filename, num_steps_str
    real(dp) :: energy, step_size
    real(dp), allocatable :: points(:,:), forces(:,:), workspace(:,:)

    if (command_argument_count() < 2) then
        error stop "Needs 2 arguments: <input-file-name> <num-steps>"
    end if

    call get_command_argument(1, length=arglen)
    allocate (character(len=arglen) :: filename)
    call get_command_argument(1, value=filename)

    open(newunit=u, file=filename, action='READ')
    read(u,*) num_points, dim
    allocate(points(dim, num_points), forces(dim, num_points), &
           & workspace(dim, num_points))
    read(u,*) points
    close(u)

    call get_command_argument(2, length=arglen)
    allocate (character(len=arglen) :: num_steps_str)
    call get_command_argument(2, value=num_steps_str)

    read(num_steps_str,*) num_steps
    step_size = 1.0e-3_dp;
    call coulomb_forces(points, energy, forces)
    call normalize_forces(points, forces)

    do step = 1, num_steps
        call quadratic_line_search(points, energy, forces, step_size, workspace)
        points = points + step_size * forces
        call normalize_points(points)
        call coulomb_forces(points, energy, forces)
        call normalize_forces(points, forces)
    end do

    write(*,*) energy
    doi = 1, num_points
        write(*,*) points(:,i)
    end do

contains

    pure subroutine normalize_points(points)
        real(dp), intent(inout) :: points(:,:)
        integer :: i
        do i = 1, size(points, 2)
            points(:,i) = points(:,i) / norm2(points(:,i))
        end do
    end subroutine normalize_points

    pure subroutine normalize_forces(points, forces)
        real(dp), intent(in) :: points(:,:)
        real(dp), intent(inout) :: forces(:,:)
        integer :: i
        do i = 1, size(forces, 2)
            forces(:,i) = forces(:,i) - &
                & dot_product(forces(:,i), points(:,i)) * points(:,i)
        end do
    end subroutine normalize_forces

    pure real(dp) function coulomb_energy(points) result (energy)
        real(dp), intent(in) :: points(:,:)
        integer :: i, j
        energy = 0.0
        do i = 2, size(points, 2)
            do j = 1, i - 1
                energy = energy + 1.0_dp / norm2(points(:,i) - points(:,j))
            end do
        end do
    end function coulomb_energy

    pure subroutine coulomb_forces(points, energy, forces)
        real(dp), intent(in) :: points(:,:)
        real(dp), intent(out) :: energy, forces(:,:)
        real(dp) :: displ(size(points, 1)), inv_dist
        integer :: i, j
        energy = 0.0_dp
        forces = 0.0_dp
        do i = 2, size(points, 2)
            do j = 1, i - 1
                displ = points(:,i) - points(:,j)
                inv_dist = 1.0_dp / norm2(displ)
                energy = energy + inv_dist
                displ = displ * inv_dist**3
                forces(:,i) = forces(:,i) + displ
                forces(:,j) = forces(:,j) - displ
            end do
        end do
    end subroutine coulomb_forces

    pure subroutine quadratic_line_search(points, energy, &
                                        & step_direction, step_size, workspace)
        real(dp), intent(in) :: points(:,:), energy, step_direction(:,:)
        real(dp), intent(inout) :: step_size
        real(dp), intent(out) :: workspace(:,:)
        real(dp) :: new_energy, newer_energy
        workspace = points + step_size * step_direction
        call normalize_points(workspace)
        new_energy = coulomb_energy(workspace)
        if (new_energy < energy) then
            do
                step_size = 2.0_dp * step_size
                workspace = points + step_size * step_direction
                call normalize_points(workspace)
                newer_energy = coulomb_energy(workspace)
                if (newer_energy >= new_energy) then
                    step_size = 0.25_dp * step_size * &
                        & (4.0_dp * new_energy - newer_energy - 3.0_dp * energy) / &
                        & (2.0_dp * new_energy - newer_energy - energy)
                    return
                end if
                new_energy = newer_energy
            end do
        else
            do
                step_size = 0.5_dp * step_size
                workspace = points + step_size * step_direction
                call normalize_points(workspace)
                newer_energy = coulomb_energy(workspace)
                if (newer_energy < new_energy) then
                    step_size = 0.5_dp * step_size * &
                        & (new_energy - 4.0_dp * newer_energy + 3.0_dp * energy) / &
                        & (new_energy - 2.0_dp * newer_energy + energy)
                    return
                end if
                new_energy = newer_energy
            end do
        end if
    end subroutine quadratic_line_search

end program CoulombOptimizer


      subroutine yalin(effsh,tottc)
c********************************************************************
c                                                                   *
c      This routine is called from FN TRCOEF to compute sediment    *
c      transport capacity using the Yalin equation. It is called    *
c      FN SHIELD.                                                   *
c                                                                   *
c********************************************************************
      real effsh
c                                                                   *
c      Arguments                                                    *
c         effsh - effective sheer stress                            *
c         tottc - total sediment transport capacity                 *
c                                                                   *
c********************************************************************
c
      include 'pmxnsl.inc'
      include 'pmxpln.inc'
      include 'pmxprt.inc'
      include 'pmxhil.inc'
      include 'pmxelm.inc'
c
c********************************************************************
c                                                                   *
c     Common Blocks                                                 *
c                                                                   *
c********************************************************************
c
      include 'cconsta.inc'
c
      include 'cpart.inc'
c
      include 'csolva1.inc'
c
      include 'cstruc.inc'
      include 'ctcfrac.inc'
c
c********************************************************************
c                                                                   *
c  tcfrac variables updated                                         *
c     tcf1(mxpart,mxplan)                                           *
c                                                                   *
c********************************************************************
c
c********************************************************************
c                                                                   *
c    local variables:                                               *
c       ws     : sediment transport capacity for particle calss k   *
c                (kg/m*s)                                           *
c       coef   : portion of yalin equation solution                 *
c       ycrit  : ordinate from shields diagram for dimensionless    *
c                critical shear for transport                       *
c       delta  : portion of yalin equation solution                 *
c       sigma  : portion of yalin equation solution                 *
c       p      : sediment transport capacity for particle class     *
c                k (nondimensional)                                 *
c       dltrat : portion of yalin equation solution                 *
c       tottc  : total sediment transport capacity (kg/m*s)         *
c                                                                   *
c********************************************************************
c
      save
      real ws(mxpart), coef(mxpart), ycrit(mxpart), delta(mxpart),
     1    sigma(mxpart), p(mxpart), dltrat(mxpart), tottc, reyn, shield,
     1    t, vstar, yalcon, oldtot, adjtc
      integer k
c
c Initialization:
c
      yalcon = 0.635
      t = 0.0
      tottc = 0.0
c
c     The constant 0.635 was derived empirically by Yalin
c
c     compute shear velocity (vstar):
c
      vstar = sqrt(effsh/msdens)
c
c     compute coefficient coef=vstar*msdens*dia*spg for each
c     particle classes:
c
      coef(npart) = vstar * msdens
      do 10 k = 1, npart
        coef(k) = coef(npart) * dia(k,iplane) * spg(k)
   10 continue
c
c     compute Reynold's number (reyn), dimensionless critical shear
c     parameter from the Shields diagram (ycrit), parameters delta and
c     sigma, and the dimensionless sediment transport capacity (p) for
c     each particle class:
c
      do 20 k = 1, npart
        reyn = vstar * dia(k,iplane) / kinvis
        ycrit(k) = shield(reyn)
        delta(k) = (vstar**2/(spg(k)-1.0)/accgav/dia(k,iplane)/
     1      ycrit(k)) - 1.0
c
        if (delta(k).gt.0.0) then
          sigma(k) = delta(k) * 2.45 * spg(k) ** (-0.4) *
     1        sqrt(ycrit(k))
          p(k) = yalcon * delta(k) * (1.0-1.0/sigma(k)*
     1        alog(1.0+sigma(k)))
          t = t + delta(k)
        else
          delta(k) = 0.0
          p(k) = 0.0
        end if
   20 continue
c
c
c
c     compute the transport capacity (mass per unit width per unit time)
c     ws for each particle class:
c
      if (t.eq.0.0) t = 1000.0
      do 30 k = 1, npart
        dltrat(k) = delta(k) / t
        ws(k) = p(k) * dltrat(k) * coef(k)
c
c RISSE 9/20/94 Add weighting scheme to transport capacity to account
c for the amount of each sediment class being transported
c NOTE: This will increase TC for clays and silts and decrease for sands
c
c       ws(k)=ws(k)*(frac(k,iplane)/0.2)
c Replaced above equation with following to take care of situation
c where other than 5 particle size classes have been used.
c dcf 11/18/94
c XXX Additional question on this is whether we want to use
c XXX "frac(k,iplane)"  OR  "frcflw(k,iplane)" for the weighting?????
c
        ws(k)=ws(k)*(frac(k,iplane)*float(npart))
c
        tottc = tottc + ws(k)
   30 continue
c
c
c XXX Add changes to include Nearing alteration to Tc that was
c XXX previously included in TCEND calculation in PARAM.FOR.  dcf
      oldtot = tottc
      if(sand(1,iplane).gt.0.5)then
        adjtc=0.3 + 0.7 * exp(-12.52*(sand(1,iplane)-0.5))
        if(adjtc.lt.0.30)adjtc=0.30
        tottc = tottc * adjtc
      endif
c
      do 35 k= 1,npart
        if(oldtot.gt.0.0) ws(k) = (ws(k)/oldtot)*tottc
   35 continue
c
      do 40 k = 1, npart
c
c       Lines added 10/16/89 to prevent divide by zero
c       if tottc is zero.
c
        if (tottc.gt.0.0) then
          tcf1(k,iplane) = ws(k) / tottc
        else
          tcf1(k,iplane) = 0.0
        end if
   40 continue
      return
      end


C
C**********************************************************************C
C**********************************************************************C
C**********************************************************************C
C
      SUBROUTINE SETOBC2T(DE_T,D_LTD2,DE_TI)
C
C     TT Version (Only used with TT versions of CALPUV)
C
C CHANGE RECORD
C
C ** SUBROUTINE SETOBC2T SETS OPEN BOUNDARY CONDITIONS FOR CALPUV2T AND
C ** CALPUV2C
C
      USE GLOBAL
C
C**********************************************************************C
C
C **  SET OPEN BOUNDARY SURFACE ELEVATIONS
C
C----------------------------------------------------------------------C
C
      IF(ISDYNSTP.EQ.0)THEN
        TN=DT*FLOAT(N)+TCON*TBEGIN
      ELSE
        TN=TIMESEC
      ENDIF
C
      DO M=1,MTIDE
      TM=MOD(TN,TCP(M))
      TM=PI2*TM/TCP(M)
      CCCOS(M)=COS(TM)
      SSSIN(M)=SIN(TM)
      ENDDO
C
C----------------------------------------------------------------------C
C
      DO LL=1,NPBW
      L=LPBW(LL)
       CC(L)=DE_TI*DXYP(L)
       CS(L)=0.
       CW(L)=0.
       CE(L)=0.
       CN(L)=0.
      FP1(L)=PSERT(NPSERW(LL))
      DO M=1,MTIDE
      TC=CCCOS(M)
      TS=SSSIN(M)
      FP1(L)=FP1(L)+PCBW(LL,M)*TC+PSBW(LL,M)*TS
      ENDDO
      CET=0.5*D_LTD2*G*HRUO(LEAST(L))*RCX(LEAST(L))*HUTMP(LEAST(L))
      IF(ISPBW(LL).GE.1)THEN
       TMP=D_LTD2*SQRT(G*HMU(LEAST(L)))*DXIU(LEAST(L))
       CC(L)=CET*(1.+TMP)/TMP
       CE(L)=-CET
       FP1(L)=CET*(2.*FP1(L)
     & -SQRT(G*HMU(LEAST(L)))*FUHDYE(LEAST(L))*DYIU(LEAST(L))*HUI(LEAST(L)))/TMP
      ELSE
       FP1(LEAST(L))=CET*FP1(L)
       FP1(L)=CC(L)*FP1(L)
      ENDIF
      ENDDO
C
C----------------------------------------------------------------------C
C
      DO LL=1,NPBE
      L=LPBE(LL)
       CC(L)=DE_TI*DXYP(L)
       CS(L)=0.
       CW(L)=0.
       CE(L)=0.
       CN(L)=0.
      FP1(L)=PSERT(NPSERE(LL))
      DO M=1,MTIDE
      TC=CCCOS(M)
      TS=SSSIN(M)
      FP1(L)=FP1(L)+PCBE(LL,M)*TC+PSBE(LL,M)*TS
      ENDDO
      CWT=0.5*D_LTD2*G*HRUO(L  )*RCX(L)*HUTMP(L  )
      IF(ISPBE(LL).GE.1)THEN
       TMP=D_LTD2*SQRT(G*HMU(L))*DXIU(L)
       CC(L)=CWT*(1.+TMP)/TMP
       CW(L)=-CWT
       FP1(L)=CWT*(2.*FP1(L)
     & +SQRT(G*HMU(L))*FUHDYE(L)*DYIU(L)*HUI(L))/TMP
      ELSE
       FP1(LWEST(L))=CWT*FP1(L)
       FP1(L)=CC(L)*FP1(L)
      ENDIF
      ENDDO
C
C----------------------------------------------------------------------C
C
      DO LL=1,NPBS
      L=LPBS(LL)
       CC(L)=DE_TI*DXYP(L)
       CS(L)=0.
       CW(L)=0.
       CE(L)=0.
       CN(L)=0.
      LN=LNC(L)
      FP1(L)=PSERT(NPSERS(LL))
      DO M=1,MTIDE
      TC=CCCOS(M)
      TS=SSSIN(M)
      FP1(L)=FP1(L)+PCBS(LL,M)*TC+PSBS(LL,M)*TS
      ENDDO
      CNT=0.5*D_LTD2*G*HRVO(LN )*RCY(LN)*HVTMP(LN )
      IF(ISPBS(LL).GE.1)THEN
       TMP=D_LTD2*SQRT(G*HMV(LN))*DYIV(LN)
       CC(L)=CNT*(1.+TMP)/TMP
       CN(L)=-CNT
       FP1(L)=CNT*(2.*FP1(L)
     & -SQRT(G*HMV(LN))*FVHDXE(LN)*DXIV(LN)*HVI(LN))/TMP
      ELSE
       FP1(LN)=CNT*FP1(L)
       FP1(L)=CC(L)*FP1(L)
      ENDIF
      ENDDO
C
C----------------------------------------------------------------------C
C
      DO LL=1,NPBN
      L=LPBN(LL)
       CC(L)=DE_TI*DXYP(L)
       CS(L)=0.
       CW(L)=0.
       CE(L)=0.
       CN(L)=0.
      LS=LSC(L)
      FP1(L)=PSERT(NPSERN(LL))
      DO M=1,MTIDE
      TC=CCCOS(M)
      TS=SSSIN(M)
      FP1(L)=FP1(L)+PCBN(LL,M)*TC+PSBN(LL,M)*TS
      ENDDO
      CST=0.5*D_LTD2*G*HRVO(L  )*RCY(L)*HVTMP(L  )
      IF(ISPBN(LL).GE.1)THEN
       TMP=D_LTD2*SQRT(G*HMV(L))*DYIV(L)
       CC(L)=CST*(1.+TMP)/TMP
       CS(L)=-CST
       FP1(L)=CST*(2.*FP1(L)
     & +SQRT(G*HMV(L))*FVHDXE(L)*DXIV(L)*HVI(L))/TMP
      ELSE
       FP1(LS)=CST*FP1(L)
       FP1(L)=CC(L)*FP1(L)
      ENDIF
      ENDDO
      RETURN
      END


MODULE ClassPressao

   USE ClassArrayFonteP
   USE ClassMatrizA

   ! Modulo que implementa a classe para Pressao

   IMPLICIT  NONE

   PRIVATE

   TYPE, PUBLIC :: Pressao

      ! Atritubos Externos serao atualizados por outra rotina
      ! Atributos Internos serao atualizados na classe

      !---------Atributos Externos-----------------------
      INTEGER           :: Tn, Threads                                   ! Numero de passos de tempo resolvidos em paralelo
      INTEGER           :: Nx, Ny                                        ! Nx e Ny da malha
      DOUBLE PRECISION  :: Lx, Ly                                        ! Comprimento do dominio
      DOUBLE PRECISION  :: ht                                            ! Refinamento temporal (necessario por causa dos coeficientes)
      DOUBLE PRECISION  :: Re                                            ! Numero de Reynolds (necessario por causa dos coeficientes)
      DOUBLE PRECISION  :: Qui
      DOUBLE PRECISION  :: Tolerancia                                    ! Tolerancia no criterio de parada dos solvers
      !--------------------------------------------------

      !--------Atributos Internos------------------------
      DOUBLE PRECISION                                 :: integral_de_p  ! A integral da pressao
      DOUBLE PRECISION                                 :: hx, hy         ! hx e hy da malha
      DOUBLE PRECISION                                 :: C_Normalizacao ! Constante de Normalizacao

      ! ATENCAO: DEFINIR UMA VETOR DE VETORES E RESERVAR VARIAVEIS v0, v1, v2, v3, ..., vt dinamicamente (de acordo com o a ordem do metodo de projecao no futuro)
      DOUBLE PRECISION, DIMENSION(:, :), ALLOCATABLE      :: p, q, p_bp, p_ex, residuo       ! Vetores da pressao
      DOUBLE PRECISION, DIMENSION(:), ALLOCATABLE         :: p_ref
      INTEGER, DIMENSION(:, :), ALLOCATABLE :: threads_if
      DOUBLE PRECISION, DIMENSION(:),    ALLOCATABLE      :: normas_L2

      ! A matriz A de V
      TYPE ( MatrizA ) :: A

      TYPE (ArrayFonteP),  DIMENSION(9) :: FonteP
      !--------------------------------------------------

      CONTAINS

         ! Metodos da Classe
         procedure :: init                      ! Metodo que inicia a classe
         procedure :: calcula_integral_da_pressao  ! Metodo que calcula a integral da pressao para garantir a unicidade da solucao
         procedure :: corrige_pressao_com_a_integral
         procedure :: aplica_condicao_inicial
         procedure :: normaliza_pressao
         procedure :: calcula_termo_fonte
         procedure :: single_grid
         procedure :: multi_grid
         procedure :: atualiza_pressao
         procedure :: escreve_matriz
         procedure :: escreve_comparacao
         procedure :: calcula_subdominios
         procedure :: calcula_residuo
         procedure :: calcula_norma_L2

   END TYPE Pressao

   CONTAINS

      SUBROUTINE init(self, Nx, Ny, Tn, Lx, Ly, ht, Re, Qui, Tolerancia, Threads)

         USE Subrotinas_MG_GS

         !------------ Metodo que inicia as variaveis gerais da malha ---------------------

         CLASS( Pressao ), INTENT(INOUT) :: self

         INTEGER         , INTENT(IN) :: Nx, Ny, Tn, Threads
         DOUBLE PRECISION, INTENT(IN) :: Lx, Ly, ht, Re, Qui, Tolerancia

         INTEGER :: stencil

         ! Atualiando os atributos da classe
         self % Tn = Tn
         self % Lx  = Lx
         self % Ly  = Ly
         self % Re  = Re
         self % Qui = Qui
         self % ht  = ht
         self % Tolerancia = Tolerancia
         self % Threads = Threads

         ! Calculando hx e hy (o mesmo valor para u, v e p)
         self % hx = self % Lx / Nx
         self % hy = self % Ly / Ny

         self % Nx = Nx
         self % Ny = Ny

         ! Reservando os vetores p, q
         ALLOCATE( self % p( self % Nx * self % Ny, self % Tn ), &
                   self % q( self % Nx * self % Ny, self % Tn ), &
                   self % p_bp( self % Nx * self % Ny, self % Tn ), &
                   self % p_ex( self % Nx * self % Ny, self % Tn ), &
                   self % threads_if(self % Threads, 2 ), &
                   self % residuo( self % Nx * self % Ny, self % Tn ), &
                   self % normas_L2( self % Tn ), &
                   self % p_ref( self % Nx * self % Ny ) )

         ! Iniciando os vetores
         self % p(:, :) = 0.0d0
         self % q(:, :)  = 0.0d0
         self % p_bp(:, :)  = 0.0d0
         self % p_ex(:, :) = 0.0d0
         self % p_ref(:) = 0.0d0
         self % residuo(:, :) = 0.0d0
         self % normas_L2( self % Tn ) = 0.d0

         !-------------------------- matriz A de P --------------------------------
         ! Inicializando a matriz A de U
         CALL self % A % init(self % Nx, self % Ny, &
                              self % hx, self % hy, self % ht, &
                              self % Re)

         ! Atribuindo um stencil a cada volume de A
         CALL self % A % atribui_stencils()

         ! Calculandos os coeficientes e posicoes dos stencils
         CALL self % A % stencils_p()
         !-------------------------- matriz A de P --------------------------------

         ! Inicializando o array de fontes de P
          DO stencil = 1, 9
            CALL self % FonteP(stencil) % init(stencil)
         END DO

         CALL calcula_exata(self)
         CALL calcula_subdominios(self)

         CALL Atualiza_Variaveis_MG_GS_P(self % Lx, self % Ly, &
                                         self % Nx, self % Ny, &
                                         self % Tolerancia, &
                                         3, 3, &
                                         50, &
                                         self % ht, &
                                         self % Re, self % Tn )

      END SUBROUTINE init

      SUBROUTINE calcula_subdominios(self)

          CLASS (Pressao), INTENT(INOUT) :: self

          INTEGER :: Nsub, tc, C, thread, it, ft

          ! Estimando incorretamente o numero de passos de tempo por thread
          Nsub = FLOOR(DBLE(self % Nx * self % Ny)/DBLE(self % Threads))

          ! tc e a thread de referencia
          tc = self % Threads + 1 - (self % Nx* self % Ny - self % Threads * Nsub)

          ! Calculando o inicio e fim de cada thread, isto e, quais passos de tempo cada thread vai assumir
          DO thread = 1, self % Threads
              IF (thread >= tc) THEN
                  C = 1
              ELSE
                  C = 0
              END IF

              it = (thread -1) * Nsub + 1 + C * (thread - tc)
              ft = thread * Nsub + C * (thread - tc + 1)

              ! Salvando estas informacoes
              self % threads_if(thread, 1) = it
              self % threads_if(thread, 2) = ft

          END DO

      END SUBROUTINE calcula_subdominios

      SUBROUTINE calcula_integral_da_pressao( self, t )

         CLASS (Pressao), INTENT(INOUT) :: self

         INTEGER, INTENT(IN) :: t

         ! Regra do Retangulo
         self % integral_de_p = self % hx* self % hy * SUM( self % p(:, t) )

         WRITE(*,*) "Integral da pressao: ", self % integral_de_p

      END SUBROUTINE calcula_integral_da_pressao

      SUBROUTINE aplica_condicao_inicial( self )
         USE FuncoesAlias

         CLASS (Pressao), INTENT(INOUT) :: self

         INTEGER :: volume

         DO volume = 1, self % Nx * self % Ny

            self % p(volume, :) = pressao_inicial(0, volume)

         END DO

      END SUBROUTINE aplica_condicao_inicial

      SUBROUTINE normaliza_pressao( self )
         CLASS (Pressao) :: self

         !self % C_Normalizacao = SUM(self % p1 * self % hx * self % hy) / SUM((self % p1 * 0.0d0+1.0d0)* self % hx * self %hy)

         self % C_Normalizacao = SUM(self % p(:,1)) / (self % Nx * self % Ny)

         self % p(:,:) = self % p(:,:) - self % C_Normalizacao

      END SUBROUTINE normaliza_pressao

      SUBROUTINE calcula_exata( self )
         USE FuncoesAlias

         CLASS (Pressao), INTENT(INOUT) :: self

         INTEGER :: volume, t
         DOUBLE PRECISION :: C

         DO t = 1, self % Tn

             DO volume = 1, self % Nx * self % Ny

                 self % p_ex(volume, t) = pressao_analitica(t, volume)

             END DO

            C = SUM(self % p_ex(:,t)) / (self % Nx * self % Ny)

            self % p_ex(:,t) = self % p_ex(:,t) - C

         END DO

      END SUBROUTINE calcula_exata

      SUBROUTINE calcula_termo_fonte( self, t, t0, ut, vt)

         CLASS(Pressao), INTENT(INOUT) :: self

         INTEGER, INTENT(IN) :: t, t0
         DOUBLE PRECISION, DIMENSION(:, :), INTENT(IN) :: ut, vt

         INTEGER :: volume, stencil

         DO volume = 1, self % Nx * self % Ny

            ! Stencil do volume
            stencil = self % A % v(volume)
            !stencil = self % A % calcula_stencil(volume)

            self % p_bp(volume, t) = self % FonteP( stencil ) % FonteStencil(t0, &
                                                                             volume, &
                                                                             self % Nx, &
                                                                             self % hx, self % hy, self % ht, &
                                                                             ut(:, t), &
                                                                             vt(:, t))
            self % p_bp(volume, t) = self % p_bp(volume, t)
            !write(*,*) self % p_bp(volume), volume

         END DO

      END SUBROUTINE calcula_termo_fonte

      SUBROUTINE single_grid( self, t, iteracoes )

         USE GS

         CLASS (Pressao) :: self

         INTEGER, INTENT(IN) :: t, iteracoes

         ! Resolvendo com Gauss Seidel
         CALL GaussSeidel( self % A, self % q(:, t), self % p_bp(:, t), iteracoes )

      END SUBROUTINE single_grid

      SUBROUTINE calcula_residuo( self, t )

          USE Residuo

          CLASS (Pressao) :: self

          INTEGER, INTENT(IN) :: t

          ! Calcula residuo
          CALL calcula_residuo_P( self % A, self % q, self % p_bp, self % residuo, t )

      END SUBROUTINE calcula_residuo

      SUBROUTINE calcula_norma_L2( self, t )

          USE Residuo

          CLASS (Pressao) :: self

          INTEGER, INTENT(IN) :: t

          ! Calcula residuo
          self % normas_L2(t) = SUM(self % residuo(:, t) * self % residuo(:, t))

      END SUBROUTINE calcula_norma_L2

      SUBROUTINE multi_grid( self, t, ciclos_V )

         USE Subrotinas_MG_GS

         CLASS (Pressao) :: self

         INTEGER, INTENT(IN) :: t, ciclos_V

         CALL MG_GS_P( self % q(:, t), self % p_bp(:, t), ciclos_V, t )

      END SUBROUTINE multi_grid

      SUBROUTINE corrige_pressao_com_a_integral(self, t)

         CLASS (Pressao) :: self

         INTEGER, INTENT(IN) :: t
         DOUBLE PRECISION :: integral_q, integral_q_antes, integral_q_depois

         !write(*,*)
         integral_q_antes = self % hx* self % hy * SUM( self % q(:, t) ) ! Calcula integral
         !write(*,*) "Integral de Q ANTES", integral_q_antes

         !---------------- Para descobrir o fator da integral (VERIFICAR SE ESTA PARTE NAO E EQUIVALENTE AO CP/C1 NORMALIZACAO)
         self % q(:, t) = self % q(:, t) - integral_q_antes               ! Atualiza vetor com integral
         integral_q_depois = self % hx* self % hy * SUM( self % q(:, t) ) ! Recalcula a integral
         self % q(:, t) = self % q(:, t) + integral_q_antes               ! Remove o valor da integral, ou seja, volta pra q original
         !--------------------------------------------------------------

         self % q(:, t) = self % q(:, t) - integral_q_antes/(1.0d0 - integral_q_depois/integral_q_antes)  ! Agora sim esta garantido integral nula

         !integral_q = self % hx* self % hy * SUM( self % q(:) ) ! Calcula integral
         !write(*,*) "Integral de Q DEPOIS", integral_q

      END SUBROUTINE corrige_pressao_com_a_integral

      SUBROUTINE atualiza_pressao( self, t, t0, thread )

         CLASS (Pressao) :: self

         INTEGER, INTENT(IN) :: t, t0, thread
         INTEGER :: volume

         !self % ps(:) = 2 * self % p1(:) - self % p0(:)
         !self % p0(:) = self % p1(:)

         DO volume = self % threads_if(thread, 1), self % threads_if(thread, 2)

            self % p(volume, t) = self % p(volume, t0) +  &
                                  self % q(volume, t) + &
                                  self % Qui*(self % ht/(1.0d0 * self % Re)) * self % p_bp(volume, t)

         END DO

         !self % ps(:) = 2 * self % p1(:) - self % p0(:)

      END SUBROUTINE atualiza_pressao

      SUBROUTINE escreve_matriz(self, nome, vetor)

         CLASS (Pressao) :: self

         CHARACTER(len=*), INTENT(IN) :: nome
         DOUBLE PRECISION, DIMENSION(:), INTENT(IN) :: vetor

         INTEGER :: pp, i, j
         CHARACTER(len=(self % Nx*23)) :: linha
         CHARACTER(len=23) :: valor

         OPEN(UNIT=41, FILE=TRIM(ADJUSTL(nome)), STATUS='REPLACE', ACTION='WRITE')

         ! Para pontos internos
         DO j = 1, self % Ny
            DO i = 1, self % Nx

               pp = i + (j-1)* self % Nx

               WRITE(valor, "(1PE22.15)") vetor(pp)
               linha((i-1)*23+1:(i-1)*23+23) = valor

            END DO

            WRITE(41,1) linha

         END DO
         CLOSE(41)

         1 FORMAT( A )

      END SUBROUTINE escreve_matriz

      SUBROUTINE escreve_comparacao(self, tempo, nome, vetor)

         USE FuncoesAlias

         CLASS (Pressao) :: self

         CHARACTER(len=*), INTENT(IN) :: nome
         INTEGER, INTENT(IN) :: tempo
         DOUBLE PRECISION, DIMENSION(:), INTENT(IN) :: vetor

         INTEGER :: pp, i, j, p_Linf
         DOUBLE PRECISION :: C1, CP, Linf

         1 FORMAT( 1PE22.15, 3X, 1PE22.15, 3X, 1PE22.15, 3X, I10, 3X, I10 )
         2 FORMAT( 1PE22.15, 1X, "= ht", /,  &
                   1PE22.15, 1X, "= Erro Maximo", /,  &
                   1PE22.15, 1X, "= Analitica (onde o erro aconteceu)", /, &
                   1PE22.15, 1X, "=  Numerica (onde o erro aconteceu)" )

        ! Obtendo as constantes de normalizacao
        CP = 0.0d0
        C1 = 0.0d0
        DO pp = 1, self % Nx * self % Ny

            CP = CP +  pressao_analitica(tempo, pp) * self % hx * self % hy
            C1 = C1 + self % hx * self % hy

        END DO

         OPEN(UNIT=41, FILE=TRIM(ADJUSTL(nome)), STATUS='REPLACE', ACTION='WRITE')

         Linf   = 0.0d0
         p_Linf  = 0

         ! Para pontos internos
         DO j = 1, self % Ny
            DO i = 1, self % Nx

               pp = i + (j-1)* self % Nx

               !WRITE(41, 1) pressao_analitica(tempo, pp) - CP/C1, vetor(pp), pressao_analitica(tempo, pp) - CP/C1 - vetor(pp)

               IF ( ABS(pressao_analitica(tempo, pp) - CP/C1 - vetor(pp)) > Linf ) THEN
                  Linf = ABS(pressao_analitica(tempo, pp) - CP/C1 - vetor(pp))
                  p_Linf  = pp
               END IF

            END DO

         END DO

         WRITE(41, 2) self % ht, Linf, pressao_analitica(tempo, p_Linf) - CP/C1, vetor(p_Linf)

         CLOSE(41)

      END SUBROUTINE escreve_comparacao

END MODULE ClassPressao


module mt19937
  implicit none

  ! integer precision constants
  integer, parameter :: int32 = selected_int_kind(9)

  ! Period parameters
  integer, parameter :: N = 624
  integer, parameter :: M = 397
  integer(int32), parameter :: MATRIX_A = z'9908b0df'   ! constant vector a
  integer(int32), parameter :: UPPER_MASK = z'80000000' ! most signif. w-r bits
  integer(int32), parameter :: LOWER_MASK = z'7fffffff' ! least signif. r bits

  ! Tempering parameters
  integer(int32), parameter :: TMASKB = z'9d2c5680'
  integer(int32), parameter :: TMASKC = z'efc60000'

  ! state
  integer(int32), dimension(N) :: mt ! the array for the state vector
  integer(int32) :: mti

  integer(int32), dimension(0:1), parameter :: mag01 = [ 0, MATRIX_A ]
  ! mag01(x) = x * MATRIX_A  for x=0,1

contains

  ! initializes mt[N] with a seed
  subroutine init_genrand(s)
    integer(int32), intent(in) :: s

    mt(1) = s
    do mti = 2, N
       mt(mti) = 1812433253 * ieor(mt(mti-1), ishft(mt(mti-1), -30)) + mti - 1
    end do
  end subroutine init_genrand

  ! generates a random number on [0,0xffffffff]-interval
  function genrand_int32() result(y)
    integer(int32) :: y
    integer :: kk

    ! generate N words at once
    if (mti >= N + 1) then
        do kk = 1, N - M
           y = ior(iand(mt(kk), UPPER_MASK), iand(mt(kk+1), LOWER_MASK))
           mt(kk) = ieor(ieor(mt(kk+M), ishft(y, -1)), mag01(iand(y,1)))
        end do
        do kk = N - M + 1, N - 1
           y = ior(iand(mt(kk), UPPER_MASK), iand(mt(kk+1), LOWER_MASK))
           mt(kk) = ieor(ieor(mt(kk + (M - N)), ishft(y, -1)), mag01(iand(y, 1)))
        end do
        y = ior(iand(mt(N), UPPER_MASK), iand(mt(1), LOWER_MASK))
        mt(N) = ieor(ieor(mt(M), ishft(y, -1)), mag01(iand(y, 1)))
        mti = 1
    end if

    y = mt(mti)

    ! Tempering
    y = ieor(y, ishft(y, -11))
    y = ieor(y, iand(ishft(y, 7), TMASKB))
    y = ieor(y, iand(ishft(y, 15), TMASKC))
    y = ieor(y, ishft(y, -18))

    mti = mti + 1
  end function genrand_int32

end module mt19937

program test
  use mt19937
  implicit none

  integer(int32), parameter :: init = 4357
  integer(int32), parameter :: shift = 2147483649
  integer(int32) :: u
  integer :: i

  call init_genrand(init)

  do i = 1, 100000
     u = genrand_int32()
  end do
  print '(i0)', u
  print '(b0.32)', u

  if (u /= 1275196716) then
     stop 'ERROR'
  end if
end program test


!-----------------------------------------------------------------------
!
! MODULE: sn_module
!> @brief
!> This module contains the variables and setup subroutines related to
!> the mock discrete ordinates treament in SNAP.
!
!-----------------------------------------------------------------------

MODULE sn_module

  USE global_module, ONLY: i_knd, r_knd, zero, one

  IMPLICIT NONE

  PUBLIC

  SAVE
!_______________________________________________________________________
!
! Module Input Variables
!
! nang     - number of discrete ordinates per octant
! nmom     - scattering order
!_______________________________________________________________________

  INTEGER(i_knd) :: nang=1, nmom=1
!_______________________________________________________________________
!
! Run-time variables
!
! cmom      - computational number of moments according to nmom & ndimen
! noct      - number of directional octants
! mu(nang)  - x direction cosines
! eta(nang) - y direction cosines
! xi(nang)  - z direction cosines
! w(nang)   - angle weights
!
! wmu(nang)  - w*mu
! weta(nang) - w*eta
! wxi(nang)  - w*xi
!
! ec(nang,cmom,noct) - Scattering expansion coefficients
! lma(nmom)          - number of 'm' moments per order l
!_______________________________________________________________________

  INTEGER(i_knd) :: cmom, noct

  INTEGER(i_knd), ALLOCATABLE, DIMENSION(:) :: lma

  REAL(r_knd), ALLOCATABLE, DIMENSION(:) :: mu, eta, xi, w, wmu, weta, &
    wxi

  REAL(r_knd), ALLOCATABLE, DIMENSION(:,:,:) :: ec

  CONTAINS

  SUBROUTINE sn_allocate ( ndimen, istat )

!-----------------------------------------------------------------------
!
! Allocate sn_module arrays.
!
!-----------------------------------------------------------------------

    INTEGER(i_knd), INTENT(IN) :: ndimen

    INTEGER(i_knd), INTENT(INOUT) :: istat
!_______________________________________________________________________
!
!   Allocate cosines and weights
!_______________________________________________________________________

    cmom = nmom
    noct = 2

    ALLOCATE( mu(nang), w(nang), wmu(nang), STAT=istat )
    IF ( istat > 0 ) RETURN

    IF ( ndimen > 1 ) THEN
      cmom = nmom*(nmom+1)/2
      noct = 4
      ALLOCATE( eta(nang), weta(nang), STAT=istat )
    ELSE
      ALLOCATE( eta(0), weta(0), STAT=istat )
    END IF
    IF ( istat > 0 ) RETURN

    IF ( ndimen > 2 ) THEN
      cmom = nmom**2
      noct = 8
      ALLOCATE( xi(nang), wxi(nang), STAT=istat )
    ELSE
      ALLOCATE( xi(0), wxi(0), STAT=istat )
    END IF
    IF ( istat > 0 ) RETURN

    ALLOCATE( ec(nang,cmom,noct), lma(nmom), STAT=istat )
    IF ( istat > 0 ) RETURN

    wmu = zero
    IF ( ndimen > 1 ) weta = zero
    IF ( ndimen > 2 ) wxi = zero
    ec = zero
    lma = 0
!_______________________________________________________________________
!_______________________________________________________________________

  END SUBROUTINE sn_allocate

  SUBROUTINE sn_deallocate

!-----------------------------------------------------------------------
!
! Deallocate sn_module arrays.
!
!-----------------------------------------------------------------------
!_______________________________________________________________________

    DEALLOCATE( mu, w, eta, xi, wmu, weta, wxi, ec, lma )
!_______________________________________________________________________
!_______________________________________________________________________

  END SUBROUTINE sn_deallocate

  SUBROUTINE expcoeff ( ndimen )

!-----------------------------------------------------------------------
!
! Compute and store the scattering expansion coefficients. Coefficient
! polynomial is (mu*eta*xi)^l, where l is the moment, starting at 0.
!
!-----------------------------------------------------------------------

    INTEGER(i_knd), INTENT(IN) :: ndimen
!_______________________________________________________________________
!
!   Local variables
!_______________________________________________________________________

    INTEGER(i_knd) :: id, jd, kd, is, js, ks, l, oct, m, mom
!_______________________________________________________________________
!
!   Set the coefficient as simply the angle raised to a power equal to
!   the moment-1. Set octant and loop according to dimension.
!_______________________________________________________________________

    SELECT CASE ( ndimen )

      CASE ( 1 )
        wmu = w*mu
        lma = 1
        DO id = 1, 2
          is = -1
          IF ( id == 2 ) is = 1
          ec(:,1,id) = one
          DO l = 2, nmom
            ec(:,l,id) = (is*mu)**(2*l-3)
          END DO
        END DO

      CASE ( 2 )

        wmu = w*mu
        weta = w*eta

        DO l = 1, nmom
          lma(l) = l
        END DO

        DO jd = 1, 2
          js = -1
          IF ( jd == 2 ) js = 1
          DO id = 1, 2
            is = -1
            IF ( id == 2 ) is = 1
            oct = 2*(jd-1) + id
            ec(:,1,oct) = one
            mom = 2
            DO l = 2, nmom
              DO m = 1, l
                ec(:,mom,oct) = (is*mu)**(2*l-3)*(js*eta)**(m-1)
                mom = mom + 1
              END DO
            END DO
          END DO
        END DO

      CASE ( 3 )

        wmu = w*mu
        weta = w*eta
        wxi = w*xi

        DO l = 1, nmom
          lma(l) = 2*l - 1
        END DO

        DO kd = 1, 2
          ks = -1
          IF ( kd == 2 ) ks = 1
          DO jd = 1, 2
            js = -1
            IF ( jd == 2 ) js = 1
            DO id = 1, 2
              is = -1
              IF ( id == 2 ) is = 1
              oct = 4*(kd-1) + 2*(jd-1) + id
              ec(:,1,oct) = one
              mom = 2
              DO l = 2, nmom
                DO m = 1, 2*l-1
                  ec(:,mom,oct) = (is*mu)**(2*l-3)*(ks*xi*js*eta)**(m-1)
                  mom = mom + 1
                END DO
              END DO
            END DO
          END DO
        END DO

    END SELECT
!_______________________________________________________________________
!_______________________________________________________________________

  END SUBROUTINE expcoeff

END MODULE sn_module


************************************************************************
*
*     Subroutine PPCCTEST                            Called by: AMLSTAT
*
*     Probability Plot Correlation Coefficient normality test for left-
*     censored data (Royston, 1993). Extended to multiply censored data
*     by Millard in the Environmental Stats for S+ library.  This
*     subroutine and several subordinate subroutines were provided by
*     Dave Lorenz (February, 2013); modified by R. Runkel (March, 2013).
*
*     This procedure is similar to the PPCC test for uncensored data.
*     It computes the correlation coefficient between the uncensored
*     quantiles of the data and the corresponding normal quantiles.
*     The test statistic is the correlation. The attained p-values are
*     empirical and only approximate for multiply-censored data.
*
*     The null hypothesis that the data are normally distributed is
*     rejected if the value of the test statistic is less than the
*     critical value for the number of observations and censoring.
*
*     Note that IPPCCERR is 0 for the singly censored test and -1 for
*     the multiply censored test. The attained p-value for the multiply
*     censored test is only approximate. IPPCCERR > 0 indicates an
*     error and the results are not valid.
*
************************************************************************
      SUBROUTINE PPCCTEST(CENSFLAG,NOBSC,RESPPCC,PPCC,PLEV,IPPCCERR)
*
*     subroutine arguments
*
      LOGICAL CENSFLAG(*)
      INTEGER*4 IPPCCERR,NOBSC
      DOUBLE PRECISION PPCC,PLEV,RESPPCC(*)
*
*     local vars
*
      INTEGER*4 I,NDL,NUNCEN,NUY,IX(NOBSC)
      INTEGER*4, DIMENSION(:), ALLOCATABLE :: AJ,BJ,CJ
      DOUBLE PRECISION CDL,D,DELTA,MU,QTB2,SIGMA,U,W,Z,ZSMOOTH1,
     &                 ZSMOOTH2,ZSMOOTH3
      DOUBLE PRECISION PP(NOBSC),QQ(NOBSC),Y(NOBSC)
*
*     function declarations
*
      DOUBLE PRECISION CORR,ERFC,QNORM01
*
*     sort the data and count unique detection limits for load
*
      IPPCCERR = 0
      IF(NOBSC .GT. 5000) THEN
         IPPCCERR = 4
         RETURN
      ENDIF
      CALL PPORDER(CENSFLAG,RESPPCC,NOBSC,IX)
      CDL = -1.D99
      NDL = 0
      DO I=1,NOBSC
         IF(CENSFLAG(IX(I)) .AND. RESPPCC(IX(I)) .GT. CDL) THEN
            NDL = NDL + 1
            CDL = RESPPCC(IX(I))
         ENDIF
      ENDDO
*
*     compute the plotting positions and convert to quantiles of the
*     standard normal
*
      ALLOCATE(AJ(NDL+2))
      ALLOCATE(BJ(NDL+2))
      ALLOCATE(CJ(NDL+2))
      CALL PPARRANGE(RESPPCC,CENSFLAG,NOBSC,AJ,BJ,CJ,NDL,Y,NUNCEN,IX)
      CALL PPLOT(AJ,BJ,NDL,PP,IPPCCERR)
*
*     error check on QNORM01 and unique values of Y
*
      NUY = 0
      DO I=1,NUNCEN
         QQ(I) = QNORM01(PP(I))
         IF (QQ(I) .EQ. 0.D0 .AND. PP(I) .NE. 0.5D0) IPPCCERR=3
         IF (I .GT. 1 .AND. Y(I) .GT. Y(I-1)) NUY = NUY + 1
      ENDDO
      IF(NUY .LT. 2) THEN
         IPPCCERR=2
         RETURN
      ENDIF
*
*     compute the correlation and test stat between the uncensored and
*     qnormals
*
      U = LOG(DBLE(NOBSC))
      PPCC = CORR(NUNCEN, QQ, Y)
      W = LOG(1.D0 - PPCC**2)
      Z = (W - (-1.2725D0 + 1.0521D0 * (LOG(U) - U))) /
     &    (1.0308D0 - 0.26758D0 * (LOG(U) + 2.D0/U))
*
*     compute the uncensored (first part of IF) or censored ppcc.test
*     (ELSE), as appropriate.
*
*     For the censored test, compute the linear regression between the
*     smoothed Zs and the actual Z scores for the 0.9, .95, and .99
*     pvalues.  This is directly accomplished by application of the QR
*     decomposition of the matrix of 1s and the Z scores.
*
      IF(NDL .EQ. 0) THEN
         PLEV = ERFC(Z/1.4142135623731D0)/2.D0
      ELSE
         D = 0.76676D0 * U + 0.015814D0 * U**2
         DELTA = -LOG(DBLE(NOBSC - NUNCEN)/DBLE(NOBSC))
         ZSMOOTH1 = 1.2815515655446D0 + D *
     &              (0.164D0 + 0.533D0 * 0.556D0**U)**DELTA
         ZSMOOTH2 = 1.64485362695147D0 + D *
     &              (0.1736D0 + 0.315D0 * 0.622D0**U)**DELTA
         ZSMOOTH3 = 2.32634787404084D0 + D *
     &              (0.256D0 - 0.00635D0 * U)**DELTA
         QTB2 = ((0.625724703684127D0 * ZSMOOTH1) + 0.141396876352102D0
     &           * ZSMOOTH2) - 0.767121580036229D0 * ZSMOOTH3
         MU = (-0.577350269189626D0 * (((-0.577350269189626D0*ZSMOOTH1)
     &         - 0.577350269189626D0*ZSMOOTH2) - 0.577350269189626D0 *
     &         ZSMOOTH3)) + 2.33419583910949D0 * QTB2
         SIGMA = -1.33312711041740D0 * QTB2
         PLEV = ERFC((Z - MU)/SIGMA/1.4142135623731D0)/2.D0
      ENDIF

      RETURN
      END


MODULE BALL_TREE_I8
  USE ISO_FORTRAN_ENV, ONLY: REAL64, INT64, INT8
  USE SWAP
  USE FAST_SELECT
  USE FAST_SORT
  IMPLICIT NONE

CONTAINS

  ! Re-arrange elements of POINTS into a binary ball tree.
  RECURSIVE SUBROUTINE BUILD_TREE(POINTS, SQ_SUMS, RADII, ORDER,&
       ROOT, LEAF_SIZE, COMPUTED_SQ_SUMS)
    INTEGER(KIND=INT8),  INTENT(INOUT), DIMENSION(:,:) :: POINTS
    INTEGER(KIND=INT64), INTENT(OUT),   DIMENSION(:) :: SQ_SUMS
    REAL(KIND=REAL64),   INTENT(OUT),   DIMENSION(:) :: RADII
    INTEGER(KIND=INT64), INTENT(INOUT), DIMENSION(:) :: ORDER
    INTEGER(KIND=INT64), INTENT(IN), OPTIONAL :: ROOT, LEAF_SIZE
    LOGICAL,             INTENT(IN), OPTIONAL :: COMPUTED_SQ_SUMS
    ! Local variables
    INTEGER(KIND=INT64) :: CENTER_IDX, MID, I, J, LS
    INTEGER(KIND=INT64), DIMENSION(SIZE(POINTS,1)) :: PT
    INTEGER(KIND=INT64), DIMENSION(SIZE(ORDER))    :: SQ_DISTS
    INTEGER(KIND=INT64) :: MAX_SQ_DIST, SQ_DIST
    ! Set the leaf size to 1 by default (most possible work required,
    ! but guarantees successful use with any leaf size).
    IF (PRESENT(LEAF_SIZE)) THEN ; LS = LEAF_SIZE
    ELSE                         ; LS = 1
    END IF
    ! If no squared sums were provided, compute them.
    IF (.NOT. PRESENT(COMPUTED_SQ_SUMS) .OR. &
         .NOT. COMPUTED_SQ_SUMS) THEN
       DO I = 1, SIZE(POINTS,2)
          SQ_SUMS(I) = SUM(POINTS(:,I)**2)
       END DO
    END IF
    ! Set the index of the 'split'
    IF (PRESENT(ROOT)) THEN ; CENTER_IDX = ROOT
    ELSE
       ! 1) Compute distances between first point (random) and all others.
       ! 2) Pick the furthest point (on conv hull) from first as the center node.
       J = ORDER(1)
       PT(:) = INT(POINTS(:,J), INT64)
       CENTER_IDX = 1
       MAX_SQ_DIST = 0
       DO I = 2, SIZE(ORDER)
          SQ_DIST = SQ_SUMS(J) + SQ_SUMS(ORDER(I)) - &
               2 * DOT_PRODUCT(POINTS(:,ORDER(I)), PT(:))
          ! If this is a new max distance, record this point.
          IF (SQ_DIST .GT. MAX_SQ_DIST) THEN
             MAX_SQ_DIST = SQ_DIST
             CENTER_IDX = I
          END IF
       END DO
       ! Now CENTER_IDX is the selected center for this node in tree.
    END IF
    ! Move the "center" to the first position.
    CALL SWAP_I64(ORDER(1), ORDER(CENTER_IDX))
    ! Measure squared distance beween "center" node and all other points.
    J = ORDER(1)
    PT(:) = INT(POINTS(:,J), INT64)
    SQ_DISTS(1) = 0
    CENTER_TO_ALL : DO I = 2, SIZE(ORDER)
       SQ_DISTS(I) = SQ_SUMS(J) + SQ_SUMS(ORDER(I)) - &
            2 * DOT_PRODUCT(POINTS(:,ORDER(I)), PT(:))
    END DO CENTER_TO_ALL
    ! Base case for recursion, once we have few enough points, exit.
    IF (SIZE(ORDER) .LE. LS) THEN
       RADII(ORDER(1)) = SQRT( REAL(MAXVAL(SQ_DISTS),REAL64) )
       IF (SIZE(ORDER) .GT. 1) RADII(ORDER(2:)) = 0.0_REAL64
       RETURN
    ELSE IF (SIZE(ORDER) .EQ. 2) THEN
       ! If the leaf size is 1 and there are only 2 elements, store
       ! the radius and exit (since there are no further steps.
       RADII(ORDER(1)) = SQRT( REAL(SQ_DISTS(2),REAL64) )
       RADII(ORDER(2)) = 0.0_REAL64
       RETURN
    END IF
    ! Rearrange "SQ_DISTS" about the median value.
    ! Compute the last index that will belong "inside" this node.
    MID = (SIZE(ORDER) + 2) / 2
    CALL ARGSELECT_I64(SQ_DISTS(2:), ORDER(2:), MID - 1)
    ! Now ORDER has been rearranged such that the median distance
    ! element of POINTS is at the median location.
    ! Identify the furthest point (must be in second half of list).
    I = MID + MAXLOC(SQ_DISTS(MID+1:),1)
    ! Store the "radius" of this ball, the furthest point.
    RADII(ORDER(1)) = SQRT( REAL(SQ_DISTS(I),REAL64) )
    ! Move the furthest point into the spot after the median (outer root).
    CALL SWAP_I64(ORDER(I), ORDER(MID+1))
    ! Move the median point (furthest "interior") to the front (inner root).
    CALL SWAP_I64(ORDER(2), ORDER(MID))
    !$OMP PARALLEL NUM_THREADS(2)
    !$OMP SECTIONS
    !$OMP SECTION
    ! Recurisively create this tree.
    !   build a tree with the root being the furthest from this center
    !   for the remaining "interior" points of this center node.
    CALL BUILD_TREE(POINTS, SQ_SUMS, RADII, ORDER(2:MID), 1_INT64, LS, .TRUE.)
    !$OMP SECTION
    !   build a tree with the root being the furthest from this center
    !   for the remaining "exterior" points of this center node.
    !   Only perform this operation if there are >0 points available.
    IF (MID < SIZE(ORDER)) &
         CALL BUILD_TREE(POINTS, SQ_SUMS, RADII, &
         ORDER(MID+1:), 1_INT64, LS, .TRUE.)
    !$OMP END SECTIONS
    !$OMP END PARALLEL
  END SUBROUTINE BUILD_TREE

  ! Compute the K nearest elements of TREE to each point in POINTS.
  SUBROUTINE NEAREST(POINTS, K, TREE, SQ_SUMS, RADII, ORDER, &
       LEAF_SIZE, INDICES, DISTS, TO_SEARCH)
    INTEGER(KIND=INT8),  INTENT(IN), DIMENSION(:,:) :: POINTS, TREE
    INTEGER(KIND=INT64), INTENT(IN), DIMENSION(:)   :: SQ_SUMS
    REAL(KIND=REAL64),   INTENT(IN), DIMENSION(:)   :: RADII
    INTEGER(KIND=INT64), INTENT(IN), DIMENSION(:)   :: ORDER
    INTEGER(KIND=INT64), INTENT(IN)                 :: K, LEAF_SIZE
    INTEGER(KIND=INT64), INTENT(OUT), DIMENSION(K,SIZE(POINTS,2)) :: INDICES
    REAL(KIND=REAL64),   INTENT(OUT), DIMENSION(K,SIZE(POINTS,2)) :: DISTS
    INTEGER(KIND=INT64), INTENT(IN),  OPTIONAL      :: TO_SEARCH
    ! Local variables.
    INTEGER(KIND=INT64) :: I, B, BUDGET
    INTEGER(KIND=INT64), DIMENSION(K+LEAF_SIZE+2) :: INDS_BUFFER
    REAL(KIND=REAL64),   DIMENSION(K+LEAF_SIZE+2) :: DISTS_BUFFER
    IF (PRESENT(TO_SEARCH)) THEN ; BUDGET = TO_SEARCH
    ELSE ; BUDGET = SIZE(ORDER) ; END IF
    ! For each point in this set, use the recursive branching
    ! algorithm to identify the nearest elements of TREE.
    !$OMP PARALLEL DO PRIVATE(INDS_BUFFER, DISTS_BUFFER, B)
    DO I = 1, SIZE(POINTS,2)
       B = BUDGET
       CALL PT_NEAREST(INT(POINTS(:,I),INT64), K, TREE, SQ_SUMS, RADII, &
            ORDER, LEAF_SIZE, INDS_BUFFER, DISTS_BUFFER, B)
       ! Sort the first K elements of the temporary arry for return.
       INDICES(:,I) = INDS_BUFFER(:K)
       DISTS(:,I) = DISTS_BUFFER(:K)
    END DO
    !$OMP END PARALLEL DO
  END SUBROUTINE NEAREST

  ! Compute the K nearest elements of TREE to each point in POINTS.
  RECURSIVE SUBROUTINE PT_NEAREST(POINT, K, TREE, SQ_SUMS, RADII, &
       ORDER, LEAF_SIZE, INDICES, DISTS, CHECKS, FOUND, PT_SS)
    INTEGER(KIND=INT64), INTENT(IN), DIMENSION(:)   :: POINT
    INTEGER(KIND=INT8),  INTENT(IN), DIMENSION(:,:) :: TREE
    INTEGER(KIND=INT64), INTENT(IN), DIMENSION(:)   :: SQ_SUMS
    REAL(KIND=REAL64),   INTENT(IN), DIMENSION(:)   :: RADII
    INTEGER(KIND=INT64), INTENT(IN), DIMENSION(:)   :: ORDER
    INTEGER(KIND=INT64), INTENT(IN)                 :: K, LEAF_SIZE
    INTEGER(KIND=INT64), INTENT(OUT), DIMENSION(:) :: INDICES
    REAL(KIND=REAL64),   INTENT(OUT), DIMENSION(:) :: DISTS
    INTEGER(KIND=INT64), INTENT(INOUT), OPTIONAL   :: CHECKS
    INTEGER(KIND=INT64), INTENT(INOUT), OPTIONAL   :: FOUND
    INTEGER(KIND=INT64), INTENT(IN),    OPTIONAL   :: PT_SS
    ! Local variables
    INTEGER(KIND=INT64) :: F, I, I1, I2, MID, ALLOWED_CHECKS
    REAL(KIND=REAL64)   :: D, D1, D2
    INTEGER(KIND=INT64) :: PS
    ! Initialize FOUND for first call, if FOUND is present then
    ! this must not be first and all are present.
    INITIALIZE : IF (PRESENT(FOUND)) THEN
       ALLOWED_CHECKS = CHECKS
       IF (ALLOWED_CHECKS .LE. 0) RETURN
       F = FOUND
       PS = PT_SS
    ELSE
       ! Initialize the remaining checks to search.
       IF (PRESENT(CHECKS)) THEN ; ALLOWED_CHECKS = CHECKS - 1
       ELSE ; ALLOWED_CHECKS = SIZE(ORDER) - 1 ; END IF
       ! Start at index 0 (added onto current index). Compute squared sum.
       PS = SUM(POINT(:)**2)
       ! Measure distance to root.
       INDICES(1) = ORDER(1)
       DISTS(1) = SQRT( REAL(PS + SQ_SUMS(ORDER(1)) - &
            2*DOT_PRODUCT(POINT(:), TREE(:,ORDER(1))), REAL64) )
       ! Set the "points found" to be 1.
       F = 1
    END IF INITIALIZE
    ! If this is NOT a leaf node, then recurse.
    BRANCH_OR_LEAF : IF (SIZE(ORDER) .GT. LEAF_SIZE) THEN
       ALLOWED_CHECKS = ALLOWED_CHECKS - 1
       ! Measure distance to inner child.
       I1 = ORDER(2)
       D1 = SQRT( REAL(PS + SQ_SUMS(I1) - &
            2*DOT_PRODUCT(POINT(:),TREE(:,I1)), REAL64) )
       ! Store this distance calculation and index.
       F = F + 1
       INDICES(F) = I1
       DISTS(F) = D1
       ! Measure distance to outer child the same as above, after
       ! checking to see if there *is* an outer child.
       MID = (SIZE(ORDER) + 2) / 2
       IF (MID+1 > SIZE(ORDER)) THEN
          I2 = 0
          D2 = HUGE(D2)
       ELSE
          I2 = ORDER(MID + 1)
          IF (I2 .NE. I1) THEN
             ALLOWED_CHECKS = ALLOWED_CHECKS - 1
             D2 = SQRT( REAL(PS + SQ_SUMS(I2) - &
                  2*DOT_PRODUCT(POINT(:),TREE(:,I2)), REAL64) )
             ! Store this distance calculation and index.
             F = F + 1
             INDICES(F) = I2
             DISTS(F) = D2
          ELSE ; D2 = HUGE(D2)
          END IF
       END IF
       ! Re-organize the list of closest points, pushing them to first K spots.
       CALL ARGSELECT_R64(DISTS(:F), INDICES(:F), MIN(K,F))
       F = MIN(K,F)
       ! Store the maximum distance.
       D = MAXVAL(DISTS(:F),1)
       ! Determine which child to search (depth-first search) based
       ! on which one is closer to the approximation point.
       INNER_CHILD_CLOSER : IF (D1 .LE. D2) THEN
          ! Search the inner child if it could contain a nearer point.
          SEARCH_INNER1 : IF ((F .LT. K) .OR. (D .GT. D1 - RADII(I1))) THEN
             CALL PT_NEAREST(POINT, K, TREE, SQ_SUMS, RADII, ORDER(2:MID), &
                  LEAF_SIZE, INDICES, DISTS, ALLOWED_CHECKS, F, PS)
          END IF SEARCH_INNER1
          ! Search the outer child if it could contain a nearer point.
          SEARCH_OUTER1 : IF (((F .LT. K) .OR. (D .GT. D2 - RADII(I2))) &
               .AND. (I2 .NE. I1) .AND. (I2 .GT. 0)) THEN
             CALL PT_NEAREST(POINT, K, TREE, SQ_SUMS, RADII, ORDER(MID+1:), &
                  LEAF_SIZE, INDICES, DISTS, ALLOWED_CHECKS, F, PS)
          END IF SEARCH_OUTER1
       ELSE
          ! Search the outer child if it could contain a nearer point.
          SEARCH_OUTER2 : IF (((F .LT. K) .OR. (D .GT. D2 - RADII(I2))) &
               .AND. (I2 .NE. I1) .AND. (I2 .GT. 0)) THEN
             CALL PT_NEAREST(POINT, K, TREE, SQ_SUMS, RADII, ORDER(MID+1:), &
                  LEAF_SIZE, INDICES, DISTS, ALLOWED_CHECKS, F, PS)
          END IF SEARCH_OUTER2
          ! Search the inner child if it could contain a nearer point.
          SEARCH_INNER2 : IF ((F .LT. K) .OR. (D .GT. D1 - RADII(I1))) THEN
             CALL PT_NEAREST(POINT, K, TREE, SQ_SUMS, RADII, ORDER(2:MID), &
                  LEAF_SIZE, INDICES, DISTS, ALLOWED_CHECKS, F, PS)
          END IF SEARCH_INNER2
       END IF INNER_CHILD_CLOSER
    ! Since this is a leaf node, we measure distance to all children.
    ELSE
       DIST_TO_CHILDREN : DO I = 2, SIZE(ORDER)
          ALLOWED_CHECKS = ALLOWED_CHECKS - 1
          ! Measure distance to all children of this node.
          D = SQRT( REAL(PS + SQ_SUMS(ORDER(I)) - &
               2*DOT_PRODUCT(POINT(:),TREE(:,ORDER(I))), REAL64) )
          ! Store this distance.
          F = F + 1
          DISTS(F) = D
          INDICES(F) = ORDER(I)
       END DO DIST_TO_CHILDREN
       ! Reduce the kept points to only those that are closest.
       CALL ARGSELECT_R64(DISTS(:F), INDICES(:F), MIN(K,F))
       F = MIN(K, F)
    END IF BRANCH_OR_LEAF

    ! Handle closing operations..
    SORT_K : IF (PRESENT(FOUND)) THEN
       ! This is not the root, We need to pass the updated value of
       ! FOUND back up the recrusion stack.
       FOUND = F
       CHECKS = ALLOWED_CHECKS
    ELSE
       ! This is the root, initial caller. Sort the distances for return.
       CALL ARGSELECT_R64(DISTS(:F), INDICES(:F), K)
       CALL ARGSORT(DISTS(:K), INDICES(:K))
    END IF SORT_K
  END SUBROUTINE PT_NEAREST

  ! Re-organize a built tree so that it is more usefully packed in memory.
  SUBROUTINE FIX_ORDER(POINTS, SQ_SUMS, RADII, ORDER)
    INTEGER(KIND=INT8),  INTENT(INOUT), DIMENSION(:,:) :: POINTS
    INTEGER(KIND=INT64), INTENT(INOUT), DIMENSION(:)   :: SQ_SUMS
    REAL(KIND=REAL64),   INTENT(INOUT), DIMENSION(:)   :: RADII
    INTEGER(KIND=INT64), INTENT(INOUT), DIMENSION(:)   :: ORDER
    INTEGER(KIND=INT64) :: I
    ! Execute the swap operation.
    POINTS(:,:) = POINTS(:,ORDER)
    SQ_SUMS(:) = SQ_SUMS(ORDER)
    RADII(:) = RADII(ORDER)
    FORALL (I=1:SIZE(ORDER)) ORDER(I) = I
  END SUBROUTINE FIX_ORDER

END MODULE BALL_TREE_I8


program PlanarTriScatteredInterpTester

use NumberKindsModule
use LoggerModule
use PolyMesh2dModule
use ParticlesModule
use EdgesModule
use FacesModule
use STDIntVectorModule
use FieldModule
use TriScatteredInterpModule

implicit none

type(Logger) :: exeLog

type(PolyMesh2d) :: triMesh
integer(kint), parameter :: meshSeed = TRI_HEX_SEED
integer(kint) :: initNest
integer(kint) :: maxNest
integer(kint) :: amrLimit
real(kreal) :: ampFactor

real(kreal), parameter :: b = 3.0_kreal

type(Field) :: scalarField, gradientField, secondPartials, gradError, exactGrad, exactPartials, estLaplacian, lapError

type(TriScatteredInterp) :: gaussianInterp

real(kreal), parameter :: xc = 0.25_kreal, yc = 0.01_kreal

integer(kint) :: i, j

integer(kint), parameter :: nn = 101
real(kreal), parameter :: dx = 8.0_kreal/100.0_kreal
real(kreal), parameter :: xmin = -4.0_kreal
real(kreal), parameter :: xmax = 4.0_kreal
real(kreal), parameter :: ymin = xmin
real(kreal), parameter :: ymax = xmax
real(kreal) :: x(nn), y(nn), xVec(3), xVec1(3)
real(kreal) :: interpolatedScalar(nn,nn), interpGradX(nn,nn), interpGradY(nn,nn), interpLap(nn,nn)
real(kreal) :: exactScalar(nn,nn), exactGradX(nn,nn), exactGradY(nn,nn), exactLap(nn,nn)

integer(kint) :: narg
character(len=MAX_STRING_LENGTH) :: filename, arg

!
!----------------
! PROGRAM START
!----------------
!
call New(exeLog, DEBUG_LOGGING_LEVEL)

narg = IARGC()
if (  narg == 0 ) then
	initNest = 0
else
	call GETARG(1, arg)
	read(arg,*) initNest
endif

maxNest = initNest
amrLimit = 0
ampFactor = 3.0_kreal
call New(triMesh, meshSeed, initNest, maxNest, amrLimit, ampFactor)

call New(scalarField, 1, triMesh%particles%N, "Gaussian", "n/a")
call New(gradientField, 2, triMesh%particles%N, "estGrad", "n/a")
call New(secondPartials, 3, triMesh%particles%N, "est2ndDerivs", "n/a")
call New(gradError, 1, triMesh%particles%N, "estGradError", "length")
call New(exactGrad, 2, triMesh%particles%N, "exactGrad","n/a")
call New(exactPartials,3,triMesh%particles%N,"exact2ndDerivs","n/a")
call New(estLaplacian,1,triMesh%particles%N, "estLap","n/a")
call New(lapError,1,triMesh%particles%N,"lapError", "n/a")

xVec = 0.0_kreal
xVec1 = 0.0_kreal
do i = 1, triMesh%particles%N
	call InsertScalarToField( scalarField, Gaussian([ triMesh%particles%x(i), triMesh%particles%y(i)], b))
	call InsertVectorToField( exactGrad, GaussGrad([triMesh%particles%x(i), triMesh%particles%y(i)], b) )
	call InsertVectorToField( exactPartials, Gauss2ndDerivs([triMesh%particles%x(i), triMesh%particles%y(i)], b) )
enddo
call LogStats(scalarField, exeLog)
call LogMessage(exeLog,DEBUG_LOGGING_LEVEL,"...", "all fields allocated.")

call New(gaussianInterp, triMesh)

call generateFaceUVs(gaussianInterp, triMesh)
call estimatePartialDerivatives(gaussianInterp, triMesh, scalarField, gradientField, secondPartials)
call SetPlanarCoefficients(gaussianInterp, triMesh, scalarField, gradientField, secondPartials)
!call SetPlanarCoefficients(gaussianInterp, triMesh, scalarField, exactGrad, exactPartials)

xVec = 0.0_kreal
xVec1 = 0.0_kreal
do i = 1, triMesh%particles%N
	xVec(1:2) = GaussGrad([triMesh%particles%x(i),triMesh%particles%y(i)],b)
	xVec1(1:2) = [gradientField%xComp(i), gradientField%yComp(i)]
	call InsertScalarToField( gradError, sqrt(sum( (xVec1 - xVec)*(xVec1-xVec))))
	call InsertScalarToField( estLaplacian, secondPartials%xComp(i) + secondPartials%zComp(i))
	call InsertScalarToField( lapError, secondPartials%xComp(i) + secondPartials%zComp(i) - &
					GaussLap( [triMesh%particles%x(i), triMesh%particles%y(i)], b) )
enddo

do i = 1, nn
	x(i) = xmin + dx * (i-1)
	y(i) = ymin + dx * (i-1)
enddo

do j = 1, nn
	do i = 1, nn
		exactScalar(i,j) = Gaussian( [x(j), y(i)], b)
		xVec(1:2) = GaussGrad([x(j), y(i)], b)
		exactGradX(i,j) = xVec(1)
		exactGradY(i,j) = xVec(2)
		exactLap(i,j) = GaussLap([x(j), y(i)], b)

		interpolatedScalar(i,j) = InterpolateScalar( gaussianInterp, triMesh, [x(j), y(i)] )
		xVec(1:2) = InterpolateGradient( gaussianInterp, triMesh, [x(j), y(i)])
		interpGradX(i,j) = xVec(1)
		interpGradY(i,j) = xVec(2)
		interpLap(i,j) = InterpolateLaplacian(gaussianInterp, triMesh, [x(j),y(i)])
	enddo
enddo

write(filename,'(A,I1,A)') "triScatteredInterpTest_", initNest, ".m"
open(unit=WRITE_UNIT_1, file=filename, status="REPLACE", action="WRITE")

call WriteParticlesToMatlab(triMesh%particles, WRITE_UNIT_1)
call WriteFieldToMatlab(scalarField, WRITE_UNIT_1)
call WriteFieldToMatlab(gradientField, WRITE_UNIT_1)
call WriteFieldToMatlab(secondPartials, WRITE_UNIT_1)
call WriteFieldToMatlab(gradError, WRITE_UNIT_1)
call WriteFieldToMatlab(estLaplacian, WRITE_UNIT_1)
call WriteFieldToMatlab(lapError, WRITE_UNIT_1)

write(WRITE_UNIT_1,'(A,F8.2,A,F12.8,A,F8.2,A)') "unifx = ", xmin,":", dx,":", xmax, ";"
write(WRITE_UNIT_1,'(A)') "unify = unifx;"
write(WRITE_UNIT_1,'(A)') "[xx, yy] = meshgrid(unifx, unify);"

call WriteMeshToMatlab(triMesh, WRITE_UNIT_1)

write(WRITE_UNIT_1,'(A)',advance='NO') "exactScalar = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactScalar(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') exactScalar(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactScalar(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') exactScalar(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "exactGradX = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactGradX(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') exactGradX(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactGradX(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') exactGradX(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "exactGradY = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactGradY(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') exactGradY(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactGradY(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') exactGradY(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "exactLap = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactLap(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') exactLap(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') exactLap(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') exactLap(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "interpolatedScalar = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpolatedScalar(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') interpolatedScalar(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpolatedScalar(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') interpolatedScalar(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "interpGradX = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpGradX(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') interpGradX(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpGradX(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') interpGradX(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "interpGradY = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpGradY(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') interpGradY(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpGradY(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') interpGradY(nn,nn), "];"

write(WRITE_UNIT_1,'(A)',advance='NO') "interpLap = ["
do i = 1, nn - 1
	do j = 1, nn - 1
		write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpLap(i,j), ","
	enddo
	write(WRITE_UNIT_1,'(F12.7,A)') interpLap(i,nn), "; ..."
enddo
do j = 1, nn - 1
	write(WRITE_UNIT_1, '(F12.7,A)',advance='NO') interpLap(nn,j), ","
enddo
write(WRITE_UNIT_1,'(F12.7,A)') interpLap(nn,nn), "];"

close(WRITE_UNIT_1)

!
!----------------
! PROGRAM END
!----------------
!
call Delete(lapError)
call Delete(estLaplacian)
call Delete(exactGrad)
call Delete(exactPartials)
call Delete(gaussianInterp)
call Delete(gradError)
call Delete(secondPartials)
call Delete(gradientField)
call Delete(scalarField)
call Delete(triMesh)
call Delete(exeLog)

contains

function Gaussian( xy, b )
	real(kreal) :: Gaussian
	real(kreal), intent(in) :: xy(2)
	real(kreal), intent(in) :: b

	Gaussian = exp( - b * b * ( (xy(1)-xc)*(xy(1)-xc) + (xy(2)-yc)*(xy(2)-yc)))
end function

function GaussGrad(xy, b)
	real(kreal) :: GaussGrad(2)
	real(kreal), intent(in) :: xy(2)
	real(kreal), intent(in) :: b
	GaussGrad(1) = xy(1)-xc
	GaussGrad(2) = xy(2)-yc
	GaussGrad = -2.0_kreal * GaussGrad * b * b * exp( - b * b * ( (xy(1)-xc)*(xy(1)-xc) + (xy(2)-yc)*(xy(2)-yc) ))
end function

function GaussLap(xy, b)
	real(kreal) :: GaussLap
	real(kreal), intent(in) :: xy(2)
	real(kreal), intent(in) :: b
	GaussLap = ( b*b * ( (xy(1)-xc)*(xy(1)-xc) + (xy(2)-yc)*(xy(2)-yc) ) - 1.0_kreal ) * 4.0_kreal * b * b * &
			exp( - b * b * ( (xy(1)-xc)*(xy(1)-xc) + (xy(2)-yc)*(xy(2)-yc) ))
end function

function Gauss2ndDerivs( xy, b)
	real(kreal) :: Gauss2ndDerivs(3)
	real(kreal), intent(in) :: xy(2)
	real(kreal), intent(in) :: b
	Gauss2ndDerivs(1) = 2.0_kreal * b * b *(2.0_kreal * b*b *(xy(1)-xc)*(xy(1)-xc) - 1.0_kreal)
	Gauss2ndDerivs(2) = 4.0_kreal * b**4 * (xy(1)-xc)*(xy(2)-yc)
	Gauss2ndDerivs(3) = 2.0_kreal * b * b *(2.0_kreal * b*b * (xy(2)-yc)*(xy(2)-yc) - 1.0_kreal)
	Gauss2ndDerivs = Gauss2ndDerivs * exp( - b * b * ( (xy(1)-xc)*(xy(1)-xc) + (xy(2)-yc)*(xy(2)-yc) ))
end function

end program


c Triangular Solve  dtrsl()
c ----------------
c     solves systems of the form
c
c                   t * x = b
c     or
c                   trans(t) * x = b
c
c     where t is a triangular matrix of order n. here trans(t)
c     denotes the transpose of the matrix t.
c
c     on entry
c
c         t         double precision(ldt,n)
c                   t contains the matrix of the system. the zero
c                   elements of the matrix are not referenced, and
c                   the corresponding elements of the array can be
c                   used to store other information.
c
c         ldt       integer
c                   ldt is the leading dimension of the array t.
c
c         n         integer
c                   n is the order of the system.
c
c         b         double precision(n).
c                   b contains the right hand side of the system.
c
c         job       integer
c                   job specifies what kind of system is to be solved.
c                   if job is
c
c                        00   solve t*x=b, t lower triangular,
c                        01   solve t*x=b, t upper triangular,
c                        10   solve trans(t)*x=b, t lower triangular,
c                        11   solve trans(t)*x=b, t upper triangular.
c
c     on return
c
c         b         b contains the solution, if info .eq. 0.
c                   otherwise b is unaltered.
c
c         info      integer
c                   info contains zero if the system is nonsingular.
c                   otherwise info contains the index of
c                   the first zero diagonal element of t.
c
c     linpack. this version dated 08/14/78 .
c     g. w. stewart, university of maryland, argonne national lab.
c
c     subroutines and functions
c
c     blas:     daxpy,ddot
c     fortran   mod
c
      subroutine dtrsl(t,ldt,n,b,job,info)
      integer ldt,n,job,info
      double precision t(ldt,*),b(*)
c
c     internal variables
c
      double precision ddot,temp
      integer case,j,jj
c
c     begin block permitting ...exits to 150
c
c        check for zero diagonal elements.
c
      do 10 info = 1, n
         if (t(info,info) .eq. 0.0d0) go to 150
c     ......exit
 10   continue
      info = 0
c
c     determine the task and go to it.
c
      case = 1
      if (mod(job,10) .ne. 0) case = 2
      if (mod(job,100)/10 .ne. 0) case = case + 2
      go to (20,50,80,110), case
c
C Case 1 (job = 00):
c        solve t*x=b for t lower triangular
c
 20   continue
      b(1) = b(1)/t(1,1)
      if (n .ge. 2) then
      do 30 j = 2, n
         temp = -b(j-1)
         call daxpy(n-j+1,temp,t(j,j-1),1,b(j),1)
         b(j) = b(j)/t(j,j)
 30   continue
      endif
      go to 140
c
C Case 2 (job = 01):
c        solve t*x=b for t upper triangular.
c
 50   continue
      b(n) = b(n)/t(n,n)
      if (n .ge. 2) then
         do 60 jj = 2, n
            j = n - jj + 1
            temp = -b(j+1)
            call daxpy(j,temp,t(1,j+1),1,b(1),1)
            b(j) = b(j)/t(j,j)
 60      continue
      endif
      go to 140
c
C Case 3 (job = 10):
c        solve trans(t)*x=b for t lower triangular.
c
 80   continue
      b(n) = b(n)/t(n,n)
      if (n .ge. 2) then
         do 90 jj = 2, n
            j = n - jj + 1
            b(j) = b(j) - ddot(jj-1,t(j+1,j),1,b(j+1),1)
            b(j) = b(j)/t(j,j)
 90      continue
      endif
      go to 140
c
C Case 4 (job = 11):
c        solve trans(t)*x=b for t upper triangular.
c
 110  continue
      b(1) = b(1)/t(1,1)
      if (n .ge. 2) then
         do 120 j = 2, n
            b(j) = b(j) - ddot(j-1,t(1,j),1,b(1),1)
            b(j) = b(j)/t(j,j)
 120     continue
      endif
C
 140  continue
c     EXIT:
 150  continue
      return
      end


      subroutine qzit(nm,n,a,b,eps1,matz,z,ierr)
c
      integer i,j,k,l,n,en,k1,k2,ld,ll,l1,na,nm,ish,itn,its,km1,lm1,
     x        enm2,ierr,lor1,enorn
      double precision a(nm,n),b(nm,n),z(nm,n)
      double precision r,s,t,a1,a2,a3,ep,sh,u1,u2,u3,v1,v2,v3,ani,a11,
     x       a12,a21,a22,a33,a34,a43,a44,bni,b11,b12,b22,b33,b34,
     x       b44,epsa,epsb,eps1,anorm,bnorm,epslon
      logical matz,notlas
c
c     this subroutine is the second step of the qz algorithm
c     for solving generalized matrix eigenvalue problems,
c     siam j. numer. anal. 10, 241-256(1973) by moler and stewart,
c     as modified in technical note nasa tn d-7305(1973) by ward.
c
c     this subroutine accepts a pair of real matrices, one of them
c     in upper hessenberg form and the other in upper triangular form.
c     it reduces the hessenberg matrix to quasi-triangular form using
c     orthogonal transformations while maintaining the triangular form
c     of the other matrix.  it is usually preceded by  qzhes  and
c     followed by  qzval  and, possibly,  qzvec.
c
c     on input
c
c        nm must be set to the row dimension of two-dimensional
c          array parameters as declared in the calling program
c          dimension statement.
c
c        n is the order of the matrices.
c
c        a contains a real upper hessenberg matrix.
c
c        b contains a real upper triangular matrix.
c
c        eps1 is a tolerance used to determine negligible elements.
c          eps1 = 0.0 (or negative) may be input, in which case an
c          element will be neglected only if it is less than roundoff
c          error times the norm of its matrix.  if the input eps1 is
c          positive, then an element will be considered negligible
c          if it is less than eps1 times the norm of its matrix.  a
c          positive value of eps1 may result in faster execution,
c          but less accurate results.
c
c        matz should be set to .true. if the right hand transformations
c          are to be accumulated for later use in computing
c          eigenvectors, and to .false. otherwise.
c
c        z contains, if matz has been set to .true., the
c          transformation matrix produced in the reduction
c          by  qzhes, if performed, or else the identity matrix.
c          if matz has been set to .false., z is not referenced.
c
c     on output
c
c        a has been reduced to quasi-triangular form.  the elements
c          below the first subdiagonal are still zero and no two
c          consecutive subdiagonal elements are nonzero.
c
c        b is still in upper triangular form, although its elements
c          have been altered.  the location b(n,1) is used to store
c          eps1 times the norm of b for later use by  qzval  and  qzvec.
c
c        z contains the product of the right hand transformations
c          (for both steps) if matz has been set to .true..
c
c        ierr is set to
c          zero       for normal return,
c          j          if the limit of 30*n iterations is exhausted
c                     while the j-th eigenvalue is being sought.
c
c     questions and comments should be directed to burton s. garbow,
c     mathematics and computer science div, argonne national laboratory
c
c     this version dated august 1983.
c
c     ------------------------------------------------------------------
c
      ierr = 0
c     .......... compute epsa,epsb ..........
      anorm = 0.0d0
      bnorm = 0.0d0
c
      do 30 i = 1, n
         ani = 0.0d0
         if (i .ne. 1) ani = dabs(a(i,i-1))
         bni = 0.0d0
c
         do 20 j = i, n
            ani = ani + dabs(a(i,j))
            bni = bni + dabs(b(i,j))
   20    continue
c
         if (ani .gt. anorm) anorm = ani
         if (bni .gt. bnorm) bnorm = bni
   30 continue
c
      if (anorm .eq. 0.0d0) anorm = 1.0d0
      if (bnorm .eq. 0.0d0) bnorm = 1.0d0
      ep = eps1
      if (ep .gt. 0.0d0) go to 50
c     .......... use roundoff level if eps1 is zero ..........
      ep = epslon(1.0d0)
   50 epsa = ep * anorm
      epsb = ep * bnorm
c     .......... reduce a to quasi-triangular form, while
c                keeping b triangular ..........
      lor1 = 1
      enorn = n
      en = n
      itn = 30*n
c     .......... begin qz step ..........
   60 if (en .le. 2) go to 1001
      if (.not. matz) enorn = en
      its = 0
      na = en - 1
      enm2 = na - 1
   70 ish = 2
c     .......... check for convergence or reducibility.
c                for l=en step -1 until 1 do -- ..........
      do 80 ll = 1, en
         lm1 = en - ll
         l = lm1 + 1
         if (l .eq. 1) go to 95
         if (dabs(a(l,lm1)) .le. epsa) go to 90
   80 continue
c
   90 a(l,lm1) = 0.0d0
      if (l .lt. na) go to 95
c     .......... 1-by-1 or 2-by-2 block isolated ..........
      en = lm1
      go to 60
c     .......... check for small top of b ..........
   95 ld = l
  100 l1 = l + 1
      b11 = b(l,l)
      if (dabs(b11) .gt. epsb) go to 120
      b(l,l) = 0.0d0
      s = dabs(a(l,l)) + dabs(a(l1,l))
      u1 = a(l,l) / s
      u2 = a(l1,l) / s
      r = dsign(dsqrt(u1*u1+u2*u2),u1)
      v1 = -(u1 + r) / r
      v2 = -u2 / r
      u2 = v2 / v1
c
      do 110 j = l, enorn
         t = a(l,j) + u2 * a(l1,j)
         a(l,j) = a(l,j) + t * v1
         a(l1,j) = a(l1,j) + t * v2
         t = b(l,j) + u2 * b(l1,j)
         b(l,j) = b(l,j) + t * v1
         b(l1,j) = b(l1,j) + t * v2
  110 continue
c
      if (l .ne. 1) a(l,lm1) = -a(l,lm1)
      lm1 = l
      l = l1
      go to 90
  120 a11 = a(l,l) / b11
      a21 = a(l1,l) / b11
      if (ish .eq. 1) go to 140
c     .......... iteration strategy ..........
      if (itn .eq. 0) go to 1000
      if (its .eq. 10) go to 155
c     .......... determine type of shift ..........
      b22 = b(l1,l1)
      if (dabs(b22) .lt. epsb) b22 = epsb
      b33 = b(na,na)
      if (dabs(b33) .lt. epsb) b33 = epsb
      b44 = b(en,en)
      if (dabs(b44) .lt. epsb) b44 = epsb
      a33 = a(na,na) / b33
      a34 = a(na,en) / b44
      a43 = a(en,na) / b33
      a44 = a(en,en) / b44
      b34 = b(na,en) / b44
      t = 0.5d0 * (a43 * b34 - a33 - a44)
      r = t * t + a34 * a43 - a33 * a44
      if (r .lt. 0.0d0) go to 150
c     .......... determine single shift zeroth column of a ..........
      ish = 1
      r = dsqrt(r)
      sh = -t + r
      s = -t - r
      if (dabs(s-a44) .lt. dabs(sh-a44)) sh = s
c     .......... look for two consecutive small
c                sub-diagonal elements of a.
c                for l=en-2 step -1 until ld do -- ..........
      do 130 ll = ld, enm2
         l = enm2 + ld - ll
         if (l .eq. ld) go to 140
         lm1 = l - 1
         l1 = l + 1
         t = a(l,l)
         if (dabs(b(l,l)) .gt. epsb) t = t - sh * b(l,l)
         if (dabs(a(l,lm1)) .le. dabs(t/a(l1,l)) * epsa) go to 100
  130 continue
c
  140 a1 = a11 - sh
      a2 = a21
      if (l .ne. ld) a(l,lm1) = -a(l,lm1)
      go to 160
c     .......... determine double shift zeroth column of a ..........
  150 a12 = a(l,l1) / b22
      a22 = a(l1,l1) / b22
      b12 = b(l,l1) / b22
      a1 = ((a33 - a11) * (a44 - a11) - a34 * a43 + a43 * b34 * a11)
     x     / a21 + a12 - a11 * b12
      a2 = (a22 - a11) - a21 * b12 - (a33 - a11) - (a44 - a11)
     x     + a43 * b34
      a3 = a(l1+1,l1) / b22
      go to 160
c     .......... ad hoc shift ..........
  155 a1 = 0.0d0
      a2 = 1.0d0
      a3 = 1.1605d0
  160 its = its + 1
      itn = itn - 1
      if (.not. matz) lor1 = ld
c     .......... main loop ..........
      do 260 k = l, na
         notlas = k .ne. na .and. ish .eq. 2
         k1 = k + 1
         k2 = k + 2
         km1 = max0(k-1,l)
         ll = min0(en,k1+ish)
         if (notlas) go to 190
c     .......... zero a(k+1,k-1) ..........
         if (k .eq. l) go to 170
         a1 = a(k,km1)
         a2 = a(k1,km1)
  170    s = dabs(a1) + dabs(a2)
         if (s .eq. 0.0d0) go to 70
         u1 = a1 / s
         u2 = a2 / s
         r = dsign(dsqrt(u1*u1+u2*u2),u1)
         v1 = -(u1 + r) / r
         v2 = -u2 / r
         u2 = v2 / v1
c
         do 180 j = km1, enorn
            t = a(k,j) + u2 * a(k1,j)
            a(k,j) = a(k,j) + t * v1
            a(k1,j) = a(k1,j) + t * v2
            t = b(k,j) + u2 * b(k1,j)
            b(k,j) = b(k,j) + t * v1
            b(k1,j) = b(k1,j) + t * v2
  180    continue
c
         if (k .ne. l) a(k1,km1) = 0.0d0
         go to 240
c     .......... zero a(k+1,k-1) and a(k+2,k-1) ..........
  190    if (k .eq. l) go to 200
         a1 = a(k,km1)
         a2 = a(k1,km1)
         a3 = a(k2,km1)
  200    s = dabs(a1) + dabs(a2) + dabs(a3)
         if (s .eq. 0.0d0) go to 260
         u1 = a1 / s
         u2 = a2 / s
         u3 = a3 / s
         r = dsign(dsqrt(u1*u1+u2*u2+u3*u3),u1)
         v1 = -(u1 + r) / r
         v2 = -u2 / r
         v3 = -u3 / r
         u2 = v2 / v1
         u3 = v3 / v1
c
         do 210 j = km1, enorn
            t = a(k,j) + u2 * a(k1,j) + u3 * a(k2,j)
            a(k,j) = a(k,j) + t * v1
            a(k1,j) = a(k1,j) + t * v2
            a(k2,j) = a(k2,j) + t * v3
            t = b(k,j) + u2 * b(k1,j) + u3 * b(k2,j)
            b(k,j) = b(k,j) + t * v1
            b(k1,j) = b(k1,j) + t * v2
            b(k2,j) = b(k2,j) + t * v3
  210    continue
c
         if (k .eq. l) go to 220
         a(k1,km1) = 0.0d0
         a(k2,km1) = 0.0d0
c     .......... zero b(k+2,k+1) and b(k+2,k) ..........
  220    s = dabs(b(k2,k2)) + dabs(b(k2,k1)) + dabs(b(k2,k))
         if (s .eq. 0.0d0) go to 240
         u1 = b(k2,k2) / s
         u2 = b(k2,k1) / s
         u3 = b(k2,k) / s
         r = dsign(dsqrt(u1*u1+u2*u2+u3*u3),u1)
         v1 = -(u1 + r) / r
         v2 = -u2 / r
         v3 = -u3 / r
         u2 = v2 / v1
         u3 = v3 / v1
c
         do 230 i = lor1, ll
            t = a(i,k2) + u2 * a(i,k1) + u3 * a(i,k)
            a(i,k2) = a(i,k2) + t * v1
            a(i,k1) = a(i,k1) + t * v2
            a(i,k) = a(i,k) + t * v3
            t = b(i,k2) + u2 * b(i,k1) + u3 * b(i,k)
            b(i,k2) = b(i,k2) + t * v1
            b(i,k1) = b(i,k1) + t * v2
            b(i,k) = b(i,k) + t * v3
  230    continue
c
         b(k2,k) = 0.0d0
         b(k2,k1) = 0.0d0
         if (.not. matz) go to 240
c
         do 235 i = 1, n
            t = z(i,k2) + u2 * z(i,k1) + u3 * z(i,k)
            z(i,k2) = z(i,k2) + t * v1
            z(i,k1) = z(i,k1) + t * v2
            z(i,k) = z(i,k) + t * v3
  235    continue
c     .......... zero b(k+1,k) ..........
  240    s = dabs(b(k1,k1)) + dabs(b(k1,k))
         if (s .eq. 0.0d0) go to 260
         u1 = b(k1,k1) / s
         u2 = b(k1,k) / s
         r = dsign(dsqrt(u1*u1+u2*u2),u1)
         v1 = -(u1 + r) / r
         v2 = -u2 / r
         u2 = v2 / v1
c
         do 250 i = lor1, ll
            t = a(i,k1) + u2 * a(i,k)
            a(i,k1) = a(i,k1) + t * v1
            a(i,k) = a(i,k) + t * v2
            t = b(i,k1) + u2 * b(i,k)
            b(i,k1) = b(i,k1) + t * v1
            b(i,k) = b(i,k) + t * v2
  250    continue
c
         b(k1,k) = 0.0d0
         if (.not. matz) go to 260
c
         do 255 i = 1, n
            t = z(i,k1) + u2 * z(i,k)
            z(i,k1) = z(i,k1) + t * v1
            z(i,k) = z(i,k) + t * v2
  255    continue
c
  260 continue
c     .......... end qz step ..........
      go to 70
c     .......... set error -- all eigenvalues have not
c                converged after 30*n iterations ..........
 1000 ierr = en
c     .......... save epsb for use by qzval and qzvec ..........
 1001 if (n .gt. 1) b(n,1) = epsb
      return
      end