module circular_mesh
implicit none
private
double precision, parameter :: pi = 3.14159265358979d0
public :: calc_mesh, create_points, calc_points_circular, calc_n_tetras, calc_n_verts
integer :: n_field_periods

contains

subroutine create_points(verts_per_ring, n_slices, points_rphiz, points_sthetaphi, efit_vmec,n_field_periods_in,n_verts, &
                         verts_theta_vmec,r_scaling_func, theta_scaling_func,repeat_center_point)
    use points_2d, only: create_points_2d, create_points_2d_vmec, scaling_func

    integer, intent(in) :: n_slices,efit_vmec,n_field_periods_in
    integer, dimension(:), intent(in) :: verts_per_ring ! without venter vert; e.g. (/6, 8, 10/)
    double precision, dimension(:, :), allocatable, intent(out) :: points_rphiz !(r, phi, z)
    double precision, dimension(:, :), allocatable, intent(out) :: points_sthetaphi !(s, theta, phi)
    double precision, dimension(:), allocatable, intent(out),optional :: verts_theta_vmec !(theta in VMEC coordinates)
    integer, intent(out) :: n_verts ! without venter vert; e.g. (/6, 8, 10/)
    procedure(scaling_func), optional :: r_scaling_func, theta_scaling_func
    logical, optional, intent(in) :: repeat_center_point ! set true if we are insym flux coords

    integer :: verts_per_slice,phi_position, i
    double precision :: s,theta,vartheta,varphi,A_phi,A_theta,dA_phi_ds,dA_theta_ds,aiota,       &
                        R,Z,alam,dR_ds,dR_dt,dR_dp,dZ_ds,dZ_dt,dZ_dp,dl_ds,dl_dt,dl_dp
!
    !Write field periodicity in module
    n_field_periods = n_field_periods_in
!
    n_verts = calc_n_verts(verts_per_ring, n_slices, repeat_center_point)
    verts_per_slice = n_verts / n_slices

    if (allocated(points_rphiz)) deallocate(points_rphiz)
    if (allocated(points_sthetaphi)) deallocate(points_sthetaphi)
!
    if (present(verts_theta_vmec)) then
        if (allocated(verts_theta_vmec)) deallocate(verts_theta_vmec)
    endif

    allocate(points_rphiz(3, n_verts), points_sthetaphi(3, n_verts))

    !Distinguish inbetween axisymmetric (EFIT) and non-axisymmetric (VMEC) data
!
    select case(efit_vmec)
      case(1) !EFIT (axisymmetric)
!
!        call create_points_2d(verts_per_ring, points_rphiz(:, :verts_per_slice), &
        call create_points_2d(1,verts_per_ring, points_rphiz(:, :verts_per_slice), &
                               & points_sthetaphi(:, :verts_per_slice), &
                               & r_scaling_func, theta_scaling_func, repeat_center_point)
        phi_position = 2 !In coordinate system (R,Phi,Z) --> Phi is at second position
        call extrude_points(verts_per_slice, n_slices,phi_position, points_rphiz)
!
        phi_position = 3 !In coordinate system (s,theta,Phi) --> Phi is at third position
        call extrude_points(verts_per_slice, n_slices,phi_position, points_sthetaphi)
!
      case(2) !VMEC (non-axisymmetric)
!
        allocate(verts_theta_vmec(n_verts))
!
        call create_points_2d_vmec(verts_per_ring,&
                                &  points_sthetaphi(:, :verts_per_slice), &
                               & r_scaling_func, theta_scaling_func,repeat_center_point)
!
        !For the VMEC field theta is not vartheta !! vartheta = theta + alam
        !But VMEC-theta must be kept in order to compute respective R and Z values
        !VMEC-theta is stored in separate vector verts_theta_vmec
        !Theta value in points_sthetaphi IS SFC theta
!
        phi_position = 3 !In coordinate system (s,theta,Phi) --> Phi is at third position
        call extrude_points(verts_per_slice, n_slices,phi_position, points_sthetaphi)
!
        do i = 1,n_verts
!
            s = points_sthetaphi(1,i)
            theta = points_sthetaphi(2,i)
            varphi = points_sthetaphi(3,i)
!
            !Find corresponding VMEC-theta for SFC-theta
            verts_theta_vmec(i) = theta_sym_flux2theta_vmec(s,theta,varphi)
!
            !Read VMEC Spline for Cylindrical variables
            call splint_vmec_data(s,verts_theta_vmec(i),varphi,A_phi,A_theta,dA_phi_ds,dA_theta_ds,aiota,       &
                                  R,Z,alam,dR_ds,dR_dt,dR_dp,dZ_ds,dZ_dt,dZ_dp,dl_ds,dl_dt,dl_dp)
!
            points_rphiz(1,i) = R
            points_rphiz(2,i) = varphi
            points_rphiz(3,i) = Z
!
        enddo
!
    end select

!    open(123, file='points_rphiz.dat')
!    do i=1, verts_per_slice
!        write(123,101) points_rphiz(:, i)
!    end do
!    close(123)
!
!    101 format(1000(e21.14,x))

end subroutine create_points

subroutine calc_points_circular(verts_per_ring, n_slices, R0, Rmax, Z0,  points, repeat_center_point)

    double precision, intent(in) :: R0, Z0, Rmax
    integer, intent(in) :: n_slices
    integer, dimension(:), intent(in) :: verts_per_ring ! without venter vert; e.g. (/6, 8, 10/)
    double precision, dimension(:, :), intent(inout) :: points !(r, phi, z)
    logical, optional, intent(in) :: repeat_center_point ! set true if we are insym flux coords

    integer :: vert, ring, n_rings, n_verts, verts_per_slice, vert_idx, phi_position, n_verts_current
    double precision :: r, theta

    integer :: n_center_point

    n_center_point = 1
    if (present(repeat_center_point)) then
        if (repeat_center_point .eqv. .true.) then
            n_center_point = verts_per_ring(1)
        end if
    end if

    n_rings = size(verts_per_ring)
    n_verts = calc_n_verts(verts_per_ring, n_slices, repeat_center_point)
    verts_per_slice = n_verts / n_slices

    vert_idx = 1
    do ring = 1, n_rings
        if (ring == 0) then
            n_verts_current = n_center_point
        else
            n_verts_current = verts_per_ring(ring)
        end if
        r = (Rmax * ring) / n_rings
        do vert = 1, n_verts_current
            theta = (2.d0 * pi * (vert - 1)) / n_verts_current
            points(:, vert_idx) = r*(/cos(theta), 0.d0, sin(theta)/)
            vert_idx = vert_idx + 1
        end do
    end do

    points(1, :verts_per_slice) = points(1, :verts_per_slice) + R0
    points(3, :verts_per_slice) = points(3, :verts_per_slice) + Z0

    phi_position = 2 !In coordinate system (R,Phi,Z) --> Phi is at second position
    call extrude_points(verts_per_slice, n_slices,phi_position, points)

end subroutine calc_points_circular

subroutine extrude_points(verts_per_slice, n_slices,phi_position,  points)
    integer, intent(in) :: verts_per_slice, n_slices,phi_position
    double precision, dimension(:, :), intent(inout) :: points !(r, phi, z)

    integer :: slice, vert_idx
    double precision :: phi
    ! copy and rotate slice
    do slice = 2, n_slices
        vert_idx = (slice- 1) * verts_per_slice + 1
        phi = (2.d0 * pi / n_field_periods * (slice - 1)) / n_slices
        points(:, vert_idx:vert_idx + verts_per_slice - 1) = points(:, 1:verts_per_slice)
        points(phi_position, vert_idx:vert_idx + verts_per_slice - 1) = phi
    end do
end subroutine

function theta_vmec2theta_sym_flux(s,theta_vmec,varphi) result(theta_sym_flux)
!
        implicit none
!
        double precision :: s,theta_vmec,theta_sym_flux,varphi,A_phi,A_theta,dA_phi_ds,dA_theta_ds,aiota,       &
                            R,Z,alam,dR_ds,dR_dt,dR_dp,dZ_ds,dZ_dt,dZ_dp,dl_ds,dl_dt,dl_dp
!
        call splint_vmec_data(s,theta_vmec,varphi,A_phi,A_theta,dA_phi_ds,dA_theta_ds,aiota,       &
                                R,Z,alam,dR_ds,dR_dt,dR_dp,dZ_ds,dZ_dt,dZ_dp,dl_ds,dl_dt,dl_dp)
!
        theta_sym_flux = theta_vmec + alam
!
end function theta_vmec2theta_sym_flux
!
function theta_sym_flux2theta_vmec(s,theta_sym_flux,varphi) result(theta_vmec)
!
  implicit none
!
  double precision :: s,varphi,theta_sym_flux
  double precision :: theta_vmec
  double precision :: dl_dt,alam,deltheta
  integer :: iter
  double precision, parameter :: epserr = 1.d-14
!
!
  ! Begin Newton iteration to find VMEC theta
!
  theta_vmec = theta_sym_flux
!
  do iter=1,100
!
    call splint_lambda(s,theta_vmec,varphi,alam,dl_dt)
!
    deltheta = (theta_sym_flux-theta_vmec-alam)/(1.d0+dl_dt)
    theta_vmec = theta_vmec + deltheta
    if(abs(deltheta).lt.epserr) exit
  enddo
!
end function theta_sym_flux2theta_vmec
!
subroutine calc_mesh(verts_per_ring, n_slices, points, n_tetras, &
                     verts, neighbours, neighbour_faces, perbou_phi, perbou_theta, repeat_center_point)
    integer, intent(in) :: n_slices
    integer, dimension(:), intent(in) :: verts_per_ring! without venter vert; e.g. (/6, 8, 10/)
    double precision, dimension(:, :), intent(in) :: points !points in first (r, phi, z)
    integer, intent(out) :: n_tetras
    integer, dimension(:, :), allocatable, intent(out) :: verts, neighbours, neighbour_faces, perbou_phi, perbou_theta
    logical, optional, intent(in) :: repeat_center_point ! set true if we are insym flux coords

    integer, dimension(4, 6) :: tetra_conf, mask_theta, mask_phi, mask_r
    integer, dimension(4, 3) :: slice_offset, ring_offset, segment_offset
    integer, dimension(size(verts_per_ring)) :: prisms_per_ring
    integer, allocatable, dimension(:) :: top_facing_prisms

    integer :: n_rings, n_verts, verts_per_slice, tetras_per_slice, &
               mask_idx, n_verts_lower, n_verts_upper, prism_orientation, &
               slice, ring, segment, base_idx, prism_idx, tetra_idx, lower_off, upper_off, i, f

    double precision, dimension(2) :: u, v, p, q
    integer :: n_center_point

    n_center_point = 1
    if (present(repeat_center_point)) then
        if (repeat_center_point .eqv. .true.) then
            n_center_point = verts_per_ring(1)
        end if
    end if

    n_rings = size(verts_per_ring)

    if (any(verts_per_ring < 3) .or. n_rings < 1 .or. n_slices < 3) then
        print *, "Invalid parameters for function calc_mesh"
        stop
    end if

    verts_per_slice = sum(verts_per_ring) + n_center_point ! verts per slice
    n_verts = verts_per_slice * n_slices

    n_tetras = calc_n_tetras(verts_per_ring, n_slices, repeat_center_point)
    if (.not. size(verts, dim=2) == n_tetras .and. size(neighbours, dim=2) == n_tetras &
            .and. size(neighbour_faces, dim=2) == n_tetras) then
        print *, "Invalid size of output arrays for function calc_mesh"
        stop
    end if

    tetras_per_slice = n_tetras / n_slices

    prisms_per_ring = verts_per_ring
    prisms_per_ring(2:) = prisms_per_ring(2:) + verts_per_ring(:n_rings - 1)
    if (n_center_point /= 1) then
        prisms_per_ring(1) = prisms_per_ring(1) * 2
    end if

    if (allocated(verts)) deallocate(verts)
    if (allocated(neighbours)) deallocate(neighbours)
    if (allocated(neighbour_faces)) deallocate(neighbour_faces)
    if (allocated(perbou_phi)) deallocate(perbou_phi)
    if (allocated(perbou_theta)) deallocate(perbou_theta)
    allocate(verts(4, n_tetras), neighbours(4, n_tetras), neighbour_faces(4, n_tetras), &
             perbou_phi(4, n_tetras), perbou_theta(4, n_tetras))
    allocate(top_facing_prisms(verts_per_slice - 1))

    !
    !     7------6
    !    /|     /|
    !   3------2 |
    !   | 5----|-4           r
    !   |/     |/            ^ ,phi
    !   1------0 *           |/
    !               theta <--*

    ! top facing prism
    ! top edge from 2 - 7
    tetra_conf(:, :3) = reshape((/ &
        0, 2, 3, 7,  & !1
        0, 2, 6, 7,  & !1
        0, 4, 6, 7  & !1
        /), (/4, 3/))

    ! calculate the bottom facing prism tetraeder configurations from the top facing one one
    ! bottom facing prism
    tetra_conf(:, 4:) = tetra_conf(:, :3)
    where (iand(tetra_conf(:, 4:) , 2) > 0)
        tetra_conf(:, 4:) = ieor(tetra_conf(:, 4:) , 1)
    end where
    where (iand(tetra_conf(:, 4:) , 1) > 0)
        tetra_conf(:, 4:) = ieor(tetra_conf(:, 4:) , 2)
    end where

    do i = 1, 6
        print *, tetra_conf(:, i)
        if (mod(i, 3) == 0) print *, ''
    end do

    ! extract masks from tetra_conf
    mask_theta = iand(tetra_conf, 1)
    mask_r = ishft(iand(tetra_conf, 2), -1)
    mask_phi = ishft(iand(tetra_conf, 4), -2)

    ! initialize neighbour_faces
    neighbour_faces = - 1

    ! initialize periodic boundary for theta
    perbou_theta = 0

    ! first slice
    prism_idx = 1
    tetra_idx = 1
    base_idx = 1
    do ring = 1, n_rings
        upper_off = 0
        lower_off = 0
        n_verts_upper = verts_per_ring(ring)

        if (ring == 1) then
            n_verts_lower = n_center_point
        else
            n_verts_lower = verts_per_ring(ring - 1)
        end if

        do segment = 1, prisms_per_ring(ring)

            u = points((/1, 3/), base_idx + modulo(lower_off, n_verts_lower))
            v = points((/1, 3/), base_idx + n_verts_lower + modulo(upper_off, n_verts_upper))
            p = points((/1, 3/), base_idx + n_verts_lower + modulo(upper_off + 1, n_verts_upper))
            q = points((/1, 3/), base_idx + modulo(lower_off + 1, n_verts_lower))

            ! if ((q(1) - v(1))*(p(2) - v(2)) - (q(2) - v(2))*(p(1) - v(1)) > 0.d0) then
            !     print *, "ERROR: points in outer ring are too far apart or meshed in wrong coordinate system"
            !     stop
            ! end if

            ! --- calculate prism orientation ---
            if (verts_per_ring(ring) == n_verts_lower) then
                prism_orientation = mod((segment - 1), 2)
            else if (lower_off > n_verts_lower) then
                prism_orientation = 0
            else if (upper_off > n_verts_upper) then
                prism_orientation = 1
            else
                if ((ring == 1 .and. n_center_point == 1).or. delaunay_condition(u, v, p, q, .true.)) then
                    ! triangle [0, 2, 3] satisfies delaunay condition => prism is up facing
                    prism_orientation  = 0
                else
                    ! prism is down facing
                    prism_orientation = 1
                end if
            end if

            ! --- get the correct offset masks for up- or down facing prism ---
            mask_idx = 1 + prism_orientation * 3

            slice_offset = mask_phi(:, mask_idx:mask_idx + 2) * verts_per_slice
            ring_offset = mask_r(:, mask_idx:mask_idx + 2)
            segment_offset = mask_theta(:, mask_idx:mask_idx + 2)

            where (ring_offset /= 0) segment_offset = modulo(segment_offset + upper_off, n_verts_upper)
            where (ring_offset == 0) segment_offset = modulo(segment_offset + lower_off, n_verts_lower)
            ring_offset = ring_offset * n_verts_lower

            ! --- with offset masks calculate verts ---
            verts(:, tetra_idx:tetra_idx + 2) = base_idx + slice_offset + ring_offset + segment_offset

            ! --- connect neighbouring tetras  ---
            ! connect neighbouring tetras in this prism
            call connect_prisms(prism_idx, prism_idx, verts, neighbours, neighbour_faces)

            ! connect with prisms in neighbouring slices
            neighbours(4, tetra_idx) = tetra_idx + 2 - tetras_per_slice
            neighbour_faces(4, tetra_idx) = 1
            neighbours(1, tetra_idx + 2) = tetra_idx + tetras_per_slice
            neighbour_faces(1, tetra_idx + 2) = 4

            ! connect with previous prism in the same ring
            if (.not. segment == 1) then
                call connect_prisms(prism_idx, prism_idx - 1, verts, neighbours, neighbour_faces)
            end if

            if (prism_orientation == 0) then ! top facing prism
                ! add idx of this prism to array of top facing prisms
                top_facing_prisms(base_idx + (n_verts_lower - 1) + upper_off) = prism_idx

                ! periodic boundary for theta
                if (segment == 1) then
                    perbou_theta(4, tetra_idx + 1) = -1
                    perbou_theta(4, tetra_idx + 2) = -1
                else if (segment == prisms_per_ring(ring)) then
                    perbou_theta(2, tetra_idx) = 1
                    perbou_theta(3, tetra_idx + 2) = 1
                end if

                upper_off = upper_off + 1
            else ! bottom facing prism
                ! connect with prism on previous ring
                if (.not. ring == 1) then
                    call connect_prisms(prism_idx, top_facing_prisms(base_idx - 1 + lower_off), &
                                        verts, neighbours, neighbour_faces)
                end if

                ! periodic boundary for theta
                if (segment == 1) then
                    perbou_theta(2, tetra_idx) = -1
                    perbou_theta(3, tetra_idx + 2) = -1
                else if (segment == prisms_per_ring(ring)) then
                    perbou_theta(1, tetra_idx) = 1
                    perbou_theta(1, tetra_idx + 1) = 1
                end if

                lower_off = lower_off + 1
            end if

            prism_idx = prism_idx + 1
            tetra_idx = (prism_idx - 1) * 3 + 1

        end do

        ! connect first and last prism in ring
        call connect_prisms(prism_idx - 1, prism_idx - prisms_per_ring(ring), verts, neighbours, neighbour_faces)

        base_idx = base_idx + n_verts_lower

    end do

    deallocate(top_facing_prisms)

    ! for all the other slices we can calculate the verts by incrementing the
    ! vert indices of the first(idx = 0) slice by slice * verts_per_slice
    ! and the neighbours by incrementing the neighbours of the first slice by slice * tetras_per_slice
    ! neighbour_faces and perbou_theta can just be copied

    do slice = 1, n_slices - 1 ! slice is the 0-based index of the slice
        tetra_idx = 1 + slice * tetras_per_slice

        verts(:, tetra_idx:tetra_idx + tetras_per_slice - 1) = verts(:, 1:tetras_per_slice) + slice * verts_per_slice

        ! wrap indices around in the last slice
        if (slice == n_slices - 1) then
            call wrap_idx_inplace(verts(:, tetra_idx:tetra_idx + tetras_per_slice - 1), n_verts)
        end if
    end do

    do slice = 1, n_slices - 1 ! slice is the 0-based index of the slice
        tetra_idx = 1 + slice * tetras_per_slice

        neighbours(:, tetra_idx:tetra_idx + tetras_per_slice - 1) = &
                neighbours(:, 1:tetras_per_slice) + slice * tetras_per_slice

        ! wrap indices around in the last slice
        if (slice == n_slices - 1) then
            call wrap_idx_inplace(neighbours(:, tetra_idx:tetra_idx + tetras_per_slice - 1), n_tetras)
        end if
    end do

    ! we dont wrap the neighbours index of the first slice earlier otherwise we would need to wrap the
    ! index of every incrementally calculated slice
    call wrap_idx_inplace(neighbours(:, 1:tetras_per_slice), n_tetras)

    do slice = 1, n_slices - 1 ! slice is the 0-based index of the slice
        tetra_idx = 1 + slice * tetras_per_slice

        neighbour_faces(:, tetra_idx:tetra_idx + tetras_per_slice - 1) = neighbour_faces(:, 1:tetras_per_slice)
    end do

    ! remove outer faces after modulo operation
    where (neighbour_faces == -1)
        neighbours = -1
    end where

    do slice = 1, n_slices - 1 ! slice is the 0-based index of the slice
        tetra_idx = 1 + slice * tetras_per_slice

        perbou_theta(:, tetra_idx:tetra_idx + tetras_per_slice - 1) = perbou_theta(:, 1:tetras_per_slice)
    end do

    ! periodic boundary
    perbou_phi = 0
    perbou_phi(4, :tetras_per_slice:3) = -1
    perbou_phi(1, n_tetras - tetras_per_slice + 3::3) = 1

    do i = 1, n_tetras
        do f = 1, 4
            if (.not. (neighbours(f, i) == -1 .and. neighbour_faces(f, i) == -1)) then
                if (.not. neighbours(neighbour_faces(f, i), neighbours(f, i)) == i) then
                    print *, "!!! CONSISTENCY CHECK FOR NEIGHBOURS FAILED, MESH IS BROKEN !!!"
                    stop
                end if
                if (.not. perbou_phi(neighbour_faces(f, i), neighbours(f, i)) == - perbou_phi(f, i)) then
                    print *, "!!! CONSISTENCY CHECK FOR PERBOU FAILED, MESH IS BROKEN !!!"
                    stop
                end if
                if (.not. perbou_theta(neighbour_faces(f, i), neighbours(f, i)) == - perbou_theta(f, i)) then
                    print *, "!!! CONSISTENCY CHECK FOR PERBOU THETA FAILED, MESH IS BROKEN !!!"
                    stop
                end if
            end if
        end do
    end do
    print *, "mesh consistency check ok"

end subroutine calc_mesh

pure function calc_n_verts(verts_per_ring, n_slices, repeat_center_point) result(n_verts)
    ! calculate the number of vertices in a torus given verts_per_ring and n_slices
    integer, dimension(:), intent(in) :: verts_per_ring
    integer, intent(in) :: n_slices
    logical, optional, intent(in) ::repeat_center_point

    integer :: n_verts

    if (present(repeat_center_point)) then
        if (repeat_center_point .eqv. .true.) then
            n_verts = (sum(verts_per_ring) + verts_per_ring(1)) * n_slices
            return
        end if
    end if

    n_verts = (sum(verts_per_ring) + 1) * n_slices

end function calc_n_verts

pure function calc_n_tetras(verts_per_ring, n_slices, repeat_center_point) result(n_tetras)
    ! calculate the number of tetraeders in a torus given verts_per_ring, n_rings and n_slices
    integer, dimension(:), intent(in) :: verts_per_ring
    integer, intent(in) :: n_slices
    logical, optional, intent(in) ::repeat_center_point

    integer :: n_tetras

    if (present(repeat_center_point)) then
        if (repeat_center_point .eqv. .true.) then
            n_tetras = (verts_per_ring(1) + &
                       sum(verts_per_ring) + sum(verts_per_ring(:size(verts_per_ring) - 1))) * 3 * n_slices
            return
        end if
    end if

    n_tetras = (sum(verts_per_ring) + sum(verts_per_ring(:size(verts_per_ring) - 1))) * 3 * n_slices

end function calc_n_tetras

pure function delaunay_condition(u, v, p, q, fixed_order) result(valid)
    ! check if the triangle [u, v, p] satisfies the delaunay condition with respect to point q
    ! returns:
    !   valid: .true. the triangle [u, v, p] satisfies the delaunay condition
    !          .false. otherwise

    double precision, dimension(2), intent(in) :: u, v, p, q
    logical, intent(in), optional :: fixed_order
    logical :: valid

    double precision :: a, b, c, d, e, f, g, h, i
    double precision :: delta, gamma

    a = u(1) - q(1)
    b = u(2) - q(2)
    c = (u(1) - q(1)) ** 2.d0 + (u(2) - q(2)) ** 2.d0
    d = v(1) - q(1)
    e = v(2) - q(2)
    f = (v(1) - q(1)) ** 2.d0 + (v(2) - q(2)) ** 2.d0
    g = p(1) - q(1)
    h = p(2) - q(2)
    i = (p(1) - q(1)) ** 2.d0 + (p(2) - q(2)) ** 2.d0

    delta = a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h

    if (.not.present(fixed_order) .or. fixed_order .eqv. .false.) then
        ! needed for result to be invariant to order of points
        gamma = ((u(1) - p(1)) * (v(2) - p(2))) - ((v(1) - p(1)) * (u(2) - p(2)))
    else
        ! if the order of the points is counterclockwise we dont need to calculate gamma
        gamma = 1.d0
    end if

    valid = (delta * gamma < 0.0d0)

end function delaunay_condition

subroutine connect_prisms(prism_1_idx, prism_2_idx, verts, neighbours, neighbour_faces)
    integer, intent(in) :: prism_1_idx, prism_2_idx
    integer, dimension(:, :), intent(in) :: verts
    integer, dimension(:, :), intent(inout) :: neighbours, neighbour_faces

    integer :: tetra_1_base, tetra_2_base, tetra_1_idx, tetra_2_idx, tetra_1_off, tetra_2_off, &
               tetra_1_face, tetra_2_face, i

    logical, dimension(4) :: same_vert_1, same_vert_2

    tetra_1_base = (prism_1_idx - 1) * 3 + 1
    tetra_2_base = (prism_2_idx - 1) * 3 + 1

    do tetra_1_off = 0, 2
        tetra_1_idx = tetra_1_base + tetra_1_off
        do tetra_2_off = 0, 2
            tetra_2_idx = tetra_2_base + tetra_2_off

            if (tetra_1_idx == tetra_2_idx) cycle

            do i = 1, 4
                same_vert_1(i) = any(verts(:, tetra_2_idx) == verts(i, tetra_1_idx))
                same_vert_2(i) = any(verts(i, tetra_2_idx) == verts(:, tetra_1_idx))
            end do

            if (count(same_vert_1) == 3) then
                tetra_1_face = minloc(abs(transfer(same_vert_1 , 1, size=4)), dim=1)
                tetra_2_face = minloc(abs(transfer(same_vert_2 , 1, size=4)), dim=1)

                neighbours(tetra_1_face, tetra_1_idx) = tetra_2_idx
                neighbours(tetra_2_face, tetra_2_idx) = tetra_1_idx

                neighbour_faces(tetra_1_face, tetra_1_idx) = tetra_2_face
                neighbour_faces(tetra_2_face, tetra_2_idx) = tetra_1_face
            end if
        end do
    end do

end subroutine connect_prisms

elemental function wrap_idx(index, period) result(wrapped_index)
    ! wrap around the 1-based periodic index which may be larger or smaller than the period
    integer, intent(in) :: index
    integer, intent(in) :: period
    integer :: wrapped_index
    wrapped_index = modulo(index - 1, period) + 1
end function wrap_idx

elemental subroutine wrap_idx_inplace(index, period)
    integer, intent(inout) :: index
    integer, intent(in) :: period
    index = wrap_idx(index, period)
end subroutine wrap_idx_inplace

end module circular_mesh

! program test
!     use circular_mesh, only: calc_mesh, calc_points_circular, calc_n_tetras, calc_n_verts
!     implicit none
!
!     integer, allocatable, dimension(:, :) :: v, n, nf, pb
!     double precision, allocatable, dimension(:, :) :: points
!     integer :: n_tetras, n_points, i, f
!
!     integer :: n_slices = 3
!     !integer, dimension(3) :: vpr = (/6, 9, 12/)
!     integer, dimension(3) :: vpr
!     vpr = (/(i, i=6, 6 + size(vpr) * 2 - 1, 2)/)
!
!     n_tetras = calc_n_tetras(vpr, n_slices)
!     n_points = calc_n_verts(vpr, n_slices)
!
!     allocate(v(4, n_tetras))
!     allocate(n(4, n_tetras))
!     allocate(nf(4, n_tetras))
!     allocate(pb(4, n_tetras))
!     allocate(points(3, n_points))
!
!     call calc_points_circular(vpr, n_slices, 171.d0, 96.d0, 0.d0, points)
!     call calc_mesh(vpr, n_slices, points(:, :n_points / n_slices), v, n, nf, pb)
!     !do i = 1, size(points, 2)
!     !    print *, i, points(:, i)
!     !end do
!     ! simple consistency check
!
!     do i = 1, size(v, 2)
!         do f = 1, 4
!             if (.not. (n(f, i) == -1 .and. nf(f, i) == -1)) then
!                 if (.not. n(nf(f, i), n(f, i)) == i) then
!                     print *, "!!! CONCISTENCY CHECK FAILED, MESH IS BROKEN !!!"
!                     stop
!                 end if
!             end if
!         end do
!         !print *, v(:, i)
!         !if (mod(i, 3) == 0) print *,
!     end do
!     print *, "consistency check ok"
! end program test
!


module matrix_utilities

  use bl_types
  use bl_prof_module
  use probin_multispecies_module, only: nspecies
  use probin_common_module, only: molmass

  implicit none

  private

  public :: Dbar2chi_iterative, choldc

contains

    ! nspecies is number of species
    ! num_iterations is the number of terms in the sum to use: 3-5 are reasonable values
    ! D_bar is matrix of Maxwell-Stefan binary diffusion coefficient
    ! chi is the multispecies diffusion matrix
    ! Xk is mole fractions --- MUST NOT BE ZERO
    subroutine Dbar2chi_iterative(num_iterations,D_bar,Xk,chi)
      integer, intent(in) :: num_iterations
      real(kind=dp_t), intent(in) :: Xk(1:nspecies), D_bar(1:nspecies,1:nspecies)
      real(kind=dp_t), intent(out) :: chi(1:nspecies,1:nspecies)

      ! Local variables
      real(kind=dp_t) :: term1, term2, MWmix
      real(kind=dp_t) :: Di(1:nspecies)
      real(kind=dp_t) :: Deltamat(1:nspecies,1:nspecies), Zmat(1:nspecies,1:nspecies)
      real(kind=dp_t), dimension(1:nspecies,1:nspecies) :: Pmat, Jmat
      real(kind=dp_t), dimension(1:nspecies) :: Minv, Mmat
      real(kind=dp_t), dimension(1:nspecies,1:nspecies) :: PJ, matrix1, matrix2
      real(kind=dp_t) :: scr
      real(kind=dp_t) :: Ykp(1:nspecies), Xkp(1:nspecies)

      integer :: i, j, k, ii, jj

      type(bl_prof_timer), save :: bpt

      call build(bpt,"Dbar2chi_iterative")

      ! mole fractions correction
      ! Turned this off since it should be done in the caller
      do ii = 1, nspecies
       Xkp(ii) = Xk(ii) ! + fraction_tolerance*(sum(Xk(:))/dble(nspecies)-Xk(ii))
      end do

      ! molecular weight of mixture - EGLIB
      Mwmix = 0.0d0
      do ii = 1, nspecies
       MWmix = MWmix + Xkp(ii)*molmass(ii)
      end do

      ! mass fractions correction - EGLIB
      do ii = 1, nspecies
       Ykp(ii) = molmass(ii)/MWmix*Xkp(ii)
      end do

      ! Find Di matrix
      do i = 1, nspecies
       term2 = 0.0d0
       do j = 1, nspecies
        if(j.ne.i) then
          term2 = term2 + Xkp(j)/D_bar(i,j)
        end if
       end do
       Di(i) = (1.d0-Ykp(i))/term2
      end do

      ! Compute Mmat and Minv
      do i = 1, nspecies
       Mmat(i) = Xkp(i)/Di(i)
       Minv(i) = Di(i)/Xkp(i)
      end do

      ! Compute P matrix
      Pmat = 0.0d0
      do i = 1, nspecies
       do j = 1, nspecies
         Pmat(i,j) = - Ykp(j)
         if(i.eq.j) then
          Pmat(i,j) =  Pmat(i,j) + 1.0d0
         end if
       end do
      end do

      ! Compute Deltamat
      Deltamat = 0.0d0
      do i = 1, nspecies
       do j = 1, nspecies
         if(i.eq.j) then
          term1 = 0.0d0
          do k = 1, nspecies
           if(k.ne.i) then
            term1 = term1 + Xkp(i)*Xkp(k)/D_bar(i,k)
           end if
          end do
          Deltamat(i,i) = term1
         else
          Deltamat(i,j) = -Xkp(i)*Xkp(j)/D_bar(i,j)
         end if
          Zmat(i,j) = -Deltamat(i,j)
       end do
      end do

      ! Compute Zmat
      do i = 1, nspecies
        Zmat(i,i) = Zmat(i,i) + Mmat(i)
      end do

      ! Compute Jmat
      do i = 1, nspecies
       do j = 1, nspecies
         Jmat(i,j) = Minv(i)*Zmat(i,j)
        end do
       end do

      ! Compute PJ
      PJ = 0.0d0
      do i = 1, nspecies
       do j = 1, nspecies
        do k = 1, nspecies
         PJ(i,j) = PJ(i,j) + Pmat(i,k)*Jmat(k,j)
        end do
       end do
      end do

      ! Compute P M^-1 Pt; store it in matrix2
      do i = 1, nspecies
       do j = 1, nspecies
        scr = 0.d0
        do k = 1, nspecies
         scr = scr + Pmat(i,k)*Minv(k)*Pmat(j,k)
            ! notice the change in indices for Pmat to represent Pmat^t
        end do
         matrix2(i,j) = scr
         chi(i,j) = scr
       end do
      end do

      do jj = 1,num_iterations
       do i = 1, nspecies
        do j = 1, nspecies
         scr = 0.d0
         do k = 1, nspecies
            scr = scr + PJ(i,k)*chi(k,j)
         end do
          matrix1(i,j) = scr+matrix2(i,j)
        end do
       end do
       chi=matrix1
      end do

      call destroy(bpt)

  end subroutine

   ! a is input matrix.
   ! upon return the lower triangle and diagonal are overwritten by the cholesky factor
   subroutine choldc(a,np)
       integer :: np
       real(kind=dp_t), intent(inout) :: a(np,np)

       real(kind=dp_t) :: p(np), dij(np,np)
       real(kind=dp_t) :: dd(np,np)
       real(kind=dp_t) :: yy(np), mwmix

       integer :: i, j, k, ii, jj
       real(kind=dp_t) :: sum1
       real(kind=dp_t) :: small_number = 0.0d0 ! Some tolerance

       integer :: idiag,ising

       type(bl_prof_timer), save :: bpt

       call build(bpt,"choldc")

       do i = 1, np

           ising = 0

        do j = i, np

           sum1 = a(i,j)

           do k = i-1, 1, -1

              sum1 = sum1 - a(i,k)*a(j,k)

           end do

           if(i.eq.j) then

             if(sum1.le.small_number) then

             p(i) = 0.d0

             ising = 1

             else

             p(i) = sqrt(sum1)

             end if

           else

             if(ising.eq.0)then

                a(j,i) = sum1/p(i)

             else

                a(j,i) = 0.d0

             end if

           end if

        end do

       end do

       do i = 1, np

          do j = i+1, np

           a(i,j) = 0.0d0 ! Zero upper triangle

          end do

          a(i,i) = p(i)

       end do

       call destroy(bpt)

    end subroutine

end module


!
!     CalculiX - A 3-dimensional finite element program
!     Copyright (C) 1998-2021 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
      subroutine con2phys(vold,vcon,nk,ntmat_,shcon,nshcon,rhcon,nrhcon,
     &     physcon,ithermal,compressible,iturbulent,inomat,mi,
     &     ierr,ifreesurface,dgravity,depth)
!
!     calculates the physical variable from the conservative
!     variables:
!     for gases: temperature,velocity and pressure
!     for thermal liquids: temperature, velocity and density
!     for athermal liquids: velocity and density
!
      implicit none
!
      integer compressible,ifreesurface,nrhcon(*),ntmat_,iturbulent,
     &     mi(*),nshcon(*),nk,ithermal(*),i,j,k,imat,ierr,inomat(*)
!
      real*8 vold(0:mi(2),*),vcon(nk,0:mi(2)),rhcon(0:1,ntmat_,*),rho,
     &     c1,cp,r,temp,temp0,c2,shcon(0:3,ntmat_,*),physcon(*),
     &     dgravity,depth(*)
!
      if(ithermal(1).gt.1) then
!
        do i=1,nk
          imat=inomat(i)
          temp=vold(0,i)
!
          if(compressible.eq.1) then
!
!     gas : rho*epsilon_t, rho and rho*v known
!
!     calculation of the static temperature
!
            rho=vcon(i,4)
            r=shcon(3,1,imat)
            c1=(vcon(i,0)-(vcon(i,1)**2+vcon(i,2)**2+
     &           vcon(i,3)**2)/(2.d0*rho))/rho
!
!     the energy density per volume unit cannot be
!     negative
!
            if(c1.lt.1.d-10) c1=1.d-10
!
            temp0=temp
            j=0
            do
              call materialdata_cp_sec(imat,ntmat_,temp,shcon,
     &             nshcon,cp,physcon)
              temp=c1/(cp-r)+physcon(1)
              j=j+1
              if((dabs(temp-temp0).lt.1.d-4*dabs(temp)).or.
     &             (dabs(temp-temp0).lt.1.d-10)) then
                vold(0,i)=temp
                exit
              endif
              if(j.gt.100) then
                write(*,*)
     &               '*ERROR in con2phys: too many iterations'
                write(*,*) '       for node',i
                write(*,*) '       increment is recalculated'
                write(*,*) '       with a higher shock smoothing'
                ierr=1
                return
              endif
              temp0=temp
            enddo
!
            if(ifreesurface.eq.0) then
!
!     determining the pressure (gas equation)
!
              vold(4,i)=rho*r*(temp-physcon(1))
            else
!
!     determining the pressure (shallow water)
!
              vold(4,i)=dgravity*(vcon(i,4)**2-depth(i)**2)/2.d0
            endif
!
!     calculating the velocity from the conservative variables
!
            do k=1,3
              vold(k,i)=vcon(i,k)/rho
            enddo
!
!     calculating the turbulent quantities from the conservative
!     variables
!
            if(iturbulent.ne.0) then
              do k=5,6
                vold(k,i)=vcon(i,k)/rho
              enddo
            endif
!
            cycle
          else
!
!     thermal liquid
!
            c1=vcon(i,0)
            c2=(vcon(i,1)**2+vcon(i,2)**2+vcon(i,3)**2)/2.d0
            temp0=temp
            j=0
!
!     iterating to find the temperature
!
            do
              call materialdata_cp_sec(imat,ntmat_,temp,shcon,
     &             nshcon,cp,physcon)
              call materialdata_rho(rhcon,nrhcon,imat,rho,
     &             temp,ntmat_,ithermal)
              temp=(c1-c2/rho)/(rho*cp)+physcon(1)
              j=j+1
              if((dabs(temp-temp0).lt.1.d-4*dabs(temp)).or.
     &             (dabs(temp-temp0).lt.1.d-10)) then
                vold(0,i)=temp
                exit
              endif
              if(j.gt.100) then
                write(*,*)
     &               '*ERROR in con2phys: too many iterations'
                write(*,*) '       for node',i
                write(*,*) '       actual temperature ',temp,' K'
                stop
              endif
              temp0=temp
            enddo
!
!     storing the density
!
            vcon(i,4)=rho
!
!     calculating the velocity
!
            do k=1,3
              vold(k,i)=vcon(i,k)/rho
            enddo
!
!     calculating the turbulent quantities
!
            if(iturbulent.ne.0) then
              do k=5,6
                vold(k,i)=vcon(i,k)/rho
              enddo
            endif
!
          endif
        enddo
      else
!
!     athermal calculations
!
        do i=1,nk
!
!     calculating the fictitious pressure for shallow water
!     calculations
!
          if(ifreesurface.eq.1) then
            vold(4,i)=dgravity*(vcon(i,4)**2-depth(i)**2)/2.d0
          endif
!
!     calculating the velocity
!
          rho=vcon(i,4)
          do k=1,3
            vold(k,i)=vcon(i,k)/rho
          enddo
!
!     calculating the turbulent quantities
!
          if(iturbulent.ne.0) then
            do k=5,6
              vold(k,i)=vcon(i,k)/rho
            enddo
          endif
!
        enddo
      endif
!
      return
      end


      subroutine dangle(x1,x2,x3,a,da,d2a,nflag)
      implicit double precision(a-h,o-z)

c checked by finite difference 230414

      dimension x1(3),x2(3),x3(3),da(3,3),d2a(3,3,3,3)
      dimension dr1(2,3),dr2(2,3),dr3(2,3)
      dimension d2r1(2,3,2,3),d2r2(2,3,2,3),d2r3(2,3,2,3)

      dimension df1(3,3),df2(3,3),df3(3,3)
      dimension d2f1(3,3,3,3),d2f2(3,3,3,3),d2f3(3,3,3,3)

c the angle x1...x2...x3

c actually calculate cos of the angle
c and derivatives of cos

      call dbond(x1,x2,r1,dr1,d2r1,nflag)
      call dbond(x3,x2,r2,dr2,d2r2,nflag)
      call dbond(x1,x3,r3,dr3,d2r3,nflag)

      denom=2.d0*r1*r2
      denom2=denom**2
      anom=r1**2+r2**2-r3**2

c cos of the angle
      a=anom/denom

      do k=1,3
       df1(1,k)=dr1(1,k)
       df1(2,k)=dr1(2,k)
       df1(3,k)=0.d0
       df2(1,k)=0.d0
       df2(2,k)=dr2(2,k)
       df2(3,k)=dr2(1,k)
       df3(1,k)=dr3(1,k)
       df3(2,k)=0.d0
       df3(3,k)=dr3(2,k)
      enddo

      do k1=1,3
      do k2=1,3

       d2f1(1,k1,1,k2)=d2r1(1,k1,1,k2)
       d2f1(1,k1,2,k2)=d2r1(1,k1,2,k2)
       d2f1(2,k1,1,k2)=d2r1(1,k1,2,k2)
       d2f1(1,k1,3,k2)=0.d0
       d2f1(3,k1,1,k2)=0.d0
       d2f1(2,k1,2,k2)=d2r1(2,k1,2,k2)
       d2f1(2,k1,3,k2)=0.d0
       d2f1(3,k1,2,k2)=0.d0
       d2f1(3,k1,3,k2)=0.d0

       d2f2(1,k1,1,k2)=0.d0
       d2f2(1,k1,2,k2)=0.d0
       d2f2(2,k1,1,k2)=0.d0
       d2f2(1,k1,3,k2)=0.d0
       d2f2(3,k1,1,k2)=0.d0
       d2f2(2,k1,2,k2)=d2r2(2,k1,2,k2)
       d2f2(2,k1,3,k2)=d2r2(1,k1,2,k2)
       d2f2(3,k1,2,k2)=d2r2(1,k1,2,k2)
       d2f2(3,k1,3,k2)=d2r2(1,k1,1,k2)

       d2f3(1,k1,1,k2)=d2r3(1,k1,1,k2)
       d2f3(1,k1,2,k2)=0.d0
       d2f3(2,k1,1,k2)=0.d0
       d2f3(1,k1,3,k2)=d2r3(1,k1,2,k2)
       d2f3(3,k1,1,k2)=d2r3(1,k1,2,k2)
       d2f3(2,k1,2,k2)=0.d0
       d2f3(2,k1,3,k2)=0.d0
       d2f3(3,k1,2,k2)=0.d0
       d2f3(3,k1,3,k2)=d2r3(2,k1,2,k2)

      enddo
      enddo

c first derivatives
      do k=1,3
      do n=1,3
       da(n,k)=(r1*df1(n,k)+r2*df2(n,k)-r3*df3(n,k)
     .         -df1(n,k)*r2*a-r1*df2(n,k)*a)/(r1*r2)
      enddo
      enddo

c second derivatives
      if(nflag.eq.2)then

      do k1=1,3
      do k2=1,3
      do n1=1,3
      do n2=1,3

      d2a(n1,k1,n2,k2)=(df1(n1,k1)*df1(n2,k2)+r1*d2f1(n1,k1,n2,k2)
     .                 +df2(n1,k1)*df2(n2,k2)+r2*d2f2(n1,k1,n2,k2)
     .                 -df3(n1,k1)*df3(n2,k2)-r3*d2f3(n1,k1,n2,k2)
     .                 -d2f1(n1,k1,n2,k2)*r2*a-df1(n1,k1)*df2(n2,k2)*a
     .                 -df1(n1,k1)*r2*da(n2,k2)
     .                 -d2f2(n1,k1,n2,k2)*r1*a-df2(n1,k1)*df1(n2,k2)*a
     .                 -df2(n1,k1)*r1*da(n2,k2)
     .                 -df1(n2,k2)*r2*da(n1,k1)-df2(n2,k2)*r1*da(n1,k1))
     .                 /(r1*r2)

      enddo
      enddo
      enddo
      enddo

      endif

      return
      end


module mod1
implicit none
save
integer, parameter :: precision=15, range=307
integer, parameter :: dp = selected_real_kind(precision, range)
real(dp), parameter :: gravity=9.81d0
real(dp), parameter :: pi=3.1415926539
real(dp), parameter :: RU=8314d0
real(dp), parameter :: zero=0._dp, one=1._dp
real(dp), parameter :: cd=1.1, rhob=1.225,tamb=300d0
real(dp), parameter :: mwair=28.96,surfrocket=pi/4
! assuminng a 1.1 drag coefficient and

real(dp):: cp,cv,g,rgas,mw,vol=one,dia,cf,id,od,length,rref,rhos,psipa,pref
real(dp):: db=zero,dt,tmax,Tflame
real(dp):: thrust=zero, area, r, n, surf,mdotgen,mdotout,edotgen,edotout,energy
real(dp):: mdotos=zero, edotos, texit, dsigng,pamb,p,t
real(dp):: mcham,echam,time=zero,propmass=zero,drag=zero,netthrust=zero
integer nsteps,i
real(dp):: accel=zero, vel=zero, altitude=zero, rocketmass=zero
real(dp), allocatable :: output(:,:)
real(dp) den ! air density
end module

subroutine propwt ! calculate weight of propellent
  use mod1
  implicit none
  propmass=pi/4*(od**2-id**2)*length*rhos
  rocketmass=0.15*propmass ! assume 85% propellant loading and 15% extra wt of rocket
end subroutine

subroutine burnrate
  use mod1
  implicit none
  r=rref*(p/pref)**n ! calculate burn rate
  db=db+r*dt  ! calculate incremental burn distance
end subroutine

subroutine calcsurf
  ! cylinder burning from id outward and from both ends along the length
  use mod1
  implicit none

  surf=pi*(id+2.0d0*db)*(length-2.0d0*db)+pi*(od**2.0d0-(id+2.0*db)**2.0d0)*0.5

  if(id+2d0*db.gt.od.or.db.gt.length/2d0) THEN
     surf=0d0  ! we hit the wall and burned out
     r=0  ! turn off burn rate so burn distance stops increasing
   endif

vol=vol+r*surf*dt ! increment the interior volume of the chamber a little
end subroutine

subroutine calmdotgen
  use mod1
  implicit none
  mdotgen=rhos*r*surf
  edotgen=mdotgen*cp*Tflame
end subroutine

subroutine massflow
   USE mod1
   implicit none
   REAL (8)::mdtx,engyx
   REAL (8)::tx,gx,rx,px,cpx,pcrit,facx,term1,term2,pratio,cstar,ax,hx
   REAL (8):: p1,p2

   mdotos=0.
   edotos=0.  ! initially set them to zero prior to running this loop

     p1=p
     p2=pamb
     ax=area
     IF(p1.GT.p2) THEN
        dsigng=1
        tx=t
        gx=g
        rx=rgas
        px=p
        cpx=cp
        hx=cp*t
        pratio=p1/p2
     else
        dsigng=-1
        tx=tamb
        gx=g
        rx=rgas
        px=pamb
        cpx=cp
        hx=cp*tamb
        pratio=p2/p1
    end if

    pcrit=(2./(gx+1.))**(gx/(gx-1.))
    IF((1./pratio).LT.pcrit) then
        ! choked flow
        cstar=sqrt((1./gx)*((gx+1.)/2.)**((gx+1.)/(gx-1.))*rx*tx)
        mdtx=px*ax/cstar
    else
        ! unchoked flow
      facx=pratio**((gx-1.)/gx)
      term1=SQRT(gx*rx*tx/facx)
      term2=SQRT((facx-1.)/(gx-1.))
      mdtx=SQRT(2.)*px/pratio/rx/tx*facx*term1*term2*ax
    end if
    engyx=mdtx*hx  ! reformulate based on enthalpy of the chamber
    mdotos=mdtx*dsigng ! exiting mass flow (could be negative "dsigng")
    edotos=engyx*dsigng ! exiting enthalpy
end subroutine

subroutine addmass
    use mod1
    implicit none
    mcham=mcham+(mdotgen-mdotos)*dt
    echam=echam+(edotgen-edotos)*dt
end subroutine

subroutine calct
    use mod1
    implicit none
    t=echam/mcham/cv
end subroutine

subroutine calcp
    use mod1
    implicit none
    p=mcham*rgas*t/vol
end subroutine

subroutine calcthrust
    use mod1
    implicit none
    thrust=(p-pamb)*area*cf ! correction to thrust (actual vs vacuum thrust)
    den=rhob*exp(-gravity*mwair*altitude/RU/tamb)
    drag=-cd*0.5*den*vel*abs(vel)*surfrocket

    netthrust=thrust+drag
end subroutine

subroutine height
  use mod1
  implicit none
  propmass=propmass-mdotgen*dt ! incremental change in propellant mass
  accel=netthrust/(propmass+rocketmass+mcham)-gravity
  vel=vel+accel*dt
  altitude=altitude+vel*dt
end subroutine

!!  Main program

function legacy_rocket( &
    dt_, &
    t_max_, &
    c_p_, &
    MW_, &
    temperature_, &
    pressure_, &
    T_flame_, &
    r_ref_, &
    n_, &
    id_, &
    od_, &
    length_, &
    rho_solid_, &
    dia_, &
    C_f_)
  !! this is a basic program of a single stage
  !! rocket motor flowing out of a nozzle, assuming
  !! a thrust coefficient and ignoring the complexities of
  !! what happens to thrust at low pressures, i.e. shock in the nozzle

use mod1
implicit none

real(dp), intent(in) :: dt_, t_max_
real(dp), intent(in) :: c_p_, MW_
real(dp), intent(in) :: temperature_, pressure_
real(dp), intent(in) :: T_flame_, r_ref_, n_
real(dp), intent(in) :: id_, od_, length_, rho_solid_
real(dp), intent(in) :: dia_, C_f_
real(dp), allocatable :: legacy_rocket(:,:)

dt   = dt_
tmax = t_max_
cp = c_p_
mw = MW_
t = temperature_
p = pressure_
Tflame = T_flame_
rref   = r_ref_
n      = n_
id     = id_
od     = od_
length = length_
rhos   = rho_solid_
dia = dia_
cf  = C_f_

!  propellent grain is a cylinder burning radially outward and axially inward from one end.
!  the other end is considered inhibited.
! outer diameter is inhibited because this is a cast propellent: it was poured
! into the tube/chamber and only the inner diameter burns when ignited.

  ! propellant burn rate information
  psipa=6894.76d0 ! pascals per psi (constant)
  pref=3000d0*psipa ! reference pressure (constant)

  nsteps=nint(tmax/dt) ! number of time steps

! preallocate an output file for simulation infomration
  allocate(output(0:nsteps,11))

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11

!! now begin calculating and initializing
! gas variables
  rgas=ru/mw
  cv=cp-rgas
  g=cp/cv

  area=pi/4d0*dia**2.0d0 ! nozzle area

  pamb=101325d0 ! atmospheric pressure

!  calculate initial mass and energy in the chamber
  mcham=p*vol/rgas/t  ! use ideal gas law to determine mass in chamber
  echam=mcham*cv*t ! initial internal energy in chamber

  output(0,:) = [time,p,t,mdotos,thrust,drag,netthrust,vol,accel,vel,altitude]

  call propwt
  do i=1,nsteps
   call burnrate
   call calcsurf
   call calmdotgen  ! [mdot,engy,dsign]= massflow(p1,pamb,t1,tamb,cp,cp,rgas,rgas,g,g,area)
   call massflow
   call addmass
   call calct
   call calcp
   call calcthrust
   call height
   time=time+dt
   output(i,:)=[time,p,t,mdotos,thrust,drag,netthrust,vol,accel,vel,altitude]

  enddo

  legacy_rocket = output

end function legacy_rocket


      subroutine woflam(trigsu, trigsv, ifaxu, ifaxv, irho)

c  Evaluate the lambda-dependent part of the functions W1(lambda)
c  for all LAMBDA.
c  Calculates the sum over n and m of (mR+nS)/(m-nq) * dalphamn/dlambda
c      * <V||/V (mn)>/<V||/V>*lambda*(-2).
c  Here, m is the poloidal mode number and n is the toroidal mode
c  number/periods.
C-----------------------------------------------
C   M o d u l e s
C-----------------------------------------------
      use parambs
      use vmec0
      implicit none
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      integer :: irho
      integer, dimension(13) :: ifaxu, ifaxv
      real(rprec), dimension(3*nthetah/2 + 1) :: trigsu
      real(rprec), dimension(2*nzetah) :: trigsv
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
      real(rprec), parameter :: zero = 0, one = 1,
     1   d18 = 1.0e-18_dp, xlam = 0.96_dp
      integer, parameter :: n_lam_coarse = 97, n_lam = 137

c  as discussed below, the mesh is split with 96 + 40 intervals

C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      integer :: l, j, i, n, m
      real(rprec), dimension(:,:), allocatable :: a11
      complex(rprec), dimension(:,:), allocatable ::
     1   alphamn, vmn
      real(rprec) :: qn, numer, avg_vpov, denom
      real(rprec), dimension(n_lam) :: xlam_f
      real(rprec), dimension(n_lam-1) :: xlam_h
C-----------------------------------------------
C   E x t e r n a l   F u n c t i o n s
C-----------------------------------------------
      real(rprec) , EXTERNAL :: sumit
C-----------------------------------------------
c
c
      allocate (a11(nthetah+2,nzetah),
     5   alphamn(-mbuse:mbuse,0:nbuse),
     8   vmn(-mbuse:mbuse,0:nbuse), stat=l)

      if (l .ne. 0) stop 'allocation error in woflam'

c-----------------------------------------------------------------------
c///////////////////////////////////////////////////////////////////////
c-----------------------------------------------------------------------

c  Meshing for lamda integral.  The lambda integral has a near singular
c  value at lambda = 1.  To evaluate, construct dalpha/dlamda on full
c  mesh.  Evaluate the rest on the full mesh.  Becasue of the (almost)
c  singularity, split the mesh into coarse (0 to 0.96) and fine (.96 to
c  1.0) components.  This was tested on both qas and qos configuratons
c  and gives results that are a percent or better on the integral and
c  ten to twenty times better for the total current.  To save storage,
c  there is only one loop on lambda.  All grids are local.

c-----------------------------------------------------------------------
c///////////////////////////////////////////////////////////////////////
c-----------------------------------------------------------------------

      if (isymm0 .ne. 0) then
         aiterm1(irho) = zero
         return
      endif

c  First contruct the meshes.  This scheme is hardwired.

      xlam_f(1) = zero
      xlam_h(1) = 0.005_dp

      do l = 2, n_lam_coarse-1
         xlam_f(l) = xlam_f(l-1) + 0.01_dp
         xlam_h(l) = xlam_h(l-1) + 0.01_dp
      enddo

      xlam_f(n_lam_coarse) = 0.96_dp               !!End coarse mesh/Begin fine mesh
      xlam_h(n_lam_coarse) = 0.9605_dp             !!Begin of fine half-mesh

      do l = n_lam_coarse+1, n_lam-1
         xlam_f(l) = xlam_f(l-1) + 0.001_dp
         xlam_h(l) = xlam_h(l-1) + 0.001_dp
      enddo

      xlam_f(n_lam) = one

c  Loop over LAMBDA = 0 to 1, calculating alpha on the full mesh, and
c  the rest of the integral on the half mesh.  The jacobian for alpha
c  puts in a b2avg but one of the b values in the denominator cancels the
c  b/bmax in alpha.  This form of the expression paralles that in the
c  hamada paper but with flux surface averages for the alpha.  This results
c  in a form that does not have the beta found in the boozer paper
c  see refereces at beginning.

      do l = 1, n_lam
         a11(:nthetah,:nzetah) =
     2      sqrt(abs(one - xlam_f(l)*bfield(:nthetah,:nzetah))+D18)
     3      *b2avg(irho)/bmax1(irho)**2/bfield(:nthetah,:nzetah)
         a11(nthetah+1,:nzetah) = zero
         a11(nthetah+2,:nzetah) = zero
         call do_fft (a11, fmn, trigsu, trigsv, ifaxu, ifaxv, nthetah,
     1      nzetah, mbuse, nbuse)
         if(l .eq. 1) then
            alphamn = fmn
            cycle                       !need to calculate to alphas and difference
         end if                         !before a term for the integral can be evaluated.

c  save alpha at lambda = 1

         if(l .eq. n_lam) alpha1mn = fmn

c  form d_alphalmn  at this point we have the value of alpha for l in fmn
c  and the previous value of alpha (l-1) in alphamn.  Store the difference in
c  fmn using vmn as a temp variable to hold alpha, then update alpha for
c  the next cycle.

         vmn = fmn
         fmn = fmn - alphamn
         alphamn = vmn

c- Now do the fft to get <exp(i(m*theta-n*zeta)V||/V> on the half mesh

         a11(:nthetah,:nzetah) =
     1      sqrt(abs(one - xlam_h(l-1)*bfield(:nthetah,:nzetah)) + D18)
     2      *(b2avg(irho)/bfield(:nthetah,:nzetah)*bmax1(irho))**2

         a11(nthetah+1,:nzetah) = zero
         a11(nthetah+2,:nzetah) = zero

         call do_fft (a11, vmn, trigsu, trigsv, ifaxu, ifaxv, nthetah,
     1      nzetah, mbuse, nbuse)

c  This needs to be divided by <V||/V>.  But this is exactly the real part of the
c  0,0 term of this transform.
         avg_vpov = real(vmn(0,0))

c- for only those harmonics that are going to be used in the sum.

         qn = periods*qsafety(irho)*zetasign
         rfmn(0,0) = zero
         do m = -mbuse, mbuse
            do n = 0, nbuse

               denom = m + n*qn
               if (n.ne.0 .or. m.ne.0) then

                  numer = denom/(denom**2 + (damp_bs*m)**2)

                  rfmn(m,n) = (m*capr(irho) + n*periods*caps(irho))*
     1             real(fmn(m,n)*vmn(m,n))*(-2*numer)
               endif
            end do
         end do
c
c- The sum in W is the SUM over all n and m (excluding (0,0)) of rfmn.
c  But only the nonegative m have been calculated and stored.  Therefore
c  the sum becomes
c    2 * SUM over m = -mbuse to mbuse and n = 1 to nbuse of rfmn
c     plus SUM over n = -mbuse to mbuse of fn0
c     minus f(0,0)

         w1(l-1) = xlam_h(l-1)*sumit(rfmn,mbuse,nbuse)/avg_vpov

c- End loop over lambda.

      end do
c  evaluate integral

      aiterm1(irho) = sum(w1)
      aiterm1(irho) = -0.75_dp*aiterm1(irho)*qsafety(irho)
     1    /ftrapped(irho) * (one + aiogar(irho)/qsafety(irho))

      deallocate (a11, alphamn, vmn, stat=l)

      end subroutine woflam


      subroutine circle (xa,ya,th0,thf,r0,rf,l)
C      circle 01/09/78
      common /gftab/irdb(5),rdb(17)
      i2= 1
      i5 = l + 1
      x=xa
      y=ya
      call move (x,y,0)
      knt=5
      rorrf = sqrt(abs(r0)+abs(rf)+0.00001)
      fctr=rdb(17)/rdb(1)
      dth= (0.3/rorrf) * fctr
      t0= th0/57.2958
      tf= thf/57.2958
      c= x- r0 * cos(t0)
      tn=(tf-t0) / dth
      if(tf.ge.t0) go to 104
      tn=abs(tn)
      dth=-dth
 104  b=y-r0*sin(t0)
      n=tn
      if(n.le.0) go to 115
      tn=(rf-r0)/tn
      rn=r0-tn
      do 110 i=1,n
      t0=t0+dth
      rn=rn+tn
      x=rn*cos(t0)+c
      y=rn*sin(t0)+b
      if(knt.gt.0) go to 111
      i2=i5-i2
      knt=5
 111  knt=knt-1
 110  call move(x,y,i2)
 115  x=rf*cos(tf)+c
      y=rf*sin(tf)+b
      call move(x,y,i2)
      return
      end


!====================================================================
! Ordinary Bessel functions.
!====================================================================
RECURSIVE real*8 function funBessel(alpha,x) result(res)
implicit none
real*8,intent(in)::alpha,x
integer::m,m_max
real*8::pi=3.141592653589793d0
real*8::a_0,a_m,djmuz

  if(alpha.gt.0.d0)then
    res=djmuz(alpha,x)  !==function from DIN for arbitrary alpha>0 and x>0==!
  else
    res=2.d0*(alpha+1.d0)/x*funBessel(alpha+1.d0,x)-funBessel(alpha+2.d0,x)
  end if
  goto 122
  !===============================================!

  !if(x.gt.1.d2*abs(alpha**2-0.25d0))then
  !if(x.gt.1.d2*abs(alpha**2-0.25d0))then
  !  funBessel=sqrt(2.d0/pi/x)*cos(x-alpha*pi/2.d0-pi/4.d0)
  !else
  !  if(abs(alpha-nint(alpha)).le.1.d-4)then
  !    funBessel=BESSEL_JN(nint(alpha),x)    !==Fortran internal function==!
  !  else
  !    m_max=100   !==has to be checked==!
  !    m=0; funBessel=0.d0; a_0=1.d0/gamma(alpha+1.d0)*(x/2.d0)**alpha; a_m=a_0
  !    do while(m.le.m_max)
  !      !funBessel=funBessel+((-1.d0)**m)*((x/2.d0)**(2.d0*m+alpha))/gamma(1.d0*m+1.d0)/gamma(1.d0*m+alpha+1.d0)
  !      funBessel=funBessel+a_m
  !      a_m=a_m*(-1.d0)/(m+1.d0)/(m+1.d0+alpha)*(x/2.d0)**2
  !      m=m+1
  !    end do
  !  end if
  !end if
122 return
end function funBessel

!=====================================================================
! Has to be revritten for integer alpha!
!=====================================================================
RECURSIVE real*8 function funNeumann(alpha,x) result(res)
implicit none
real*8,intent(in)::alpha,x
real*8::funBessel  !==functions==!
real*8::pi=3.141592653589793d0
real*8::delta,a_0,a_k,b_0,b_k
integer::k

  if(alpha.gt.0.d0)then
    if(abs(alpha-nint(alpha)).le.1.d-4)then
      res=BESSEL_YN(nint(alpha),x)    !==Fortran internal function==!
    else
      res=funNeumann_alpha_nonint(alpha-nint(alpha),nint(alpha),x)
    end if
  else
    res=-2.d0*(alpha+1.d0)/x*funNeumann(alpha+1.d0,x)-funNeumann(alpha+2.d0,x)
  end if
  goto 122
  !===========================================!

  !if(x.gt.1.d2*abs(alpha**2-0.25d0))then
  !  funNeumann=sqrt(2.d0/pi/x)*sin(x-alpha*pi/2.d0-pi/4.d0)
  !else
  !  delta=alpha/1-int(alpha/1)
  !  if((delta.le.1.d-6).or.(delta.ge.0.999999d0))then
  !    funNeumann=funNeumann_int(idnint(alpha),x)
  !  else
  !    k=0
  !    a_0=(x/2.d0)**alpha/gamma(1.d0+alpha)*cos(alpha*pi)/sin(alpha*pi)
  !    b_0=-(x/2.d0)**(-alpha)/gamma(1.d0-alpha)/sin(alpha*pi)
  !    funNeumann=a_0+b_0
  !    a_k=a_0; b_k=b_0
  !    do while(k.le.30)
  !      k=k+1; a_k=a_k*(-1.d0)*(x/2.d0)**2/k/(alpha+k); b_k=b_k*(-1.d0)*(x/2.d0)**2/k/(1.d0*k-alpha)
  !      funNeumann=funNeumann+a_k+b_k
  !    end do
  !  end if
  !end if
122 return
contains

  !==positive orders only==!
  real*8 function funNeumann_alpha_nonint(alpha_frac,alpha_int,x)
  implicit none
  real*8,intent(in)::alpha_frac,x
  integer,intent(in)::alpha_int
  real*8:: res
  dimension res(alpha_int+1)
  integer::ncalc
    call rybesl ( x, alpha_frac, alpha_int+1, res, ncalc )   !==we use the subroutine from numerical library "specfun"==!
    funNeumann_alpha_nonint=res(alpha_int+1)
  return
  end function funNeumann_alpha_nonint

  real*8 function funNeumann_int(n,x)
  implicit none
  integer,intent(in)::n
  real*8,intent(in)::x
  real*8::funBessel  !==function==!
  real*8::sum1,sum2,sum3,sum31,sum32
  real*8::C,pi  !==constants
  integer::k,k_max,m
    k_max=40
    C=0.5772156649015325d0  !==Euler constant
    pi=3.141592653589793d0
    funNeumann_int=2.d0/pi*funBessel(1.d0*n,x)*(log(x/2.d0)+C)
    !==============
    k=1; sum1=0.d0
    do while(k.le.n)
      sum1=sum1+1.d0/(k*1.d0); k=k+1
    end do
    sum1=sum1/pi*((x/2.d0)**n)/gamma(1.d0*n+1.d0)
    !==============
    k=1; sum2=0.d0
    do while(k.le.n)
      sum2=sum2+gamma(1.d0*n-1.d0*k+1.d0)/gamma(1.d0*k)*((x/2.d0)**(-1.d0*n+2.d0*k-2.d0))
      k=k+1
    end do
    sum2=sum2/pi
    !==============
    k=1; sum3=0.d0
    do while(k.le.k_max)
      sum31=0.d0; m=1
      do while(m.le.(n+k)); sum31=sum31+1.d0/(1.d0*m); m=m+1; end do
      sum32=0.d0; m=1
      do while(m.le.k); sum32=sum32+1.d0/(1.d0*m); m=m+1; end do
      sum3=sum3+((-1.d0)**k)*((x/2.d0)**(n+2.d0*k))/gamma(k+1.d0)/gamma(k+n+1.d0)*(sum31+sum32)
      k=k+1
    end do
    sum3=sum3/pi
    funNeumann_int=funNeumann_int-sum1-sum2-sum3
  return
  end function funNeumann_int

end function funNeumann
!=============================================================================

!====================================================================
! Modified Bessel functions of the first kind.
!====================================================================
real*8 function modifiedBessel1(alpha,x)
implicit none
real*8,intent(in)::alpha,x
integer::i,k

 if(x.le.(0.013d0*sqrt(alpha+1.d0)))then
   !==aproximation for small parameter==!
   modifiedBessel1=1.d0/gamma(alpha+1.d0)*(x/2.d0)**alpha
   !write(*,*)"use"
 else
    k=100
    i=0; modifiedBessel1=0.d0
    do while(i.le.k)
      modifiedBessel1=modifiedBessel1+((x**2/4.d0)**i)/dgamma(1.d0*i+1.d0)/dgamma(alpha+1.d0*i+1.d0)
      i=i+1
    end do
    modifiedBessel1=modifiedBessel1*((x/2.d0)**alpha)
  end if
return
end function modifiedBessel1


C	COMMON /TCALDATA/ A(6),B(6),POFFS(6),NOFFS(6)
	REAL A(6),B(6),POFFS(6),NOFFS(6),EFFLEN(4)
	INTEGER*4 CHANNEL,SPEED,TM,INDEX
	DATA B /.08316,.08151,.08099,.08212,.0834,.0834/
	DATA A /3.19E-4,2.14E-4,3.89E-4,2.40E-4,1.79E-4,2.03E-4/
	DATA POFFS /4.01,6.31, 0., 0., 0., 3.5/
	DATA NOFFS /1.59,1.79, 0., 0., 0., -6.6/
C	DATA EFFLEN /50.,7.5,5.,1./
C


c EQUISUBMU_cont evalua la presion en equilibrio hidrostatico
c_______________________________________________________________
c equisubmu rutina que evalua la presion en equilibrio hidrostatico
c teniendo en cuenta el numero de electrones para el calculo de mu
c Luis Bellot y Basilio Ruiz 27/7/95
c Cambio del metodo de integracion Basilio Ruiz 3/9/96
c _______________________________________________________________
	subroutine equisubmu_cont(ntau,tau1,t,pe,pg0,pg,z,ro)

	implicit real*4 (a-h,o-z)

	include 'PARAMETER'   !solo por kt, que era igual a 1000
	parameter (nex=28,cgases=83145100.)
	real*4 tau(kt),t(kt),pe(kt),pg(kt),kac,d2,x(kt),kappa(kt),taue(kt)
        real*4 z(kt),z1(kt),ro(*),y(kt)
        real*4 tau1(kt)
  	real*4 mu
	integer*4 nmaxitera

	real*4 wgt,abu,ei1,ei2,pp(10),tsi,psi,d1(10)
        common/constantes/g,avog	!gravedad,n. avogadro/pmu
	common/mu/cth                   !esto esta deshabilitado desde sir (entra 1)
        common/precisoitera/precitera
        common/anguloheliocent/mu
        common/nmaxitera/nmaxitera
        precitera=1.e-5
        nmaxitera=250

	g=mu*2.7414e+4		!gravedad cm/s^2 en fotosfera solar
       	avog=6.023e23

        do i=1,ntau
           tau(i)=tau1(i)+alog10(abs(cth))  !this is the optical depth in
        end do                        !the VERTICAL direction for
	                              !hydrostatic equilib. computations

        tau0=1.
        imin=1
        do i=2,ntau
           if(abs(tau1(i)).lt.tau0)then
              imin=i        !zero of z is at log tau (LOS) =0
              tau0=tau1(i)
           end if
        end do
c        print*,'imin tau(imin)=',imin,tau0,tau1(imin)

        do i=1,10
           d1(i)=0
        end do
c calculamos el peso molecular medio pmu
	pmusum=0.0
        asum=0.0
	do i=1,92
  	   ii=i
	   call neldatb(ii,0.,wgt,abu,ei1,ei2)
	   pmusum=pmusum+wgt*abu
           asum=asum+abu
	end do

	do i=1,ntau
           taue(i)=10.**tau(i)
        end do

	do i=1,ntau-1
           x(i+1)=g*(tau(i)-tau(i+1))*2.3025851
        end do

c inicializamos
	tsi=t(ntau)
        psi=pe(ntau)

        call pefrompg11(tsi,pg0,psi)
        pe(ntau)=psi
        call gasc(tsi,psi,pg(ntau),pp)
        pg(ntau)=pg0
	call kappach(5.e-5,tsi,psi,pp,d1,d1,kac,d2,d2)
        pesomedio=pmusum/(asum+pp(8)) !peso molec. medio
        kappa(ntau)=kac*avog/pmusum
        ro(ntau)=pesomedio*pg(ntau)/tsi/cgases
        y(ntau)=taue(ntau)/kappa(ntau)/ro(ntau)

c integramos
        do i=ntau-1,1,-1
           pgpr=pg(i+1)+x(i+1)*(taue(i+1)/kappa(i+1))
	   tsi=t(i)

           call pefrompg11(tsi,pgpr,psi)
           pe(i)=psi

           call gasc(tsi,psi,pgpr,pp)
	   call kappach(5.e-5,tsi,psi,pp,d1,d1,kac,d2,d2)
           kappa(i)=kac*avog/pmusum

           pg(i)=pg(i+1)+x(i+1)*( taue(i+1)/kappa(i+1) + taue(i)/kappa(i) )/2.

           nit=1
           dif=2.*abs((pg(i)-pgpr))/(pg(i)+pgpr)

           do while (nit.lt.30.and.dif.gt.1.e-5)
              nit=nit+1
              pgpr=pg(i)
              call pefrompg11(tsi,pgpr,psi)
              pe(i)=psi
              call gasc(tsi,psi,pgpr,pp)
	      call kappach(5.e-5,tsi,psi,pp,d1,d1,kac,d2,d2)
              kappa(i)=kac*avog/pmusum
              pg(i)=pg(i+1)+x(i+1)*(taue(i+1)/kappa(i+1)+taue(i)/kappa(i) )/2.
              dif=2.*abs((pg(i)-pgpr))/(pg(i)+pgpr)
            end do

            if(dif.gt.0.1) print*,'WARNING:
     & Hydrostatic equilibrium is resulting in inaccurate electronic pressures '
            call pefrompg11(tsi,pg(i),psi)

            pe(i)=psi
            ro(i)=pesomedio*pg(i)/tsi/cgases

            y(i)=taue(i)/kappa(i)/ro(i)
c	    print*,'equisubmu_cont 3',i,'pg(i) ro kappa= ',pg(i),ro(i),pesomedio,tsi,cgases,kappa(i),y(i)
	end do

        z1(1)=0.
        do i=2,ntau   !this is the z scale along the vertical direction,: NO MORE, NOW its along line of sight
           z1(i)=z1(i-1)+x(i)/g*(y(i-1)+y(i))/2.
        end do
	z00=z1(imin)

        do i=1,ntau
           z(i)=(z1(i)-z00)*1.e-5
c           print*,i,z(i)
        end do
c        print*,imin,abs(tau(imin)),z(imin)
c        do i=1,ntau   !we output geometrical heights ALONG the LOS
c          z(i)=z(i)/abs(cth)
c        enddo

       return
       end


program main
  implicit none
  real(8),parameter :: pi=4d0*atan(1d0)
  complex(8),parameter :: zI=(0d0,1d0)
  integer,parameter :: Nt = 50000, Nw = 6000
!  real(8),parameter :: ww_max = 120d0/27.211d0, dw = ww_max/Nw
  real(8),parameter :: ww_max = 300d0, dw = ww_max/Nw
  real(8)  :: Tprop = 100d0/0.02418d0
  real(8) :: tt(0:Nt),pz(0:Nt),weight(0:Nt)
  integer :: it,iw
  real(8) :: f1,f2,f3,f4,f5,f6,f7,f8,xx,ww,dt
  complex(8) :: zpzw

  do it = 0,Nt
    read(*,*)tt(it),f1,f2,f3,f4,f5,f6,f7,f8,pz(it)
  end do
  dt = tt(1) - tt(0)
  Tprop = tt(Nt)

  weight = 0d0
  do it = 0,Nt
!    if(tt(it) < 0.5d0*Tprop)then
!      weight(it) = 1d0
!    else
!      xx = (tt(it)-0.5d0*Tprop)/(tt(Nt)-0.5d0*Tprop)
      xx = (tt(it)-tt(0))/(tt(Nt)-tt(0))
!      weight(it) = 1d0 - 3d0*xx**2 + 2d0*xx**3
!      weight(it) = exp(-1d0/27.2d0*(tt(it)-Tprop))
!    end if
  end do
  weight = 1d0

  open(21,file="zpzw.out")
  do iw = 0,Nw
    ww = dw*iw
    zpzw = 0d0
    do it = 0,Nt
      zpzw = zpzw +exp(zI*ww*tt(it))*weight(it)*pz(it)
    end do
    zpzw = zpzw*dt
    write(21,"(999e26.16e3)")ww,abs(zpzw)**2
  end do
  close(21)

end program main


!##############################################################################
!# Tutorial 010a: Create a 2x2 block system with Mass and Laplace on the diag.
!##############################################################################

module tutorial010a

  ! Include basic Feat-2 modules
  use fsystem
  use storage
  use genoutput

  use triangulation
  use meshgeneration

  use derivatives
  use element
  use cubature
  use spatialdiscretisation
  use linearsystemscalar
  use linearsystemblock
  use bilinearformevaluation
  use stdoperators

  use blockmatassemblybase
  use blockmatassembly
  use blockmatassemblystdop
  use collection

  use matrixio

  implicit none
  private

  public :: start_tutorial010a

contains

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

  subroutine start_tutorial010a

    ! Declare some variables.
    type(t_triangulation) :: rtriangulation
    type(t_spatialDiscretisation) :: rspatialDiscr
    type(t_blockDiscretisation) :: rblockDiscr

    type(t_scalarCubatureInfo), target :: rcubatureInfo
    type(t_matrixScalar) :: rtemplateMatrix
    type(t_matrixBlock) :: rmatrix

    type(t_collection) :: rcollection
    character(LEN=SYS_STRLEN) :: spostdir

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

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

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

    ! =================================
    ! Create a block discretisation that encapsules
    ! two equations, both discretised with Q1.
    ! =================================

    ! Set up a Q1 discretisation
    call spdiscr_initDiscr_simple (rspatialDiscr,EL_Q1_2D,rtriangulation)

    ! Set up a block discretisation for two equations. Both equations are Q1.
    call spdiscr_initBlockDiscr (rblockDiscr,rtriangulation)
    call spdiscr_appendBlockComponent (rblockDiscr,rspatialDiscr)
    call spdiscr_appendBlockComponent (rblockDiscr,rspatialDiscr)
    call spdiscr_commitBlockDiscr (rblockDiscr)

    ! =================================
    ! Use a 3-point Gauss Formula for the assembly
    ! =================================

    call spdiscr_createDefCubStructure (rspatialDiscr,rcubatureInfo,CUB_GEN_AUTO_G3)

    ! =================================
    ! Create a 2x2 block matrix with
    ! entries in (1,1) and (2,2)
    ! =================================

    ! Create a template matrix for the FEM space. CSR structure.
    call bilf_createMatrixStructure (rspatialDiscr,LSYSSC_MATRIX9,rtemplateMatrix)

    ! Use the block discretisation to create a basic system matrix.
    call lsysbl_createMatrix (rblockDiscr,rmatrix)

    ! Create (1,1) and (2,2) using the template matrix.
    ! Structure is "shared" (no new memory is allocated), content is allocated.
    call lsysbl_duplicateMatrix (rtemplateMatrix,rmatrix,1,1,&
        LSYSSC_DUP_SHARE,LSYSSC_DUP_EMPTY)

    call lsysbl_duplicateMatrix (rtemplateMatrix,rmatrix,2,2,&
        LSYSSC_DUP_SHARE,LSYSSC_DUP_EMPTY)

    ! =================================
    ! Create Mass and Laplace.
    ! Use default block assembly routines.
    ! =================================

    ! Clear the matrix
    call lsysbl_clearMatrix (rmatrix)

    ! Put the mass matrix to (1,1).
    rcollection%DquickAccess(1) = 1.0_DP    ! Multiplier
    rcollection%IquickAccess(1) = 1         ! y-position
    rcollection%IquickAccess(2) = 1         ! x-position
    call bma_buildMatrix (rmatrix,BMA_CALC_STANDARD,bma_fcalc_mass,rcollection,&
        rcubatureInfo=rcubatureInfo)

    ! Put the Laplace matrix to (2,2)
    rcollection%DquickAccess(1) = 1.0_DP    ! Multiplier
    rcollection%IquickAccess(1) = 2         ! y-position
    rcollection%IquickAccess(2) = 2         ! x-position
    call bma_buildMatrix (rmatrix,BMA_CALC_STANDARD,bma_fcalc_laplace,rcollection,&
        rcubatureInfo=rcubatureInfo)

    ! =================================
    ! Output of the matrix structure
    ! =================================
    call output_line ("Writing matrix to text files...")

    if (sys_getenv_string("POSTDIR",spostdir)) then
      ! Write the matrix to a text file, omit nonexisting entries in the matrix.
      call matio_writeBlockMatrixHR (rmatrix, "matrix", .true., 0, &
          trim(spostdir)//"/tutorial010a_matrix.txt", "(E11.2)")

      ! Write the matrix to a MATLAB file.
      call matio_spyBlockMatrix(&
          trim(spostdir)//"/tutorial010a_matrix.m","matrix",rmatrix,.true.)

      ! Write the matrix to a MAPLE file
      call matio_writeBlockMatrixMaple (rmatrix, "matrix", 0, &
          trim(spostdir)//"/tutorial010a_matrix.maple", "(E11.2)")
    else
      ! Write the matrix to a text file, omit nonexisting entries in the matrix.
      call matio_writeBlockMatrixHR (rmatrix, "matrix", .true., 0, &
          "./post/tutorial010a_matrix.txt", "(E11.2)")

      ! Write the matrix to a MATLAB file.
      call matio_spyBlockMatrix(&
          "./post/tutorial010a_matrix.m","matrix",rmatrix,.true.)

      ! Write the matrix to a MAPLE file
      call matio_writeBlockMatrixMaple (rmatrix, "matrix", 0, &
          "./post/tutorial010a_matrix.maple", "(E11.2)")
    end if

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

    ! Release the cubature formula
    call spdiscr_releaseCubStructure (rcubatureInfo)

    ! Release the matrix
    call lsysbl_releaseMatrix (rmatrix)

    ! ... and the FEM template matrix
    call lsyssc_releaseMatrix (rtemplateMatrix)

    ! Release the block discretisation
    call spdiscr_releaseBlockDiscr (rblockDiscr)

    ! Release the Q1-discretisation
    call spdiscr_releaseDiscr (rspatialDiscr)

    ! Release the triangulation
    call tria_done (rtriangulation)

  end subroutine

end module


PROGRAM calc_pebble_accretion
  ! Program reads in output from sgd_grid
  ! Computes properties of the disc related to pebble accretion:
  ! i) Where streaming instability may occur, and
  ! ii) Pebble accretion rates of fragments
  ! Assumes either a fixed grain size or fixed dimensionless stopping time

  use eosdata
  implicit none

  integer :: irad,imdot,irrchoice
  integer :: nrad, nmdot

  real :: mdotmax,mdotmin, mdotvisc, metallicity, mstar
  real :: qratio, T, gamma_sigma,gamma_omega
  real :: Q_irr, T_irr,dr, rmax,rmin, betac, vrgas, mpebble
  real :: gapcriterion, tcross,tgap,tmig1,aspectratio,dustaspectratio,mratio
  real :: tstop_turb, mturb,maxgrow, miso_peb, effpeb, dv_drift, sigma_deltav
  real :: percentcount, displaypercent,increment

  real, parameter :: mu = 2.4

  real, parameter :: gamma1 = 1.0
  real, parameter :: vfrag = 1.0e3 ! Empirically determined fragmentation velocity (cm/s)

  real, parameter :: alpha_max = 1.0e20 ! Maximum turbulence for streaming instability

  integer, allocatable,dimension(:) :: stream_unstable
  real, allocatable, dimension(:) :: r, sigma,cs,omega,alpha,mjeans,Hp_to_Hg,rhop_rhog
  real, allocatable, dimension(:) :: H, eta, vrpeb, rhogas,etadash,tstop,grainsize
  real,allocatable,dimension(:) :: tstop_frag,maxgrainsize, sigma_peb, sigma_peb_max,mpebtot

  integer :: ipebrad,npebrad, irmin_unstable,irmax_unstable
  real :: zpeb, fpeb,beta_peb, rmax_peb, mplanet, Hp, dlogrhodr
  real :: grainsize_in,tstop_in, rhosolid,mfp, actual_width

  real :: rpeb, rdotpeb, tpeb, mdotpebble, rhill
  real :: rmin_unstable, rmax_unstable, width_unstable, h_unstable
  real :: bcross_reduce, bcross, rhill_reduce, zeta, Chi
  real :: planet_pebaccrete, mcross, eff_pebble
  real :: rpeb_accretemax, tpeb_accretemax, mdotpebble_accretemax, mjeans_accretemax
  real :: planet_accretemax, mcross_accretemax, eff_accretemax

  logical :: inner_radius
  logical :: outer_radius

  character(1) :: fixparam
  character(100) ::inputfile, inputprefix,outputfile, outputlog

  ! Get input parameters and set up header:
  print*, " "
  print*, "-----------------------------------------------"
  print*, " PEBBLE ACCRETION IN SELF-GRAVITATING DISC MODELS "
  print*, "     Created by D. Forgan, Dec 2017            "
  print*, " (Relies on input from sgd_grid) "
  print*, "-----------------------------------------------"
  print*, " "
  print*, " "
  print*, " input files: ./calc_pebble_accretion.params"
  print*, " "
  print*, "-----------------------------------------------"
  print*, " "

  OPEN(10,file='calc_pebble_accretion.params', status='unknown')
  read(10,*) inputprefix
  read(10,*) fixparam ! Fix either tstop or grain size
  READ(10,*) tstop_in ! Input Stopping time (dimensionless)
  read(10,*) grainsize_in ! Input Grain size (cm)
  read(10,*) rhosolid ! Density of grains (g cm ^-3)
  read(10,*) zpeb ! Metallicity of disc
  read(10,*) fpeb ! Fraction of disc in pebbles
  read(10,*) effpeb ! Pebble accretion efficiency
  read(10,*) beta_peb ! growth rate of pebbles t_peb = beta_peb *(zpeb*omega(irad)^-1
  read(10,*) rmax_peb ! Maximum radius to consider pebble accretion  (AU)
  close(10)

  inputfile = trim(inputprefix)//'.sgdmodel'
  print*, 'Reading file ', inputfile
  outputfile = trim(inputprefix)//'.pebble'
  print*, 'Outputting pebble accretion data to file ', outputfile

  outputlog = trim(outputfile)//".max"
  print*, "Maximal pebble accretion data per gas accretion rates to be written to ", trim(outputlog)

  !******************************************
  ! 1. Read in disc and recompute essential gas  properties
  !******************************************

  OPEN(10,file=inputfile ,status='unknown')

  ! Read input file header

  read(10,*) nrad, nmdot, rmin,rmax,mdotmin,mdotmax,Mstar, metallicity,&
       gamma_sigma,gamma_omega,irrchoice,Q_irr,T_irr

  mstar = mstar*umass
  mplanet = mplanet*mearth

  print*, 'There are ', nrad, ' radii in the input file'

  allocate(r(nrad))
  allocate(sigma(nrad))
  allocate(cs(nrad))
  allocate(H(nrad))
  allocate(Hp_to_Hg(nrad))
  allocate(rhop_rhog(nrad))
  allocate(sigma_peb(nrad))
  allocate(mpebtot(nrad))
  allocate(sigma_peb_max(nrad))
  allocate(rhogas(nrad))
  allocate(omega(nrad))
  allocate(alpha(nrad))
  allocate(mjeans(nrad))
  allocate(eta(nrad))
  allocate(etadash(nrad))
  allocate(vrpeb(nrad))
  allocate(stream_unstable(nrad))
  allocate(grainsize(nrad))
  allocate(tstop(nrad))
  allocate(tstop_frag(nrad))
  allocate(maxgrainsize(nrad))

  ! Find index of maximum radius for pebble accretion (npebrad)
  dr = (rmax-rmin)/REAL(nrad)
  dr = dr*udist
  npebrad = nrad

  if(npebrad > nrad) npebrad = nrad

  ! Write header for main output file
  open(30, file=outputfile, status='unknown')
  write(30,*) npebrad,nmdot,rmin,rmax,Mstar,fixparam,zpeb,beta_peb,&
       metallicity,gamma_sigma,gamma_omega,irrchoice,Q_irr,T_irr

  ! Write header for global maxima output file
  OPEN(20,file=outputlog,status='unknown')
  write(20,*) nrad,nmdot,rmin,rmax,mdotmin,mdotmax,Mstar,&
       tstop,zpeb,fpeb,beta_peb,&
       metallicity,gamma_sigma,gamma_omega,irrchoice,Q_irr,T_irr

  !*******************************************************************
  ! 1 Compute disc properties needed for pebble accretion calculations
  !********************************************************************

  increment = 10.0
  displaypercent = 10.0

  ! Loop over accretion rates
  DO imdot = 1, nmdot

     percentcount = real(imdot)*100.0/real(nmdot)
     if(percentcount > displaypercent) then
        print('(F5.0,A)'), displaypercent, '% complete'
        displaypercent = displaypercent +increment
     endif

     r(:) = 0.0
     sigma(:) = 0.0
     cs(:) = 0.0
     H(:) = 0.0
     Hp_to_Hg(:) = 0.0
     rhop_rhog(:) = 0.0
     rhogas(:) = 0.0
     omega(:) = 0.0
     alpha(:) = 0.0
     eta(:) = 0.0
     vrpeb(:) = 0.0
     mjeans(:) = 0.0
     stream_unstable(:) = 0

     inner_radius = .false.
     outer_radius = .false.
     rmin_unstable = 0.0
     rmax_unstable = 0.0
     irmin_unstable = 0
     irmax_unstable = 0
     width_unstable = 0.0

     ! Fill tstop, grainsize arrays with default values (changed later)
     tstop(:) = tstop_in
     grainsize(:) = grainsize_in
     tstop_frag(:) = 0.0
     maxgrainsize(:) = 0.0

     ! Read entire disc at this accretion rate
     DO irad = 1,nrad

        read(10,*) r(irad), mdotvisc, qratio, sigma(irad), cs(irad),omega(irad),&
             T,betac,alpha(irad), mjeans(irad)

        ! Be careful with units here
        H(irad) = (cs(irad)/omega(irad)) ! in units of cm
        rhogas(irad) = sigma(irad)/(2.0*H(irad))
        r(irad) = r(irad)*udist

        mpebtot(irad) = fpeb*zpeb*mstar*qratio

        if(irad>1) then
           sigma_peb_max(irad) = (mpebtot(irad)-mpebtot(irad-1))/(twopi*r(irad)*dr)
        else
           sigma_peb_max(irad) = 0.0
        endif

     enddo

     !print*, 'Total pebble mass: ',mpebtot(nrad)/mearth ,' Earth masses'

     mpebble = 0.0
     do irad = 1,nrad

        !**********************************************************************
        ! 1a  Calculate pressure gradient and eta - sub Keplerian parameter
        !**********************************************************************

        if(irad>1) then
           dlogrhodr = (log(rhogas(irad)*cs(irad)*cs(irad)) - log(rhogas(irad-1)*cs(irad-1)*cs(irad-1)))/&
                (log(r(irad) - log(r(irad-1))))
        else
           dlogrhodr = 0.0
        endif

        eta(irad) = abs(0.5*(H(irad)*H(irad))/(r(irad)*r(irad)) * dlogrhodr)

        !*********************************************************************
        ! 1b Compute either grain size or stopping time depending on inputs
        !**********************************************************************

        ! Compute mean free path
        mfp = sqrt(twopi)*mu*mH*H(irad)/(coll_H*sigma(irad))

        ! Compute maximum stopping time (from grain fragmentation)
        ! (see Birnstiel et al 2009, A&A, 503, L5-L8)

        tstop_frag(irad) = vfrag*vfrag/(alpha(irad)*cs(irad)*cs(irad))
        call calc_grainsize(maxgrainsize(irad),mfp,rhosolid,sigma(irad),tstop_frag(irad))

        ! If stopping time fixed
        if(fixparam=='t') then

           if(tstop(irad)>tstop_frag(irad)) tstop(irad) = tstop_frag(irad)
           call calc_grainsize(grainsize(irad),mfp,rhosolid,sigma(irad),tstop(irad))

           ! Otherwise if grain size fixed
        else

           call calc_tstop(tstop(irad),mfp,rhosolid,sigma(irad),grainsize(irad))

           ! if tstop > tstop_frag, must recompute both tstop and grainsize
           if(tstop(irad) > tstop_frag(irad)) then
              tstop(irad) = tstop_frag(irad)
              call calc_grainsize(grainsize(irad),mfp,rhosolid,sigma(irad),tstop(irad))
           endif

        endif

        ! Now we have tstop, compute radial velocity of pebbles

        Chi = sqrt((1.0 + 4.0*tstop(irad)*tstop(irad)))/(1.0+tstop(irad)*tstop(irad))

        ! Compute etadash function (for pebble accretion rates)
        etadash(irad) = Chi*eta(irad)

        ! Pebble radial velocity = drift velocity + viscous radial velocity
        ! (Takeuchi & Lin 2002)

        ! Drift velocity first
        vrpeb(irad) = 2.0*eta(irad)*omega(irad)*r(irad)*tstop(irad)/(1.0 + tstop(irad)*tstop(irad))

        ! now gas viscous radial velocity

	if(irad>1) then
        vrgas = (sqrt(r(irad)*alpha(irad)*cs(irad)*H(irad)*sigma(irad)-  &
                sqrt(r(irad-1)*alpha(irad-1)*cs(irad-1)*H(irad-1)*sigma(irad-1))))/&
                (r(irad)-r(irad-1))
	else
	vrgas = 0.0
	endif

        vrgas = -3.0*vrgas/(sqrt(r(irad))*sigma(irad))

        vrpeb(irad) = abs(vrgas/(1.0+tstop(irad)*tstop(irad)) + vrpeb(irad))

        rpeb = r(irad)

        !**************************************************
        ! 1c Compute susceptibility to streaming instability
        !***************************************************

        ! Need to compute mdotpebble first to do this here

        ! Pebble growth timescale at this radius
        tpeb = beta_peb/(zpeb*omega(irad))

        ! Pebble front growth rate
        rdotpeb = 0.6666*(G*Mstar*zpeb*zpeb/(tpeb*beta_peb*beta_peb))**(0.333)

        ! Compute mdotpebble (g s^-1)
        mdotpebble = 2.0*pi*rpeb*rdotpeb*zpeb*fpeb*sigma(irad)

        ! Scale height ratio of dust to gas (Dubrulle et al 1995)
        Hp_to_Hg(irad) = sqrt(alpha(irad)/(alpha(irad)+tstop(irad)))

        ! Surface density of pebbles
        if(vrpeb(irad)>1.0e-30) then
           sigma_peb(irad) = (rdotpeb/vrpeb(irad))*zpeb*fpeb*sigma(irad)
        else
           sigma_peb(irad) = 0.0
        endif

        ! Ensure surface density of pebbles can't exceed local maximum
        if(sigma_peb(irad)>sigma_peb_max(irad))then
           sigma_peb(irad) = sigma_peb_max(irad)
        endif

        if(Hp_to_Hg(irad)*sigma(irad) >1.0e-30) then
           ! Ratio of dust to gas density at this radius
           rhop_rhog(irad) = sigma_peb(irad)/(sigma(irad)*Hp_to_Hg(irad))
        else
           rhop_rhog(irad) = 0.0
        endif

        ! Double check if total pebble supply used up - compute pebble mass generated so far
        mpebble = mpebble + twopi*r(irad)*sigma_peb(irad)*dr

        ! If pebble supply exhausted, then set pebble density to zero
        if(mpebble>(mpebtot(nrad)/fpeb)) then
           rhop_rhog(irad) = 0.0
           Hp_to_Hg(irad) = 0.0
        endif

        ! If rhop/rhog >1 and not in fragmentation zone,
        ! mark this radius as streaming unstable region
        if(rhop_rhog(irad)>1.0 .and.mjeans(irad)<1.0e-30 .and.alpha(irad)<alpha_max) then
           stream_unstable(irad) = 1

           ! Record minimum and maximum values of streaming regions
           ! (assuming single region only)
           ! If in a fragmentation zone, curtail streaming region

           if(inner_radius.eqv..false.) then
              rmin_unstable = r(irad)
              irmin_unstable = irad
              inner_radius = .true.
           endif
        else
           if(irad>1) then
              if(stream_unstable(irad-1)==1) then
                 rmax_unstable = r(irad)
                 irmax_unstable = irad
                 outer_radius = .true.
                 width_unstable = rmax_unstable-rmin_unstable
              endif
           endif
           endif

        enddo

       ! print*, rmin_unstable/udist, rmax_unstable/udist, outer_radius,sum(stream_unstable)>0,&
       !      (outer_radius .eqv. .false.) .and. sum(stream_unstable)>0

        if((outer_radius .eqv. .false.) .and. sum(stream_unstable)>0) then
           rmax_unstable = r(nrad)
           irmax_unstable = nrad
           width_unstable = rmax_unstable - rmin_unstable
        endif

        !print*, rmin_unstable/udist, rmax_unstable/udist
        !if(sum(stream_unstable)<1) STOP

     !******************************************************************
     ! 2. Now compute pebble accretion properties as a function of rpeb
     !******************************************************************

     ! Variables store maximum values at this gas accretion rate
     planet_accretemax = 0.0
     mjeans_accretemax = 0.0
     mdotpebble_accretemax = 0.0
     rpeb_accretemax = 0.0
     tpeb_accretemax = 0.0
     mcross_accretemax = 0.0
     eff_accretemax = 0.0

     ! convert mdot into cgs
     mdotvisc = mdotvisc*umass/year

     ! Now loop again over radius, where radius now refers to rpeb

     do ipebrad =1,npebrad

        rpeb = r(ipebrad)

        !**************************************
        ! 2 Calculate growth rate of pebble front
        !**************************************

        ! Pebble growth timescale at this radius
        tpeb = beta_peb/(zpeb*omega(ipebrad))

        ! Pebble front growth rate
        rdotpeb = 0.6666*(G*Mstar*zpeb*zpeb/(tpeb*beta_peb*beta_peb))**(0.333)

        ! Compute mdotpebble (g s^-1)
        mdotpebble = 2.0*pi*rpeb*rdotpeb*zpeb*fpeb*sigma(ipebrad)

        ! Initialise parameters for calculation
        planet_pebaccrete = 0.0
        eff_pebble = 0.0
        mcross = 0.0
        mturb = 0.0
        h_unstable = 0.0

        !*************************************************************
        ! 3 Compute the pebble accretion rate of any fragment present
        ! (according to equations of Ida et al (2016)
        !*************************************************************

        ! Compute some useful expressions out front
        Hp = Hp_to_Hg(ipebrad)*H(ipebrad)
        aspectratio = H(ipebrad)/r(ipebrad)
        dustaspectratio = Hp/r(ipebrad)
        mratio = mjeans(ipebrad)*mjup/mstar

        ! Pebble isolation mass
        miso_peb = (dustaspectratio*dustaspectratio*dustaspectratio)*mstar

        if (mjeans(ipebrad)>0.0) then

           ! Convenience functions for calculating pebble accretion
           ! (Ida et al 2016)

           zeta = 1/(1+tstop(ipebrad)*tstop(ipebrad))
           Chi = sqrt((1.0 + 4.0*tstop(ipebrad)*tstop(ipebrad)))/(1.0+tstop(ipebrad)*tstop(ipebrad))

           ! Compute Hill Radius
           rhill_reduce = (mjeans(ipebrad)*mjup/(3.0*mstar))**0.3333
           rhill = rhill_reduce*r(ipebrad)

           !***********************************************************
           ! Does fragment drive a gap in pebbles? If so, no accretion
           !***********************************************************

           gapcriterion = 0.75*Hp/rhill + &
                50.0*alpha(ipebrad)*dustaspectratio**2/mratio

           ! Also check that migration slow enough to allow gap formation

           ! Type I migration timescale (Ormel et al 2017)
           tmig1 = mstar*aspectratio*aspectratio/(gamma1*mratio*sigma(ipebrad)*r(ipebrad)*r(ipebrad)*omega(ipebrad))

           ! Gap crossing timescale
           tcross = 2.5*rhill*tmig1/r(ipebrad)

           ! Gap formation timescale
           tgap = 1.0e2*(dustaspectratio**5)/(omega(ipebrad)*mratio*mratio)

           ! If a gap is opened in the pebbles, then no pebble accretion
           if(gapcriterion<1.0 .and. tgap< tcross) then
              planet_pebaccrete = 0.0

           else

              ! Compute cross section of pebble flow for accretion

              bcross_reduce = 3*tstop(ipebrad)**0.333*rhill_reduce/etadash(ipebrad)

              if(bcross_reduce>1.0) bcross_reduce = 1.0

              bcross_reduce = bcross_reduce*2.0*tstop(ipebrad)**0.333*rhill_reduce
              bcross = bcross_reduce*r(ipebrad)

              ! Now compute disc fragment's pebble accretion rate

              planet_pebaccrete = sqrt(8.0/pi)*Hp/bcross

              if(planet_pebaccrete>1.0) planet_pebaccrete = 1.0

              sigma_deltav =  mdotpebble/(4.0*pi*r(ipebrad)*tstop(ipebrad)*zeta)
              sigma_deltav = sigma_deltav*Chi*(1.0+ 3*bcross_reduce/(2*Chi*eta(ipebrad)*r(ipebrad)))

              dv_drift = sigma_deltav/sigma_peb(ipebrad)

              planet_pebaccrete = sqrt(pi/2)*(bcross*bcross/Hp)*sigma_deltav

              if(planet_pebaccrete>mdotpebble) planet_pebaccrete = mdotpebble

              ! If the fragment can open a gap *in the pebbles*, i.e. mjeans > miso
              ! then dv is given by migration rate, not pebble drift
              ! must modify pebble accretion accordingly

              if(mratio>dustaspectratio**3) then
                 planet_pebaccrete = planet_pebaccrete*(1.0- r(ipebrad)/(tmig1*dv_drift))
                 !print*, mratio, dustaspectratio**3, &
                 !     (1.0- r(ipebrad)/(tmig1*dv_drift))
                 if (planet_pebaccrete<0.0) planet_pebaccrete = 0.0
              endif

           endif
           ! measure pebble accretion efficiency
           eff_pebble = planet_pebaccrete/mdotpebble

        endif

        !****************************************************
        ! 4 compute crossing radius for planetesimal growth
        !****************************************************

        mcross = 0.0
        actual_width = 0.0

        ! Only do the calculations for non-zero widths
        ! Do calculation assuming ipebrad = inner distance of streaming
        if(width_unstable > 1.0e-40) then
           if(r(ipebrad)>rmin_unstable .and. r(ipebrad)<rmax_unstable) then

           actual_width = rmax_unstable-r(ipebrad)
           h_unstable = H(ipebrad)/r(ipebrad)

           ! Compute crossing mass at this rpeb (Ormel et al 2017)
           ! with our imposed pebble accretion efficiency (effpeb parameter)

           mcross = sqrt(3.0*pi*actual_width*alpha(ipebrad)*mdotpebble*effpeb/&
                (rmin_unstable*mdotvisc*gamma1))*h_unstable * h_unstable* Mstar

           ! If crossing mass exceeds local isolation mass, restrict it here

           if(mcross > miso_peb) mcross = miso_peb

           ! If entire pebble disc required to create embryo,
           ! ensure mass cannot exceed total pebble mass (at given efficiency)

           if(mcross > mpebtot(nrad)*effpeb) then
              !print*, 'Pebble disc mass exceeded: ',mpebtot(nrad)*effpeb/mearth
              mcross = mpebtot(nrad)*effpeb
           endif

           ! Compute maximum growable mass in turbulent disc

           ! Find maximum allowed stopping time for bidisperse grain population
           ! (Booth & Clarke 2016, MNRAS, 458, 2676)

           tstop_turb = vfrag/(cs(ipebrad)*4.47*sqrt(alpha(ipebrad))) + tstop(ipebrad)

           ! Compute mean free path
           mfp = sqrt(twopi)*mu*mH*H(ipebrad)/(coll_H*sigma(ipebrad))

           call calc_grainsize(maxgrow,mfp,rhosolid,sigma(ipebrad),tstop_turb)

           ! maximum turbulent mass
           mturb = 4.0*pi*maxgrow*maxgrow*maxgrow*rhosolid

           !print*, mcross/mjup, miso_peb/mjup, dustaspectratio, aspectratio

           if(mturb>1.0e7) then
              print*, ipebrad, mturb/1.0e3, maxgrow, tstop_turb, tstop(ipebrad), tstop_turb/tstop(ipebrad)
           endif
           !write(*,'(11(1P,e8.1,1X))'), mcross/mjup, &
           !     actual_width/udist, gamma1,h_unstable,mdotpebble/mdotvisc, &
           !     alpha(irmin_unstable), mdotpebble, effpeb, mdotvisc, Mstar/umass

           endif
        endif

        !****************************
        ! 4. Write output data to files
        !****************************

        !print*, r(ipebrad)/udist, Hp_to_Hg(ipebrad)*H(ipebrad)/r(ipebrad)
        ! Check if fragment pebble accretion rate is at a maximum
        ! Record maximal values

        if(planet_pebaccrete > planet_accretemax) then
           planet_accretemax = planet_pebaccrete
           mjeans_accretemax = mjeans(ipebrad)
           mdotpebble_accretemax = mdotpebble
           rpeb_accretemax = rpeb
           tpeb_accretemax = tpeb
           mcross_accretemax = mcross
           eff_accretemax = eff_pebble
        endif

        ! Write to main output file
        write(30,*) mdotvisc*year/umass, rpeb/udist,grainsize(ipebrad),tstop(ipebrad),&
             tstop(ipebrad)/tstop_frag(ipebrad),maxgrainsize(ipebrad),tpeb/year, &
             rdotpeb*year/udist, mdotpebble*year/mjup,  &
             Hp_to_Hg(ipebrad), rhop_rhog(ipebrad),vrpeb(ipebrad),&
             actual_width/udist, rmin_unstable/udist, &
             mcross/mearth, maxgrow, miso_peb/mearth, mjeans(ipebrad),planet_pebaccrete*year/mjup, eff_pebble

     enddo

     ! Now write to global maxima file for this mdot

     write(20,*) mdotvisc*year/umass, rpeb_accretemax/udist, tpeb_accretemax/3.15e7, &
          mdotpebble_accretemax*year/mjup, mcross_accretemax/mearth, &
          mjeans_accretemax, planet_accretemax*year/mjup, &
          eff_accretemax

  enddo

  close(20)
  close(30)

end PROGRAM calc_pebble_accretion

subroutine calc_tstop(tstop,mfp,rhosolid,sigmagas,grainsize)
  ! Calculates the stopping time for Epstein and Stokes drag regimes
  ! (Ida et al 2016)
  use eosdata, only: roottwopi
  implicit none

  real, intent(in) :: mfp,rhosolid,sigmagas,grainsize
  real, intent(inout) :: tstop

  if(grainsize<9.0*mfp/4.0) then
     tstop = roottwopi*rhosolid*grainsize/sigmagas
  else
     tstop = 4.0*roottwopi*rhosolid*grainsize*grainsize &
          /(9.0*mfp*sigmagas)
  endif

end subroutine calc_tstop

subroutine calc_grainsize(grainsize,mfp,rhosolid,sigmagas,tstop)
  ! Calculates the grain size for a given stopping time
  ! (Ida et al 2016)

  use eosdata, only: roottwopi
  implicit none

  real, intent(in) :: mfp,rhosolid,sigmagas,tstop
  real, intent(inout) :: grainsize

  ! Calculate assuming Epstein drag, then check
  grainsize = tstop*sigmagas/(roottwopi*rhosolid)

  ! If in Stokes regime, recompute
  if(grainsize >= 9.0*mfp/4.0) then
     grainsize = 9.0*mfp*sigmagas*tstop/(4.0*roottwopi*rhosolid)
     grainsize = sqrt(grainsize)
  endif

end subroutine calc_grainsize


!
!     CalculiX - A 3-dimensional finite element program
!              Copyright (C) 1998-2018 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
!     d{K(X)}/dTeint
!
!     author: Yannick Muller
!
      subroutine dKdt(x,u,uprime,rpar,ipar)
!
      implicit none
      integer ipar
      real*8 x,u(1),uprime(1),rpar(*),zk0,phi,Tup,
     &     xflow,Pup,f1_x,K_x,lambda1,df1dk,Rurd,f_k,kup
!
      external f_k
!
!     defining the parameters
      phi=rpar(1)
      lambda1=rpar(2)
      zk0=rpar(3)
      Pup=rpar(4)
      Tup=rpar(5)
      rurd=rpar(6)
      xflow=rpar(7)
      kup=rpar(8)
!
!     find K(X) for the given x

      k_x=f_k(x,phi,lambda1,zk0,Pup,Tup,rurd,xflow,kup)
!
      k_x=dsqrt(K_x/x)
!
!     f1_x
      f1_x= (zk0*K_x)**(7.d0/4.d0)
     &     -(1-K_x)/dabs(1-K_x)*dabs(1-K_x)**(7d0*4d0)
!
!     df1dK
      df1dK=7d0/4d0*zk0**(7d0/4d0)*K_x**(3.d0/4.d0)
     &     +7d0/4d0*dabs(1-K_x)**(3.d0/4.d0)
!
!
      uprime(1)=-0.8d0*x**1.6d0*lambda1*Pup**(0.8d0)
     &     /(xflow*Tup**1.8d0)*f1_x+u(1)
     &     *(lambda1*x**1.6d0*Pup**0.8d0/(xflow*Tup**0.8d0)
     &     *df1dK-2/x)
!
      return
!
      end


program main
    use std
    implicit none

    type(LinearSolver_) :: app
    type(time_) :: time
    integer(int32) :: i

    call time%start()

    ! set matrix (only non-zero value)
    call app%set(init=.true.)
    call app%set(1,1, entryvalue=  1.0d0)
    call app%set(2,2, entryvalue=  2.0d0)
    call app%set(2,3, entryvalue= -1.0d0)
    call app%set(3,2, entryvalue= -1.0d0)
    call app%set(3,3, entryvalue=  2.0d0)
    call app%set(4,4, entryvalue=  1.0d0)

    ! set Right-hand side vector (only non-zero value)
    call app%set(1, entryvalue=  0.5d0)
    call app%set(2, entryvalue=  0.5d0)
    call app%set(3, entryvalue=  0.2d0)
    call app%set(4, entryvalue=  0.2d0)

    ! solve!
    call app%solve(Solver="BiCGSTAB",CRS=.true.)

    ! show result
    print *, app%x

    call time%show()

end program


double precision function invfn (x, qn, mode)
    implicit none
    integer, intent (in) :: qn, mode
    double precision, intent (in) :: x
    double precision :: x0, x1, ans0, ans1, new
    intrinsic Abs
    ans0 = 2.0d0
    x0 = fn(ans0, qn, mode) - x
    ans1 = ans0 * 1.1d0
    do
      x1 = fn(ans1, qn, mode) - x
      if (Abs (x0-x1) < 1.d-10) exit
      new = ans0 + x0 * (ans1-ans0) / (x0-x1)
      if (Abs(x1) < 1.d-10) exit
      x0 = x1
      ans0 = ans1
      ans1 = new
    end do
    invfn = new
contains
    double precision function fn (x, qn, mode)
   !
   !.. Implicit Declarations ..
      implicit none
   !
   !.. Formal Arguments ..
      integer, intent (in) :: qn, mode
      double precision, intent (in) :: x
   !
   !.. External Calls ..
      double precision, external :: rsc
      select case (mode)
      case (0)
    !
    !  Calculate Gss given the principal quantum number
    !  and Slater exponent for the "s" orbital.
    !
        fn = rsc (0, qn, x, qn, x, qn, x, qn, x)
      case (1)
    !
    !  Calculate Gpp given the principal quantum number
    !  and Slater exponent for the "p" orbital.
    !
        fn = rsc (0, qn, x, qn, x, qn, x, qn, x) + 0.16d0 * rsc (2, qn, x, qn, &
       & x, qn, x, qn, x)
      case (2)
    !
    !  Calculate Gdd given the principal quantum number
    !  and Slater exponent for the "d" orbital.
    !
        fn = rsc (0, qn, x, qn, x, qn, x, qn, x) + 4.d0 / 49.d0 * (rsc (2, qn, &
       & x, qn, x, qn, x, qn, x)+rsc (4, qn, x, qn, x, qn, x, qn, x))
      end select
      return
    end function fn
end function invfn