program allcostf
!
!**********************************************************************
! *              .      .    .
! * PROGRAM:    allcostf
! * PRGMMR:     M. ZUPANSKI     ORG: CIRA/CSU   DATE: 2003-09-05
! *
! * ABSTRACT:   TOTAL COST FUNCTION
! *             Calculate total cost function 
! *             Add obs + background
! *
! * PROGRAM LOG:
! *
! * 09/05/2003    ..... M. ZUPANSKI:
! *
! **********************************************************************
!   INPUT:    z(i)     = control variable (ensemble subspace)
!
!   INPUT:    sqrtIPA_inv = (I+A)**(-1/2) matrix
!--------------------------------------------------------------------
!-----
  integer,parameter::IpA_file=21     ! (I+A)**(-1/2)
  integer,parameter::icntrl=22       ! cntrl vrbl (ensemble space)
  integer,parameter::icostobs=23     ! observational cost function
  integer,parameter::icost=51        ! total cost function 
!-----
 real COST_b,COST_obs,COST_pen,COST_total
 integer iout,ioutm1,index

 real,allocatable::sqrtIPA_inv(:,:)
 real,allocatable::z(:)
 real,allocatable::zz(:)

 integer NENS
 integer NENS_START
!
!      DECLARE NAMELIST
!
      NAMELIST /ENSEMBLE_SIZE/ NENS_START,NENS
      NAMELIST /CURRENT_ITERATION/ iout,ioutm1,index

!=========================================================
!==============start calculation===================
        write(*,*) "start allcostf"

!-- read current iteration

       REWIND 13
       READ(13,CURRENT_ITERATION)

       write(*,*) "allcostf: iout,ioutm1,index=",iout,ioutm1,index

!-- read ensemble size

       REWIND 15
       READ(15,ENSEMBLE_SIZE)

       write(*,*) "allcostf: N_START, NENS=",NENS_START,NENS

!--- read (I+A)**(-1/2) ---------------------------
      allocate(sqrtIPA_inv(NENS_START:NENS,NENS_START:NENS))

      REWIND IpA_file
      READ(IpA_file) ((sqrtIPA_inv(i,j),i=NENS_START,NENS),j=NENS_START,NENS)

!    read the cntrl vrbl in ensemble space (z)

        allocate(z(NENS_START:NENS))

      CLOSE(icntrl)
      OPEN(UNIT=icntrl,FILE='z_cntrl',FORM='UNFORMATTED',IOSTAT=IER)
      IF(IER.NE.0) WRITE(*,*)' icntrl OPEN UNIT ERROR IER=',IER
      READ(icntrl) z
      CLOSE(icntrl,status='KEEP')

!==================================================
!-- zz=(I+A)**(-1/2)*z

         allocate(zz(NENS_START:NENS))

         do i=NENS_START,NENS
           zz(i)=0.0
          do j=NENS_START,NENS
           zz(i)=zz(i)+sqrtIPA_inv(i,j)*z(j)
          end do
         end do

         deallocate(z)
         deallocate(sqrtIPA_inv)

!-- background cost function

       COST_b=0.0
      do i=NENS_START,NENS
       COST_b = COST_b + 0.5*zz(i)*zz(i)
      end do

         deallocate(zz)

!-- observational cost function

      CLOSE(icostobs)
      OPEN(UNIT=icostobs,FILE='costfobs',FORM='UNFORMATTED',IOSTAT=IER)
      READ(icostobs) COST_obs
      CLOSE(icostobs,STATUS='KEEP')

      write(*,*) "Allcostf: COST_b=",COST_b," COST_obs=",COST_obs

      COST_total=COST_b+COST_obs

      write(*,*) "Allcostf: COST_total=",COST_total

!-- no gravity wave penalty at this time 
      COST_pen=0.0

            WRITE(3,*) "=============================="
            WRITE(3,*) "== COST FUNCTION ============="
            WRITE(3,*) "=============================="
            WRITE(3,10) COST_obs
            WRITE(3,20) COST_pen
            WRITE(3,30) COST_b
            WRITE(3,100) COST_total
            WRITE(3,*) "=============================="

 10   FORMAT('OBS COST FUNCTION  = ',E15.5)
 20   FORMAT('PEN COST FUNCTION  = ',E15.5)
 30   FORMAT('BACK COST FUNCTION = ',E15.5)
 100  FORMAT('TOTAL COST FUNCTION (OBS+PEN+BACK) = ',E20.10)


      CLOSE(icost)
      OPEN(UNIT=icost,FILE='outcostc',FORM='FORMATTED',IOSTAT=IER)
      WRITE(icost,200) ioutm1,COST_obs,COST_pen,COST_b,COST_total
      CLOSE(icost,STATUS='KEEP')
 200  FORMAT(I5,4E20.10)

!====================================================================

 stop
 end