module GeometryClass
    use, intrinsic :: iso_fortran_env
    use MathClass
    use ArrayClass
    implicit none

    type Point_
        real(real64),allocatable :: coord(:)
        character*30 :: name
    contains
        procedure :: Init   => InitPoint
        procedure :: set    => setPoint
        procedure :: show   => showPoint
    end type

    type Line_
        real(real64),allocatable :: coord(:,:)
    contains
        procedure :: Init       => InitLine
        procedure :: setNode    => SetNodeLine
        procedure :: import     => importLine
        procedure :: show       => showLine
    end type

    type Circle_
        real(real64) :: radius
        real(real64),allocatable :: center(:)
    contains
        procedure :: Init       => InitCircle
        procedure :: SetCenter  => InitSetCenterCircle
        procedure :: SetRadius  => InitSetRadiusCircle
        procedure :: getArea    => getAreaCircle
        procedure :: show       => showCircle
    end type

    type Sphere_
        real(real64) :: radius
        real(real64) :: center(3)
    contains
        procedure :: Init       => InitSphere
        procedure :: SetCenter  => InitSetCenterSphere
        procedure :: SetRadius  => InitSetRadiusSphere
        procedure :: show       => showSphere
    end type

    type Triangle_
        real(real64),allocatable :: NodCoord(:,:)
        real(real64),allocatable :: OuterNormal(:)
        real(real64),allocatable :: Center(:)
    contains
        procedure :: Init       => InitTriangle
        procedure :: setNode    => setNodeTriangle
        procedure :: import     => importTriangle
        procedure :: getCircle  => getCircleTriangle
        procedure :: getArea    => getAreaTriangle
        procedure :: show       => showTriangle
        procedure :: GetOuterNormal => GetOuterNormalTriangle
    end type

    type Rectangle_
        real(real64),allocatable :: NodCoord(:,:)
    contains
        procedure :: Init       => InitRectangle
        procedure :: create     => createRectangle
        procedure :: move       => moveRectangle
        procedure :: setNode    => setNodeRectangle
        procedure :: import     => importRectangle
        procedure :: getCircle  => getCircleRectangle
        procedure :: getArea    => getAreaRectangle
        procedure :: show       => showRectangle
        procedure :: contact   => contactRectangle
    end type

    type Tetragon_
        real(real64),allocatable :: NodCoord(:,:)
    end type

    type Tetrahedron_
        real(real64) :: NodCoord(4,3)
        real(real64) :: radius
        real(real64) :: center(3)
    contains
        procedure :: Init => InitTetrahedron
        procedure :: getCircle => getCircleTetrahedron
    end type

    type Octahedron_
        real(real64),allocatable :: NodCoord(:,:)
    end type
contains

!#########################################################
subroutine InitPoint(obj,dim)
    class(Point_),intent(inout)::obj
    integer(int32),optional,intent(in)::dim

    ! default == 3
    if(allocated(obj%coord) )then
        deallocate(obj%coord)
    endif
    allocate(obj%coord(input(default=3,option=dim) ) )
    obj%coord(:)=0.0d0

end subroutine
!#########################################################

!#########################################################
subroutine showPoint(obj,Name)
    class(Point_),intent(in)::obj
    character(*),optional,intent(in)::Name

    if(present(Name) )then
        open(10,file=Name)
        write(10,*) obj%coord(:)
        close(10)
    else
        print *, "Point> x(:) = ",obj%coord(:)
    endif
end subroutine
!#########################################################

!#########################################################
subroutine setPoint(obj,x,y,z,xvec)
    class(Point_),intent(inout)::obj
    real(real64),optional,intent(in)::x,y,z
    real(real64),optional,intent(in)::xvec(:)
    integer(int32) :: n

    n=size(obj%coord)
    if(present(xvec) )then
        if(n/=size(xvec) )then
            print *, "ERROR :: setPoint :: n/=size(xvec)"
            return
        endif
        obj%coord(:)=xvec(:)
        return
    endif

    if(present(x) )then
        obj%coord(1)=x

    endif

    if(present(y) )then
        obj%coord(2)=y

    endif

    if(size(obj%coord)<=2 )then
        return
    endif

    if(present(z) )then
        obj%coord(3)=z
    endif

end subroutine
!#########################################################

!#########################################################
subroutine InitLine(obj,dim)
    class(Line_),intent(inout)::obj
    integer(int32),optional,intent(inout)::dim

    ! default = 3D
    if(allocated(obj%coord) )then
        deallocate(obj%coord)
    endif
    allocate(obj%coord(2,input(default=3,option=dim) ) )
    obj%coord(:,:)=0.0d0

end subroutine
!#########################################################

!#########################################################
subroutine SetNodeLine(obj,point,position)
    class(Line_),intent(inout)::obj
    class(Point_),intent(in)::Point
    integer(int32),intent(in)::position

    if(position <=0 .or. position >= 3)then
        print *, "ERROR :: Line%SetNode => (position <=0 .or. position >= 3)",position
        return
    endif

    if(size(obj%coord,2)/=size(point%coord) )then
        print *, "ERROR :: Line%SetNode => (size(obj%coord,2)/=size(point%coord) )"
        return
    else
        obj%coord(position,:)=point%coord(:)
    endif

end subroutine
!#########################################################

!#########################################################
subroutine importLine(obj,NodCoord)
    class(Line_),intent(inout)::obj
    integer(int32),intent(in)::NodCoord(:,:)

    if(size(obj%coord,2)/=size(NodCoord,2) )then
        print *, "ERROR :: importLine :: size(obj%NodCoord,2)/=size(NodCoord,2)"
        return
    endif
    obj%coord(1:2,:)=NodCoord(1:2,:)

end subroutine
!#########################################################

!#########################################################
subroutine showLine(obj,Name)
    class(Line_),intent(in)::obj
    character(*),optional,intent(in)::Name

    if(present(Name) )then
        open(10,file=Name)
        write(10,*) obj%coord(1,:)
        write(10,*) obj%coord(2,:)
        close(10)
    else
        print *, "Line> x1(:) = ",obj%coord(1,:)
        print *, "Line> x2(:) = ",obj%coord(2,:)
    endif
end subroutine
!#########################################################

!#########################################################
subroutine InitCircle(obj,dim)
    class(Circle_),intent(inout)::obj
    integer(int32),optional,intent(inout)::dim

    ! default = 3D
    if(allocated(obj%center) )then
        deallocate(obj%center)
    endif
    allocate(obj%center(input(default=3,option=dim) ) )
    obj%center(:)=0.0d0
    obj%radius=1.0d0

end subroutine
!#########################################################

!#########################################################
subroutine InitSetCenterCircle(obj,point)
    class(Circle_),intent(inout)::obj
    class(Point_),intent(in)::point

    if( size(obj%center)/=size(point%coord) )then
        print *, "ERROR ::InitSetCenterCircle ::  ( size(obj%center)/=size(point%coord) )"
        return
    endif

    obj%center(:)=point%coord(:)

end subroutine
!#########################################################

!#########################################################
subroutine InitSetRadiusCircle(obj,radius)
    class(Circle_),intent(inout)::obj
    real(real64),intent(in)::radius

    obj%radius=radius

end subroutine
!#########################################################

!#########################################################
function getAreaCircle(obj) result(area)
    class(Circle_),intent(inout)::obj
    real(real64) :: area,pi
    pi=3.14159260d0

    area=obj%radius*obj%radius*pi

end function
!#########################################################

!#########################################################
subroutine showCircle(obj,Name)
    class(Circle_),intent(in)::obj
    character(*),optional,intent(in)::Name
    real(real64) :: angle,dtheta,pi
    integer(int32) :: i

    pi=3.14159260d0

    if(present(Name) )then
        open(10,file=Name)
        angle=0.0d0
        dtheta=2.0d0*pi/360.0
        do i=1, 360
            write(10,*) obj%center(1)+obj%radius*cos(angle) , obj%center(2)+obj%radius*sin(angle)
            angle = angle + dtheta
        enddo
        close(10)
    else
        print *, "center> O(:) = ", obj%center(:)
        print *, "radius = ", obj%radius
    endif

end subroutine
!#########################################################

!#########################################################
subroutine InitSphere(obj,dim)
    class(Sphere_),intent(inout)::obj
    integer(int32),optional,intent(inout)::dim

    obj%center(:)=0.0d0
    obj%radius=1.0d0

end subroutine
!#########################################################

!#########################################################
subroutine InitSetCenterSphere(obj,point)
    class(Sphere_),intent(inout)::obj
    class(Point_),intent(in)::point

    if( size(obj%center)/=size(point%coord) )then
        print *, "ERROR ::InitSetCenterSphere ::  ( size(obj%center)/=size(point%coord) )"
        return
    endif

    obj%center(:)=point%coord(:)

end subroutine
!#########################################################

!#########################################################
subroutine InitSetRadiusSphere(obj,radius)
    class(Sphere_),intent(inout)::obj
    real(real64),intent(in)::radius

    obj%radius=radius

end subroutine
!#########################################################

!#########################################################
subroutine showSphere(obj,Name)
    class(Sphere_),intent(in)::obj
    character(*),optional,intent(in)::Name
    real(real64) :: angle1,angle2,dtheta,pi
    integer(int32) :: i,j

    pi=3.14159260d0

    if(present(Name) )then
        open(10,file=Name)
        angle1=0.0d0
        angle2=0.0d0
        dtheta=2.0d0*pi/360.0
        do i=1,360
            do j=1,360
                write(10,*) obj%center(1)+obj%radius*sin(angle1)*cos(angle2),&
                    obj%center(2)+obj%radius*sin(angle1)*sin(angle2),&
                    obj%center(3)+obj%radius*cos(angle1)
            enddo
        enddo
        close(10)
    else
        print *, "center> O(:) = ",obj%center(:)
        print *, "radius = ",obj%radius
    endif

end subroutine
!#########################################################

!#########################################################
subroutine InitTriangle(obj,dim)
    class(Triangle_),intent(inout)::obj
    integer(int32),optional,intent(in)::dim

    if(allocated(obj%NodCoord) )then
        deallocate(obj%NodCoord)
    endif
    allocate(obj%NodCoord(3,input(default=3,option=dim) ) )
    obj%NodCoord(:,:)=0.0d0

end subroutine
!#########################################################

!#########################################################
subroutine setNodeTriangle(obj,point,order)
    class(Triangle_),intent(inout)::obj
    class(Point_),intent(in)::point
    integer(int32),intent(in)::order

    if( size(obj%NodCoord,2)/=size(point%coord) )then
        print *, "ERROR ::InitSetNodeTriangle ::  ( size(obj%NodCoord,2)/=size(point%coord)  )"
        return
    endif
    obj%NodCoord(order,:)=point%coord(:)
end subroutine
!#########################################################

!#########################################################
subroutine importTriangle(obj,NodCoord,FileName)
    class(Triangle_),intent(inout)::obj
    integer(int32),optional,intent(in)::NodCoord(:,:)

    integer(int32) :: n,i
    character(*),optional,intent(in)::FileName

    if(present(FileName) )then
        open(30,file=trim(FileName))
        read(30,*) n
        call obj%init(dim=n)
        do i=1,size(obj%NodCoord,1)
            read(30,*) obj%NodCoord(i,:)
        enddo
        close(30)
        print *, "Imported triangle from ",trim(FileName)
        return
    endif

    if(present(NodCoord) )then

        if(size(obj%NodCoord,2)/=size(NodCoord,2) )then
            print *, "ERROR :: importTriangle :: size(obj%NodCoord,2)/=size(NodCoord,2)"
            return
        endif
        obj%NodCoord(1:3,:)=NodCoord(1:3,:)
    endif

end subroutine
!#########################################################

!#########################################################
subroutine getCircleTriangle(obj,type_of_circle,circle)
    class(Triangle_),intent(in)::obj
    type(Circle_),intent(inout) :: circle
    character(*),intent(in) :: type_of_circle
    real(real64) :: x1,x2,x3,y1,y2,y3
    real(real64),allocatable :: a(:),b(:),c(:)
    integer(int32) :: i,n

    n=size(obj%NodCoord,2)
    call initCircle(circle,dim=n)

    if(type_of_circle == "center_of_gravity")then

        do i=1,3
            circle%center(:)=circle%center(:)+1.0d0/3.0d0*obj%NodCoord(i,:)
        enddo
        circle%radius=1.0d0

    elseif( type_of_circle == "circumcenter"  )then

        ! 外心を計算する
        allocate(a( size(obj%NodCoord,2) ) )
        a(:) = obj%NodCoord(1,:)
        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        circle%center(1)=  0.50d0*( &
                    (x1*x1+y1*y1)*(y2-y3) + &
                    (x2*x2+y2*y2)*(y3-y1) + &
                    (x3*x3+y3*y3)*(y1-y2)  &
                    )
        circle%center(1)=  circle%center(1)/(&
                    x1*(y2-y3) + &
                    x2*(y3-y1) + &
                    x3*(y1-y2)  &
                    )
        circle%center(2)=  0.50d0*( &
                    (x1*x1+y1*y1)*(x2-x3) + &
                    (x2*x2+y2*y2)*(x3-x1) + &
                    (x3*x3+y3*y3)*(x1-x2)  &
                    )
        circle%center(2)=  -circle%center(2)/(&
                    x1*(y2-y3) + &
                    x2*(y3-y1) + &
                    x3*(y1-y2)  &
                    )
        circle%radius = distance(circle%center,a )

    elseif( type_of_circle == "innter_center"  )then

        ! 外心を計算する
        allocate(a( size(obj%NodCoord,2) ) )
        allocate(b( size(obj%NodCoord,2) ) )
        allocate(c( size(obj%NodCoord,2) ) )
        a(:) = obj%NodCoord(1,:)
        b(:) = obj%NodCoord(2,:)
        c(:) = obj%NodCoord(3,:)
        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        circle%center(1)=  sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) )*x1 + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) )*x2 + &
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) )*x3
        circle%center(1)=  circle%center(1)/(&
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) ) + &
                    sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) ) + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) ) &
                    )
        circle%center(2)=  sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) )*y1 + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) )*y2 + &
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) )*y3
        circle%center(2)=  circle%center(2)/(&
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) ) + &
                    sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) ) + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) ) &
                    )
        circle%radius = 2.0d0*obj%getArea()/(distance(a,b )+ distance(b,c )+ distance(c,a ) )

    elseif( type_of_circle == "excenter"  )then
        print *, "ERROR :: getCircleTriangle :: type_of_circle/excenter is now being implemented"
    elseif( type_of_circle == "orthocenter"  )then
        print *, "ERROR :: getCircleTriangle :: type_of_circle/orthocenter is now being implemented"
    endif

end subroutine
!#########################################################

!#########################################################
function getAreaTriangle(obj) result(area)
    class(Triangle_),intent(in)::obj
    real(real64) :: area
    real(real64) :: x1,x2,x3,y1,y2,y3
    real(real64) :: a(3),b(3),c(3)

    if(size(obj%NodCoord,2)==2 )then

        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        area = abs(0.50d0*( x1*y2 + x2*y3 + x3*y1 - y1*x2 -y2*x3 - y3*x1 ))

    elseif( size(obj%NodCoord,2)==3 )then
        a(:)=obj%NodCoord(1,:)
        b(:)=obj%NodCoord(2,:)
        c(:)=obj%NodCoord(3,:)
        a(:)=a(:)-c(:)
        b(:)=b(:)-c(:)
        area=(a(1)*a(1) + a(2)*a(2) + a(3)*a(3))*(b(1)*b(1) + b(2)*b(2) + b(3)*b(3))
        area=area-(a(1)*b(1)+a(2)*b(2)+a(3)*b(3))*(a(1)*b(1)+a(2)*b(2)+a(3)*b(3))
        area=0.50d0*area
        area=sqrt(area)
    else
        print *, "ERROR :: getAreaTriangle, size(obj%NodCoord,2)==2 "
        return
    endif

end function
!#########################################################

!#########################################################
subroutine showTriangle(obj,Name,option)
    class(Triangle_),intent(in)::obj
    character(*),optional,intent(in)::Name,option

    if(present(Name) )then
        if(present(option) )then
            open(10,file=Name)
            write(10,*) obj%NodCoord(1,:)
            write(10,*) obj%NodCoord(2,:)
            write(10,*) obj%NodCoord(3,:)
            write(10,*) obj%NodCoord(1,:)
            close(10)
        else
            open(10,file=Name)
            write(10,*) obj%NodCoord(1,:),obj%NodCoord(2,:)-obj%NodCoord(1,:)
            write(10,*) obj%NodCoord(2,:),obj%NodCoord(3,:)-obj%NodCoord(2,:)
            write(10,*) obj%NodCoord(3,:),obj%NodCoord(1,:)-obj%NodCoord(3,:)
            close(10)
        endif
    else
        print *, "Triangle> x1(:) = ",obj%NodCoord(1,:)
        print *, "Triangle> x2(:) = ",obj%NodCoord(2,:)
        print *, "Triangle> x3(:) = ",obj%NodCoord(3,:)
    endif
end subroutine
!#########################################################

!#########################################################
subroutine GetOuterNormalTriangle(obj)
    class(Triangle_),intent(inout) :: obj
    real(real64) :: x1(3),x2(3),x3(3),on(3)
    integer(int32) :: n

    if(.not.allocated(obj%OuterNormal) )then
        n=size(obj%NodCoord,2)
        allocate(obj%OuterNormal(n) )
        obj%OuterNormal(:)=0.0d0
    endif

    x1(:)=0.0d0
    x2(:)=0.0d0
    x1(:)=obj%NodCoord(2,1:n)-obj%NodCoord(1,1:n)
    x2(:)=obj%NodCoord(3,1:n)-obj%NodCoord(1,1:n)
    on(:)=1.0d0/norm(cross_product(x1,x2)) *cross_product(x1,x2)
    obj%OuterNormal(1:n)=on(1:n)

    x1(1:n) = obj%NodCoord(1,1:n)
    x2(1:n) = obj%NodCoord(2,1:n)
    x3(1:n) = obj%NodCoord(3,1:n)
    if(allocated(obj%center) ) then
        deallocate(obj%center)
    endif
    allocate(obj%center(n) )
    obj%center(:)=0.0d0
    obj%center(1:n)=obj%center(1:n)+x1(1:n)
    obj%center(1:n)=obj%center(1:n)+x2(1:n)
    obj%center(1:n)=obj%center(1:n)+x3(1:n)
    obj%center(1:n)=1.0d0/3.0d0*obj%center(1:n)

end subroutine
!#########################################################

!#########################################################
subroutine InitRectangle(obj,dim)
    class(Rectangle_),intent(inout)::obj
    integer(int32),optional,intent(in)::dim

    if(allocated(obj%NodCoord) )then
        deallocate(obj%NodCoord)
    endif
    allocate(obj%NodCoord(4,input(default=3,option=dim) ) )
    obj%NodCoord(:,:)=0.0d0

end subroutine
!#########################################################

!#########################################################
subroutine createRectangle(obj)
    class(Rectangle_),intent(inout) :: obj

    ! create unit one
    allocate(obj%NodCoord(8,3) )
    obj%NodCoord(1,1)=-1.0d0; obj%NodCoord(1,2)=-1.0d0; obj%NodCoord(1,3)=-1.0d0;
    obj%NodCoord(2,1)= 1.0d0; obj%NodCoord(2,2)=-1.0d0; obj%NodCoord(2,3)=-1.0d0;
    obj%NodCoord(3,1)= 1.0d0; obj%NodCoord(3,2)= 1.0d0; obj%NodCoord(3,3)=-1.0d0;
    obj%NodCoord(4,1)=-1.0d0; obj%NodCoord(4,2)= 1.0d0; obj%NodCoord(4,3)=-1.0d0;
    obj%NodCoord(5,1)=-1.0d0; obj%NodCoord(5,2)=-1.0d0; obj%NodCoord(5,3)= 1.0d0;
    obj%NodCoord(6,1)= 1.0d0; obj%NodCoord(6,2)=-1.0d0; obj%NodCoord(6,3)= 1.0d0;
    obj%NodCoord(7,1)= 1.0d0; obj%NodCoord(7,2)= 1.0d0; obj%NodCoord(7,3)= 1.0d0;
    obj%NodCoord(8,1)=-1.0d0; obj%NodCoord(8,2)= 1.0d0; obj%NodCoord(8,3)= 1.0d0;
end subroutine createRectangle
!#########################################################

!#########################################################
subroutine moveRectangle(obj,x,y,z)
    class(Rectangle_),intent(inout) :: obj
    real(real64),optional,intent(in) :: x, y, z

    obj%NodCoord(:,1)=obj%NodCoord(:,1)+input(default=0.0d0,option=x)
    obj%NodCoord(:,2)=obj%NodCoord(:,2)+input(default=0.0d0,option=y)
    obj%NodCoord(:,3)=obj%NodCoord(:,3)+input(default=0.0d0,option=z)

end subroutine moveRectangle
!#########################################################

!#########################################################
subroutine setNodeRectangle(obj,point,order)
    class(Rectangle_),intent(inout)::obj
    class(Point_),intent(in)::point
    integer(int32),intent(in)::order

    if( size(obj%NodCoord,2)/=size(point%coord) )then
        print *, "ERROR ::InitSetNodeRectangle ::  ( size(obj%NodCoord,2)/=size(point%coord)  )"
        return
    endif
    obj%NodCoord(order,:)=point%coord(:)
end subroutine
!#########################################################

!#########################################################
subroutine importRectangle(obj,NodCoord,FileName)
    class(Rectangle_),intent(inout)::obj
    integer(int32),optional,intent(in)::NodCoord(:,:)
    integer(int32) :: n,i
    character(*),optional,intent(in)::FileName

    if(present(FileName) )then
        open(30,file=trim(FileName))
        read(30,*) n
        call obj%init(dim=n)
        do i=1,size(obj%NodCoord,1)
            read(30,*) obj%NodCoord(i,:)
        enddo
        close(30)
        print *, "Imported rectangle from ",trim(FileName)
        return
    endif

    if(present(NodCoord) )then
        if(size(obj%NodCoord,2)/=size(NodCoord,2) )then
            print *, "ERROR :: importRectangle :: size(obj%NodCoord,2)/=size(NodCoord,2)"
            return
        endif
        obj%NodCoord(1:4,:)=NodCoord(1:4,:)
    endif
end subroutine
!#########################################################

!#########################################################
subroutine getCircleRectangle(obj,type_of_circle,circle)
    class(Rectangle_),intent(in)::obj
    type(Circle_),intent(inout) :: circle
    character(*),intent(in) :: type_of_circle
    real(real64) :: x1,x2,x3,x4,y1,y2,y3,y4
    real(real64),allocatable :: a(:),b(:),c(:)
    integer(int32) :: i,n

    n=size(obj%NodCoord,2)
    call initCircle(circle,dim=n)

    if(type_of_circle == "center_of_gravity")then

        do i=1,4
            circle%center(:)=circle%center(:)+1.0d0/4.0d0*obj%NodCoord(i,:)
        enddo
        circle%radius=1.0d0

    elseif( type_of_circle == "circumcenter"  )then

        ! 外心を計算する
        allocate(a( size(obj%NodCoord,2) ) )
        a(:) = obj%NodCoord(1,:)
        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        x4 = obj%NodCoord( 4,1 )
        y4 = obj%NodCoord( 4,2 )

        circle%center(1)=  0.50d0*( &
                    (x1*x1+y1*y1)*(y2-y3) + &
                    (x2*x2+y2*y2)*(y3-y1) + &
                    (x3*x3+y3*y3)*(y1-y2)  &
                    )
        circle%center(1)=  circle%center(1)/(&
                    x1*(y2-y3) + &
                    x2*(y3-y1) + &
                    x3*(y1-y2)  &
                    )
        circle%center(2)=  0.50d0*( &
                    (x1*x1+y1*y1)*(x2-x3) + &
                    (x2*x2+y2*y2)*(x3-x1) + &
                    (x3*x3+y3*y3)*(x1-x2)  &
                    )
        circle%center(2)=  -circle%center(2)/(&
                    x1*(y2-y3) + &
                    x2*(y3-y1) + &
                    x3*(y1-y2)  &
                    )
        circle%radius = distance(circle%center,a )

    elseif( type_of_circle == "innter_center"  )then

        ! 外心を計算する
        allocate(a( size(obj%NodCoord,2) ) )
        allocate(b( size(obj%NodCoord,2) ) )
        allocate(c( size(obj%NodCoord,2) ) )
        a(:) = obj%NodCoord(1,:)
        b(:) = obj%NodCoord(2,:)
        c(:) = obj%NodCoord(3,:)
        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        circle%center(1)=  sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) )*x1 + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) )*x2 + &
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) )*x3
        circle%center(1)=  circle%center(1)/(&
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) ) + &
                    sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) ) + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) ) &
                    )
        circle%center(2)=  sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) )*y1 + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) )*y2 + &
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) )*y3
        circle%center(2)=  circle%center(2)/(&
                    sqrt( (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) ) + &
                    sqrt( (x3-x2)*(x3-x2)+(y3-y2)*(y3-y2) ) + &
                    sqrt( (x1-x3)*(x1-x3)+(y1-y3)*(y1-y3) ) &
                    )
        circle%radius = 2.0d0*obj%getArea()/(distance(a,b )+ distance(b,c )+ distance(c,a ) )

    elseif( type_of_circle == "excenter"  )then
        print *, "ERROR :: getCircleRectangle :: type_of_circle/excenter is now being implemented"
    elseif( type_of_circle == "orthocenter"  )then
        print *, "ERROR :: getCircleRectangle :: type_of_circle/orthocenter is now being implemented"
    endif

end subroutine
!#########################################################

!#########################################################
function getAreaRectangle(obj) result(area)
    class(Rectangle_),intent(in)::obj
    real(real64) :: area
    real(real64) :: x1,x2,x3,x4,y1,y2,y3,y4

    if(size(obj%NodCoord,2)==2 )then

        x1 = obj%NodCoord( 1,1 )
        y1 = obj%NodCoord( 1,2 )
        x2 = obj%NodCoord( 2,1 )
        y2 = obj%NodCoord( 2,2 )
        x3 = obj%NodCoord( 3,1 )
        y3 = obj%NodCoord( 3,2 )
        area = abs(0.50d0*( x1*y2 + x2*y3 + x3*y1 - y1*x2 -y2*x3 - y3*x1 ))

        x2 = obj%NodCoord( 1,1 )
        y2 = obj%NodCoord( 1,2 )
        x3 = obj%NodCoord( 2,1 )
        y3 = obj%NodCoord( 2,2 )
        x4 = obj%NodCoord( 3,1 )
        y4 = obj%NodCoord( 3,2 )
        area = area+abs(0.50d0*( x1*y2 + x2*y3 + x3*y1 - y1*x2 -y2*x3 - y3*x1 ))

    elseif( size(obj%NodCoord,2)==3 )then
        print *, " getAreaRectangle >> not ready for 3D"
        stop
    else
        print *, "ERROR :: getAreaRectangle, size(obj%NodCoord,2)==2 "
        return
    endif

end function
!#########################################################

!#########################################################
subroutine showRectangle(obj,Name,option)
    class(Rectangle_),intent(in)::obj
    character(*),optional,intent(in)::Name,option

    if(present(Name) )then
        if(present(option) )then
            open(10,file=Name)
            write(10,*) obj%NodCoord(1,:)
            write(10,*) obj%NodCoord(2,:)
            write(10,*) obj%NodCoord(3,:)
            write(10,*) obj%NodCoord(4,:)
            write(10,*) obj%NodCoord(1,:)
            close(10)
        else
            open(10,file=Name)
            write(10,*) obj%NodCoord(1,:),obj%NodCoord(2,:)-obj%NodCoord(1,:)
            write(10,*) obj%NodCoord(2,:),obj%NodCoord(3,:)-obj%NodCoord(2,:)
            write(10,*) obj%NodCoord(3,:),obj%NodCoord(4,:)-obj%NodCoord(3,:)
            write(10,*) obj%NodCoord(4,:),obj%NodCoord(1,:)-obj%NodCoord(4,:)
            close(10)
        endif
    else
        print *, "Rectangle> x1(:) = ",obj%NodCoord(1,:)
        print *, "Rectangle> x2(:) = ",obj%NodCoord(2,:)
        print *, "Rectangle> x3(:) = ",obj%NodCoord(3,:)
        print *, "Rectangle> x3(:) = ",obj%NodCoord(4,:)
    endif
end subroutine
!#########################################################

!#########################################################
function contactRectangle(obj,Rectangle,threshold) result(contact)
    class(Rectangle_),intent(in) :: obj
    class(Rectangle_),intent(in) :: Rectangle
    integer(int32),optional,intent(in) :: threshold
    logical :: contact, inside
    integer(int32) :: i,j,k,n,m,dim,inside_score,th
    real(real64)::dist1,dist2,dist3,x2d(2),x3d(3),x_max(3),x_min(3)

    th=input(default=1,option=threshold)

    ! Contact Rectangle to Rectangle
    dim = size(obj%NodCoord,2)
    if(dim==3)then
        ! get range
        do i=1,dim
            x_max(i)=maxval(Rectangle%NodCoord(:,i))
            x_min(i)=minval(Rectangle%NodCoord(:,i))
        enddo
        inside_score=0
        inside=.false.
        do i=1,size(obj%NodCoord,1)
            x3d(1:3)=obj%NodCoord(i,1:3)
            inside=InOrOut(x=x3d,xmax=x_max,xmin=x_min,DimNum=dim)
            if(inside .eqv. .true.)then
                inside_score=inside_score+1
            endif
        enddo
        if(inside_score >= th)then
            contact=.true.
        else
            contact=.false.
        endif
    else
        print *, "Please implement contactRectangle for",dim,"D"
        stop
    endif

end function contactRectangle
!#########################################################

!#########################################################
subroutine InitTetrahedron(obj,NodCoord)
    class(Tetrahedron_),intent(inout) :: obj
    real(real64),intent(in) :: NodCoord(4,3)

    obj%NodCoord(:,:)=NodCoord(:,:)

end subroutine
!#########################################################

!#########################################################
subroutine getCircleTetrahedron(obj)
    class(Tetrahedron_),intent(inout) :: obj
    real(real64) :: a(3),b(3),c(3),d(3),e(3),f(3),g(3),s,t,r,N(3)
    real(real64) :: a_(3),b_(3),c_(3),d_(3),aA,aB,aC,aD,V,k

    a(:)=obj%NodCoord(1,:)
    b(:)=obj%NodCoord(2,:)
    c(:)=obj%NodCoord(3,:)
    d(:)=obj%NodCoord(4,:)

    a_(:) = a(:) - d(:)
    b_(:) = b(:) - d(:)
    c_(:) = c(:) - d(:)
    d_(:) = 0.0d0

    aA = 0.50d0*norm(cross_product(a_,b_) )
    aB = 0.50d0*norm(cross_product(b_,c_) )
    aC = 0.50d0*norm(cross_product(c_,d_) )
    aD = 0.50d0*norm(cross_product(d_,a_) )

    V=1.0d0/6.0d0*dot_product(cross_product(a_(:),b_(:) ),c_(:) )
    r = 3.0d0*V/( aA+aB+aC+aD )
    obj%radius = r

end subroutine
!#########################################################

!#########################################################
!subroutine
!end subroutine
!#########################################################
end module GeometryClass


      SUBROUTINE SB10ED( N, M, NP, NCON, NMEAS, A, LDA, B, LDB, C, LDC,
     $                   D, LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK,
     $                   RCOND, TOL, IWORK, DWORK, LDWORK, BWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrices of the H2 optimal n-state controller
C
C                           | AK | BK |
C                       K = |----|----|
C                           | CK | DK |
C
C     for the discrete-time system
C
C                   | A  | B1  B2  |   | A | B |
C               P = |----|---------| = |---|---| ,
C                   | C1 |  0  D12 |   | C | D |
C                   | C2 | D21 D22 |
C
C     where B2 has as column size the number of control inputs (NCON)
C     and C2 has as row size the number of measurements (NMEAS) being
C     provided to the controller.
C
C     It is assumed that
C
C     (A1) (A,B2) is stabilizable and (C2,A) is detectable,
C
C     (A2) D12 is full column rank and D21 is full row rank,
C
C               j*Theta
C     (A3) | A-e       *I  B2  | has full column rank for all
C          |    C1         D12 |
C
C          0 <= Theta < 2*Pi ,
C
C
C               j*Theta
C     (A4) | A-e       *I  B1  |  has full row rank for all
C          |    C2         D21 |
C
C          0 <= Theta < 2*Pi .
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     NCON    (input) INTEGER
C             The number of control inputs (M2).  M >= NCON >= 0,
C             NP-NMEAS >= NCON.
C
C     NMEAS   (input) INTEGER
C             The number of measurements (NP2).  NP >= NMEAS >= 0,
C             M-NCON >= NMEAS.
C
C     A       (input/worksp.) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain the
C             system state matrix A.
C             This array is modified internally, but it is restored on
C             exit.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= max(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain the
C             system input matrix B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= max(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading NP-by-N part of this array must contain the
C             system output matrix C.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= max(1,NP).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading NP-by-M part of this array must contain the
C             system input/output matrix D.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     AK      (output) DOUBLE PRECISION array, dimension (LDAK,N)
C             The leading N-by-N part of this array contains the
C             controller state matrix AK.
C
C     LDAK    INTEGER
C             The leading dimension of the array AK.  LDAK >= max(1,N).
C
C     BK      (output) DOUBLE PRECISION array, dimension (LDBK,NMEAS)
C             The leading N-by-NMEAS part of this array contains the
C             controller input matrix BK.
C
C     LDBK    INTEGER
C             The leading dimension of the array BK.  LDBK >= max(1,N).
C
C     CK      (output) DOUBLE PRECISION array, dimension (LDCK,N)
C             The leading NCON-by-N part of this array contains the
C             controller output matrix CK.
C
C     LDCK    INTEGER
C             The leading dimension of the array CK.
C             LDCK >= max(1,NCON).
C
C     DK      (output) DOUBLE PRECISION array, dimension (LDDK,NMEAS)
C             The leading NCON-by-NMEAS part of this array contains the
C             controller input/output matrix DK.
C
C     LDDK    INTEGER
C             The leading dimension of the array DK.
C             LDDK >= max(1,NCON).
C
C     RCOND   (output) DOUBLE PRECISION array, dimension (7)
C             RCOND contains estimates the reciprocal condition
C             numbers of the matrices which are to be inverted and the
C             reciprocal condition numbers of the Riccati equations
C             which have to be solved during the computation of the
C             controller. (See the description of the algorithm in [2].)
C             RCOND(1) contains the reciprocal condition number of the
C                      control transformation matrix TU;
C             RCOND(2) contains the reciprocal condition number of the
C                      measurement transformation matrix TY;
C             RCOND(3) contains the reciprocal condition number of the
C                      matrix Im2 + B2'*X2*B2;
C             RCOND(4) contains the reciprocal condition number of the
C                      matrix Ip2 + C2*Y2*C2';
C             RCOND(5) contains the reciprocal condition number of the
C                      X-Riccati equation;
C             RCOND(6) contains the reciprocal condition number of the
C                      Y-Riccati equation;
C             RCOND(7) contains the reciprocal condition number of the
C                      matrix Im2 + DKHAT*D22 .
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             Tolerance used for controlling the accuracy of the
C             transformations applied for diagonalizing D12 and D21,
C             and for checking the nonsingularity of the matrices to be
C             inverted. If TOL <= 0, then a default value equal to
C             sqrt(EPS) is used, where EPS is the relative machine
C             precision.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (max(2*M2,2*N,N*N,NP2))
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) contains the optimal
C             LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= N*M + NP*(N+M) + M2*M2 + NP2*NP2 +
C                       max(1,LW1,LW2,LW3,LW4,LW5,LW6), where
C             LW1 = (N+NP1+1)*(N+M2) + max(3*(N+M2)+N+NP1,5*(N+M2)),
C             LW2 = (N+NP2)*(N+M1+1) + max(3*(N+NP2)+N+M1,5*(N+NP2)),
C             LW3 = M2 + NP1*NP1 + max(NP1*max(N,M1),3*M2+NP1,5*M2),
C             LW4 = NP2 + M1*M1 + max(max(N,NP1)*M1,3*NP2+M1,5*NP2),
C             LW5 = 2*N*N+max(1,14*N*N+6*N+max(14*N+23,16*N),M2*(N+M2+
C                             max(3,M1)),NP2*(N+NP2+3)),
C             LW6 = max(N*M2,N*NP2,M2*NP2,M2*M2+4*M2),
C             with M1 = M - M2 and NP1 = NP - NP2.
C             For good performance, LDWORK must generally be larger.
C             Denoting Q = max(M1,M2,NP1,NP2), an upper bound is
C             2*Q*(3*Q+2*N)+max(1,(N+Q)*(N+Q+6),Q*(Q+max(N,Q,5)+1),
C                     2*N*N+max(1,14*N*N+6*N+max(14*N+23,16*N),
C                               Q*(N+Q+max(Q,3)))).
C
C     BWORK   LOGICAL array, dimension (2*N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C                                      j*Theta
C             = 1:  if the matrix | A-e       *I  B2  | had not full
C                                 |      C1       D12 |
C                   column rank in respect to the tolerance EPS;
C                                      j*Theta
C             = 2:  if the matrix | A-e       *I  B1  |  had not full
C                                 |      C2       D21 |
C                   row rank in respect to the tolerance EPS;
C             = 3:  if the matrix D12 had not full column rank in
C                   respect to the tolerance TOL;
C             = 4:  if the matrix D21 had not full row rank in respect
C                   to the tolerance TOL;
C             = 5:  if the singular value decomposition (SVD) algorithm
C                   did not converge (when computing the SVD of one of
C                   the matrices |A-I  B2 |, |A-I  B1 |, D12 or D21).
C                                |C1   D12|  |C2   D21|
C             = 6:  if the X-Riccati equation was not solved
C                   successfully;
C             = 7:  if the matrix Im2 + B2'*X2*B2 is not positive
C                   definite, or it is numerically singular (with
C                   respect to the tolerance TOL);
C             = 8:  if the Y-Riccati equation was not solved
C                   successfully;
C             = 9:  if the matrix Ip2 + C2*Y2*C2' is not positive
C                   definite, or it is numerically singular (with
C                   respect to the tolerance TOL);
C             =10:  if the matrix Im2 + DKHAT*D22 is singular, or its
C                   estimated condition number is larger than or equal
C                   to 1/TOL.
C
C     METHOD
C
C     The routine implements the formulas given in [1].
C
C     REFERENCES
C
C     [1] Zhou, K., Doyle, J.C., and Glover, K.
C         Robust and Optimal Control.
C         Prentice-Hall, Upper Saddle River, NJ, 1996.
C
C     [2] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
C         Fortran 77 routines for Hinf and H2 design of linear
C         discrete-time control systems.
C         Report 99-8, Department of Engineering, Leicester University,
C         April 1999.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of the result depends on the condition numbers of the
C     matrices which are to be inverted and on the condition numbers of
C     the matrix Riccati equations which are to be solved in the
C     computation of the controller. (The corresponding reciprocal
C     condition numbers are given in the output array RCOND.)
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, May 1999.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 1999,
C     Sept. 1999, Feb. 2000, Nov. 2005.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, H2 optimal control, optimal regulator,
C     robust control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDD,
     $                   LDDK, LDWORK, M, N, NCON, NMEAS, NP
      DOUBLE PRECISION   TOL
C     ..
C     .. Array Arguments ..
      INTEGER            IWORK( * )
      DOUBLE PRECISION   A( LDA, * ), AK( LDAK, * ), B( LDB, * ),
     $                   BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ),
     $                   D( LDD, * ), DK( LDDK, * ), DWORK( * ),
     $                   RCOND( * )
      LOGICAL            BWORK( * )
C     ..
C     .. Local Scalars ..
      INTEGER            I, INFO2, IWC, IWD, IWRK, IWTU, IWTY, IWX, IWY,
     $                   LW1, LW2, LW3, LW4, LW5, LW6, LWAMAX, M1, M2,
     $                   M2L, MINWRK, NL, NLP, NP1, NP2, NPL
      DOUBLE PRECISION   TOLL
C     ..
C     .. External Functions ..
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH
C     ..
C     .. External Subroutines ..
      EXTERNAL           DLACPY, SB10PD, SB10SD, SB10TD, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          DBLE, INT, MAX, SQRT
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      M1  = M - NCON
      M2  = NCON
      NP1 = NP - NMEAS
      NP2 = NMEAS
      NL  = MAX( 1, N )
      NPL = MAX( 1, NP )
      M2L = MAX( 1, M2 )
      NLP = MAX( 1, NP2 )
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
         INFO = -4
      ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
         INFO = -5
      ELSE IF( LDA.LT.NL ) THEN
         INFO = -7
      ELSE IF( LDB.LT.NL ) THEN
         INFO = -9
      ELSE IF( LDC.LT.NPL ) THEN
         INFO = -11
      ELSE IF( LDD.LT.NPL ) THEN
         INFO = -13
      ELSE IF( LDAK.LT.NL ) THEN
         INFO = -15
      ELSE IF( LDBK.LT.NL ) THEN
         INFO = -17
      ELSE IF( LDCK.LT.M2L ) THEN
         INFO = -19
      ELSE IF( LDDK.LT.M2L ) THEN
         INFO = -21
      ELSE
C
C        Compute workspace.
C
         LW1 = ( N + NP1 + 1 )*( N + M2 ) + MAX( 3*( N + M2 ) + N + NP1,
     $                                           5*( N + M2 ) )
         LW2 = ( N + NP2 )*( N + M1 + 1 ) + MAX( 3*( N + NP2 ) + N +
     $                                           M1, 5*( N + NP2 ) )
         LW3 = M2 + NP1*NP1 + MAX( NP1*MAX( N, M1 ), 3*M2 + NP1, 5*M2 )
         LW4 = NP2 + M1*M1  + MAX( MAX( N, NP1 )*M1, 3*NP2 + M1, 5*NP2 )
         LW5 = 2*N*N + MAX( 1, 14*N*N +
     $                         6*N + MAX( 14*N + 23, 16*N ),
     $                         M2*( N + M2 +  MAX( 3, M1 ) ),
     $                         NP2*( N + NP2 + 3 ) )
         LW6 = MAX( N*M2, N*NP2, M2*NP2, M2*M2 + 4*M2 )
         MINWRK = N*M + NP*( N + M ) + M2*M2 + NP2*NP2 +
     $            MAX( 1, LW1, LW2, LW3, LW4, LW5, LW6 )
         IF( LDWORK.LT.MINWRK )
     $      INFO = -26
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10ED', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .AND. MAX( M2, NP2 ).EQ.0 ) THEN
          RCOND( 1 ) = ONE
          RCOND( 2 ) = ONE
          RCOND( 3 ) = ONE
          RCOND( 4 ) = ONE
          RCOND( 5 ) = ONE
          RCOND( 6 ) = ONE
          RCOND( 7 ) = ONE
          DWORK( 1 ) = ONE
          RETURN
      END IF
C
      TOLL = TOL
      IF( TOLL.LE.ZERO ) THEN
C
C        Set the default value of the tolerance for rank tests.
C
         TOLL = SQRT( DLAMCH( 'Epsilon' ) )
      END IF
C
C     Workspace usage.
C
      IWC  = N*M  + 1
      IWD  = IWC  + NP*N
      IWTU = IWD  + NP*M
      IWTY = IWTU + M2*M2
      IWRK = IWTY + NP2*NP2
C
      CALL DLACPY( 'Full', N,  M, B, LDB, DWORK, NL )
      CALL DLACPY( 'Full', NP, N, C, LDC, DWORK( IWC ), NPL )
      CALL DLACPY( 'Full', NP, M, D, LDD, DWORK( IWD ), NPL )
C
C     Transform the system so that D12 and D21 satisfy the formulas
C     in the computation of the H2 optimal controller.
C     Since SLICOT Library routine SB10PD performs the tests
C     corresponding to the continuous-time counterparts of the
C     assumptions (A3) and (A4), for the frequency w = 0, the
C     next SB10PD routine call uses A - I.
C
      DO 10 I = 1, N
         A(I,I) = A(I,I) - ONE
   10 CONTINUE
C
      CALL SB10PD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, NL,
     $             DWORK( IWC ), NPL, DWORK( IWD ), NPL, DWORK( IWTU ),
     $             M2L, DWORK( IWTY ), NLP, RCOND, TOLL, DWORK( IWRK ),
     $             LDWORK-IWRK+1, INFO2 )
C
      DO 20 I = 1, N
         A(I,I) = A(I,I) + ONE
   20 CONTINUE
C
      IF( INFO2.GT.0 ) THEN
         INFO = INFO2
         RETURN
      END IF
      LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
      IWX  = IWRK
      IWY  = IWX + N*N
      IWRK = IWY + N*N
C
C     Compute the optimal H2 controller for the normalized system.
C
      CALL SB10SD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, NL,
     $             DWORK( IWC ), NPL, DWORK( IWD ), NPL, AK, LDAK, BK,
     $             LDBK, CK, LDCK, DK, LDDK, DWORK( IWX ), NL,
     $             DWORK( IWY ), NL, RCOND( 3 ), TOLL, IWORK,
     $             DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = INFO2 + 5
         RETURN
      END IF
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
      IWRK = IWX
C
C     Compute the H2 optimal controller for the original system.
C
      CALL SB10TD( N, M, NP, NCON, NMEAS, DWORK( IWD ), NPL,
     $             DWORK( IWTU ), M2L, DWORK( IWTY ), NLP, AK, LDAK, BK,
     $             LDBK, CK, LDCK, DK, LDDK, RCOND( 7 ), TOLL, IWORK,
     $             DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = 10
         RETURN
      END IF
C
      DWORK( 1 ) = DBLE( LWAMAX )
      RETURN
C *** Last line of SB10ED ***
      END


      SUBROUTINE TG01HD( JOBCON, COMPQ, COMPZ, N, M, P, A, LDA, E, LDE,
     $                   B, LDB, C, LDC, Q, LDQ, Z, LDZ, NCONT, NIUCON,
     $                   NRBLCK, RTAU, TOL, IWORK, DWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute orthogonal transformation matrices Q and Z which
C     reduce the N-th order descriptor system (A-lambda*E,B,C)
C     to the form
C
C                ( Ac  *  )             ( Ec  *  )           ( Bc )
C       Q'*A*Z = (        ) ,  Q'*E*Z = (        ) ,  Q'*B = (    ) ,
C                ( 0  Anc )             ( 0  Enc )           ( 0  )
C
C          C*Z = ( Cc Cnc ) ,
C
C     where the NCONT-th order descriptor system (Ac-lambda*Ec,Bc,Cc)
C     is a finite and/or infinite controllable. The pencil
C     Anc - lambda*Enc is regular of order N-NCONT and contains the
C     uncontrollable finite and/or infinite eigenvalues of the pencil
C     A-lambda*E.
C
C     For JOBCON = 'C' or 'I', the pencil ( Bc Ec-lambda*Ac ) has full
C     row rank NCONT for all finite lambda and is in a staircase form
C     with
C                     _      _          _        _
C                   ( E1,0   E1,1  ...  E1,k-1   E1,k  )
C                   (        _          _        _     )
C       ( Bc Ec ) = (  0     E2,1  ...  E2,k-1   E2,k  ) ,  (1)
C                   (              ...  _        _     )
C                   (  0       0   ...  Ek,k-1   Ek,k  )
C
C                     _          _        _
C                   ( A1,1  ...  A1,k-1   A1,k  )
C                   (            _        _     )
C         Ac      = (   0   ...  A2,k-1   A2,k  ) ,         (2)
C                   (       ...           _     )
C                   (   0   ...    0      Ak,k  )
C           _
C     where Ei,i-1 is an rtau(i)-by-rtau(i-1) full row rank matrix
C                            _
C     (with rtau(0) = M) and Ai,i is an rtau(i)-by-rtau(i)
C     upper triangular matrix.
C
C     For JOBCON = 'F', the pencil ( Bc Ac-lambda*Ec ) has full
C     row rank NCONT for all finite lambda and is in a staircase form
C     with
C                     _     _          _        _
C                   ( A1,0  A1,1  ...  A1,k-1   A1,k  )
C                   (       _          _        _     )
C       ( Bc Ac ) = (  0    A2,1  ...  A2,k-1   A2,k  ) ,   (3)
C                   (             ...  _        _     )
C                   (  0      0   ...  Ak,k-1   Ak,k  )
C
C                     _          _        _
C                   ( E1,1  ...  E1,k-1   E1,k  )
C                   (            _        _     )
C         Ec      = (   0   ...  E2,k-1   E2,k  ) ,         (4)
C                   (       ...           _     )
C                   (   0   ...    0      Ek,k  )
C           _
C     where Ai,i-1 is an rtau(i)-by-rtau(i-1) full row rank matrix
C                            _
C     (with rtau(0) = M) and Ei,i is an rtau(i)-by-rtau(i)
C     upper triangular matrix.
C
C     For JOBCON = 'C', the (N-NCONT)-by-(N-NCONT) regular pencil
C     Anc - lambda*Enc has the form
C
C                         ( Ainc - lambda*Einc         *          )
C      Anc - lambda*Enc = (                                       ) ,
C                         (        0           Afnc - lambda*Efnc )
C
C     where:
C       1) the NIUCON-by-NIUCON regular pencil Ainc - lambda*Einc,
C          with Ainc upper triangular and nonsingular, contains the
C          uncontrollable infinite eigenvalues of A - lambda*E;
C       2) the (N-NCONT-NIUCON)-by-(N-NCONT-NIUCON) regular pencil
C          Afnc - lambda*Efnc, with Efnc upper triangular and
C          nonsingular, contains the uncontrollable finite
C          eigenvalues of A - lambda*E.
C
C     Note: The significance of the two diagonal blocks can be
C           interchanged by calling the routine with the
C           arguments A and E interchanged. In this case,
C           Ainc - lambda*Einc contains the uncontrollable zero
C           eigenvalues of A - lambda*E, while Afnc - lambda*Efnc
C           contains the uncontrollable nonzero finite and infinite
C           eigenvalues of A - lambda*E.
C
C     For JOBCON = 'F', the pencil Anc - lambda*Enc has the form
C
C        Anc - lambda*Enc = Afnc - lambda*Efnc ,
C
C     where the regular pencil Afnc - lambda*Efnc, with Efnc
C     upper triangular and nonsingular, contains the uncontrollable
C     finite eigenvalues of A - lambda*E.
C
C     For JOBCON = 'I', the pencil Anc - lambda*Enc has the form
C
C        Anc - lambda*Enc = Ainc - lambda*Einc ,
C
C     where the regular pencil Ainc - lambda*Einc, with Ainc
C     upper triangular and nonsingular, contains the uncontrollable
C     nonzero finite and infinite eigenvalues of A - lambda*E.
C
C     The left and/or right orthogonal transformations Q and Z
C     performed to reduce the system matrices can be optionally
C     accumulated.
C
C     The reduced order descriptor system (Ac-lambda*Ec,Bc,Cc) has
C     the same transfer-function matrix as the original system
C     (A-lambda*E,B,C).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBCON  CHARACTER*1
C             = 'C':  separate both finite and infinite uncontrollable
C                     eigenvalues;
C             = 'F':  separate only finite uncontrollable eigenvalues:
C             = 'I':  separate only nonzero finite and infinite
C                     uncontrollable eigenvalues.
C
C     COMPQ   CHARACTER*1
C             = 'N':  do not compute Q;
C             = 'I':  Q is initialized to the unit matrix, and the
C                     orthogonal matrix Q is returned;
C             = 'U':  Q must contain an orthogonal matrix Q1 on entry,
C                     and the product Q1*Q is returned.
C
C     COMPZ   CHARACTER*1
C             = 'N':  do not compute Z;
C             = 'I':  Z is initialized to the unit matrix, and the
C                     orthogonal matrix Z is returned;
C             = 'U':  Z must contain an orthogonal matrix Z1 on entry,
C                     and the product Z1*Z is returned.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The dimension of the descriptor state vector; also the
C             order of square matrices A and E, the number of rows of
C             matrix B, and the number of columns of matrix C.  N >= 0.
C
C     M       (input) INTEGER
C             The dimension of descriptor system input vector; also the
C             number of columns of matrix B.  M >= 0.
C
C     P       (input) INTEGER
C             The dimension of descriptor system output vector; also the
C             number of rows of matrix C.  P >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N state matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the transformed state matrix Q'*A*Z,
C
C                                ( Ac   *  )
C                       Q'*A*Z = (         ) ,
C                                ( 0   Anc )
C
C             where Ac is NCONT-by-NCONT and Anc is
C             (N-NCONT)-by-(N-NCONT).
C             If JOBCON = 'F', the matrix ( Bc Ac ) is in the
C             controllability staircase form (3).
C             If JOBCON = 'C' or 'I', the submatrix Ac is upper
C             triangular.
C             If JOBCON = 'C', the Anc matrix has the form
C
C                             ( Ainc   *  )
C                       Anc = (           ) ,
C                             (  0   Afnc )
C
C             where the NIUCON-by-NIUCON matrix Ainc is nonsingular and
C             upper triangular.
C             If JOBCON = 'I', Anc is nonsingular and upper triangular.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     E       (input/output) DOUBLE PRECISION array, dimension (LDE,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N descriptor matrix E.
C             On exit, the leading N-by-N part of this array contains
C             the transformed descriptor matrix Q'*E*Z,
C
C                                ( Ec   *  )
C                       Q'*E*Z = (         ) ,
C                                ( 0   Enc )
C
C             where Ec is NCONT-by-NCONT and Enc is
C             (N-NCONT)-by-(N-NCONT).
C             If JOBCON = 'C' or 'I', the matrix ( Bc Ec ) is in the
C             controllability staircase form (1).
C             If JOBCON = 'F', the submatrix Ec is upper triangular.
C             If JOBCON = 'C', the Enc matrix has the form
C
C                             ( Einc   *  )
C                       Enc = (           ) ,
C                             (  0   Efnc )
C
C             where the NIUCON-by-NIUCON matrix Einc is nilpotent
C             and the (N-NCONT-NIUCON)-by-(N-NCONT-NIUCON) matrix Efnc
C             is nonsingular and upper triangular.
C             If JOBCON = 'F', Enc is nonsingular and upper triangular.
C
C     LDE     INTEGER
C             The leading dimension of array E.  LDE >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the N-by-M input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the transformed input matrix
C
C                              ( Bc )
C                       Q'*B = (    ) ,
C                              ( 0  )
C
C              where Bc is NCONT-by-M.
C              For JOBCON = 'C' or 'I', the matrix ( Bc Ec ) is in the
C              controllability staircase form (1).
C              For JOBCON = 'F', the matrix ( Bc Ac ) is in the
C              controllability staircase form (3).
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the state/output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed matrix C*Z.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
C             If COMPQ = 'N': Q is not referenced.
C             If COMPQ = 'I': on entry, Q need not be set;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix Q,
C                             where Q' is the product of transformations
C                             which are applied to A, E, and B on
C                             the left.
C             If COMPQ = 'U': on entry, the leading N-by-N part of this
C                             array must contain an orthogonal matrix
C                             Qc;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix
C                             Qc*Q.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.
C             LDQ >= 1,        if COMPQ = 'N';
C             LDQ >= MAX(1,N), if COMPQ = 'U' or 'I'.
C
C     Z       (input/output) DOUBLE PRECISION array, dimension (LDZ,N)
C             If COMPZ = 'N': Z is not referenced.
C             If COMPZ = 'I': on entry, Z need not be set;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix Z,
C                             which is the product of transformations
C                             applied to A, E, and C on the right.
C             If COMPZ = 'U': on entry, the leading N-by-N part of this
C                             array must contain an orthogonal matrix
C                             Zc;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix
C                             Zc*Z.
C
C     LDZ     INTEGER
C             The leading dimension of array Z.
C             LDZ >= 1,        if COMPZ = 'N';
C             LDZ >= MAX(1,N), if COMPZ = 'U' or 'I'.
C
C     NCONT   (output) INTEGER
C             The order of the reduced matrices Ac and Ec, and the
C             number of rows of reduced matrix Bc; also the order of
C             the controllable part of the pair (A-lambda*E,B).
C
C     NIUCON  (output) INTEGER
C             For JOBCON = 'C', the order of the reduced matrices
C             Ainc and Einc; also the number of uncontrollable
C             infinite eigenvalues of the pencil A - lambda*E.
C             For JOBCON = 'F' or 'I', NIUCON has no significance
C             and is set to zero.
C
C     NRBLCK  (output) INTEGER
C             For JOBCON = 'C' or 'I', the number k, of full row rank
C                    _
C             blocks Ei,i in the staircase form of the pencil
C             (Bc Ec-lambda*Ac) (see (1) and (2)).
C             For JOBCON = 'F', the number k, of full row rank blocks
C             _
C             Ai,i in the staircase form of the pencil (Bc Ac-lambda*Ec)
C             (see (3) and (4)).
C
C     RTAU    (output) INTEGER array, dimension (N)
C             RTAU(i), for i = 1, ..., NRBLCK, is the row dimension of
C                                     _         _
C             the full row rank block Ei,i-1 or Ai,i-1 in the staircase
C             form (1) or (3) for JOBCON = 'C' or 'I', or
C             for JOBCON = 'F', respectively.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determinations when
C             transforming (A-lambda*E, B). If the user sets TOL > 0,
C             then the given value of TOL is used as a lower bound for
C             reciprocal condition numbers in rank determinations; a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by  TOLDEF = N*N*EPS,  is used instead, where EPS
C             is the machine precision (see LAPACK Library routine
C             DLAMCH).  TOL < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (M)
C
C     DWORK   DOUBLE PRECISION array, dimension (MAX(N,2*M))
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The subroutine is based on the reduction algorithms of [1].
C
C     REFERENCES
C
C     [1] A. Varga
C         Computation of Irreducible Generalized State-Space
C         Realizations.
C         Kybernetika, vol. 26, pp. 89-106, 1990.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically backward stable and requires
C     0( N**3 )  floating point operations.
C
C     FURTHER COMMENTS
C
C     If the system matrices A, E and B are badly scaled, it is
C     generally recommendable to scale them with the SLICOT routine
C     TG01AD, before calling TG01HD.
C
C     CONTRIBUTOR
C
C     A. Varga, German Aerospace Center, DLR Oberpfaffenhofen.
C     March 1999. Based on the RASP routine RPDSCF.
C
C     REVISIONS
C
C     July 1999, V. Sima, Research Institute for Informatics, Bucharest.
C
C     KEYWORDS
C
C     Controllability, minimal realization, orthogonal canonical form,
C     orthogonal transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          COMPQ, COMPZ, JOBCON
      INTEGER            INFO, LDA, LDB, LDC, LDE, LDQ, LDZ,
     $                   M, N, NCONT, NIUCON, NRBLCK, P
      DOUBLE PRECISION   TOL
C     .. Array Arguments ..
      INTEGER            IWORK( * ), RTAU( * )
      DOUBLE PRECISION   A( LDA, * ), B( LDB, * ), C( LDC, *  ),
     $                   DWORK( * ),  E( LDE, * ), Q( LDQ, * ),
     $                   Z( LDZ, * )
C     .. Local Scalars ..
      CHARACTER          JOBQ, JOBZ
      LOGICAL            FINCON, ILQ, ILZ, INFCON
      INTEGER            ICOMPQ, ICOMPZ, LBA, NR
C     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
C     .. External Subroutines ..
      EXTERNAL           TG01HX, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          MAX
C
C     .. Executable Statements ..
C
C     Decode JOBCON.
C
      IF( LSAME( JOBCON, 'C' ) ) THEN
         FINCON = .TRUE.
         INFCON = .TRUE.
      ELSE IF( LSAME( JOBCON, 'F' ) ) THEN
         FINCON = .TRUE.
         INFCON = .FALSE.
      ELSE IF( LSAME( JOBCON, 'I' ) ) THEN
         FINCON = .FALSE.
         INFCON = .TRUE.
      ELSE
         FINCON = .FALSE.
         INFCON = .FALSE.
      END IF
C
C     Decode COMPQ.
C
      IF( LSAME( COMPQ, 'N' ) ) THEN
         ILQ = .FALSE.
         ICOMPQ = 1
      ELSE IF( LSAME( COMPQ, 'U' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 2
      ELSE IF( LSAME( COMPQ, 'I' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 3
      ELSE
         ICOMPQ = 0
      END IF
C
C     Decode COMPZ.
C
      IF( LSAME( COMPZ, 'N' ) ) THEN
         ILZ = .FALSE.
         ICOMPZ = 1
      ELSE IF( LSAME( COMPZ, 'U' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 2
      ELSE IF( LSAME( COMPZ, 'I' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 3
      ELSE
         ICOMPZ = 0
      END IF
C
C     Test the input scalar parameters.
C
      INFO = 0
      IF( .NOT.FINCON .AND. .NOT.INFCON ) THEN
         INFO = -1
      ELSE IF( ICOMPQ.LE.0 ) THEN
         INFO = -2
      ELSE IF( ICOMPZ.LE.0 ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( P.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDE.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -14
      ELSE IF( ( ILQ .AND. LDQ.LT.N ) .OR. LDQ.LT.1 ) THEN
         INFO = -16
      ELSE IF( ( ILZ .AND. LDZ.LT.N ) .OR. LDZ.LT.1 ) THEN
         INFO = -18
      ELSE IF( TOL.GE.ONE ) THEN
         INFO = -23
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'TG01HD', -INFO )
         RETURN
      END IF
C
      JOBQ = COMPQ
      JOBZ = COMPZ
C
      IF( FINCON ) THEN
C
C        Perform finite controllability form reduction.
C
         CALL TG01HX( JOBQ, JOBZ, N, N, M, P, N, MAX( 0, N-1 ), A, LDA,
     $                E, LDE, B, LDB, C, LDC, Q, LDQ, Z, LDZ, NR,
     $                NRBLCK, RTAU, TOL, IWORK, DWORK, INFO )
         IF( NRBLCK.GT.1 ) THEN
            LBA = RTAU(1) + RTAU(2) - 1
         ELSE IF( NRBLCK.EQ.1 ) THEN
            LBA = RTAU(1) - 1
         ELSE
            LBA = 0
         END IF
         IF( ILQ ) JOBQ = 'U'
         IF( ILZ ) JOBZ = 'U'
      ELSE
         NR = N
         LBA = MAX( 0, N-1 )
      END IF
C
      IF( INFCON ) THEN
C
C        Perform infinite controllability form reduction.
C
         CALL TG01HX( JOBQ, JOBZ, N, N, M, P, NR, LBA, E, LDE,
     $                A, LDA, B, LDB, C, LDC, Q, LDQ, Z, LDZ, NCONT,
     $                NRBLCK, RTAU, TOL, IWORK, DWORK, INFO )
         IF( FINCON ) THEN
            NIUCON = NR - NCONT
         ELSE
            NIUCON = 0
         END IF
      ELSE
         NCONT  = NR
         NIUCON = 0
      END IF
C
      RETURN
C
C *** Last line of TG01HD ***
      END


    !-------------------------------------------------------------------------------------------------------------
    !
    !> \file    CompGibbsMagnetic.f90
    !> \brief   Compute magnetic contributions to the Gibbs energy terms.
    !> \author  M.H.A. Piro
    !> \date    Apr. 24, 2012
    !> \sa      CompThermoData.f90
    !> \param[in] i Gibbs energy equation coefficient
    !> \param[in] j Species index
    !> \param[in,out] dChemicalPotential A double real vector representing the chemical potential.
    !
    !
    !
    ! Revisions:
    ! ==========
    !
    !    Date          Programmer        Description of change
    !    ----          ----------        ---------------------
    !    10/20/2011    M.H.A. Piro       Original code
    !
    !
    ! Purpose:
    ! ========
    !
    !> \details The purpose of this subroutine is to compute the magnetic contribution to the standard molar
    !! Gibbs energy of a pure species.  This contribution is given by
    !! \f$ \Delta g_{mag} = RT ln(B_o + 1) g(\tau ) \f$, where \f$ B_o \f$ is the average magnetic moment per
    !! atom, \f$ \tau \f$  is the absolute temperature divided by the critical temperature (i.e., the Curie
    !! temperature for ferromagnetic materials or the Neel temperature for antiferromagnetic materials) and
    !! \f$ g \f$ is a function of \f$ \tau \f$, given by:
    !! \f$ g(\tau ) = 1 - \left( \frac{79\tau^-1}{140p} + \frac{474}{497}(\frac{1}{p} - 1)(\frac{\tau ^3}{6}
    !!   + \frac{\tau ^9}{135} \frac{\tau ^15}{600}   )  \right) /D, \tau \leq 1 \f$
    !! and \f$ g(\tau ) = - \left( \frac{\tau ^{-5}}{10} + \frac{\tau ^{-15}}{315} + \frac{\tau ^{-25}}{1500}
    !!   \right) /D, \tau > 1 \f$, where \f$ D = \frac{518}{1125} + \frac{11692}{15975} \left( \frac{1}{p} -1
    !!   \right) \f$.
    !!
    !
    ! References:
    ! ===========
    !
    !> \details The following references explain the magnetic contribution to the Gibbs energy term that is
    !! used in this subroutine:
    !!
    !!   A.T. Dinsdale, "SGTE Data for Pure Elements," CALPHAD, 15, 4 (1991) 317-425.
    !!
    !!   H.L. Lukas, S.G. Fries and B. Sundman, "Computational Thermodynamics: The Calphad Method," Cambridge
    !!   University Press, New York (2007).
    !
    ! Pertinent variables:
    ! ====================
    !
    !> \param[in]   i   An integer scalar corresponding to the Gibbs energy index.
    !> \param[in]   j   An integer scalar representing the species index.
    !
    !-------------------------------------------------------------------------------------------------------------

subroutine CompGibbsMagnetic(i,j)

    USE ModuleParseCS
    USE ModuleThermo
    USE ModuleThermoIO

    implicit none

    integer :: i, j
    real(8) :: B, D, p, invpmone, tau, Tcritical, g, StructureFactor

    ! Assign the critical temperature can either be the Curie temperature for ferromagnetic materials
    ! or the Neel temperature for antiferromagnetic materials:
    Tcritical       = dGibbsMagneticCS(i,1)
    B               = dGibbsMagneticCS(i,2)
    StructureFactor = dGibbsMagneticCS(i,3)
    p               = dGibbsMagneticCS(i,4)
    invpmone        = 1D0/p - 1D0

    ! ChemSage files store the critical (i.e., Neel) temperature for antiferromagnetic materials
    ! as a negative real value divided by the structure factor.  Correct Tcritical and B:
    if (Tcritical < 0D0) then
        Tcritical = -Tcritical * StructureFactor
        B         = -B * StructureFactor
    end if

    tau = dTemperature / Tcritical
    D   = 518D0/1125D0 + (11692D0/15975D0) * invpmone

    if (tau > 1D0) then
        g = -((tau**(-5))/10D0 + (tau**(-15))/315D0 + (tau**(-25))/1500D0) / D
    else
        g = 1D0 - (79D0/(140D0*p*tau) + (474D0/497D0)*invpmone*((tau**3)/6D0 + (tau**9)/135D0 + (tau**15)/600D0)) / D
    end if

    ! Add the magnetic contribution to the Gibbs energy term:
    dChemicalPotential(j) = dChemicalPotential(j) + dIdealConstant * dTemperature * DLOG(B + 1D0) * g

    return

end subroutine CompGibbsMagnetic


**************************************************
      SUBROUTINE grid
      implicit none
      include 'usr.com'
      integer i,j,k
*     EXTERNAL
      real  dfdz,fz

      write(99,*) '============GRID==========='
      write(99,10) xmin*180/pi,xmax*180/pi,ymin*180/pi,ymax*180/pi
 10   format(1x,'configuration:[',f6.1,1x,f6.1,'] x [',f6.1,1x,f6.1,']')
      write(99,20) n,m,l
 20   format(1x,'resolution:',i8,'x',i8,'x',i8)
      write(99,30) qz
 30   format(1x,'stretching parameter',g12.4)
      write(99,*) '============GRID==========='

      IF (abs(xmax-xmin-2*pi).LT.1.0e-3) periodic = .true.
      dx = (xmax-xmin)/N
      dy = (ymax-ymin)/M
      dz = (zmax-zmin)/L
      if (la.eq.1) then
         periodic =.true.
      endif
      DO i=1,n
        x(i) = (real(i)-0.5)*dx + xmin
        xu(i)= (real(i)    )*dx + xmin
      ENDDO
      xu(0) = xmin
      DO j=1,m
        y(j) = (real(j)-0.5)*dy + ymin
        yv(j)= (real(j)    )*dy + ymin
      ENDDO
      yv(0) = ymin
      DO k=1,l
        ze(k)  = (real(k)-0.5)*dz + zmin
        zwe(k) = (real(k)    )*dz + zmin
        z(k)   = fz(ze(k) ,qz)
        zw(k)  = fz(zwe(k),qz)
* compute derivatives of mapping at T- points
        dfzT(k) = dfdz(ze(k),qz)
* compute derivatives of mapping at w- points
        dfzW(k) = dfdz(zwe(k),qz)
      ENDDO
      zw(0) = zmin
      dfzw(0) = dfdz(zmin,qz)
      end
**************************************************
      REAL FUNCTION fz(z,qz)
      implicit none
* INPUT/OUTPUT
      real z,qz
* LOCAL
      real th,tth
      th = tanh (qz * (z+1))
      tth = tanh (qz)
      IF (qz.gt.1.0) THEN
      fz = -1 + th/tth
* for testing
      ELSE
*      fz = z
      fz = z + (1.-qz) * z * (1-z)
      ENDIF
      RETURN
      END
************************************************
      REAL FUNCTION dfdz(z,qz)
      implicit none
* INPUT/OUTPUT
      real z,qz
* LOCAL
      real tth,ch
      ch = cosh (qz * (z+1))
      tth = tanh (qz)
      IF (qz.gt.1.0) THEN
      dfdz = qz  / (tth * ch * ch)
      ELSE
*      dfdz = 1.0
      dfdz = 1.0 + (1.-qz)*(1.-2.*z)
      ENDIF
      RETURN
      END


SUBROUTINE CSYTRD_F95( A, TAU, UPLO, INFO )
!
!  -- LAPACK95 interface driver routine (version 3.0) --
!     UNI-C, Denmark; Univ. of Tennessee, USA; NAG Ltd., UK
!     September, 2000
!
!  .. USE STATEMENTS ..
   USE LA_PRECISION, ONLY: WP => SP
   USE LA_AUXMOD, ONLY: ERINFO, LSAME
   USE F77_LAPACK, ONLY: SYTRD_F77 => LA_SYTRD, ILAENV_F77 => LA_ILAENV
!  .. IMPLICIT STATEMENT ..
   IMPLICIT NONE
!  .. SCALAR ARGUMENTS ..
   CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: UPLO
   INTEGER, INTENT(OUT), OPTIONAL :: INFO
!  .. ARRAY ARGUMENTS ..
   COMPLEX(WP), INTENT(INOUT) :: A(:,:)
   COMPLEX(WP), INTENT(OUT) :: TAU(:)
!  .. LOCAL PARAMETERS ..
   CHARACTER(LEN=8), PARAMETER :: SRNAME = 'LA_SYTRD'
   CHARACTER(LEN=6), PARAMETER :: BSNAME = 'CSYTRD'
!-----------------------------------------------------------------
!
! Purpose
! =======
!
! LA_SYTRD / LA_HETRD reduces a real symmetric or complex Hermitian
! matrix A to real symmetric tridiagonal form T by an orthogonal
! or unitary similarity transformation:
! Q**H * A * Q = T.
!
! =======
!
!    SUBROUTINE LA_HETRD / LA_SYTRD|( A, TAU, UPLO, INFO )
!    .. Scalar Arguments ..
!       CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: UPLO
!       INTEGER, INTENT(OUT), OPTIONAL :: INFO
!    .. Array Arguments ..
!       <type>(<wp>), INTENT(INOUT) :: A(:,:)
!       <type>(<wp>), INTENT(OUT) :: TAU(:)
!    where
!       <type> ::= REAL | COMPLEX
!       <wp>   ::= KIND(1.0) | KIND(1.0D0)
!
! Defaults
! ========
!
! 1. If UPLO is not present then UPLO = 'U' is assumed.
!
! Arguments
! =========
!
! A       (input/output) either REAL or COMPLEX square array,
!         shape (:,:), size(A,1) == size(A,2) >= 0.
!         On entry, the symmetric (Hermitian) matrix A.
!            If UPLO = 'U', the upper triangular part of A contains
!               the upper triangular part of the matrix A.
!            If UPLO = 'L', the lower triangular part of A contains
!               the lower triangular part of the matrix A.
!         On exit:
!            If UPLO = 'U', the diagonal and first superdiagonal
!               of A are overwritten by the corresponding elements of the
!               tridiagonal matrix T, and the elements above the first
!               superdiagonal, with the array TAU, represent the unitary
!               matrix Q as a product of elementary reflectors.
!            If UPLO = 'L', the diagonal and first subdiagonal of A are
!               overwritten by the corresponding elements of the tridiagonal
!               matrix T, and the elements below the first subdiagonal, with
!               the array TAU, represent the unitary matrix Q as a product
!               of elementary reflectors.
!            See Further Details.
!
! TAU     (output) either REAL or COMPLEX array,
!         shape (:), size(TAU) == size(A,1)-1.
!         The scalar factors of the elementary reflectors.
!         See Further Details.
!
! UPLO    Optional, (input) CHARACTER*1
!         If UPLO is present then:
!            = 'U':  Upper triangle of A is stored
!            = 'L':  Lower triangle of A is stored
!         otherwise UPLO = 'U' is assumed.
!
! INFO    Optional, (output) INTEGER
!         If INFO is present:
!            = 0: successful exit
!            < 0: if INFO = -i, the i-th argument had an illegal value
!         If INFO is not present and an error occurs, then the program
!            is terminated with an error message.
!
! Further Details
! ===============
!
! If UPLO = 'U', the matrix Q is represented as a product of elementary
! reflectors
!
!    Q = H(n-1) . . . H(2) H(1).
!
! Each H(i) has the form
!
!    H(i) = I - tau * v * v'
!
! where tau is a complex scalar, and v is a complex vector with
! v(i+1:n) = 0 and v(i) = 1; v(1:i-1) is stored on exit in
! A(1:i-1,i+1), and tau in TAU(i).
!
! If UPLO = 'L', the matrix Q is represented as a product of elementary
! reflectors
!
!    Q = H(1) H(2) . . . H(n-1).
!
! Each H(i) has the form
!
!    H(i) = I - tau * v * v'
!
! where tau is a complex scalar, and v is a complex vector with
! v(1:i) = 0 and v(i+1) = 1; v(i+2:n) is stored on exit in A(i+2:n,i),
! and tau in TAU(i).
!
! The contents of A on exit are illustrated by the following examples
! with n = 5:
!
! if UPLO = 'U':                       if UPLO = 'L':
!
!   (  d   e   v2  v3  v4 )              (  d                  )
!   (      d   e   v3  v4 )              (  e   d              )
!   (          d   e   v4 )              (  v1  e   d          )
!   (              d   e  )              (  v1  v2  e   d      )
!   (                  d  )              (  v1  v2  v3  e   d  )
!
! where d and e denote diagonal and off-diagonal elements of T, and vi
! denotes an element of the vector defining H(i).
!
! --------------------------------------
!  .. LOCAL SCALARS ..
   CHARACTER(LEN=1) :: LUPLO
   INTEGER :: LINFO, N, LD, LWORK, NB, ISTAT, ISTAT1
!  .. LOCAL ARRAYS ..
   COMPLEX(WP), POINTER :: WORK(:)
   REAL(WP), POINTER :: D(:), E(:)
!  .. INTRINSIC FUNCTIONS ..
   INTRINSIC SIZE, MAX, PRESENT
!  .. EXECUTABLE STATEMENTS ..
   LINFO = 0; N = SIZE(A,1); LD = MAX(1,N); ISTAT = 0
   IF( PRESENT(UPLO) ) THEN; LUPLO = UPLO; ELSE; LUPLO = 'U'; END IF
!  .. TEST THE ARGUMENTS
   IF( SIZE( A, 2 ) /= N .OR. N < 0 )THEN; LINFO = -1
   ELSE IF( SIZE( TAU ) /= N-1 )THEN; LINFO = -2
   ELSE IF( .NOT.LSAME(LUPLO,'U') .AND. .NOT.LSAME(LUPLO,'L') )THEN; LINFO = -3
   ELSE IF( N > 0 )THEN
!  .. DETERMINE THE WORKSPACE
      NB = ILAENV_F77( 1, BSNAME, LUPLO, N, -1, -1, -1 )
      IF( NB > 1 .AND. NB < N )THEN; LWORK = N*NB; ELSE; LWORK = 1; ENDIF
      ALLOCATE(D(N), E(N-1), WORK(LWORK), STAT=ISTAT)
      IF( ISTAT /= 0 )THEN; DEALLOCATE(D, E, WORK, STAT=ISTAT1)
         LWORK = 1; ALLOCATE(D(N), E(N-1), WORK(LWORK), STAT=ISTAT)
         IF( ISTAT == 0 ) CALL ERINFO( -200, SRNAME, LINFO )
      ENDIF
      IF( ISTAT == 0 )THEN
!        .. CALL LAPACK77 ROUTINE
         CALL SYTRD_F77( LUPLO, N, A, LD, D, E, TAU, WORK, LWORK, LINFO )
      ELSE; LINFO = -100; ENDIF
      DEALLOCATE(D, E, WORK, STAT=ISTAT1)
   ENDIF
   CALL ERINFO(LINFO,SRNAME,INFO,ISTAT)
END SUBROUTINE CSYTRD_F95


c
c     Mach number vs. area for an isentropic flow.  See also the Python
c     version of this problem in the Python demos.
c
      program isentropic
      implicit double precision (a-h,o-z)
      parameter (oneatm = 1.01325d5, NPTS = 200)
      double precision a(NPTS), dmach(NPTS), t(NPTS),
     $     ratio(NPTS)

      call newIdealGasMix('gri30.cti','gri30','')
      temp = 1200.d0
      pres = 10.d0*oneatm
      call setState_TPX_String(temp, pres,'H2:1, N2:0.1')

c     stagnation state properties
      s0 = entropy_mass()
      h0 = enthalpy_mass()
      p0 = pressure()

      dmdot = 1.0d0
      amin = 1.0d14

      do n = 1, NPTS
         p = p0*n/(NPTS+1)
         call setState_SP(s0,p)
         h = enthalpy_mass()
         rho = density()

         v2 = 2.0*(h0 - h)
         v = sqrt(v2)
         area = dmdot/(rho*v)
         if (area .lt. amin) then
            amin = area
         end if

         a(n) = area
         dmach(n) = v/soundspeed()
         t(n) = temperature()
         ratio(n) = p/p0
      end do

      do n = 1, NPTS
         a(n) = a(n)/amin
         write(*,30) a(n), dmach(n), t(n), ratio(n)
 30      format(4e16.5)
      end do
      stop
      end

      double precision function soundspeed()
      implicit double precision (a-h,o-z)
      double precision meanMolarMass
      parameter (R = 8314.3d0)
      gamma = cp_mass()/cv_mass()
      soundspeed = sqrt(gamma * R * temperature()
     $     / meanMolarMass())
      return
      end


c --------------------------------------------------------------
C input
      SUBROUTINE DINDX77(X,Y,NMOS,NYRS,XMSG,IPRNT,WORK,LWORK,ZI,ZNI,IER)

c routine to call indx77x which actually does the calculations
c .   this is just a short calling list
c NOTE: indx77x can also be called directly
c .   DSJ: this is a variant of index routines

C number of "months" (eg: 12, 4, 1)
C number of years of data
C print flag (=0 mean do not print)
C length of work (>= 2*nmos*nyrs + 4*nmos + nyrs)
C error code
      INTEGER NMOS,NYRS,IPRNT,LWORK,IER

C monthly data from station/grid pt 1
C monthly data from station/grid pt 2
C missing code (if any)
C index
C noise index
C work array that will be partitioned
      DOUBLE PRECISION X(NMOS,NYRS),Y(NMOS,NYRS),XMSG,ZI(NMOS,NYRS),
     +                 ZNI(NMOS,NYRS),WORK(LWORK)

C local

C overall st dev of x anomalies
C overall st dev of y anomalies
      DOUBLE PRECISION XASTD,YASTD

      IER = 0
      IF (NMOS.LE.0 .OR. NYRS.LE.0) IER = IER + 1
      IF (LWORK.LT. (2*NMOS*NYRS+4*NMOS+NYRS)) IER = 19

c Check if either input arrays contains all missing values.
      IXMISS = 1
      IYMISS = 1
      DO NYR = 1,NYRS
          DO NMO = 1,NMOS
              IF (X(NMO,NYR).NE.XMSG) THEN
                  IXMISS = 0
              END IF
              IF (Y(NMO,NYR).NE.XMSG) THEN
                  IYMISS = 0
              END IF
          END DO
      END DO
      IF (IXMISS.EQ.1 .OR. IYMISS.EQ.1) THEN
          IER = 2
          RETURN
      END IF

      IF (IER.NE.0) THEN
          DO NYR = 1,NYRS
              DO NMO = 1,NMOS
                  ZI(NMO,NYR) = XMSG
                  ZNI(NMO,NYR) = XMSG
              END DO
          END DO

          RETURN
      END IF

C set up pointers (addresses in work array)

C beginning of "xa"   (see indx77x)
      I1 = 1
C beginning of "ya"   (see indx77x)
      I2 = I1 + NMOS*NYRS
C beginning of "xave" (see indx77x)
      I3 = I2 + NMOS*NYRS
C beginning of "yave" (see indx77x)
      I4 = I3 + NMOS
C beginning of "xstd" (see indx77x)
      I5 = I4 + NMOS
C beginning of "ystd" (see indx77x)
      I6 = I5 + NMOS
C beginning of "wyrs" (see indx77x)
      I7 = I6 + NMOS

C input
C input
C input
C output
      CALL DINDX77X(X,Y,NMOS,NYRS,XMSG,IPRNT,WORK(I1),WORK(I2),WORK(I3),
     +              WORK(I4),WORK(I5),WORK(I6),WORK(I7),XASTD,YASTD,ZI,
     +              ZNI,IER)

      RETURN
      END
c -----------------------------------------------------------
C input
C input/output
C output
      SUBROUTINE DINDX77X(X,Y,NMOS,NYRS,XMSG,IPRNT,XA,YA,XAVE,YAVE,XSTD,
     +                    YSTD,WYRS,XASTD,YASTD,ZI,ZNI,IER)
      DOUBLE PRECISION XVAR
      DOUBLE PRECISION YVAR
      DOUBLE PRECISION XABAR
      DOUBLE PRECISION XAVAR
      DOUBLE PRECISION YABAR
      DOUBLE PRECISION YAVAR

c This is the most commonly used routine.

c given two series of year-month values (eg: slp) calculate
c .  an "index" (eg: Southern Oscillation Index)
c .  the way Kevin Trenberth suggests doing it.
c .  This means using the overall anomaly standard deviation
c .  to normalize the anomalies.

c index is computed as zi = x/sigma(x) - y/sigma(y) (normalized stuff)

c Trenberth (1984), "Signal versus Noise in the Southern Oscillation"
c .  Monthly Weather Review 112:326-332

c code written for clarity

C number of "months" (eg: 12, 4, 1)
C number of years of data
C print flag (=0 means do not print)
C error code
      INTEGER NMOS,NYRS,IPRNT,IER

C monthly data from station/grid pt 1
C monthly data from station/grid pt 2
C missing code (if any)
C index
C noise index
      DOUBLE PRECISION X(NMOS,NYRS),Y(NMOS,NYRS),XMSG,ZI(NMOS,NYRS),
     +                 ZNI(NMOS,NYRS)

C upon exit the following will contain

C monthly anomalies from average
C monthly averages
C monthly st dev (interannual variability)
C work array
C overall st dev of xa anomalies
C overall st dev of ya anomalies
      DOUBLE PRECISION XA(NMOS,NYRS),YA(NMOS,NYRS),XAVE(NMOS),
     +                 YAVE(NMOS),XSTD(NMOS),YSTD(NMOS),WYRS(NYRS),
     +                 XASTD,YASTD

      IER = 0
      IF (NMOS.LE.0 .OR. NYRS.LE.0) IER = IER + 1
      IF (IER.NE.0) RETURN

c initilize index to msg values

      DO NYR = 1,NYRS
          DO NMO = 1,NMOS
              ZI(NMO,NYR) = XMSG
              ZNI(NMO,NYR) = XMSG
          END DO
      END DO

c calculate each months long-term mean and standard deviation
c .   (long-term <==> climatological)

      DO NMO = 1,NMOS
          DO NYR = 1,NYRS
              WYRS(NYR) = X(NMO,NYR)
          END DO
          CALL DSTAT2(WYRS,NYRS,XMSG,XAVE(NMO),XVAR,XSTD(NMO),KNTX,IER)

          DO NYR = 1,NYRS
              WYRS(NYR) = Y(NMO,NYR)
          END DO
          CALL DSTAT2(WYRS,NYRS,XMSG,YAVE(NMO),YVAR,YSTD(NMO),KNTY,IER)
      END DO

c calculate anomalies from climatogical mean

      DO NYR = 1,NYRS
          DO NMO = 1,NMOS
              IF (X(NMO,NYR).NE.XMSG .AND. XAVE(NMO).NE.XMSG) THEN
                  XA(NMO,NYR) = X(NMO,NYR) - XAVE(NMO)
              ELSE
                  XA(NMO,NYR) = XMSG
              END IF
              IF (Y(NMO,NYR).NE.XMSG .AND. YAVE(NMO).NE.XMSG) THEN
                  YA(NMO,NYR) = Y(NMO,NYR) - YAVE(NMO)
              ELSE
                  YA(NMO,NYR) = XMSG
              END IF
          END DO
      END DO

c calculate the overall standard deviation of the anomalies
c .   (ie: use anomalies from all year-months together)

      CALL DSTAT2(XA,NMOS*NYRS,XMSG,XABAR,XAVAR,XASTD,KNTXA,IER)
      CALL DSTAT2(YA,NMOS*NYRS,XMSG,YABAR,YAVAR,YASTD,KNTYA,IER)

c calculate each index

      IF (XASTD.NE.XMSG .AND. XASTD.GT.0.0D0 .AND. YASTD.NE.XMSG .AND.
     +    YASTD.GT.0.0D0) THEN
          DO NYR = 1,NYRS
              DO NMO = 1,NMOS
                  IF (XA(NMO,NYR).NE.XMSG .AND.
     +                YA(NMO,NYR).NE.XMSG) THEN
                      ZI(NMO,NYR) = XA(NMO,NYR)/XASTD -
     +                              YA(NMO,NYR)/YASTD
                      ZNI(NMO,NYR) = XA(NMO,NYR)/XASTD +
     +                               YA(NMO,NYR)/YASTD
                  END IF
              END DO
          END DO
      ELSE
          IER = IER + 10
      END IF

      IF (IPRNT.NE.0) THEN
          DO NYR = 1,NYRS
              WRITE (*,FMT='('' SINDEX: '',i4,12(1x,f7.1))') NYR,
     +          (ZI(NMO,NYR),NMO=1,NMOS)
          END DO
          IF (IPRNT.LT.0) THEN
              DO NYR = 1,NYRS
                  WRITE (*,FMT='('' NINDEX: '',i4,12(1x,f7.1))') NYR,
     +              (ZNI(NMO,NYR),NMO=1,NMOS)
              END DO
          END IF
      END IF

      RETURN
      END


program BVESingleGaussianVortex

use NumberKindsModule
use OutputWriterModule
use LoggerModule
use ParticlesModule
use EdgesModule
use FacesModule
use PolyMesh2dModule
use FieldModule
use MPISetupModule
use SphereGeomModule
use SphereBVEModule
use SphereBVESolverModule
use RefinementModule

implicit none

include 'mpif.h'

! mesh variables
type(BVEMesh) :: sphere
type(RefineSetup) :: refinement
integer(kint) :: initNest
integer(kint) :: maxNest
integer(kint) :: faceKind
integer(kint) :: meshSeed
integer(kint) :: amrLimit
real(kreal), parameter :: radius = 1.0_kreal
logical(klog) :: continueAMR
logical(klog) :: AMR
integer(kint) :: nParticlesBefore
integer(kint) :: nParticlesAfter
namelist /meshDefine/ faceKind, initNest, amrLimit

! test case variables
real(kreal), allocatable, dimension(:) :: kineticEnergy, enstrophy
real(kreal), save :: GAUSS_CONST = 0.0_kreal
real(kreal) :: shapeParam
real(kreal) :: rotRate
real(kreal) :: vortStrength
real(kreal) :: vortInitLat
real(kreal) :: vortInitLon
real(kreal), dimension(3) :: vortCenter
real(kreal) :: flowMapVarTol
real(kreal) :: circulationTol
namelist /gaussVort/ shapeParam, rotRate, vortStrength, vortInitLat, vortInitLon, flowMapVarTol, circulationTol

! timestepping
type(BVESolver) :: solver
real(kreal) :: dt
real(kreal) :: t
real(kreal) :: tfinal
integer(kint) :: nTimesteps
integer(kint) :: timeJ
namelist /timestepping/ dt, tfinal

! i/o
character(len=MAX_STRING_LENGTH) :: outputDir
character(len=MAX_STRING_LENGTH) :: outputRoot
character(len=MAX_STRING_LENGTH) :: vtkFile
character(len=MAX_STRING_LENGTH) :: matlabFile
character(len=MAX_STRING_LENGTH) :: vtkRoot
character(len=MAX_STRING_LENGTH) :: meshString
integer(kint) :: frameOut
integer(kint) :: frameCounter
namelist /fileIO/ outputDir, outputRoot, frameOut

! computing environment / general
type(Logger) :: exeLog
character(len=MAX_STRING_LENGTH) :: logString
integer(kint), parameter :: logLevel = DEBUG_LOGGING_LEVEL
character(len=28) :: logKey = "BVEGaussianVortex"
integer(kint) :: mpiErrCode
real(kreal) :: timeStart, timeEnd
integer(kint) :: i
real(kreal), dimension(3) :: vec3

!--------------------------------
!	initialize : setup computing environment
!--------------------------------

call MPI_INIT(mpiErrCode)
call MPI_COMM_SIZE(MPI_COMM_WORLD, numProcs, mpiErrCode)
call MPI_COMM_RANK(MPI_COMM_WORLD, procRank, mpiErrCode)

call InitLogger(exeLog, procRank)

timeStart = MPI_WTIME()

call ReadNamelistFile( procRank )

!
!	initialize mesh and spatial fields
!
t = 0.0_kreal
call New( sphere, meshSeed, initNest, maxNest, amrLimit, radius, rotRate )

vortCenter = radius * [ cos(vortInitLon) * cos(vortInitLat), sin(vortInitLon) * cos(vortInitLat), sin(vortInitLat)]
call SetInitialVorticityOnMesh( sphere, GaussianVortexVorticity)
GAUSS_CONST = SetGaussConst(sphere)
call LogMessage(exeLog, TRACE_LOGGING_LEVEL, trim(logKey)//" GAUSS_CONST = ", GAUSS_CONST)
call SetInitialVorticityOnMesh( sphere, GaussianVortexVorticity)
call LogMessage(exeLog, DEBUG_LOGGING_LEVEL, trim(logKey)//" ", "uniform mesh ready...")

AMR = (maxNest > initNest)
if ( AMR ) then
	call LogMessage(exeLog, DEBUG_LOGGING_LEVEL, trim(logKey)//" ", "starting adaptive refinement...")
	call New(refinement, sphere%mesh%faces%N_Max)
	call SetAbsoluteTolerances( sphere, circulationTol, flowMapVarTol)
	call LogMessage(exeLog, DEBUG_LOGGING_LEVEL, trim(logKey)//" ", "AMR ready.")

	do i = 1, amrLimit
		write(logString,"(A,I2)") 'amr loop ', i
		call StartSection(exeLog, trim(logString) )
		call IterateMeshRefinementOneVariableAndFlowMap( refinement, sphere%mesh, sphere%relVort, ScalarIntegralRefinement, &
			circulationTol, "circulation refinement", flowMapVarTol, nParticlesBefore, nParticlesAfter)

		call SetInitialVorticityOnMesh(sphere, GaussianVortexVorticity)
		!GAUSS_CONST = SetGaussConst(sphere)
		!call LogMessage(exeLog, TRACE_LOGGING_LEVEL, trim(logKey)//" GAUSS_CONST = ", GAUSS_CONST)
		call SetInitialVorticityOnMesh(sphere, GaussianVortexVorticity)
		call EndSection(exeLog)
	enddo

	call LoadBalance(sphere%mpiParticles, sphere%mesh%particles%N, numProcs)
endif

call SetStreamFunctionsOnMesh(sphere)
call SetVelocityOnMesh(sphere)

call AddTracers(sphere, 1, [1])
do i = 1, sphere%mesh%particles%N
	vec3 = PhysCoord(sphere%mesh%particles, i)
	call InsertScalarToField(sphere%tracers(1), Latitude(vec3))
enddo

frameCounter = 0
if ( procRank == 0 ) then
	call LogStats(sphere, exeLog)
	if (meshSeed == ICOS_TRI_SPHERE_SEED) then
		if ( initNest == maxNest) then
			write(meshString, '(A,I1,A)') '_icosTri', initNest, '_'
		else
			write(meshString, '(2(A,I1),A)') '_icosTriAMR', initNest, 'to', maxNest, '_'
		endif
	elseif (meshSeed == CUBED_SPHERE_SEED ) then
		if ( initNest == maxNest ) then
			write(meshString, '(A,I1,A)') '_cubedSphere', initNest, '_'
		else
			write(meshString, '(2(A,I1),A)') '_cubedSphereAMR', initNest, 'to', maxNest, '_'
		endif
	endif

	write(vtkRoot,'(4A)') trim(outputDir), '/vtkOut/', trim(outputRoot), trim(meshString)
	write(vtkFile,'(A,I0.4,A)') trim(vtkRoot), frameCounter, '.vtk'

	call OutputToVTK(sphere, vtkFile)
	frameCounter = frameCounter + 1

	call LogMessage(exeLog, TRACE_LOGGING_LEVEL, trim(logkey)//" t = ", t)
endif

!
!	initialize time stepping
!
call New(solver, sphere)
nTimesteps = floor(tfinal / dt)
allocate(kineticEnergy(nTimesteps+1))
allocate(enstrophy(nTimesteps+1))
kineticEnergy(1) = TotalKE(sphere)
enstrophy(1) = TotalEnstrophy(sphere)
!--------------------------------
!	run : evolve the problem in time (currently no remeshing)
!--------------------------------
do timeJ = 0, nTimesteps - 1

	call Timestep(solver, sphere, dt)

	t = real(timeJ +1, kreal) * dt
	sphere%mesh%t = t

	enstrophy(timeJ+2) = TotalEnstrophy(sphere)
	kineticEnergy(timeJ+2) = TotalKE(sphere)

	if ( procRank == 0 .AND. mod(timeJ+1, frameOut) == 0 ) then
		write(vtkFile,'(A,I0.4,A)') trim(vtkRoot), frameCounter, '.vtk'
		call OutputToVTK(sphere, vtkFile)
		frameCounter = frameCounter + 1
		call LogMessage(exelog, TRACE_LOGGING_LEVEL, trim(logKey)//" t = ", t)
	endif
enddo

!
!	write t = tfinal output
!
if ( procRank == 0 ) then
	write(matlabFile, '(5A)') trim(outputDir), '/', trim(outputRoot), trim(meshString), '.m'
	open(unit=WRITE_UNIT_1, file=matlabFile, status='REPLACE', action='WRITE')
		write(WRITE_UNIT_1,'(A,F12.9,A,F12.6,A)') "t = 0:", dt,":", tfinal, ";"
		call WriteToMatlab(kineticEnergy, WRITE_UNIT_1, "kineticEnergy")
		call WriteToMatlab(enstrophy, WRITE_UNIT_1, "enstrophy")
	close(WRITE_UNIT_1)
endif

!--------------------------------
!	finalize : clean up
!--------------------------------

timeEnd = MPI_WTIME()
write(logstring,'(A,F12.2,A)') "PROGRAM COMPLETE : elapsed time ", timeEnd - timeStart, " seconds."
call LogMessage(exeLog, TRACE_LOGGING_LEVEL, trim(logKey)//" ", logstring)

deallocate(kineticEnergy)
deallocate(enstrophy)
if ( AMR ) call Delete(refinement)
call Delete(solver)
call Delete(sphere)

call MPI_FINALIZE(mpiErrCode)

contains

subroutine SetAbsoluteTolerances( aBVEMesh, circTol, lagVarTol )
	type(BVEMesh), intent(in) :: aBVEMesh
	real(kreal), intent(inout) :: circTol
	real(kreal), intent(inout) :: lagVarTol
	!
	circTol = circTol * MaxCirculationMagnitudePerFace(aBVEMesh)
	lagVarTol = lagVarTol * MaxLagrangianVariationPerFace(aBVEMesh%mesh)
end subroutine

function GaussianVortexVorticity( x, y, z )
	real(kreal) :: GaussianVortexVorticity
	real(kreal), intent(in) :: x, y, z
	GaussianVortexVorticity = vortStrength * exp( - shapeParam * shapeParam * &
		(radius * radius - x * vortCenter(1) - y * vortCenter(2) - z * vortCenter(3)) ) - GAUSS_CONST
end function

function SetGaussConst( aBVEMesh )
	real(kreal) :: SetGaussConst
	type(BVEMesh), intent(in) :: aBVEMesh
	SetGaussConst = sum( aBVEMesh%relVort%scalar(1:aBVEMesh%mesh%particles%N) * &
						 aBVEMesh%mesh%particles%area(1:aBVEMesh%mesh%particles%N), &
						 MASK=aBVEMesh%mesh%particles%isActive(1:aBVEMesh%mesh%particles%N) )
end function

subroutine ReadNamelistFile( rank )
	integer(kint), intent(in) :: rank
	!
	character(len=MAX_STRING_LENGTH) :: namelistFilename
	integer(kint), parameter :: initBcast_intSize = 5
	integer(kint), parameter :: initBcast_realSize = 9
	integer(kint), dimension(initBcast_intSize) :: bcastIntegers
	real(kreal), dimension(initBcast_realSize) :: bcastReals
	integer(kint) :: mpiErrCode, readStat

	if ( COMMAND_ARGUMENT_COUNT() /= 1 ) then
		call LogMessage(exeLog, ERROR_LOGGING_LEVEL, trim(logKey), " ERROR: expected namelist file as 1st argument.")
		stop
	endif

	if ( rank == 0 ) then
		call GET_COMMAND_ARGUMENT(1, namelistFilename)

		open(unit=READ_UNIT, file=namelistFilename, status='OLD', action='READ', iostat=readStat)
			if ( readStat /= 0 ) then
				call LogMessage(exeLog, ERROR_LOGGING_LEVEL, trim(logKey), " ERROR: cannot read namelist file.")
				stop
			endif

			read(READ_UNIT, nml=meshDefine)
			rewind(READ_UNIT)
			read(READ_UNIT, nml=gaussVort)
			rewind(READ_UNIT)
			read(READ_UNIT, nml=timestepping)
			rewind(READ_UNIT)
			read(READ_UNIT, nml=fileIO)
		close(READ_UNIT)

		if ( faceKind == 3 ) then
			meshSeed = ICOS_TRI_SPHERE_SEED
		elseif ( faceKind == 4) then
			meshSeed = CUBED_SPHERE_SEED
		else
			call LogMessage(exeLog, WARNING_LOGGING_LEVEL, trim(logkey)//" ReadNamelistFile WARNING : ", &
				" invalid faceKind -- using triangles.")
			meshSeed = ICOS_TRI_SPHERE_SEED
		endif

		maxNest = initNest + amrLimit

!		namelist /meshDefine/ faceKind, initNest, maxNest, amrLimit, radius
!		namelist /timestepping/ dt, tfinal
!		namelist /fileIO/ outputDir, outputRoot, frameOut

		bcastIntegers(1) = meshSeed
		bcastIntegers(2) = initNest
		bcastIntegers(3) = maxNest
		bcastIntegers(4) = amrLimit
		bcastIntegers(5) = frameOut

		bcastReals(1) = dt
		bcastReals(2) = tfinal
		bcastReals(3) = shapeParam
		bcastReals(4) = rotRate
		bcastReals(5) = vortStrength
		bcastReals(6) = vortInitLon
		bcastReals(7) = vortInitLat
		bcastReals(8) = circulationTol
		bcastReals(9) = flowMapVarTol
	endif

	call MPI_BCAST(bcastIntegers, initBCAST_intSize, MPI_INTEGER, 0, MPI_COMM_WORLD, mpiErrCode)
	if ( mpiErrCode /= 0 ) then
		call LogMessage(exeLog, ERROR_LOGGING_LEVEL, trim(logKey)//" bcastIntegers, MPI_BCAST ERROR : ", mpiErrCode)
	endif

	call MPI_BCAST(bcastReals, initBCAST_realSize, MPI_DOUBLE_PRECISION, 0, MPI_COMM_WORLD, mpiErrCode)
	if ( mpiErrCode /= 0 ) then
		call LogMessage(exeLog, ERROR_LOGGING_LEVEL, trim(logKey)//" bcastReals, MPI_BCAST ERROR : ", mpiErrCode)
	endif

	meshSeed = bcastIntegers(1)
	initNest = bcastIntegers(2)
	maxNest = bcastIntegers(3)
	amrLimit = bcastIntegers(4)
	frameOut = bcastIntegers(5)

	dt = bcastReals(1)
	tfinal = bcastReals(2)
	shapeParam = bcastReals(3)
	rotRate = bcastReals(4)
	vortStrength = bcastReals(5)
	vortInitLon = bcastReals(6)
	vortInitLat = bcastReals(7)
	circulationTol = bcastReals(8)
	flowMapVarTol = bcastReals(9)
end subroutine

subroutine InitLogger(log, rank)
	type(Logger), intent(inout) :: log
	integer(kint), intent(in) :: rank
	if ( rank == 0 ) then
		call New(log, logLevel)
	else
		call New(log, ERROR_LOGGING_LEVEL)
	endif
	write(logKey,'(A,I0.2,A)') trim(logKey)//"_", rank, ":"
end subroutine

end program