! nonling.src: Collection of procedures to execute likelihood evaluation using the particle filter.
!The likelihood function is associated with the optimal growth model, outlined in Section 10.2.2.2

!The collection of procedures contained in ara.prg facilitate the construction of the policy function for determining
!consumption. Inputs provided by these procedures are denoted by lines ending with /!** 
!some more global constants
!	REAL(8), PARAMETER :: LOG_LBD=2.2250738585072013E-308_DP
!	REAL(8), PARAMETER :: LOG_UBD=1.7976931348623157E+308_DP
!	REAL(8), PARAMETER :: EXP_LBD=-708.2964185322641_DP
!	REAL(8), PARAMETER :: EXP_UBD=709.682712893384_DP


! Begin with a collection of globals 

nparts = 20000;		! size of particle swarms
lnlkadj = 400;		! adjustement to the likelihood function that prevents underflows
!big0 = zeros(nparts,1);	! not used in fortran version 
!big1 = ones(nparts,1);	! not used in fortran version 

!common random numbers
!WE USE "Mersenne Twister" random number generator
!http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/emt.html
call INIT_GENRAND(123456789)
!use for drawing for initial state of a and k in partproc
CRN_RNDN0=rndn(2,nparts) !rndn and rndu are not intrinsic fortran functions
!use for epsilons in partproc
CRN_RNDN1=rndn(nobs,nparts)
!use for resampling in partproc
CRN_RNDU0=rndu(nobs,nparts)


subroutine lkeval(datt,veet,s_1,opt_gam_from_polcfn,wmat,lnnormcons,z,k,lkvaladj)
implicit none
real(8),intent(in)::datt(2),veet,s_1(2),wmat(2,2),lnnormcons,opt_gam_from_polcfn(ordz*ordk)
real(8),intent(out)::z,k,lkvaladj
!local 
real(8)::z_1,k_1,eps,y,c,i,c_1,c_1e,y_1,i_1,ypred,cpred,cprede,ipred
real(8)::uy,uc,ui,u(2),kern,lnkern,lnlkvaladj,kmin,znormL,knormL
integer(4)::nn
real(8)::tlocal
tlocal=rtc()

    z_1 = s_1(1)
    k_1 = s_1(2)
    eps = veet
    y = datt(1)
    i = datt(2)

    ! convert yesterday's a and k into today's a and k 
    !the 'if' statements check that states lie between lower and upper bounds
	if(isnan(k_1))k_1=kl 
	if(isnan(z_1))z_1=zl 
   	znormL = z_1/omegaz
   	knormL = (k_1-kss)/omegak
	if(znormL.lt.-1.0_8)znormL=-1.0_8
	if(znormL.gt.1.0_8)znormL=1.0_8

	if(knormL.lt.-1.0_8)knormL=-1.0_8
	if(knormL.gt.1.0_8)knormL=1.0_8
	
    c_1 = ceeofzk(znormL,knormL,opt_gam_from_polcfn)
    y_1 = newexp(z_1)*(k_1**alp) ! alp = alpha
    i_1 = y_1-c_1
    k = i_1+(1 - del)*k_1; ! del = delta
    z = rho*z_1 + eps

    ! construct predictions for todays y, c, i
    
	if(isnan(k))k=kl 
	if(isnan(z))z=zl 
   	znormL = z/omegaz
   	knormL = (k-kss)/omegak
	if(znormL.lt.-1.0_8)znormL=-1.0_8
	if(znormL.gt.1.0_8)znormL=1.0_8

	if(knormL.lt.-1.0_8)knormL=-1.0_8
	if(knormL.gt.1.0_8)knormL=1.0_8

    ypred = newexp(z)*(k**alp)
    cpred = ceeofzk(znormL,knormL,opt_gam_from_polcfn)
    ipred = ypred-cpred


    ! construct implied errors 
    uy = y - ypred;!partuy=uy
    ui = i - ipred;!partui=ui
    u = (/uy,ui/)

	lnkern = -0.5*dot_product(u,matmul(wmat,u))
    lnlkvaladj = lnnormcons+lnkern+lnlkadj

	lkvaladj = exp(lnlkvaladj)
	lkevaltime=lkevaltime+(rtc()-tlocal)
end subroutine lkeval
!****************************************************************
subroutine partproc(opt_gam_from_polcfn,wmat,normcons,s0tilde,cvcvs0tilde,lnlk)
implicit none
real(8),intent(in)::opt_gam_from_polcfn(ordz*ordk),wmat(2,2),normcons,s0tilde(2),cvcvs0tilde(2,2)
real(8),intent(out)::lnlk

!local 
real(8)::rsnewz(nparts)
real(8)::rsnewk(nparts)
real(8)::rslk(nparts)
real(8)::lnlts(nobs)
real(8)::newz(nparts)
real(8)::newk(nparts)
real(8)::lk(nparts)
real(8)::probs(nparts)
real(8)::csprobs(nparts)
real(8)::s0tildedraw(2),xz,xk,xeps,udraw,sumlk


integer(4)::arows,krows,jjj,drawind,drawind1,iii
integer(4)::inddiff,di2
real(8)::tlocal
tlocal=rtc()

rsnewz = 0.0_8    ! will contain resampled updated (over time) as and ks: a' and k' 
rsnewk = 0.0_8
rslk = 0.0_8     ! will contain resampled likelihood values 

lnlts = 0.0_8      ! will contain date-t log likelihood values 
do jjj=1,nobs ! index over dates 
newz = 0.0_8         ! will contain updated as and ks passed to the next round 
newk = 0.0_8
lk = 0.0_8
	do iii=1,nparts  ! index over particles 
		! draw initial state 
		if(jjj .eq. 1)then
			s0tildedraw = matmul(cvcvs0tilde,CRN_RNDN0(:,iii))
			xz =s0tildedraw(2)
			xk = kss*newexp(s0tildedraw(1))
    		xeps = crn_rndn1(jjj,iii)*sige
		else
			xz = rsnewz(iii)
			xk = rsnewk(iii)
    		xeps = crn_rndn1(jjj,iii)*sige
		endif

		call lkeval(rawdat1(jjj,:),xeps,(/xz,xk/),opt_gam_from_polcfn,wmat,normcons,newz(iii),newk(iii),lk(iii))

	enddo !iii=1,nparts index over particles

where(lk.le.log_lbd)lk=0.0_8
sumlk=sum(lk,1)
probs = lk/sumlk
csprobs = cumsum(probs)
lnlts(jjj) = newlog(sum(lk,1)/real(nparts,8))-lnlkadj

! resampling 	
	!prev_minloc=1
	do iii=1,nparts  !756 index over particles 
		udraw=crn_rndu0(jjj,iii)
		drawind=myminloc1(csprobs,udraw) !finds the location of the min(csprobs-udraw)
	   ! draw new state 
		rsnewz(iii) = newz(drawind)
		rsnewk(iii) = newk(drawind)
		rslk(iii) = lk(drawind)
	enddo !756 index over particles
enddo !loop over data points

lnlk = sum(lnlts,1)
partproctime=partproctime+(rtc()-tlocal)
end subroutine partproc

