module stpcomod

!$$$ module documentation block
!           .      .    .                                       .
! module:   stpcomod    module for stpco
!  prgmmr:
!
! abstract: module for stpco
!
! program history log:
!   2010-06-16  todling - based on stpco
!
! subroutines included:
!   sub stpco
!   sub stpcolay_
!   sub stpcolev_
!
! attributes:
!   language: f90
!   machine:
!
!$$$ end documentation block

implicit none

PRIVATE
PUBLIC stpco

contains

!subroutine stpco(cohead,colvkhead,rval,sval,out,sges,nstep)
subroutine stpco(colvkhead,rval,sval,out,sges,nstep)
!$$$  subprogram documentation block
!                .      .    .                                       .
! subprogram:    stpco       call components to calculate contrib. to
!                            penalty and stepsize for carbon monoxide
!   prgmmr: sienkiewicz     org: GMAO                 date: 2009-01-22
!
! abstract: The routine calls individual components that calculate 
!           contribution to the penalty and step size from layer 
!           and level carbon monoxine measurements
!
! program history log:
!   2010-06-14  todling - based on Sienkiewicz carbon monoxine code
!
!   input argument list:
!     cohead
!     colvkhead
!     rval - search direction for carbon monoxine
!     sval - input carbon monoxine correction field
!     sges - step size estimates (nstep)
!     nstep- number of stepsize estimates (==0 means use outer iteration value)
!
!   output argument list:
!     out(1:nstep) - contribution of carbon monoxine data to penalty sges(1:nstep)
!
! attributes:
!   language: f90
!   machine:
!
!$$$  
  use kinds, only: r_kind,r_quad,i_kind
! use obsmod, only: co_ob_type,colvk_ob_type
  use obsmod, only: colvk_ob_type
  use gridmod, only: latlon1n
  use constants, only: zero_quad
  use gsi_bundlemod, only: gsi_bundle
  use mpimod,only :mype
  implicit none

! Declare passed variables

! type(   co_ob_type),pointer          ,intent(in   ) :: cohead
  type(colvk_ob_type),pointer          ,intent(in   ) :: colvkhead
  integer(i_kind)                     ,intent(in   ) :: nstep
  type(gsi_bundle)                    ,intent(in   ) :: sval
  type(gsi_bundle)                    ,intent(in   ) :: rval
  real(r_kind),dimension(max(1,nstep)),intent(in   ) :: sges
  real(r_quad),dimension(max(1,nstep)),intent(inout) :: out

  out=zero_quad
!  write(900+mype,*)'call stpcolev',nstep,'sges',sges,'nstep',nstep,'associated',associated(colvkhead)
!  call flush(900+mype)
!  write(6,*)mype,'top stpco'
!  call flush(6)

! if(associated(cohead))call stpcolay_(  cohead,rval,sval,out,sges,nstep)
!  write(6,*)mype,'stpco associated',associated(colvkhead)
!  write(900+mype,*)mype,'stpco associated',associated(colvkhead)
  !call flush(900+mype)
!  call flush(6)
  if(associated(colvkhead))call stpcolev_(colvkhead,rval,sval,out,sges,nstep)
!  write(900+mype,*)nstep,'finish stopcolev ',out
!  call flush(900+mype)
!  write(6,*)mype,'bot stpco',out
!  call flush(6)

  return

end subroutine stpco
subroutine stpcolev_(colvkhead,rval,sval,out,sges,nstep)
  use kinds, only: r_kind,r_quad,i_kind,r_double
  use obsmod, only: colvk_ob_type
  use constants, only: zero_quad,zero
  use gsi_bundlemod, only: gsi_bundle,gsi_bundlegetpointer
  use jfunc,only : l_foto, xhat_dt,dhat_dt
  use mpimod, only : mype
  use modairsco, only : dpairs,debugco,usewdpij
  use gridmod, only: lat2,lon2,nsig
  implicit none

! Declare passed variables
  type(colvk_ob_type),pointer          ,intent(in   ) :: colvkhead
  integer(i_kind)                     ,intent(in   ) :: nstep
  type(gsi_bundle)                    ,intent(in   ) :: sval
  type(gsi_bundle)                    ,intent(in   ) :: rval
  real(r_kind),dimension(max(1,nstep)),intent(in   ) :: sges
  real(r_quad),dimension(max(1,nstep)),intent(inout) :: out

! Declare local variables
  integer(i_kind) j1,j2,j3,j4,kk,ier,istatus,k,k1,k2,ij,i,j,nuse
  real(r_kind) co,pob
  real(r_kind),dimension(max(1,nstep))::pen
  real(r_kind) w1,w2,w3,w4,w5,w6,w7,w8, time_co
!  real(r_quad) val,val2
  real(r_double) val,val2
  real(r_kind),pointer,dimension(:) :: xhat_dt_cop
  real(r_kind),pointer,dimension(:) :: dhat_dt_cop
  real(r_kind),pointer,dimension(:,:,:) :: rco1dp,sco1dp
  real(r_kind),allocatable, dimension(:,:) :: rco1d,sco1d
  real(r_kind),allocatable,dimension(:) :: vali,vali2
!  real(r_quad),allocatable,dimension(:) :: valdp,val2dp
  real(r_double),allocatable,dimension(:) :: valdp,val2dp
!  real(r_quad),allocatable,dimension(:) ::valak,val2ak
  real(r_double),allocatable,dimension(:) ::valak,val2ak
  type(colvk_ob_type), pointer :: colvkptr
  if(.not.associated(colvkhead))return
!  write(900+mype,*)mype,'top of stpcolev ',nstep,'sges',sges
!  call flush(900+mype)

! Get pointers and return if not found
  ier=0
  call gsi_bundlegetpointer(sval,'co',sco1dp,istatus);ier=istatus+ier
  call gsi_bundlegetpointer(rval,'co',rco1dp,istatus);ier=istatus+ier
  !if(l_foto) then
  !   call gsi_bundlegetpointer(xhat_dt,'co',xhat_dt_cop,istatus);ier=istatus+ier
     !call gsi_bundlegetpointer(dhat_dt,'co',dhat_dt_cop,istatus);ier=istatus+ier
!  endif
!  write(900+mype,*)mype,'ier',ier,'lat2',lat2,lon2,'nsig',nsig
  !call flush(900+mype)
  if(ier/=0) return
!   write(540+mype,*)'stpco nstep',nstep,'sges',sges
! Initialize output variables to zero
  allocate (rco1d(lat2*lon2,nsig),sco1d(lat2*lon2,nsig))
  do k=1,nsig
    ij=0
    do j=1,lon2
      do i=1,lat2
        ij=ij+1
        rco1d(ij,k)=rco1dp(i,j,k)
        sco1d(ij,k)=sco1dp(i,j,k)
      end do
    end do
  end do
!  if(debugco)then
!  write(950+mype,*)'rco1d',maxval(rco1d),minval(rco1d)
!  write(950+mype,*)'sco1d',maxval(sco1d),minval(sco1d),'nstep',nstep
!  call flush(950+mype)
!  endif

  time_co = zero

  colvkptr => colvkhead

! Loop over level co observations
!
  nuse=0
  do while (associated(colvkptr))
    if(colvkptr%luse)then
      if(nstep > 0)then
        nuse=nuse+1
!        if(mod(nuse,10).eq.0)then
         !write(900+mype,*)'stpoz nuse',nuse
         !call flush(900+mype)
        !endif
!       Accumulate contribution from layer observations
!       This repeats the algorithm inside intrp3co
!       with several of the terms already calculated


        allocate(vali(colvkptr%nlco))
        allocate(vali2(colvkptr%nlco))
        allocate(valak(colvkptr%nlco),val2ak(colvkptr%nlco))
        allocate(valdp(colvkptr%nlco),val2dp(colvkptr%nlco))

!       Set location
        j1=colvkptr%ij(1)
        j2=colvkptr%ij(2)
        j3=colvkptr%ij(3)
        j4=colvkptr%ij(4)
        if(usewdpij)then
          do k=1,colvkptr%nlco   ! loop over MOPITT ave. ker. contribution levels
!            val=zero_quad
            val=zero
!            val2=zero_quad
            val2=zero
            do kk=1,colvkptr%nsub(k)
              k1=colvkptr%ksub(k,kk)
              w1=colvkptr%wdpij(k,kk,1)
              w2=colvkptr%wdpij(k,kk,2)
              w3=colvkptr%wdpij(k,kk,3)
              w4=colvkptr%wdpij(k,kk,4)
              val=val+w1*rco1d(j1,k1)+w2*rco1d(j2,k1)+w3*rco1d(j3,k1)+w4*rco1d(j4,k1)
              val2=val2+w1*sco1d(j1,k1)+w2*sco1d(j2,k1)+w3*sco1d(j3,k1)+w4*sco1d(j4,k1)
!              if(nuse.eq.1.and.mype.eq.1)then
!                   write(6,*)'val',k,'kk',kk,'k1',k1,val,'w1',w1,w2,w3,w4
!                write(6,*)'rco1d',rco1d(j1,k1),rco1d(j2,k1),rco1d(j3,k1),rco1d(j4,k1)
!              endif
             end do
             valdp(k)=val
             val2dp(k)=val2
!           if(nuse.eq.1)then
!              write(6,*)mype,'val',k,val,val2
!           endif
          end do
          valak=0.
          val2ak=0.
      
          do k=1,colvkptr%nlco
            do kk=1,colvkptr%nlco
! ajl found bug 8/15/2016
!              valak(k)=valak(k)+colvkptr%ak(k,kk)*valdp(k)
!              val2ak(k)=val2ak(k)+colvkptr%ak(k,kk)*val2dp(k)
              valak(k)=valak(k)+colvkptr%cohmat(k,kk)*valdp(kk)
              val2ak(k)=val2ak(k)+colvkptr%cohmat(k,kk)*val2dp(kk)
            end do
            val2ak(k)=val2ak(k)-colvkptr%res(k)
!           if(nuse.eq.1)then
!             write(6,*)mype,'val2ak',k,val2ak(k),'res',colvkptr%res(k),'valak',valak(k)
!           endif
          end do
        else
        
          do k=1,colvkptr%nlco   ! loop over MOPITT ave. ker. contribution levels
          
            pob = colvkptr%prs(k)

            k1=int(pob)
            k2=min(k1+1,nsig)
            w1=colvkptr%wij(1,k)
            w2=colvkptr%wij(2,k)
            w3=colvkptr%wij(3,k)
            w4=colvkptr%wij(4,k)
            w5=colvkptr%wij(5,k)
            w6=colvkptr%wij(6,k)
            w7=colvkptr%wij(7,k)
            w8=colvkptr%wij(8,k)
            val=w1*rco1d(j1,k1)+w2*rco1d(j2,k1)+w3*rco1d(j3,k1)+w4*rco1d(j4,k1)+ &
              w5*rco1d(j1,k2)+w6*rco1d(j2,k2)+w7*rco1d(j3,k2)+w8*rco1d(j4,k2)
            val2=w1*sco1d(j1,k1)+w2*sco1d(j2,k1)+w3*sco1d(j3,k1)+w4*sco1d(j4,k1)+ &
              w5*sco1d(j1,k2)+w6*sco1d(j2,k2)+w7*sco1d(j3,k2)+w8*sco1d(j4,k2)
            vali(k)=val
            vali2(k)=val2
          end do
!         put on layers with dp
          do k=colvkptr%nlco,2, -1
            vali(k)=.5*(vali(k)+vali(k-1))*dpairs(k)
!           vali2(k)=.5*(vali2(k)+vali2(k-1))*dpairs(k)-colvkptr%res(k)
            vali2(k)=.5*(vali2(k)+vali2(k-1))*dpairs(k)
          end do
          vali(1)=vali(2)
          vali2(1)=vali2(2)
!         do k=1,colvkptr%nlco
!           write(780+mype,*)'vali',vali(k),' dp ',valdp(k)
           !write(780+mype,*)'val2i',vali2(k),' dp ',val2dp(k)
!         end do
        
!         need to apply kernel to put on res levels
          valak=0.
          val2ak=0.
      
          do k=1,colvkptr%nlco
            do kk=1,colvkptr%nlco
!              valak(k)=valak(k)+colvkptr%ak(k,kk)*vali(k)
!              val2ak(k)=val2ak(k)+colvkptr%ak(k,kk)*vali2(k)
              valak(k)=valak(k)+colvkptr%cohmat(k,kk)*vali(kk)
              val2ak(k)=val2ak(k)+colvkptr%cohmat(k,kk)*vali2(kk)
            end do
            val2ak(k)=val2ak(k)-colvkptr%res(k)
          end do
        endif

        do k=1,colvkptr%nlco 
          if(colvkptr%raterr2(k).ne.0)then
          do kk=1,nstep
            co=val2ak(k)+sges(kk)*valak(k)
            pen(kk)=colvkptr%err2(k)*co*co
          end do
          out(1)=out(1)+pen(1)*colvkptr%raterr2(k)
          do kk=2,nstep
            out(kk)=out(kk)+(pen(kk)-pen(1))*colvkptr%raterr2(k)
          end do
          endif
        end do
        
      else
        do k=1,colvkptr%nlco 
          pen(1)=colvkptr%res(k)*colvkptr%res(k)*colvkptr%err2(k)
          out(1)=out(1)+pen(1)*colvkptr%raterr2(k)
          do kk=2,nstep
            out(kk)=out(kk)+(pen(kk)-pen(1))*colvkptr%raterr2(k)
          end do
        end do
       endif
      deallocate (vali,vali2,valak,val2ak)
      deallocate (valdp,val2dp)
    endif
    colvkptr=>colvkptr%llpoint
  enddo
  deallocate (rco1d,sco1d)
  
  return
end subroutine stpcolev_


end module stpcomod
