    !Lensing the C_l using the deflection angle from the computed lensing potential
    !power spectrum.
    !lensing_method=1: using an accurate curved-sky correlation function method
    !lensing_method=2: using the flat-sky lower order result of astro-ph/9505109
    !                  and astro-ph/9803150 as in CMBFAST
    !lensing_method=3: using inaccurate full sky harmonic method of astro-ph/0001303

    !The flat sky result is accurate to about 0.1% in TT, and 0.4% in EE and is
    !about a factor of two faster than lensing_method=1.
    !lensing_method=3 is only present for comparison and is not recommended in any regime

    !Set accurate_BB=T if you want BB accurately by integrating the full angular range
    !otherwise it saves a large amount of time by only integrating the small scales
    !accute_BB only does *not* include any non-linear corrections or ensure you have
    !chosen sufficiently high l_max and k_max, so does not neccessarily give an accurate
    !result

    !Uses the un-lensed Cls and the computed lensing potential power spectrum.
    !Usual values of k_max are fine for all but the lensed BB Cls
    !To get the lensed BB accurate around l=1000 you need to go to l_max >2000, and
    !higher for higher l. Since this probes small scales in the lensing power spectrum you
    !also need to go to higher k_max - for concordance models something like
    !k_eta_max_scalar=10000. At l>1000 you can expect to need higher k_max, and for
    !non-linear evolution to cause a significant error.

    !Correlation function routines by AL+AC Nov 2004 with flat-sky borrowings from CMBFAST
    !Curved sky results use the method of astro-ph/xxx.

    !Full sky harmonic lensing routines by Gayoung Chon and AC.
    !Ref: astro-ph/0001303 by W. Hu.
    !For better derivations see also astro-ph/0301064 and astro-ph/0301031
    !Adapted for CAMB and optimized by AL.
    !Uses f90 version of "J1-RECURSION OF 3J-COEFFICIENTS" by K. Schulten and R.G. Gordon
    !obtainable from the CPC program library (www.cpc.cs.qub.ac.uk).

    !March 2006: fixed problem with l_max when generating with tensors (thanks Chad Fendt)

    module lensing
    use Precision
    use ModelParams
    use AmlUtils
    implicit none
    integer, parameter :: lensing_method_curv_corr=1,lensing_method_flat_corr=2, &
        lensing_method_harmonic=3

    integer :: lensing_method = lensing_method_curv_corr

    real(dl) :: lensing_sanity_check_amplitude = 1e-7

    real(dl) :: ALens_Fiducial = 0._dl
    !Change from zero to set lensing smoothing by scaling amplitude of fiducial template

    private

    logical  :: lensing_includes_tensors = .false.

    !flat method stores
    real(dl), parameter :: dbessel = 0.05_dl
    real(dl), dimension(:), allocatable :: Bess0, ddBess0
    real(dl), dimension(:), allocatable :: Bess2, ddBess2
    real(dl), dimension(:), allocatable :: Bess4, ddBess4
    real(dl), dimension(:), allocatable :: Bess6, ddBess6

    integer, parameter :: lensed_convolution_margin = 100
    !Number of L less than L max at which the lensed power spectrum is calculated

    !Harmonic method stores
    integer :: lmax_donelnfa = 0
    real(dl), dimension(:), allocatable  :: lnfa

    public lens_Cls, lensing_includes_tensors, lensing_method, lensing_method_flat_corr,&
        lensing_method_curv_corr,lensing_method_harmonic, BessI, bessj0, ALens_Fiducial, &
        lensing_sanity_check_amplitude
    contains

    subroutine lens_Cls
    use lvalues

    !Must set l again in case computed tessors (thanks to Chad)
    call initlval(lSamp,CP%Max_l)
    if (lensing_method == lensing_method_curv_corr) then
        call CorrFuncFullSky()
    elseif (lensing_method == lensing_method_flat_corr) then
        call CorrFuncFlatSky()
    elseif (lensing_method == lensing_method_harmonic) then
        call BadHarmonic
    else
        error stop 'Unknown lensing method'
    end if
    end subroutine lens_Cls

    subroutine CorrFuncFullSky

    integer :: lmax_extrap

    lmax_extrap = CP%Max_l - lensed_convolution_margin + 450
    if (HighAccuracyDefault) lmax_extrap=lmax_extrap+300
    lmax_extrap = min(lmax_extrap_highl,lmax_extrap)
    call CorrFuncFullSkyImpl(max(lmax_extrap,CP%max_l))

    end subroutine CorrFuncFullSky

    subroutine CorrFuncFullSkyImpl(lmax)
    !Accurate curved sky correlation function method
    !Uses non-perturbative isotropic term with 2nd order expansion in C_{gl,2}
    !Neglects C_{gl}(theta) terms (very good approx)
    use ModelParams
    use ModelData
    use lvalues
    implicit none
    integer, intent(in) :: lmax
    integer l, i, in
    integer :: npoints
    real(dl) corr(4), Cg2, sigmasq, theta
    real(dl) dtheta
    real(dl) llp1,fac, fac1,fac2,fac3, rootllp1, rootfac1, rootfac2, rootfac3
    integer max_lensed_ix
    real(dl) P(lmax),dP(lmax)
    real(dl) sinth,halfsinth, x, T2,T4
    real(dl) roots(-1:lmax+4), lfacs(lmax), lfacs2(lmax), lrootfacs(lmax)
    real(dl) d_11(lmax),d_m11(lmax)
    real(dl) d_22(lmax),d_2m2(lmax),d_20(lmax)
    real(dl) Cphil3(lmin:lmax), CTT(lmin:lmax), CTE(lmin:lmax),CEE(lmin:lmax)
    real(dl) ls(lmax)
    real(dl) xl(lmax)
    real(dl), allocatable :: ddcontribs(:,:),corrcontribs(:,:)
    real(dl), allocatable :: lens_contrib(:,:,:)
    integer thread_ix
    real(dl) pmm, pmmp1
    real(dl) d4m4,d11,dm11,d2m2,d22,d20,d23,d2m3,d33,d3m3,d04,d1m2,d12,d13,d1m3,d2m4
    real(dl) sinfac, Cg2sq
    real(dl) X000,X022,X220,X121,X132,X242
    real(dl) dX000,dX022
    real(sp) timeprev
    integer  interp_fac
    integer j,jmax
    integer llo, lhi
    real(dl) a0,b0,ho, sc
    integer apodize_point_width
    logical :: short_integral_range
    real(dl) range_fac
    logical, parameter :: approx = .false.
    real(dl) theta_cut(lmax)

    !$ integer  OMP_GET_THREAD_NUM, OMP_GET_MAX_THREADS
    !$ external OMP_GET_THREAD_NUM, OMP_GET_MAX_THREADS

    if (lensing_includes_tensors) call MpiStop('Haven''t implemented tensor lensing')

    max_lensed_ix = lSamp%l0-1
    do while(lSamp%l(max_lensed_ix) > CP%Max_l - lensed_convolution_margin)
        max_lensed_ix = max_lensed_ix -1
    end do
    lmax_lensed = lSamp%l(max_lensed_ix)
    if (allocated(Cl_lensed)) deallocate(Cl_lensed)
    allocate(Cl_lensed(lmin:lmax_lensed,CP%InitPower%nn,1:4))

    Cl_Lensed = 0

    npoints = CP%Max_l  * 2 *AccuracyBoost
    short_integral_range = .not. CP%AccurateBB
    dtheta = pi / npoints
    if (CP%Max_l > 3500) dtheta=dtheta/1.3
    apodize_point_width = nint(0.003 / dtheta)
    npoints = int(pi/dtheta)
    if (short_integral_range) then
        range_fac= max(1._dl,32/AccuracyBoost) !fraction of range to integrate
        npoints = int(npoints /range_fac)
        !OK for TT, EE, TE but inaccurate for low l BB
        !this induces high frequency ringing on very small scales
        !which is then mitigated by the apodization below
    else
        range_fac=1
    end if

    if (DebugMsgs) timeprev=GetTestTime()

    interp_fac = max(1,min(nint(10/AccuracyBoost),int(range_fac*2)-1))

    jmax = 0
    do l=lmin,lmax
        if (l<=15 .or. mod(l-15,interp_fac)==interp_fac/2) then
            jmax =jmax+1
            ls(jmax)=l
            xl(jmax)=l
        end if
        lfacs(l) = real(l*(l+1),dl)
        lfacs2(l) = real((l+2)*(l-1),dl)
        lrootfacs(l) = sqrt(lfacs(l)*lfacs2(l))
    end do
    do l=2,lmax
        theta_cut(l) = 0.244949_dl/sqrt(3._dl*lfacs(l) - 8._dl)
    end do

    roots(-1)=0 !just so dipole doesn't screw up
    do l=0,lmax+4
        roots(l) = sqrt(real(l,dl))
    end do

    thread_ix = 1
    !$ thread_ix = OMP_GET_MAX_THREADS()
    allocate(lens_contrib(4,lmax_lensed,thread_ix))
    allocate(ddcontribs(lmax,4),corrcontribs(lmax,4))

    do in = 1, CP%InitPower%nn

        do l=lmin,CP%Max_l
            ! (2*l+1)l(l+1)/4pi C_phi_phi: Cl_scalar(l,1,C_Phi) is l^4 C_phi_phi
            Cphil3(l) = Cl_scalar(l,in,C_Phi)*(2*l+1)*(l+1)/real(l,dl)**3/(4*pi)
            fac = (2*l+1)/(4*pi) * 2*pi/(l*(l+1))
            CTT(l) =  Cl_scalar(l,in,C_Temp)*fac
            CEE(l) =  Cl_scalar(l,in,C_E)*fac
            CTE(l) =  Cl_scalar(l,in,C_Cross)*fac
        end do
        if (Cphil3(10) > lensing_sanity_check_amplitude) then
            write (*,*) 'You need to normalize realistically to use lensing.'
            write (*,*) 'see http://cosmocoffee.info/viewtopic.php?t=94'
            call MpiStop()
        end if
        if (lmax > CP%Max_l) then
            l=CP%Max_l
            sc = (2*l+1)/(4*pi) * 2*pi/(l*(l+1))
            fac2=CTT(CP%Max_l)/(sc*highL_CL_template(CP%Max_l, C_Temp))
            fac=Cphil3(CP%Max_l)/(sc*highL_CL_template(CP%Max_l, C_Phi))
            do l=CP%Max_l+1, lmax
                !Fill in tail from template
                sc = (2*l+1)/(4*pi) * 2*pi/(l*(l+1))
                Cphil3(l) = highL_CL_template(l, C_Phi)*fac*sc

                CTT(l) =  highL_CL_template(l, C_Temp)*fac2*sc
                CEE(l) =  highL_CL_template(l, C_E)*fac2 *sc
                CTE(l) =  highL_CL_template(l, C_Cross)*fac2*sc
                if (Cphil3(CP%Max_l+1) > 1e-7) then
                    call MpiStop('You need to normalize the high-L template so it is dimensionless')
                end if
            end do
        end if
        if (ALens_Fiducial > 0) then
            do l=2, lmax
                sc = (2*l+1)/(4*pi) * 2*pi/(l*(l+1))
                Cphil3(l) =  sc * highL_CL_template(l, C_Phi) * ALens_Fiducial
            end do
        end if

        lens_contrib=0

        !uncomment second line for PGF90 workaround
        !$OMP PARALLEL DO DEFAULT(PRIVATE),  &
        !OMP PRIVATE(P,dP,d11,dm11,d22,d2m2,d20,corrcontribs,ddcontribs),&
        !$OMP SHARED(lfacs,lfacs2,lrootfacs,Cphil3,CTT,CTE,CEE,lens_contrib, lmax, theta_cut), &
        !$OMP SHARED(dtheta,CP,lmax_lensed,roots, npoints,interp_fac,jmax,ls,xl,short_integral_range,apodize_point_width)
        do i=1,npoints-1

            theta = i * dtheta
            x = cos(theta)
            sinth = sin(theta)
            halfsinth = sinth/2

            pmm=1
            pmmp1=x

            Cg2=0
            sigmasq=0
            if (lmin==1) then
                d_11(1) = cos(theta/2)**2
                d_m11(1) = sin(theta/2)**2
                sigmasq = sigmasq  +  (1-d_11(1))*Cphil3(lmin)
                Cg2 = Cg2  + d_m11(1)*Cphil3(lmin)
                P(1) = x
                d_22(1)=0
                d_2m2(1)=0
                d_20(1)=0
            end if
            do l=2,lmax

                P(l)= ((2*l-1)* x *pmmp1 - (l-1)*Pmm)/ l
                dP(l) = l*(pmmp1-x*P(l))/sinth**2
                Pmm=pmmp1
                pmmp1=P(l)
                llp1 = lfacs(l)

                fac1 = (1-x)
                fac2 = (1+x)
                fac = fac1/fac2

                d_11(l) =  fac1*dP(l)/llp1 + P(l)
                d_m11(l) = fac2*dP(l)/llp1 - P(l)

                sigmasq = sigmasq  +  (1-d_11(l))*Cphil3(l)
                Cg2 = Cg2  + d_m11(l)*Cphil3(l)

                d_22(l) = ( ((4*x-8)/fac2 + llp1)*P(l) &
                    + 4*fac*( fac2 + (x - 2)/llp1)*dP(l) )/ lfacs2(l)

                !For small theta use Taylor expansion for better stability (thanks Pavel Motloch)
                if (theta > theta_cut(l)) then
                    d_2m2(l) = ( (llp1- (4*x+8)/fac1) *P(l) &
                        +4/fac*( -fac1 + (x+2)/llp1) *dP(l) )/lfacs2(l)
                else
                    d_2m2(l) = lfacs(l)*lfacs2(l)*theta**4*(1._dl/384._dl &
                        - (3._dl*lfacs(l) - 8._dl)/23040._dl*theta**2)
                endif

                d_20(l) = (2*x*dP(l) - llp1*P(l) ) / lrootfacs(l)

            end do

            do j=1,jmax
                l =ls(j)

                fac1 = (1-x)
                fac2 = (1+x)
                llp1 = lfacs(l)

                rootllp1 = roots(l)*roots(l+1)
                rootfac1 = roots(l+2)*roots(l-1)
                rootfac2 = roots(l+3)*roots(l-2)

                llp1=lfacs(l)
                dm11=d_m11(l)
                d11=d_11(l)
                if (l<2) then
                    d2m2=0
                    d22=0
                    d20=0
                    d1m2 = 0
                    d12 =  0
                else
                    d2m2=d_2m2(l)
                    d22=d_22(l)
                    d20=d_20(l)
                    d1m2 = sinth/rootfac1*(dP(l) -2/fac1*dm11)
                    d12 =  sinth/rootfac1*(dP(l) -2/fac2*d11)
                end if
                if (l<3) then
                    d1m3=0
                    d2m3=0
                    d3m3=0
                    d13 =0
                    d23 =0
                    d33 =0
                else
                    sinfac=4/sinth
                    d1m3 = (-(x+0.5_dl)*d1m2*sinfac - lfacs2(l)*dm11/rootfac1 )/rootfac2
                    d2m3 = (-fac2*d2m2*sinfac - rootfac1*d1m2)/rootfac2
                    d3m3 = (-(x+1.5_dl)*d2m3*sinfac - rootfac1*d1m3)/rootfac2
                    d13  =  ((x-0.5_dl)*d12*sinfac - lfacs2(l)*d11/rootfac1 ) /rootfac2
                    d23  = (-fac1*d22*sinfac + rootfac1*d12 ) / rootfac2
                    d33  = (-(x-1.5_dl)*d23*sinfac - rootfac1*d13)/rootfac2
                end if
                if (l<4) then
                    d04=0
                    d2m4=0
                    d4m4=0
                    rootfac3=0
                else
                    rootfac3=roots(l-3)*roots(l+4)
                    d04=( (-llp1 + (18*x**2 + 6)/sinth**2 )*d20  -&
                        6*x*lfacs2(l)*dP(l)/lrootfacs(l) ) / (rootfac2*rootfac3)
                    d2m4= (-(6*x+4)*d2m3/sinth - rootfac2*d2m2 ) / rootfac3
                    d4m4 = (-7/5._dl*(llp1-6)*d2m2 + &
                        12/5._dl*( -llp1+(9*x+26)/fac1)*d3m3 ) / (llp1-12)
                end if

                !Non perturbative isotropic integrals
                !these are approx, but extremely good approximations
                X000 = exp(-llp1*sigmasq/4)
                if (approx) then

                    X022 = X000
                    X220 = rootllp1**2/4*X000
                    X121 = -0.5_dl*rootllp1*X000
                    X132 = -0.5_dl*rootllp1*X000
                    X242 = 0.25_dl*rootllp1**2*X022

                    dX000 = -llp1/4*X000
                    dX022 = -llp1/4*X022

                else
                    X022 = X000*(1+sigmasq)   !exp(-(llp1-4)*sigmasq/4)
                    X220 = lrootfacs(l)/4*X000
                    X121 = -0.5_dl*rootfac1*X000
                    X132 = -0.5_dl*rootfac2*X000
                    X242 = 0.25_dl*rootfac2*rootfac3*X022

                    dX000 = -llp1/4*X000
                    dX022 = (1-llp1/4)*X022
                end if
                !second order
                !TT
                fac1 = dX000**2
                fac3 = X220**2
                Cg2sq = Cg2**2

                !Here we drop terms in Cgt which are down by powers of l
                !Approx good to 1e-4 level
                fac = ( (X000**2-1) + Cg2sq*fac1)*P(l)+ Cg2sq*fac3*d2m2 &
                    + 8/llp1* fac1*Cg2*dm11

                corrcontribs(j,1)=  CTT(l) * fac

                fac2=(Cg2*dX022)**2+(X022**2-1)
                !Q+U
                fac = 2*Cg2*X121*X132*d13 + fac2*d22 +Cg2sq*X242*X220*d04

                corrcontribs(j,2)= CEE(l) * fac

                !Q-U
                fac = ( fac3*P(l) + X242**2*d4m4)*Cg2sq/2 &
                    + Cg2*(X121**2*dm11+ X132**2*d3m3) + fac2*d2m2

                corrcontribs(j,3)= CEE(l) * fac

                !TE
                fac = (X000*X022-1)*d20+ &
                    2*dX000*Cg2*(X121*d11 + X132*d1m3)/rootllp1 &
                    + Cg2sq*(X220/2*d2m4*X242 +( fac3/2 + dX022*dX000)*d20)

                corrcontribs(j,4)= CTE(l) * fac

            end do

            do j=1,4
                corr(j) = sum(corrcontribs(1:14,j))+interp_fac*sum(corrcontribs(15:jmax,j))
            end do

            !if (short_integral_range .and. i>npoints-20) &
            !        corr=corr*exp(-(i-npoints+20)**2/150.0) !taper the end to help prevent ringing

            if (short_integral_range .and. i>npoints-apodize_point_width*3) &
                corr=corr*exp(-(i-npoints+apodize_point_width*3)**2/real(2*apodize_point_width**2))
            !taper the end to help prevent ringing

            !Interpolate contributions
            !Increasing interp_fac and using this seems to be slower than above
            if (.false.) then
                if (abs(sum(corrcontribs(1:jmax,1)))>1e-11) print *,i,sum(corrcontribs(1:jmax,1))
                do j=1,4
                    call spline(xl,corrcontribs(1,j),jmax,1d30,1d30,ddcontribs(1,j))
                end do
                corr=0
                llo=1
                do l=lmin,lmax
                    if ((l > ls(llo+1)).and.(llo < jmax)) then
                        llo=llo+1
                    end if
                    lhi=llo+1
                    ho=ls(lhi)-ls(llo)
                    a0=(ls(lhi)-l)/ho
                    b0=(l-ls(llo))/ho
                    fac1 = ho**2/6
                    fac2 = (b0**3-b0)*fac1
                    fac1 = (a0**3-a0)*fac1

                    corr(1) = Corr(1)+ a0*corrcontribs(llo,1)+ b0*corrcontribs(lhi,1)+ &
                        fac1* ddcontribs(llo,1) +fac2*ddcontribs(lhi,1)
                    corr(2) = Corr(2)+ a0*corrcontribs(llo,2)+ b0*corrcontribs(lhi,2)+ &
                        fac1* ddcontribs(llo,2) +fac2*ddcontribs(lhi,2)
                    corr(3) = Corr(3)+ a0*corrcontribs(llo,3)+ b0*corrcontribs(lhi,3)+ &
                        fac1* ddcontribs(llo,3) +fac2*ddcontribs(lhi,3)
                    corr(4) = Corr(4)+ a0*corrcontribs(llo,4)+ b0*corrcontribs(lhi,4)+ &
                        fac1* ddcontribs(llo,4) +fac2*ddcontribs(lhi,4)

                end do
            end if

            !$   thread_ix = OMP_GET_THREAD_NUM()+1

            do l=lmin, lmax_lensed
                !theta factors were put in earlier (already in corr)

                lens_contrib(C_Temp, l, thread_ix)= lens_contrib(C_Temp,l, thread_ix) + &
                    corr(1)*P(l)*sinth

                T2 = corr(2)* d_22(l)
                T4 = corr(3)* d_2m2(l)

                lens_contrib(CT_E, l, thread_ix)= lens_contrib(CT_E,l, thread_ix) + &
                    (T2+T4)*halfsinth
                lens_contrib(CT_B, l, thread_ix)= lens_contrib(CT_B,l, thread_ix) + &
                    (T2-T4)*halfsinth

                lens_contrib(CT_Cross, l, thread_ix)= lens_contrib(CT_Cross,l, thread_ix) + &
                    corr(4)*d_20(l)*sinth

            end do

        end do
        !$OMP END PARALLEL DO

        do l=lmin, lmax_lensed
            !sign from d(cos theta) = -sin theta dtheta
            fac = l*(l+1)/OutputDenominator*dtheta *2*pi
            Cl_lensed(l,in,CT_Temp) = sum(lens_contrib(CT_Temp,l,:))*fac &
                + Cl_scalar(l,in,C_Temp)
            Cl_lensed(l,in,CT_E) = sum(lens_contrib(CT_E,l,:))*fac &
                + Cl_scalar(l,in,C_E)
            Cl_lensed(l,in,CT_B) = sum(lens_contrib(CT_B,l,:))*fac
            Cl_lensed(l,in,CT_Cross) = sum(lens_contrib(CT_Cross,l,:))*fac &
                + Cl_scalar(l,in,C_Cross)

        end do

    end do !loop over different initial power spectra
    deallocate(ddcontribs,corrcontribs)
    deallocate(lens_contrib)

    if (DebugMsgs) write(*,*) GetTestTime()-timeprev, 'Time for corr lensing'

    end subroutine CorrFuncFullSkyImpl

    subroutine CorrFuncFlatSky
    !Do flat sky approx partially non-perturbative lensing, lensing_method=2
    use ModelParams
    use ModelData
    use lvalues
    integer l, i
    integer :: npoints
    real(dl) Cgl2,  sigmasq, theta
    real(dl) dtheta
    real(dl) dbessfac, fac, fac1,fac2,  C2term, expsig, corr(4)
    real(sp) timeprev
    real(dl) Bessel0(lmin:CP%Max_l),Bessel2(lmin:CP%Max_l)
    real(dl) Bessel4(lmin:CP%Max_l),Bessel6(lmin:CP%Max_l)
    real(dl) Cphil3(lmin:CP%Max_l), CTT(lmin:CP%Max_l), CTE(lmin:CP%Max_l),CEE(lmin:CP%Max_l)
    integer max_lensed_ix
    integer b_lo
    integer in
    real(dl) T2,T4,a0, b0
    real(dl) lfacs(CP%Max_l)
    real(dl), allocatable, dimension(:,:,:) :: lens_contrib(:,:,:)
    integer thread_ix
    !$ integer OMP_GET_THREAD_NUM, OMP_GET_MAX_THREADS
    !$ external OMP_GET_THREAD_NUM, OMP_GET_MAX_THREADS

    if (lensing_includes_tensors) stop 'Haven''t implemented tensor lensing'

    max_lensed_ix = lSamp%l0-1
    do while(lSamp%l(max_lensed_ix) > CP%Max_l -250)
        max_lensed_ix = max_lensed_ix -1
    end do
    lmax_lensed = lSamp%l(max_lensed_ix)
    if (allocated(Cl_lensed)) deallocate(Cl_lensed)
    allocate(Cl_lensed(lmin:lmax_lensed,CP%InitPower%nn,1:4))

    Cl_Lensed = 0

    npoints = CP%Max_l  * 2
    if (CP%AccurateBB) npoints = npoints * 2

    dtheta = pi / npoints
    if (.not. CP%AccurateBB) then
        npoints = int(npoints /32 *min(32._dl,AccuracyBoost))
        !OK for TT, EE, TE but inaccurate for low l BB
        !this induces high frequency ringing on very small scales
    end if

    call GetBessels(npoints*dtheta*CP%Max_l)

    if (DebugMsgs) timeprev=GetTestTime()

    dbessfac = dbessel**2/6

    thread_ix = 1
    !$ thread_ix = OMP_GET_MAX_THREADS()
    allocate(lens_contrib(4,lmax_lensed,thread_ix))

    do in = 1, CP%InitPower%nn

        do l=lmin,CP%Max_l
            ! l^3 C_phi_phi/2/pi: Cl_scalar(l,1,C_Phi) is l^4 C_phi_phi
            Cphil3(l) = Cl_scalar(l,in,C_Phi)/l /(2*pi)
            fac = l/(2*pi)*2*pi/(l*(l+1))
            CTT(l) =  Cl_scalar(l,in,C_Temp)*fac
            CEE(l) =  Cl_scalar(l,in,C_E)*fac
            CTE(l) =  Cl_scalar(l,in,C_Cross)*fac
            lfacs(l) = l**2*0.5_dl
        end do

        if (Cphil3(10) > 1e-7) then
            write (*,*) 'You need to normalize realistically to use lensing.'
            write (*,*) 'see http://cosmocoffee.info/viewtopic.php?t=94'
            stop
        end if

        lens_contrib=0

        !$OMP PARALLEL DO DEFAULT(SHARED),  &
        !$OMP PRIVATE(theta, sigmasq,cgl2,b_lo,a0,b0,fac,fac1,fac2), &
        !$OMP PRIVATE(Bessel0,Bessel2,Bessel4,Bessel6), &
        !$OMP PRIVATE(corr,expsig,C2term,T2,T4,i,l, thread_ix)

        do i=1,npoints-1

            theta = i * dtheta
            sigmasq =0
            Cgl2=0
            fac = theta /dbessel

            do l=lmin,CP%Max_l

                !Interpolate the Bessel functions, and compute sigma^2 and C_{gl,2}
                b0 = l*fac
                b_lo = int(b0) +1
                a0=  b_lo - b0
                b0=  1._dl - a0
                fac1 = a0*b0*dbessfac
                fac2 = fac1*(a0-2)
                fac1 = fac1*(b0-2)

                Bessel0(l) = a0*Bess0(b_lo)+ b0*Bess0(b_lo+1) +fac1*ddBess0(b_lo) &
                    +fac2*ddBess0(b_lo+1)
                sigmasq = sigmasq + (1-Bessel0(l))*Cphil3(l)

                Bessel2(l) = a0*Bess2(b_lo)+ b0*Bess2(b_lo+1) +fac1*ddBess2(b_lo) &
                    +fac2*ddBess2(b_lo+1)
                Cgl2 =  Cgl2 + Bessel2(l)*Cphil3(l)

                Bessel4(l) = a0*Bess4(b_lo)+ b0*Bess4(b_lo+1) +fac1*ddBess4(b_lo) &
                    +fac2*ddBess4(b_lo+1)
                Bessel6(l) = a0*Bess6(b_lo)+ b0*Bess6(b_lo+1) +fac1*ddBess6(b_lo) &
                    +fac2*ddBess6(b_lo+1)

            end do

            !Get difference between lensed and unlensed correlation function
            corr = 0
            do l=lmin,CP%Max_l
                !For 2nd order perturbative result use
                !         expsig = 1 -sigmasq*l**2/2._dl
                !         C2term = l**2*Cgl2/2._dl
                fac = sigmasq*lfacs(l)
                expsig = exp(-fac)
                C2term = Cgl2*lfacs(l)
                !Put theta factor later  in here
                fac1 = expsig*theta
                fac2 = C2term*fac1
                fac1 = fac1 - theta  !we want expsig-1 to get lensing difference

                fac = fac1*Bessel0(l) + fac2*Bessel2(l)

                !TT
                corr(1) = corr(1) + CTT(l) * fac

                !Q + U
                corr(2) = corr(2) + CEE(l) * fac
                fac2 = fac2*0.5_dl
                !Q-U
                corr(3) = corr(3) + CEE(l) * &
                    (fac1*Bessel4(l) + fac2*(Bessel2(l)+Bessel6(l)))
                !Cross
                corr(4) = corr(4) + CTE(l) * &
                    (fac1*Bessel2(l) + fac2*(Bessel0(l)+Bessel4(l)))

            end do

            !$   thread_ix = OMP_GET_THREAD_NUM()+1

            do l=lmin, lmax_lensed
                !theta factors were put in earlier (already in corr)
                lens_contrib(C_Temp, l, thread_ix)= lens_contrib(C_Temp,l, thread_ix) + &
                    corr(1)*Bessel0(l)
                T2 = corr(2)*Bessel0(l)
                T4 = corr(3)*Bessel4(l)
                lens_contrib(CT_E,l,thread_ix)  = lens_contrib(CT_E,l, thread_ix) + T2+T4
                lens_contrib(CT_B,l,thread_ix)  = lens_contrib(CT_B,l, thread_ix) + T2-T4
                lens_contrib(CT_Cross,l, thread_ix) = lens_contrib(CT_Cross,l, thread_ix) + &
                    corr(4)*Bessel2(l)
            end do

        end do
        !$OMP END PARALLEL DO

        do l=lmin, lmax_lensed
            fac = l*(l+1)* 2*pi/OutputDenominator*dtheta
            Cl_lensed(l,in,CT_Temp) = sum(lens_contrib(CT_Temp,l,:))*fac &
                + Cl_scalar(l,in,CT_Temp)
            Cl_lensed(l,in,CT_Cross) = sum(lens_contrib(CT_Cross,l,:))*fac &
                +Cl_scalar(l,in,C_Cross)
            fac = fac /2 !(factor of 1/2 should have been in T2+/-T4 above
            Cl_lensed(l,in,CT_E) = sum(lens_contrib(CT_E,l,:))*fac &
                + Cl_scalar(l,in,CT_E)
            Cl_lensed(l,in,CT_B) = sum(lens_contrib(CT_B,l,:))*fac
        end do

    end do !loop over different initial power spectra
    deallocate(lens_contrib)

    if (DebugMsgs) write(*,*) GetTestTime()-timeprev, 'Time for corr lensing'

    end subroutine CorrFuncFlatSky

    subroutine BadHarmonic
    use ModelParams
    use ModelData
    use lvalues
    use InitialPower
    integer maxl, i, in, almin, max_lensed_ix, maxl_phi
    real(dl) , dimension (:,:,:), allocatable :: bare_cls
    real(dl) pp(CP%InitPower%nn,CP%Max_l)
    real(dl) asum(CP%InitPower%nn), RR(CP%InitPower%nn), roots(CP%Max_l)
    real(dl) asum_TE(CP%InitPower%nn), asum_EE(CP%InitPower%nn), asum_BB(CP%InitPower%nn)
    integer l1,l2,al,j, j1, k, hk, llp_1, llp_al, g1
    real(dl)  F, fct
    real(dl) g2l,g2l1, norm
    real(dl) a3j(CP%Max_l*2+1), tF, expF
    logical DoPol
    real(dl) iContribs(lSamp%l0,CP%InitPower%nn, 1:4), intcontrib(lmin:lSamp%l(lSamp%l0))
    real(dl) , dimension (:,:,:), allocatable :: iCl_lensed
    integer max_j_contribs

    real(sp) timeprev

    !Otherwise use second order perturbative harmonic method

    if (DebugMsgs) timeprev=GetTestTime()

    DoPol = CP%AccuratePolarization

    maxl = CP%Max_l

    if (allocated(Cl_lensed)) deallocate(Cl_lensed)

    allocate(bare_cls(CP%InitPower%nn,maxl,1:4))

    RR = 0
    do j=lmin,maxl
        norm = OutputDenominator/(j*(j+1))
        if (lensing_includes_tensors .and. CP%WantTensors .and. j<= CP%Max_l_tensor) then !Use total Cls
            bare_cls(:,j,CT_Temp:CT_E) = (Cl_scalar(j,:,C_Temp:C_E) + &
                Cl_tensor(j,:,CT_Temp:CT_E))*norm
            bare_cls(:,j,CT_B) = Cl_tensor(j,:,CT_B)*norm
            bare_cls(:,j,CT_Cross) =  (Cl_scalar(j,:,C_Cross) + &
                Cl_tensor(j,:,CT_Cross))*norm
        else
            bare_cls(:,j,CT_Temp:CT_E) = Cl_scalar(j,:,C_Temp:C_E)*norm
            bare_cls(:,j,CT_B) = 0
            bare_cls(:,j,CT_Cross) =  Cl_scalar(j,:,C_Cross)*norm
        end if
        pp(:,j) = Cl_scalar(j,:,C_Phi)/real(j**2,dl)**2
        RR = RR + j*(j+1)*real(2*j+1,dl)*pp(:,j)
        roots(j) = sqrt(real(2*j+1,dl))
    end do

    RR = RR/2/fourpi
    if (RR(1) > 1e-5) then
        write (*,*) 'You need to normalize realistically to use lensing.'
        write (*,*) 'see http://cosmocoffee.info/viewtopic.php?t=94'
        call MpiStop()
    end if
    if (maxl > lmax_donelnfa) then
        !Get ln factorials
        if (allocated(lnfa)) deallocate(lnfa)
        allocate(lnfa(0:maxl*3+1))
        lmax_donelnfa = maxl
        lnfa(0) = 0
        do i=1,CP%Max_l*3+1
            lnfa(i)=lnfa(i-1) + log(real(i,dl))
        end do
    end if

    max_lensed_ix = lSamp%l0-1
    do while(lSamp%l(max_lensed_ix) > maxl -250)
        max_lensed_ix = max_lensed_ix -1
    end do
    lmax_lensed = lSamp%l(max_lensed_ix)

    allocate(iCl_lensed(max_lensed_ix, CP%InitPower%nn, 1:4))

    max_j_contribs = lSamp%l0-1
    if (.not. DoPol) then
        maxl_phi = min(maxl,nint(max(600,(maxl*2)/5)*scale*AccuracyBoost))
        do while (lSamp%l(max_j_contribs) > maxl_phi)
            max_j_contribs=max_j_contribs-1
        end do
    end if

    !$OMP PARALLEL DO DEFAULT(SHARED), SCHEDULE(DYNAMIC), SHARED(max_j_contribs) &
    !$OMP PRIVATE(al,g1,llp_al,llp_1,g2l,asum,l1,g2l1,l2,k,hk,F,fct,almin), &
    !$OMP PRIVATE(asum_EE,asum_BB,asum_TE,expF,tF, a3j, iContribs,in,intcontrib)
    do j=max_lensed_ix,1,-1
        !Only compute lensed spectra at lSamp%l(j). Start with slow ones.

        al=lSamp%l(j)

        llp_al = al*(al+1)
        g2l=sqrt((2*al+1)/fourpi)

        asum = 0
        asum_EE = 0
        asum_BB = 0
        asum_TE = 0

        do j1 = 1, max_j_contribs
            !  Contributions to C_al are a smooth function of l_1 - so interpolate
            l1=lSamp%l(j1)

            llp_1 = l1*(l1+1)
            g2l1=roots(l1)

            almin = max(abs(al-l1),2)

            if (DoPol) then
                call GetThreeJs(a3j(almin),l1,al,0,2)
                do l2= almin, min(maxl,al+l1)
                    g1 = llp_1+l2*(l2+1)-llp_al
                    if (g1 == 0 ) cycle

                    k=al+l1+l2
                    fct=g1*g2l*g2l1*roots(l2)/2
                    tF = fct*a3j(l2)

                    if (mod(k,2)==0) then

                        hk = k/2
                        F = lnfa(hk)-lnfa(hk-al)-lnfa(hk-l1)-lnfa(hk-l2)+(lnfa(k-2*al)+lnfa(k-2*l1)&
                            & +lnfa(k-2*l2)-lnfa(k+1))/2

                        expF = exp(F)

                        asum=asum + bare_cls(:,l2,C_Temp)*(expF*fct)**2

                        asum_EE = asum_EE + bare_cls(:,l2,CT_E)*tF**2
                        asum_BB = asum_BB + bare_cls(:,l2,CT_B)*tF**2
                        if (mod(hk,2)/=0) tF=-tF
                        asum_TE = asum_TE + bare_cls(:,l2,CT_Cross)*expF*fct*tF

                    else

                        asum_BB = asum_BB + bare_cls(:,l2,CT_E)*tF**2
                        asum_EE = asum_EE +bare_cls(:,l2,CT_B)*tF**2

                    end if

                end do

            else !No polarization
                do l2= almin +mod(al+l1+almin,2),min(maxl,al+l1), 2
                    !Only do lSamp%l's where al + l1 + l2 is even

                    g1 = llp_1+l2*(l2+1)-llp_al

                    if (g1 == 0 ) cycle  !Contribution is zero

                    k=al+l1+l2
                    hk=k/2

                    fct=g1*g2l*g2l1*roots(l2)/2
                    expF = exp(2*(lnfa(hk)-lnfa(hk-al)-lnfa(hk-l1)-lnfa(hk-l2))+lnfa(k-2*al)+lnfa(k-2*l1)&
                        & +lnfa(k-2*l2)-lnfa(k+1))
                    asum=asum + bare_cls(:,l2,CT_Temp)*expF *fct**2

                end do
            end if !No polarization

            iContribs(j1,:,CT_Temp) = asum*pp(:,l1)
            if (DoPol) then
                iContribs(j1,:,CT_E) = asum_EE*pp(:,l1)
                iContribs(j1,:,CT_B) = asum_BB*pp(:,l1)
                iContribs(j1,:,CT_Cross) = asum_TE*pp(:,l1)
            end if
            asum = 0
            asum_EE = 0
            asum_BB = 0
            asum_TE = 0

        end do

        !Interpolate contributions to sum and add up
        do in=1, CP%InitPower%nn

            call InterpolateClArr(lSamp,iContribs(1,in,CT_Temp),intcontrib,max_j_contribs)
            asum(in) = sum(intcontrib(lmin:lSamp%l(max_j_contribs)))
            if (DoPol) then
                call InterpolateClArr(lSamp,iContribs(1,in,CT_E),intcontrib,max_j_contribs)
                asum_EE(in) = sum(intcontrib(lmin:lSamp%l(max_j_contribs)))
                call InterpolateClArr(lSamp,iContribs(1,in,CT_B),intcontrib,max_j_contribs)
                asum_BB(in) = sum(intcontrib(lmin:lSamp%l(max_j_contribs)))
                call InterpolateClArr(lSamp,iContribs(1,in,CT_Cross),intcontrib,max_j_contribs)
                asum_TE(in) = sum(intcontrib(lmin:lSamp%l(max_j_contribs)))
            end if
        end do

        iCl_lensed(j,:,CT_Temp) =  ((1-al*(al+1)*RR)*bare_cls(:,al,CT_Temp)  & !Linear part
            + asum/(2*al+1))*llp_al/OutputDenominator !add quadratic part and *l(l+1)/2pi
        if (DoPol) then
            iCl_lensed(j,:,CT_E) = ((1-(al**2+al-4)*RR)*bare_cls(:,al,CT_E)  &
                + asum_EE/(2*al+1))*llp_al/OutputDenominator
            iCl_lensed(j,:,CT_B) = ((1-(al**2+al-4)*RR)*bare_cls(:,al,CT_B)  &
                + asum_BB/(2*al+1))*llp_al/OutputDenominator
            iCl_lensed(j,:,CT_Cross) =  ((1-(al**2+al-2)*RR)*bare_cls(:,al,CT_Cross) &
                + asum_TE/(2*al+1))*llp_al/OutputDenominator

        else
            iCl_lensed(j,:,CT_E:CT_Cross) = bare_cls(:,al,CT_E:CT_Cross)
        end if

    end do
    !$OMP END PARALLEL DO

    deallocate(bare_cls)

    allocate(Cl_lensed(lmin:lmax_lensed,CP%InitPower%nn,1:4))

    !Interpolate to get final spectrum
    do in=1, CP%InitPower%nn
        do j = CT_Temp, CT_Cross
            call InterpolateClArr(lSamp,iCl_lensed(1,in,j),Cl_lensed(lmin, in, j),max_lensed_ix)
        end do
    end do

    deallocate(iCl_lensed)

    if (DebugMsgs) then
        if (FeedbackLevel>0) write(*,*) GetTestTime()-timeprev,' Timing for lensing'
    end if

    end subroutine BadHarmonic

    subroutine GetBessels(MaxArg)
    real(dl), intent(in):: MaxArg
    integer i
    real(dl), allocatable, dimension(:) :: x
    integer max_bes_ix
    integer, save :: last_max = 0

    max_bes_ix = nint(MaxArg / dbessel) + 3
    if (max_bes_ix > last_max) then
        last_max = max_bes_ix
        if (allocated(Bess0)) then
            deallocate(Bess0,ddBess0)
            deallocate(Bess2,ddBess2)
            deallocate(Bess4,ddBess4)
            deallocate(Bess6,ddBess6)
        end if
        allocate(Bess0(max_bes_ix),ddBess0(max_bes_ix))

        allocate(Bess2(max_bes_ix),ddBess2(max_bes_ix))
        allocate(Bess4(max_bes_ix),ddBess4(max_bes_ix))
        allocate(Bess6(max_bes_ix),ddBess6(max_bes_ix))

        allocate(x(max_bes_ix))
        Bess0(1)=1
        Bess2(1)=0; Bess4(1)=0; Bess6(1)=0
        x(1)=0
        do i=2, max_bes_ix
            x(i) = (i-1)*dbessel
            Bess0(i) = Bessj0(x(i))
            Bess2(i) = Bessj(2,x(i))
            Bess4(i) = Bessj(4,x(i))
            Bess6(i) = Bessj(6,x(i))
        end do
        call spline(x,Bess0,max_bes_ix,spl_large,spl_large,ddBess0)
        call spline(x,Bess2,max_bes_ix,spl_large,spl_large,ddBess2)
        call spline(x,Bess4,max_bes_ix,spl_large,spl_large,ddBess4)
        call spline(x,Bess6,max_bes_ix,spl_large,spl_large,ddBess6)

        deallocate(x)
    end if

    end subroutine GetBessels

    !Wrap standard built-in F2008 funcitons (appear OK for ifort and gfortran)

    FUNCTION bessj0(x)
    real(dl) bessj0,x

    bessj0 = bessel_j0(x)

    END FUNCTION bessj0

    FUNCTION bessj(n,x)
    real(dl) bessj,x
    integer n

    bessj = bessel_jn(n,x)

    END FUNCTION bessj

    ! ----------------------------------------------------------------------
    ! Auxiliary Bessel functions for N=0, N=1
    FUNCTION BESSI0(X)
    double precision X,BESSI0,Y,P1,P2,P3,P4,P5,P6,P7,  &
        Q1,Q2,Q3,Q4,Q5,Q6,Q7,Q8,Q9,AX,BX
    DATA P1,P2,P3,P4,P5,P6,P7/1.D0,3.5156229D0,3.0899424D0,1.2067429D0,  &
        0.2659732D0,0.360768D-1,0.45813D-2/
    DATA Q1,Q2,Q3,Q4,Q5,Q6,Q7,Q8,Q9/0.39894228D0,0.1328592D-1, &
        0.225319D-2,-0.157565D-2,0.916281D-2,-0.2057706D-1,  &
        0.2635537D-1,-0.1647633D-1,0.392377D-2/
    IF(ABS(X).LT.3.75D0) THEN
        Y=(X/3.75D0)**2
        BESSI0=P1+Y*(P2+Y*(P3+Y*(P4+Y*(P5+Y*(P6+Y*P7)))))
    ELSE
        AX=ABS(X)
        Y=3.75D0/AX
        BX=EXP(AX)/SQRT(AX)
        AX=Q1+Y*(Q2+Y*(Q3+Y*(Q4+Y*(Q5+Y*(Q6+Y*(Q7+Y*(Q8+Y*Q9)))))))
        BESSI0=AX*BX
    ENDIF
    RETURN
    END FUNCTION BESSI0
    ! ----------------------------------------------------------------------
    FUNCTION BESSI1(X)
    double precision X,BESSI1,Y,P1,P2,P3,P4,P5,P6,P7,  &
        Q1,Q2,Q3,Q4,Q5,Q6,Q7,Q8,Q9,AX,BX
    DATA P1,P2,P3,P4,P5,P6,P7/0.5D0,0.87890594D0,0.51498869D0,  &
        0.15084934D0,0.2658733D-1,0.301532D-2,0.32411D-3/
    DATA Q1,Q2,Q3,Q4,Q5,Q6,Q7,Q8,Q9/0.39894228D0,-0.3988024D-1, &
        -0.362018D-2,0.163801D-2,-0.1031555D-1,0.2282967D-1, &
        -0.2895312D-1,0.1787654D-1,-0.420059D-2/
    IF(ABS(X).LT.3.75D0) THEN
        Y=(X/3.75D0)**2
        BESSI1=X*(P1+Y*(P2+Y*(P3+Y*(P4+Y*(P5+Y*(P6+Y*P7))))))
    ELSE
        AX=ABS(X)
        Y=3.75D0/AX
        BX=EXP(AX)/SQRT(AX)
        AX=Q1+Y*(Q2+Y*(Q3+Y*(Q4+Y*(Q5+Y*(Q6+Y*(Q7+Y*(Q8+Y*Q9)))))))
        BESSI1=AX*BX
    ENDIF
    RETURN
    END FUNCTION BESSI1

    FUNCTION BESSI(N,X)
    !from http://perso.orange.fr/jean-pierre.moreau/Fortran/tbessi_f90.txt
    !
    !     This subroutine calculates the first kind modified Bessel function
    !     of integer order N, for any REAL X. We use here the classical
    !     recursion formula, when X > N. For X < N, the Miller's algorithm
    !     is used to avoid overflows.
    !     REFERENCE:
    !     C.W.CLENSHAW, CHEBYSHEV SERIES FOR MATHEMATICAL FUNCTIONS,
    !     MATHEMATICAL TABLES, VOL.5, 1962.
    integer, intent(in) :: N
    integer, PARAMETER :: IACC = 40
    integer m,j
    double precision, parameter ::  BIGNO = 1.D10, BIGNI = 1.D-10
    double precision X,BESSI,TOX,BIM,BI,BIP
    IF (N.EQ.0) THEN
        BESSI = BESSI0(X)
        RETURN
    ENDIF
    IF (N.EQ.1) THEN
        BESSI = BESSI1(X)
        RETURN
    ENDIF
    IF(X.EQ.0.D0) THEN
        BESSI=0.D0
        RETURN
    ENDIF
    TOX = 2.D0/X
    BIP = 0.D0
    BI  = 1.D0
    BESSI = 0.D0
    M = 2*((N+INT(SQRT(FLOAT(IACC*N)))))
    DO J = M,1,-1
        BIM = BIP+ J*TOX*BI
        BIP = BI
        BI  = BIM
        IF (ABS(BI).GT.BIGNO) THEN
            BI  = BI*BIGNI
            BIP = BIP*BIGNI
            BESSI = BESSI*BIGNI
        ENDIF
        IF (J.EQ.N) BESSI = BIP
    END DO
    BESSI = BESSI*BESSI0(X)/BI
    RETURN
    END FUNCTION BESSI

    end module lensing


      FUNCTION CHIPER(X,Y,YE,UNC,LO,LUP,A,AE,NPARAM,NITERA)
      COMPLEX Y(1),S(4096),CEXP1,CMPLX,F,FA(3)
      COMPLEX SARG
      REAL X(1),YE(1),A(3),V(11),C(4,4),B(4,4)
      REAL AE(NPARAM)
      REAL SIG2
      INTEGER IEXC(4096)
      LOGICAL   CONV
      NPTS=0
      DO 109 J=LO,LUP
        NPTS=NPTS+1
        IEXC(J)=0
109     CONTINUE
      CHIOLD=0
      SIG2 = EXP(2. * A(2) )
      DO 110 J=LO,LUP
      S(J)=A(3)*CEXP1(X(J)**2/2.*SIG2,-X(J)*A(1))
110   CHIOLD=CHIOLD
     $      +((REAL(S(J))-REAL(Y(J)))**2
     $      +(AIMAG(S(J))-AIMAG(Y(J)))**2)/YE(J)**2

      I = 0
      IFACT=0
      call info( i, ifact, a, chiold, npts)

      DO 10 I=1,NITERA
C  AFTER TWO ITERATIONS, EXCLUDE EXTREME DATA POINTS.
C  Use I .LE. 2 for that option.
      IF(I.LE.2) GOTO 220
        DO 200 J=LO,LUP
          PROJ=REAL(Y(J)*CEXP1(0.,X(J)*A(1)))
          IEXC(J)=0
          IF(PROJ.GT.1.5) IEXC(J)=1
          IF(PROJ.LT.-0.5) IEXC(J)=1
200       CONTINUE
C  RE-EVALUATE CHIOLD
        CHIOLD=0.0
        NPTS=0
        DO 210 J=LO,LUP
          IF(IEXC(J).EQ.1) GOTO 210
          S(J)=A(3)*CEXP1(X(J)**2/2.*SIG2,-X(J)*A(1))
          CHIOLD=CHIOLD
     $          +((REAL(S(J))-REAL(Y(J)))**2
     $          +(AIMAG(S(J))-AIMAG(Y(J)))**2)/YE(J)**2
          NPTS=NPTS+1
210       CONTINUE
220   CONTINUE
      DO 20 J=1,NPARAM
      C(J,NPARAM+1)=0
      DO 20 K=1,NPARAM
      C(J,K)=0
20    CONTINUE
      DO 30 J=LO,LUP
      IF(IEXC(J).EQ.1) GOTO 30
      F=(S(J)-Y(J))/YE(J)
      SARG=S(J)/YE(J)
      FA(1)=X(J)*CMPLX(AIMAG(SARG),-REAL(SARG))
      FA(2)=-X(J)**2 * SIG2 * SARG
      FA(3)=SARG/A(3)
      C(1,4)=C(1,4)+REAL(F)*REAL(FA(1))+AIMAG(F)*AIMAG(FA(1))
      C(2,4)=C(2,4)+REAL(F)*REAL(FA(2))+AIMAG(F)*AIMAG(FA(2))
      C(3,4)=C(3,4)+REAL(F)*REAL(FA(3))+AIMAG(F)*AIMAG(FA(3))
      C(3,3)=C(3,3)+REAL(FA(3))*REAL(FA(3))+AIMAG(FA(3))*AIMAG(FA(3))
      C(2,3)=C(2,3)+REAL(FA(2))*REAL(FA(3))+AIMAG(FA(2))*AIMAG(FA(3))
      C(1,3)=C(1,3)+REAL(FA(1))*REAL(FA(3))+AIMAG(FA(1))*AIMAG(FA(3))
      C(2,2)=C(2,2)+REAL(FA(2))*REAL(FA(2))+AIMAG(FA(2))*AIMAG(FA(2))
      C(1,2)=C(1,2)+REAL(FA(1))*REAL(FA(2))+AIMAG(FA(1))*AIMAG(FA(2))
      C(1,1)=C(1,1)+REAL(FA(1))*REAL(FA(1))+AIMAG(FA(1))*AIMAG(FA(1))
30    CONTINUE
      DO 65 K=1,10
      CONV=(K.EQ.1)
      IF(K.EQ.1)FACT=0
      IF(K.NE.1)FACT=2.**IFACT
      DO 70 J=1,NPARAM
      DO 75 L=1,J
      B(L,J)=C(L,J)
      B(J,L)=C(L,J)
75    CONTINUE
      B(J,NPARAM+1)=C(J,NPARAM+1)
      B(J,J)=(1+FACT)*C(J,J)
70    CONTINUE
      DET=SIMUL(NPARAM,B,V,1E-10,0,4)
      DO 40 J=1,NPARAM
      A(J)=A(J)-V(J)
      IF ( A(J) .EQ. 0. ) A(J) = 1.E-10
      CONV=CONV.AND.(ABS(V(J)/A(J)).LE.UNC)
40    CONTINUE
      IF ( A(2) .GT. 35. )  A(2) = 35.
      IF ( A(2) .LT. -35. )  A(2) = -35.
      IF ( A(3) .LT. 0. ) A(3) = 0.01
      SIG2 = EXP( 2. * A(2) )
      CHI=0
      IF(CONV)GO TO 120
      DO 100 J=LO,LUP
      IF(IEXC(J).EQ.1) GOTO 100
      S(J)=A(3)*CEXP1(X(J)**2/2.*SIG2,-X(J)*A(1))
      CHI=CHI
     $      +((REAL(S(J))-REAL(Y(J)))**2
     $      +(AIMAG(S(J))-AIMAG(Y(J)))**2)/YE(J)**2
100   CONTINUE
120   CONTINUE

      call info(i, ifact, a, chi, npts)

      IF(K.NE.1)IFACT=IFACT-1
      IF ( IFACT .LT. -15 )  IFACT = -15
      IF(CHI.LT.(1.0001*CHIOLD))GO TO 80
      IF(K.NE.1)IFACT=IFACT+2
      IF ( IFACT .GT. 15 ) IFACT = 15
      DO 65 J=1,NPARAM
      A(J)=A(J)+V(J)
      IF ( A(J) .EQ. 0.)  A(J) = 1.E-10
65    CONTINUE
      IF ( A(2) .GT. 35. ) A(2) = 35.
      IF ( A(2) .LT. -35. ) A(2) = -35.
      IF ( A(3) .LT. 0. ) A(3) = 0.01
      SIG2 = EXP( 2. * A(2) )
      IF(IFACT.GT.10)GO TO 50
80    CONTINUE
      IF(CONV)GO TO 50
      IF(CHI.LT.CHIOLD)CHIOLD=CHI
10    CONTINUE
50    CONTINUE
      NFREE=NPTS-NPARAM
      PERDEG=CHIOLD/NFREE
C      WRITE(7,1000)
      DO 60 I=1,NPARAM
      AE(I)=SQRT(PERDEG*B(I,I))
      IF ( AE(I) .EQ. 0. )  AE(I) = 1.E-10
      DO 90 J=1,I
      C(I,J)=PERDEG*B(I,J)/AE(I)/AE(J)
90    CONTINUE

60    CONTINUE

      call info(i, ifact, a, perdeg, npts)

      CHIPER=PERDEG
      RETURN
      END


!** Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
!** See https://llvm.org/LICENSE.txt for license information.
!** SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception

!* Tests for runtime library MATMUL routines

program p

  parameter(NbrTests=176)

  complex*8, dimension(4,3) :: arr1
  complex*8, dimension(3) :: arr2
  complex*8, dimension(4) :: arr3
  complex*8, dimension(4,4) :: arr4
  complex*8, dimension(0:3,-1:1) :: arr5
  complex*8, dimension(-3:-1) :: arr6
  complex*8, dimension(-1:2,0:3) :: arr7
  complex*8, dimension(2:5,2:4) :: arr8
  complex*8, dimension(2:4) :: arr9
  complex*8, dimension(2:5) :: arr10
  complex*8, dimension(4) :: arr11

  data arr1 /(0,1),(1,2),(2,3),(3,4), &
             (4,3),(5,4),(6,5),(7,6), &
             (8,8),(9,9),(10,10),(11,11)/
  data arr2 /(0,2),(1,3),(2,1)/
  data arr3 /(0,4),(1,3),(2,2),(3,1)/
  data arr4 /(0,1),(1,2),(2,3),(3,4), &
             (4,3),(5,2),(6,1),(7,0), &
             (8,0),(9,10),(10,11),(11,12), &
             (12,11),(13,10),(14,9),(15,8)/
  data arr5 /(0,-1),(1,-2),(2,-3),(3,-4), &
             (4,-3),(5,-2),(6,-1),(7,0), &
             (8,-7),(9,-8),(10,-9),(11,11)/
  data arr6 /(0,5),(1,4),(2,3)/
  data arr7 /(0,1),(1,2),(2,3),(3,4), &
             (4,-7),(5,-6),(6,-5),(7,-4), &
             (8,11),(9,10),(10,9),(11,10), &
             (12,-15),(13,-14),(14,-13),(15,-12)/
 data arr8 /(0,1),(1,2),(2,3),(3,4), &
             (4,3),(5,4),(6,5),(7,6), &
             (8,8),(9,9),(10,10),(11,11)/
  data arr9 /(0,2),(1,3),(2,1)/
  data arr10 /(0,4),(1,3),(2,2),(3,1)/

  complex*8 :: expect(NbrTests)
  complex*8 :: results(NbrTests)

  data expect /  &
  !test 1-8
  (1.000000,39.00000), (-2.000000,48.00000), (-5.000000,57.00000), &
  (-8.000000,66.00000), &
  !test 9-16
  (0.000000,0.000000), (-2.000000,48.00000), (-5.000000,57.00000), &
  (-8.000000,66.00000), &
  !test 17-24
  (-2.000000,48.00000), (-5.000000,57.00000), (-8.000000,66.00000), &
  (0.000000,0.000000), &
  !test 25-32
  (0.000000,0.000000), (1.000000,39.00000), (-2.000000,48.00000), &
  (-5.000000,57.00000), &
  !test 33-40
  (0.000000,0.000000), (-11.00000,21.00000), (-15.00000,27.00000), &
  (-19.00000,33.00000), &
  !test 41-48
  (-22.00000,40.00000), (-26.00000,46.00000), (-30.00000,52.00000), &
  (-34.00000,58.00000), &
  !test 49-56
  (2.000000,11.00000), (1.000000,18.00000), (0.000000,25.00000), &
  (-1.000000,32.00000), &
  !test 57-64
  (0.000000,0.000000), (2.000000,11.00000), (1.000000,18.00000), &
  (0.000000,25.00000), &
  !test 65-72
  (-26.00000,46.00000), (-30.00000,52.00000), (-34.00000,58.00000), &
  (0.000000,0.000000), &
  !test 73-80
  (5.000000,29.00000), (4.000000,34.00000), (3.000000,39.00000), &
  (0.000000,0.000000), &
  !test 81-88
  (0.000000,0.000000), (2.000000,11.00000), (0.000000,0.000000), &
  (0.000000,25.00000), &
  !test 89-96
  (-26.00000,46.00000), (0.000000,0.000000), (-34.00000,58.00000), &
  (0.000000,0.000000), &
  !test 97-128
  (0.000000,0.000000), (-26.00000,46.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (-34.00000,58.00000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 129-160
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (-26.00000,46.00000), &
  (0.000000,0.000000), (-34.00000,58.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 161-192
  (0.000000,0.000000), (51.00000,53.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (-33.00000,90.00000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 192-224
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (55.00000,29.00000), &
  (0.000000,0.000000), (-4.000000,83.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 225-232
  (3.000000,39.00000), (4.000000,34.00000), (5.000000,29.00000), &
  (0.000000,0.000000), &
  !test 233-240
  (-26.00000,46.00000), (0.000000,0.000000), (-34.00000,58.00000), &
  (0.000000,0.000000), &
  !test 241-272
  (0.000000,0.000000), (-34.00000,58.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (-26.00000,46.00000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 273-304
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (-26.00000,46.00000), &
  (0.000000,0.000000), (-34.00000,58.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 305-336
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (55.00000,29.00000), &
  (0.000000,0.000000), (-4.000000,83.00000), (0.000000,0.000000), &
  (0.000000,0.000000), (0.000000,0.000000), (0.000000,0.000000), &
  (0.000000,0.000000), &
  !test 337-344
  (5.000000,29.00000), (4.000000,34.00000), (3.000000,39.00000), &
  (0.000000,0.000000), &
  !test 345-352
  (1.000000,39.00000), (-2.000000,48.00000), (-5.000000,57.00000), &
  (-8.000000,66.00000)/

  results = -1

  ! tests 1-8
  arr3=0
  arr3 = matmul(arr1,arr2)
  call assign_result(1,4,arr3,results)
  !print *,"test 1,8"
  !print *,arr3

  ! tests 9-16
  arr3=0
  arr3(2:4) = matmul(arr1(2:4,:),arr2)
  call assign_result(5,8,arr3,results)
  !print *,"test 9,16"
  !print *,arr3

  ! tests 17-24
  arr3=0
  arr3(1:3) = matmul(arr1(2:4,:),arr2)
  call assign_result(9,12,arr3,results)
  !print *,"test 17,24"
  !print *,arr3

  !tests 25-32
  arr3=0
  arr3(2:4) = matmul(arr1(1:3,:),arr2)
  call assign_result(13,16,arr3,results)
  !print *,"test 25,32"
  !print *,arr3

  !tests 33-40
  arr3=0
  arr3(2:4) = matmul(arr1(2:4,1:2),arr2(1:2))
  call assign_result(17,20,arr3,results)
  !print *,"test 33,40"
  !print *,arr3

  !tests 41-48
  arr3=0
  arr3 = matmul(arr1(:,2:3),arr2(1:2))
  call assign_result(21,24,arr3,results)
  !print *,"test 41,48"
  !print *,arr3

  !tests 49-56
  arr3=0
  arr3 = matmul(arr1(:,1:2),arr2(2:3))
  call assign_result(25,28,arr3,results)
  !print *,"test 49,56"
  !print *,arr3

  !tests 57-64
  arr3=0
  arr3(2:4)  = matmul(arr1(1:3,1:2),arr2(2:3))
  call assign_result(29,32,arr3,results)
  !print *,"test 57,64"
  !print *,arr3

  !tests 65-72
  arr3=0
  arr3(1:3)  = matmul(arr1(2:4,2:3),arr2(1:2))
  call assign_result(33,36,arr3,results)
  !print *,"test 65,72"
  !print *,arr3

  !tests 73-80
  arr3=0
  arr3(1:3) = matmul(arr1(2:4,1:3:2),arr2(1:3:2))
  call assign_result(37,40,arr3,results)
  !print *,"test 73,80"
  !print *,arr3

  !tests 81-88
  arr3=0
  arr3(2:4:2)  = matmul(arr1(1:3:2,1:2),arr2(2:3))
  call assign_result(41,44,arr3,results)
  !print *,"test 81,88"
  !print *,arr3

  !tests 89-96
  arr3=0
  arr3(1:3:2)  = matmul(arr1(2:4:2,2:3),arr2(1:2))
  call assign_result(45,48,arr3,results)
  !print *,"test 89,96"
  !print *,arr3

  !tests 97-128
  arr4=0
  arr4(2,1:3:2)  = matmul(arr1(2:4:2,2:3),arr2(1:2))
  call assign_result(49,64,arr4,results)
  !print *,"test 97,128"
  !print *,arr4

  !tests 129-160
  arr4=0
  arr4(1:3:2,3)  = matmul(arr1(2:4:2,2:3),arr2(1:2))
  call assign_result(65,80,arr4,results)
  !print *,"test 129,160"
  !print *,arr4

  !tests 161-192
  arr7=0
  arr7(0,0:2:2)  = matmul(arr5(1:3:2,0:1),arr6(-3:-2))
  call assign_result(81,96,arr7,results)
  !print *,"test 161,192"
  !print *,arr7

  !tests 193-224
  arr7=0
  arr7(-1:1:2,2)  = matmul(arr5(1:3:2,0:1),arr6(-2:-1))
  call assign_result(97,112,arr7,results)
  !print *,"test 193,224"
  !print *,arr7

  !tests 225-332
  arr3=0
  arr3(3:1:-1) = matmul(arr1(2:4,3:1:-2),arr2(3:1:-2))
  call assign_result(113,116,arr3,results)
  !print *,"test 225,332"
  !print *,arr3

  !tests 333-240
  arr3=0
  arr3(3:1:-2)  = matmul(arr1(4:2:-2,2:3),arr2(1:2))
  call assign_result(117,120,arr3,results)
  !print *,"test 333,240"
  !print *,arr3

  !tests 241,272
  arr4=0
  arr4(2,3:1:-2)  = matmul(arr1(2:4:2,2:3),arr2(1:2))
  call assign_result(121,136,arr4,results)
  !print *,"test 241,272"
  !print *,arr4

  !tests 273-304
  arr4=0
  arr4(3:1:-2,3)  = matmul(arr1(4:2:-2,2:3),arr2(1:2))
  call assign_result(137,152,arr4,results)
  !print *,"test 273,304"
  !print *,arr4

  !tests 305-336
  arr7=0
  arr7(1:-1:-2,2)  = matmul(arr5(3:1:-2,0:1),arr6(-2:-1))
  call assign_result(153,168,arr7,results)
  !print *,"test 305,336"
  !print *,arr7

  !tests 337-344
  arr3=0
  arr3(1:3) = matmul(arr1(2:4,3:1:-2),arr2(3:1:-2))
  call assign_result(169,172,arr3,results)
  !print *,"test 337,344"
  !print *,arr3

  arr11 = 0

  ! tests 337-344
  arr10=0
  arr10 = arr11 + matmul(arr8,arr9)
  call assign_result(173,176,arr10,results)
  !print *,"test 337,344"
  !print *,arr10

  call check(results, expect, NbrTests*2)

end program

subroutine assign_result(s_idx, e_idx , arr, rslt)
  complex*8, dimension(1:e_idx-s_idx+1) :: arr
  complex*8, dimension(e_idx) :: rslt
  integer:: s_idx, e_idx

  rslt(s_idx:e_idx) = arr

end subroutine


!==============================================================================!
  subroutine Turb_Mod_Compute_Variable(turb, sol, ini, phi, n_step)
!------------------------------------------------------------------------------!
!   Discretizes and solves transport equations for different turbulent         !
!   variables.                                                                 !
!------------------------------------------------------------------------------!
!----------------------------------[Modules]-----------------------------------!
  use Work_Mod, only: one => r_cell_12
!------------------------------------------------------------------------------!
  implicit none
!--------------------------------[Arguments]-----------------------------------!
  type(Turb_Type),   target :: turb
  type(Solver_Type), target :: sol
  integer                   :: ini
  type(Var_Type)            :: phi
  integer                   :: n_step
!----------------------------------[Locals]------------------------------------!
  type(Field_Type),  pointer :: flow
  type(Grid_Type),   pointer :: grid
  type(Var_Type),    pointer :: u, v, w
  type(Var_Type),    pointer :: vis
  real, contiguous,  pointer :: flux(:)
  type(Matrix_Type), pointer :: a
  real, contiguous,  pointer :: b(:)
  integer                    :: s, c, c1, c2, exec_iter
  real                       :: f_ex, f_im
  real                       :: phis
  real                       :: a0, a12, a21
  real                       :: vis_eff
  real                       :: phi_x_f, phi_y_f, phi_z_f
  real                       :: dt
  real                       :: visc_f
!==============================================================================!
!                                                                              !
!  The form of equations which are solved:                                     !
!                                                                              !
!     /               /                /                     /                 !
!    |     dphi      |                | mu_eff              |                  !
!    | rho ---- dV + | rho u phi dS = | ------ DIV phi dS + | G dV             !
!    |      dt       |                |  sigma              |                  !
!   /               /                /                     /                   !
!                                                                              !
!------------------------------------------------------------------------------!

  call Cpu_Timer_Mod_Start('Compute_Turbulence (without solvers)')

  ! Take aliases
  flow => turb % pnt_flow
  grid => flow % pnt_grid
  vis  => turb % vis
  flux => flow % m_flux % n
  dt   =  flow % dt
  call Field_Mod_Alias_Momentum(flow, u, v, w)
  call Solver_Mod_Alias_System (sol, a, b)

  ! Initialize matrix and right hand side
  a % val(:) = 0.0
  b      (:) = 0.0

  ! Old values (o) and older than old (oo)
  if(ini .eq. 1) then
    do c = 1, grid % n_cells
      phi % oo(c) = phi % o(c)
      phi % o (c) = phi % n(c)
    end do
  end if

  ! Gradients
  call Field_Mod_Grad_Variable(flow, phi)

  !---------------!
  !               !
  !   Advection   !
  !               !
  !---------------!
  one(:) = 1.0
  call Numerics_Mod_Advection_Term(phi, one, flux, sol,  &
                                   phi % x,              &
                                   phi % y,              &
                                   phi % z,              &
                                   grid % dx,            &
                                   grid % dy,            &
                                   grid % dz)

  !------------------!
  !                  !
  !     Difusion     !
  !                  !
  !------------------!

  !----------------------------!
  !   Spatial discretization   !
  !----------------------------!
  do s = 1, grid % n_faces

    c1 = grid % faces_c(1,s)
    c2 = grid % faces_c(2,s)

    visc_f =        grid % fw(s)  * flow % viscosity(c1)   &
           + (1.0 - grid % fw(s)) * flow % viscosity(c2)

    vis_eff = visc_f + (    grid % fw(s)  * turb % vis_t(c1)   &
                     + (1.0-grid % fw(s)) * turb % vis_t(c2))  &
                     / phi % sigma

    if(turb % model .eq. SPALART_ALLMARAS .or.               &
       turb % model .eq. DES_SPALART)                        &
      vis_eff = visc_f + (    grid % fw(s)  * vis % n(c1)    &
                       + (1.0-grid % fw(s)) * vis % n(c2))   &
                       / phi % sigma

    if(turb % model .eq. HYBRID_LES_RANS) then
      vis_eff = visc_f + (    grid % fw(s)  * turb % vis_t_eff(c1)   &
                       + (1.0-grid % fw(s)) * turb % vis_t_eff(c2))  &
                       / phi % sigma
    end if
    phi_x_f = grid % fw(s) * phi % x(c1) + (1.0-grid % fw(s)) * phi % x(c2)
    phi_y_f = grid % fw(s) * phi % y(c1) + (1.0-grid % fw(s)) * phi % y(c2)
    phi_z_f = grid % fw(s) * phi % z(c1) + (1.0-grid % fw(s)) * phi % z(c2)

    if(turb % model .eq. K_EPS_ZETA_F    .or.  &
       turb % model .eq. HYBRID_LES_RANS .or.  &
       turb % model .eq. K_EPS) then
      if(c2 < 0 .and. phi % name .eq. 'KIN') then
        if(Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALL .or.  &
           Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALLFL) then
          if(turb % y_plus(c1) > 4) then

            phi_x_f = 0.0
            phi_y_f = 0.0
            phi_z_f = 0.0
            vis_eff = 0.0

          end if
        end if
      end if
    end if

    ! Total (exact) diffusive flux
    f_ex = vis_eff * (  phi_x_f * grid % sx(s)  &
                      + phi_y_f * grid % sy(s)  &
                      + phi_z_f * grid % sz(s) )

    a0 = vis_eff * a % fc(s)

    ! Implicit diffusive flux
    f_im = (  phi_x_f * grid % dx(s)                      &
            + phi_y_f * grid % dy(s)                      &
            + phi_z_f * grid % dz(s) ) * a0

    ! Cross diffusion part
    phi % c(c1) = phi % c(c1) + f_ex - f_im
    if(c2  > 0) then
      phi % c(c2) = phi % c(c2) - f_ex + f_im
    end if

    ! Compute coefficients for the sysytem matrix
    a12 = a0
    a21 = a0

    a12 = a12  - min(flux(s), real(0.0))
    a21 = a21  + max(flux(s), real(0.0))

    ! Fill the system matrix
    if(c2  > 0) then
      a % val(a % pos(1,s)) = a % val(a % pos(1,s)) - a12
      a % val(a % dia(c1))  = a % val(a % dia(c1))  + a12
      a % val(a % pos(2,s)) = a % val(a % pos(2,s)) - a21
      a % val(a % dia(c2))  = a % val(a % dia(c2))  + a21
    else if(c2 < 0) then

      ! All modeled turbulent quantities except t2(!) are zero at the wall
      ! or specified otherwise in the control file
      if(phi % name .ne. 'T2') then

        ! Outflow is not included because it was causing problems
        if((Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. INFLOW)  .or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALL)    .or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. PRESSURE).or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. CONVECT) .or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALLFL) ) then
          a % val(a % dia(c1)) = a % val(a % dia(c1)) + a12
          b(c1) = b(c1) + a12 * phi % n(c2)
        end if

      ! For t2; fix the value at all these boundary condition types,
      ! but not WALLFL!!!
      else
        if((Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. INFLOW)  .or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALL)    .or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. PRESSURE).or.   &
           (Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. CONVECT) ) then
          a % val(a % dia(c1)) = a % val(a % dia(c1)) + a12
          b(c1) = b(c1) + a12 * phi % n(c2)
        end if
      end if

    end if  ! if c2 < 0

  end do  ! through faces

  ! Cross diffusion terms are treated explicity
  do c = 1, grid % n_cells
    b(c) = b(c) + phi % c(c)
  end do

  !--------------------!
  !                    !
  !   Inertial terms   !
  !                    !
  !--------------------!
  call Numerics_Mod_Inertial_Term(phi, flow % density, sol, dt)

  !-------------------------------------!
  !                                     !
  !   Source terms and wall function    !
  !                                     !
  !-------------------------------------!
  if(turb % model .eq. K_EPS) then
    if(phi % name .eq. 'KIN') call Turb_Mod_Src_Kin_K_Eps(turb, sol)
    if(phi % name .eq. 'EPS') call Turb_Mod_Src_Eps_K_Eps(turb, sol)
    if(heat_transfer) then
      if(phi % name .eq. 'T2')  call Turb_Mod_Src_T2(turb, sol)
    end if
  end if

  if(turb % model .eq. K_EPS_ZETA_F .or.  &
     turb % model .eq. HYBRID_LES_RANS) then
    if(phi % name .eq. 'KIN')  call Turb_Mod_Src_Kin_K_Eps_Zeta_F(turb, sol)
    if(phi % name .eq. 'EPS')  call Turb_Mod_Src_Eps_K_Eps_Zeta_F(turb, sol)
    if(phi % name .eq. 'ZETA')  &
      call Turb_Mod_Src_Zeta_K_Eps_Zeta_F(turb, sol, n_step)
    if(heat_transfer) then
      if(phi % name .eq. 'T2')  call Turb_Mod_Src_T2(turb, sol)
    end if
  end if

  if(turb % model .eq. SPALART_ALLMARAS .or.  &
     turb % model .eq. DES_SPALART) then
    call Turb_Mod_Src_Vis_Spalart_Almaras(turb, sol)
  end if

  !---------------------------------!
  !                                 !
  !   Solve the equations for phi   !
  !                                 !
  !---------------------------------!

  ! Under-relax the equations
  call Numerics_Mod_Under_Relax(phi, sol)

  ! Call linear solver to solve the equations
  call Cpu_Timer_Mod_Start('Linear_Solver_For_Turbulence')
  call Bicg(sol,            &
            phi % n,        &
            b,              &
            phi % precond,  &
            phi % niter,    &
            exec_iter,      &
            phi % tol,      &
            phi % res)
  call Cpu_Timer_Mod_Stop('Linear_Solver_For_Turbulence')

  do c = 1, grid % n_cells
    if( phi % n(c) < 0.0 ) phi % n(c) = phi % o(c)
    if(phi % name .eq. 'ZETA')  phi % n(c) = min(phi % n(c), 1.8)
  end do

  ! Print info on the screen
  if(turb % model .eq. K_EPS        .or.  &
     turb % model .eq. K_EPS_ZETA_F .or.  &
     turb % model .eq. HYBRID_LES_RANS) then
    if(phi % name .eq. 'KIN')  &
      call Info_Mod_Iter_Fill_At(3, 1, phi % name, exec_iter, phi % res)
    if(phi % name .eq. 'EPS')  &
      call Info_Mod_Iter_Fill_At(3, 2, phi % name, exec_iter, phi % res)
    if(phi % name .eq. 'ZETA')  &
      call Info_Mod_Iter_Fill_At(3, 3, phi % name, exec_iter, phi % res)
    if(heat_transfer) then
      if(phi % name .eq. 'T2')  &
      call Info_Mod_Iter_Fill_At(3, 5, phi % name, exec_iter, phi % res)
    end if
  end if

  call Grid_Mod_Exchange_Real(grid, phi % n)

  call Cpu_Timer_Mod_Stop('Compute_Turbulence (without solvers)')

  end subroutine


      subroutine dfluxc(ny,il,jl,ib,jb,
     & w,p,
     & porj,
     & fw, radi, radj,
c    no flo_params used
     & rfil,vis0)
c
c
c     arg list above is:
c
c     dims
c
c     flow_var
c     mesh_var
c     solv_var
c
c     flo_param
c     solv_param
c
c
c     ******************************************************************
c     *                                                                *
c     *   artificial dissipation on coarse grids                       *
c     *   first order flux scaled to the spectral radius               *
c     *                                                                *
c     ******************************************************************
c
c     w(i,j,1)  = density
c     w(i,j,2)  = momentum in x direction
c     w(i,j,3)  = momentum in y direction
c     w(i,j,4)  = total energy
c
c     ******************************************************************
c
c      use dims
c
c     ******************************************************************
c
c      use flo_var
c      use mesh_var
c      use solv_var
c
c     ******************************************************************
c
c      use solv_param
c
c     ******************************************************************
c
      implicit none

c     ******************************************************************
c
c     input variables
c
c     ******************************************************************
c     from dims
      integer, intent(in) :: ny,il,jl,ib,jb

c     from flo_var
      real(8), intent(inout), dimension(0:ib,0:jb,4) :: w
      real(8), intent(inout), dimension(0:ib,0:jb) :: p

c     from mesh_var
      real(8), intent(in), dimension(2:il,1:jl)   :: porj

c     from solv_var
      real(8), intent(inout), dimension(0:ib,0:jb,4) :: fw
      real(8), intent(inout), dimension(0:ib,0:jb)   :: radi,radj

c     from solv_param
      real(8), intent(in)     :: rfil, vis0
c
c     ******************************************************************
c
c     local variables
c
c     ******************************************************************
c
      integer  :: i,j,n
c
c     ******************************************************************
c
      real(8)     :: fis0,sfil
c
c     ******************************************************************
c
      real(8), dimension(il,jl,4)     :: fs
      real(8), dimension(il,jl)       :: dis
c
c     ******************************************************************
c
c      write(*,)
      fis0      = rfil*abs(vis0)/8.
      sfil      = 1.  -rfil
c
c     dissipation in the i direction
c
      do j=2,jl

         do i=1,il
c           dis(i,j)  = fis0*(radi(i+1,j)  +radi(i,j))
            dis(i,j)  = fis0*min(radi(i+1,j),radi(i,j))
         end do

         do n=1,4
            do i=1,il
               fs(i,j,n) = dis(i,j)*(w(i+1,j,n)  -w(i,j,n))
            end do
         end do

         do i=1,il
            fs(i,j,4) = fs(i,j,4)  +dis(i,j)*(p(i+1,j)  -p(i,j))
         end do

         do n=1,4
            do i=2,il
               fw(i,j,n) = sfil*fw(i,j,n)  -fs(i,j,n) +fs(i-1,j,n)
            end do
         end do

      end do

      if (ny.lt.3) return
c
c     dissipation in the j direction
c
      do j=1,jl
      do i=2,il
c        dis(i,j)  = fis0*(radj(i,j+1)  +radj(i,j))
         dis(i,j)  = fis0*porj(i,j)*min(radj(i,j+1),radj(i,j))
      end do
      end do

      do n=1,4
         do j=1,jl
         do i=2,il
            fs(i,j,n) = dis(i,j)*(w(i,j+1,n)  -w(i,j,n))
         end do
         end do
      end do

      do j=1,jl
      do i=2,il
         fs(i,j,4) = fs(i,j,4)  +dis(i,j)*(p(i,j+1)  -p(i,j))
      end do
      end do

      do n=1,4
         do j=2,jl
         do i=2,il
            fw(i,j,n) = fw(i,j,n)  -fs(i,j,n)  +fs(i,j-1,n)
         end do
         end do
      end do

      return

      end


Subroutine ddrho(srt, iseed, px, py, pz, dm1, pnx, pny, pnz, dm2, ppx, ppy, ppz, amp, icou1)
  Common /table/xarray(0:1000), earray(0:1000)
  Common /rndf77/nseed
  Save
  icou1 = 0
  pi = 3.1415926
  amn = 938.925/1000.
  amp = 770./1000.
  srt1 = srt - amp - 0.02
  ntrym = 0
8 Call rmasdd(srt1, 1.232, 1.232, 1.08, 1.08, iseed, 1, dm1, dm2)
  ntrym = ntrym + 1
  rhomax = srt - dm1 - dm2 - 0.02
  If (rhomax<=0 .And. ntrym<=20) Goto 8
  amp = rhomas(rhomax, iseed)
  v = 0.43
  w = -0.84
  ptmax2 = (srt**2-(dm1+dm2+amp)**2)*(srt**2-(dm1-amp-dm2)**2)/4./srt**2
  scheck = ptmax2
  If (scheck<0) Then
     Write (99, *) 'scheck24: ', scheck
     scheck = 0.
  End If
  ptmax = sqrt(scheck)*1./3.
7 pt = ptr(ptmax, iseed)
  pzmax2 = (srt**2-(dm1+dm2+amp)**2)*(srt**2-(dm1-amp-dm2)**2)/4./srt**2 - pt**2
  If ((pzmax2<0.) .And. ntrym<=100) Then
     Goto 7
  Else
     pzmax2 = 1.E-06
  End If
  pzmax = sqrt(pzmax2)
  xmax = 2.*pzmax/srt
  ntryx = 0
  fmax00 = 1.056
  x00 = 0.26
  If (abs(xmax)>0.26) Then
     f00 = fmax00
  Else
     f00 = 1. + v*abs(xmax) + w*xmax**2
  End If
9 x = xmax*(1.-2.*ranart(nseed))
  ntryx = ntryx + 1
  xratio = (1.+v*abs(x)+w*x**2)/f00
  If (xratio<ranart(nseed) .And. ntryx<=50) Goto 9
  pz = 0.5*srt*x
  fai = 2.*pi*ranart(nseed)
  px = pt*cos(fai)
  py = pt*sin(fai)
  ek = sqrt(dm1**2+pt**2+pz**2)
  eln = srt - ek
  If (eln<=0) Then
     icou1 = -1
     Return
  End If
  bx = -px/eln
  by = -py/eln
  bz = -pz/eln
  scheck = 1. - bx**2 - by**2 - bz**2
  If (scheck<=0) Then
     Write (99, *) 'scheck25: ', scheck
     Stop
  End If
  ga = 1./sqrt(scheck)
  elnc = eln/ga
  pn2 = ((elnc**2+dm2**2-amp**2)/(2.*elnc))**2 - dm2**2
  If (pn2<=0) Then
     icou1 = -1
     Return
  End If
  pn = sqrt(pn2)
  xptr = 0.33*pn
  pnt = ptr(xptr, iseed)
  fain = 2.*pi*ranart(nseed)
  pnx = pnt*cos(fain)
  pny = pnt*sin(fain)
  sig = 1
  If (x>0) sig = -1
  scheck = pn**2 - pnt**2
  If (scheck<0) Then
     Write (99, *) 'scheck26: ', scheck
     scheck = 0.
  End If
  pnz = sig*sqrt(scheck)
  en = sqrt(dm2**2+pnx**2+pny**2+pnz**2)
  ppx = -pnx
  ppy = -pny
  ppz = -pnz
  ep = sqrt(amp**2+ppx**2+ppy**2+ppz**2)
  pbeta = pnx*bx + pny*by + pnz*bz
  trans0 = ga*(ga*pbeta/(ga+1.)+en)
  pnx = bx*trans0 + pnx
  pny = by*trans0 + pny
  pnz = bz*trans0 + pnz
  If (ep==0.) ep = 1.E-09
  pbeta = ppx*bx + ppy*by + ppz*bz
  trans0 = ga*(ga*pbeta/(ga+1.)+ep)
  ppx = bx*trans0 + ppx
  ppy = by*trans0 + ppy
  ppz = bz*trans0 + ppz
  Return
End Subroutine ddrho


      SUBROUTINE SB10UD( N, M, NP, NCON, NMEAS, B, LDB, C, LDC, D, LDD,
     $                   TU, LDTU, TY, LDTY, RCOND, TOL, DWORK, LDWORK,
     $                   INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To reduce the matrices D12 and D21 of the linear time-invariant
C     system
C
C                   | A  | B1  B2  |   | A | B |
C               P = |----|---------| = |---|---|
C                   | C1 |  0  D12 |   | C | D |
C                   | C2 | D21 D22 |
C
C     to unit diagonal form, and to transform the matrices B and C to
C     satisfy the formulas in the computation of the H2 optimal
C     controller.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     NCON    (input) INTEGER
C             The number of control inputs (M2).  M >= NCON >= 0,
C             NP-NMEAS >= NCON.
C
C     NMEAS   (input) INTEGER
C             The number of measurements (NP2).  NP >= NMEAS >= 0,
C             M-NCON >= NMEAS.
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the system input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the transformed system input matrix B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= max(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading NP-by-N part of this array must
C             contain the system output matrix C.
C             On exit, the leading NP-by-N part of this array contains
C             the transformed system output matrix C.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= max(1,NP).
C
C     D       (input/output) DOUBLE PRECISION array, dimension (LDD,M)
C             On entry, the leading NP-by-M part of this array must
C             contain the system input/output matrix D.
C             The (NP-NMEAS)-by-(M-NCON) leading submatrix D11 is not
C             referenced.
C             On exit, the trailing NMEAS-by-NCON part (in the leading
C             NP-by-M part) of this array contains the transformed
C             submatrix D22.
C             The transformed submatrices D12 = [ 0  Im2 ]' and
C             D21 = [ 0  Inp2 ] are not stored. The corresponding part
C             of this array contains no useful information.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     TU      (output) DOUBLE PRECISION array, dimension (LDTU,M2)
C             The leading M2-by-M2 part of this array contains the
C             control transformation matrix TU.
C
C     LDTU    INTEGER
C             The leading dimension of the array TU.  LDTU >= max(1,M2).
C
C     TY      (output) DOUBLE PRECISION array, dimension (LDTY,NP2)
C             The leading NP2-by-NP2 part of this array contains the
C             measurement transformation matrix TY.
C
C     LDTY    INTEGER
C             The leading dimension of the array TY.
C             LDTY >= max(1,NP2).
C
C     RCOND   (output) DOUBLE PRECISION array, dimension (2)
C             RCOND(1) contains the reciprocal condition number of the
C                      control transformation matrix TU;
C             RCOND(2) contains the reciprocal condition number of the
C                      measurement transformation matrix TY.
C             RCOND is set even if INFO = 1 or INFO = 2; if INFO = 1,
C             then RCOND(2) was not computed, but it is set to 0.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             Tolerance used for controlling the accuracy of the applied
C             transformations. Transformation matrices TU and TY whose
C             reciprocal condition numbers are less than TOL are not
C             allowed. If TOL <= 0, then a default value equal to
C             sqrt(EPS) is used, where EPS is the relative machine
C             precision.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) contains the optimal
C             LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= MAX( M2 + NP1*NP1 + MAX(NP1*N,3*M2+NP1,5*M2),
C                            NP2 + M1*M1  + MAX(M1*N,3*NP2+M1,5*NP2),
C                            N*M2, NP2*N, NP2*M2, 1 )
C             where M1 = M - M2 and NP1 = NP - NP2.
C             For good performance, LDWORK must generally be larger.
C             Denoting Q = MAX(M1,M2,NP1,NP2), an upper bound is
C             MAX(1,Q*(Q+MAX(N,5)+1)).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if the matrix D12 had not full column rank in
C                   respect to the tolerance TOL;
C             = 2:  if the matrix D21 had not full row rank in respect
C                   to the tolerance TOL;
C             = 3:  if the singular value decomposition (SVD) algorithm
C                   did not converge (when computing the SVD of D12 or
C                   D21).
C
C     METHOD
C
C     The routine performs the transformations described in [1], [2].
C
C     REFERENCES
C
C     [1] Zhou, K., Doyle, J.C., and Glover, K.
C         Robust and Optimal Control.
C         Prentice-Hall, Upper Saddle River, NJ, 1996.
C
C     [2] Balas, G.J., Doyle, J.C., Glover, K., Packard, A., and
C         Smith, R.
C         mu-Analysis and Synthesis Toolbox.
C         The MathWorks Inc., Natick, Mass., 1995.
C
C     NUMERICAL ASPECTS
C
C     The precision of the transformations can be controlled by the
C     condition numbers of the matrices TU and TY as given by the
C     values of RCOND(1) and RCOND(2), respectively. An error return
C     with INFO = 1 or INFO = 2 will be obtained if the condition
C     number of TU or TY, respectively, would exceed 1/TOL.
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 1998.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 1999,
C     Feb. 2000.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, H2 optimal control, LQG, LQR, optimal
C     regulator, robust control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER            INFO, LDB, LDC, LDD, LDTU, LDTY, LDWORK, M, N,
     $                   NCON, NMEAS, NP
      DOUBLE PRECISION   TOL
C     ..
C     .. Array Arguments ..
      DOUBLE PRECISION   B( LDB, * ), C( LDC, * ), D( LDD, * ),
     $                   DWORK( * ), RCOND( 2 ), TU( LDTU, * ),
     $                   TY( LDTY, * )
C     ..
C     .. Local Scalars ..
      INTEGER            INFO2, IQ, IWRK, J, LWAMAX, M1, M2, MINWRK,
     $                   ND1, ND2, NP1, NP2
      DOUBLE PRECISION   TOLL
C     ..
C     .. External Functions
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH
C     ..
C     .. External Subroutines ..
      EXTERNAL           DGEMM, DGESVD, DLACPY, DSCAL, DSWAP, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          DBLE, INT, MAX, SQRT
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      M1  = M - NCON
      M2  = NCON
      NP1 = NP - NMEAS
      NP2 = NMEAS
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
         INFO = -4
      ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
         INFO = -5
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
         INFO = -9
      ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
         INFO = -11
      ELSE IF( LDTU.LT.MAX( 1, M2 ) ) THEN
         INFO = -13
      ELSE IF( LDTY.LT.MAX( 1, NP2 ) ) THEN
         INFO = -15
      ELSE
C
C        Compute workspace.
C
         MINWRK = MAX( 1, M2  + NP1*NP1 + MAX( NP1*N, 3*M2 + NP1,
     $                                         5*M2 ),
     $                    NP2 + M1*M1 + MAX( M1*N, 3*NP2 + M1, 5*NP2 ),
     $                    N*M2, NP2*N, NP2*M2 )
         IF( LDWORK.LT.MINWRK )
     $      INFO = -19
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10UD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
     $    .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
         RCOND( 1 ) = ONE
         RCOND( 2 ) = ONE
         DWORK( 1 ) = ONE
         RETURN
      END IF
C
      ND1  = NP1 - M2
      ND2  = M1 - NP2
      TOLL = TOL
      IF( TOLL.LE.ZERO ) THEN
C
C        Set the default value of the tolerance for condition tests.
C
         TOLL = SQRT( DLAMCH( 'Epsilon' ) )
      END IF
C
C     Determine SVD of D12, D12 = U12 S12 V12', and check if D12 has
C     full column rank. V12' is stored in TU.
C     Workspace:  need   M2 + NP1*NP1 + max(3*M2+NP1,5*M2);
C                 prefer larger.
C
      IQ   = M2 + 1
      IWRK = IQ + NP1*NP1
C
      CALL DGESVD( 'A', 'A', NP1, M2, D( 1, M1+1 ), LDD, DWORK,
     $             DWORK( IQ ), NP1, TU, LDTU, DWORK( IWRK ),
     $             LDWORK-IWRK+1, INFO2 )
      IF( INFO2.NE.0 ) THEN
          INFO = 3
          RETURN
      END IF
C
      RCOND( 1 ) = DWORK( M2 )/DWORK( 1 )
      IF( RCOND( 1 ).LE.TOLL ) THEN
          RCOND( 2 ) = ZERO
          INFO = 1
          RETURN
      END IF
      LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
C     Determine Q12.
C
      IF( ND1.GT.0 ) THEN
         CALL DLACPY( 'Full', NP1, M2, DWORK( IQ ), NP1, D( 1, M1+1 ),
     $                LDD )
         CALL DLACPY( 'Full', NP1, ND1, DWORK( IQ+NP1*M2 ), NP1,
     $                DWORK( IQ ), NP1 )
         CALL DLACPY( 'Full', NP1, M2, D( 1, M1+1 ), LDD,
     $                DWORK( IQ+NP1*ND1 ), NP1 )
      END IF
C
C     Determine Tu by transposing in-situ and scaling.
C
      DO 10 J = 1, M2 - 1
         CALL DSWAP( J, TU( J+1, 1 ), LDTU, TU( 1, J+1 ), 1 )
   10 CONTINUE
C
      DO 20 J = 1, M2
         CALL DSCAL( M2, ONE/DWORK( J ), TU( 1, J ), 1 )
   20 CONTINUE
C
C     Determine C1 =: Q12'*C1.
C     Workspace:  M2 + NP1*NP1 + NP1*N.
C
      CALL DGEMM( 'T', 'N', NP1, N, NP1, ONE, DWORK( IQ ), NP1, C, LDC,
     $            ZERO, DWORK( IWRK ), NP1 )
      CALL DLACPY( 'Full', NP1, N, DWORK( IWRK ), NP1, C, LDC )
      LWAMAX = MAX( IWRK + NP1*N - 1, LWAMAX )
C
C     Determine SVD of D21, D21 = U21 S21 V21', and check if D21 has
C     full row rank. U21 is stored in TY.
C     Workspace:  need   NP2 + M1*M1 + max(3*NP2+M1,5*NP2);
C                 prefer larger.
C
      IQ   = NP2 + 1
      IWRK = IQ + M1*M1
C
      CALL DGESVD( 'A', 'A', NP2, M1, D( NP1+1, 1 ), LDD, DWORK, TY,
     $             LDTY, DWORK( IQ ), M1, DWORK( IWRK ), LDWORK-IWRK+1,
     $             INFO2 )
      IF( INFO2.NE.0 ) THEN
          INFO = 3
          RETURN
      END IF
C
      RCOND( 2 ) = DWORK( NP2 )/DWORK( 1 )
      IF( RCOND( 2 ).LE.TOLL ) THEN
          INFO = 2
          RETURN
      END IF
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
C     Determine Q21.
C
      IF( ND2.GT.0 ) THEN
         CALL DLACPY( 'Full', NP2, M1, DWORK( IQ ), M1, D( NP1+1, 1 ),
     $                LDD )
         CALL DLACPY( 'Full', ND2, M1, DWORK( IQ+NP2 ), M1, DWORK( IQ ),
     $                M1 )
         CALL DLACPY( 'Full', NP2, M1, D( NP1+1, 1 ), LDD,
     $                DWORK( IQ+ND2 ), M1 )
      END IF
C
C     Determine Ty by scaling and transposing in-situ.
C
      DO 30 J = 1, NP2
         CALL DSCAL( NP2, ONE/DWORK( J ), TY( 1, J ), 1 )
   30 CONTINUE
C
      DO 40 J = 1, NP2 - 1
         CALL DSWAP( J, TY( J+1, 1 ), LDTY, TY( 1, J+1 ), 1 )
   40 CONTINUE
C
C     Determine B1 =: B1*Q21'.
C     Workspace:  NP2 + M1*M1 + N*M1.
C
      CALL DGEMM( 'N', 'T', N, M1, M1, ONE, B, LDB, DWORK( IQ ), M1,
     $            ZERO, DWORK( IWRK ), N )
      CALL DLACPY( 'Full', N, M1, DWORK( IWRK ), N, B, LDB )
      LWAMAX = MAX( IWRK + N*M1 - 1, LWAMAX )
C
C     Determine B2 =: B2*Tu.
C     Workspace:  N*M2.
C
      CALL DGEMM( 'N', 'N', N, M2, M2, ONE, B( 1, M1+1 ), LDB, TU, LDTU,
     $            ZERO, DWORK, N )
      CALL DLACPY( 'Full', N, M2, DWORK, N, B( 1, M1+1 ), LDB )
C
C     Determine C2 =: Ty*C2.
C     Workspace:  NP2*N.
C
      CALL DGEMM( 'N', 'N', NP2, N, NP2, ONE, TY, LDTY,
     $            C( NP1+1, 1 ), LDC, ZERO, DWORK, NP2 )
      CALL DLACPY( 'Full', NP2, N, DWORK, NP2, C( NP1+1, 1 ), LDC )
C
C     Determine D22 =: Ty*D22*Tu.
C     Workspace:  NP2*M2.
C
      CALL DGEMM( 'N', 'N', NP2, M2, NP2, ONE, TY, LDTY,
     $            D( NP1+1, M1+1 ), LDD, ZERO, DWORK, NP2 )
      CALL DGEMM( 'N', 'N', NP2, M2, M2, ONE, DWORK, NP2, TU, LDTU,
     $            ZERO, D( NP1+1, M1+1 ), LDD )
C
      LWAMAX = MAX( N*MAX( M2, NP2 ), NP2*M2, LWAMAX )
      DWORK( 1 ) = DBLE( LWAMAX )
      RETURN
C *** Last line of SB10UD ***
      END