#include "cctk.h"
#include "cctk_Arguments.h"
#include "cctk_Parameters.h"
#include "cctk_Functions.h"

subroutine SWTNS_calc_rhs (CCTK_ARGUMENTS)
   implicit none
  DECLARE_CCTK_ARGUMENTS
  DECLARE_CCTK_FUNCTIONS
  DECLARE_CCTK_PARAMETERS
  	
 CCTK_INT, parameter :: izero = 0
   CCTK_INT :: i, j, k, spec_i, spec_j, spec_k
   CCTK_REAL :: pio
    
   integer, parameter :: ik = kind (izero)
   
   integer :: na, nb, nc 
   
   CCTK_REAL, dimension(:,:,:), allocatable &
        :: TOVold, dxXi, dyXi, dzXi, &
            dxPi, dyPi, dzPi, dxH, dyH, dzH

   na = cctk_lsh(1); nb = cctk_lsh(2); nc = cctk_lsh(3)
 !   write(*,*)"allocate RHS..."
  
    pio=acos(-1.0d0)
    !write(*,*) "Surface = " , TOV_surface
allocate( dxXi(na, nb, nc), &
           dyXi(na, nb, nc), &
           dzXi(na, nb, nc), &
           dxPi(na, nb, nc), &
           dyPi(na, nb, nc), &
           dzPi(na, nb, nc), &
           dxH(na, nb, nc), &
           dyH(na, nb, nc), &
           dzH(na, nb, nc) )

!call SWTNS_derivatives (CCTK_ARGUMENTS)  
!!! Wave Eq.
     !    Xidot = Pi
  	! Hdot = drPi
  	! Pidot = A*H + B*drH + C*Xi

!write(*,*) "Define Xeta"

!Boundary Xi(r=0) = 0 =Xi(r=R)

!updating TOV surface in PostStep
!Updating TOV_surface 

surfaceloop: do k=1,cctk_lsh(3)
 	do j=1,cctk_lsh(2)
  	      do i=1,cctk_lsh(1)
  		if(patchnumber(i,j,k) == 1) then 
		 if (grid_r(i,j,k) >= (TOV_surface - 2*rprec) .AND. grid_r(i,j,k) <= (TOV_surface) .AND. Xi(i,j,k) /= 0) then
		 TOV_surface = TOV_R + Xeta(i,j,k) !or Xi
		 exit surfaceloop
		 end if
		else
		 exit surfaceloop
		end if
	      end do
	end do 
  end do surfaceloop

 
!Ableitung drXi

           call globalDiff_gv (cctkGH, 0_ik, Xi, dxXi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33,-1_ik)
           call globalDiff_gv (cctkGH, 1_ik, Xi, dyXi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)
           call globalDiff_gv (cctkGH, 2_ik, Xi, dzXi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)

	   !drXi = dxXi*dxdr + dyXi*dydr + dzXi*dzdr

!Ableitung drPi
           call globalDiff_gv (cctkGH, 0_ik, Pi, dxPi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33,-1_ik)
           call globalDiff_gv (cctkGH, 1_ik, Pi, dyPi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)
           call globalDiff_gv (cctkGH, 2_ik, Pi, dzPi, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)

	   !drPi = dxPi*dxdr + dyPi*dydr + dzPi*dzdr


!Ableitung drH
           call globalDiff_gv (cctkGH, 0_ik, H, dxH, J11, J21, J31, &
                                 J12, J22, J32, J13, J23, J33,-1_ik)
           call globalDiff_gv (cctkGH, 1_ik, H, dyH, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)
           call globalDiff_gv (cctkGH, 2_ik, H, dzH, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)

           !drH = dxH*dxdr + dyH*dydr + dzH*dzdr


!!! Wave Eq.
         !Pidot = A*H + B*drH + C*Xi 
	  !Hdot = drPi
	 !Xidot = Pi 

!$OMP PARALLEL DO private(i,j,k)
do k = 1, cctk_lsh(3)
     do j = 1, cctk_lsh(2)
        do i = 1, cctk_lsh(1)
		if (grid_r(i,j,k) <= 2*rprec ) then !r=0 boundaries
		  Xi(i,j,k) = 0
		  Xeta(i,j,k) = 0
		  drXi(i,j,k) = dxXi(i,j,k)*dxdr(i,j,k) + dyXi(i,j,k)*dydr(i,j,k) + dzXi(i,j,k)*dzdr(i,j,k)
		  Pi(i,j,k) = 0
		  Xidot(i,j,k) = Pi(i,j,k)
		  drPi(i,j,k) = dxPi(i,j,k)*dxdr(i,j,k) + dyPi(i,j,k)*dydr(i,j,k) + dzPi(i,j,k)*dzdr(i,j,k)
		  Hdot(i,j,k) = drPi(i,j,k)
		  drH(i,j,k) = dxH(i,j,k)*dxdr(i,j,k) + dyH(i,j,k)*dydr(i,j,k) + dzH(i,j,k)*dzdr(i,j,k)
		  Pidot(i,j,k) = 0
		else if (grid_r(i,j,k) < (TOV_surface - 2*rprec) .AND. grid_r(i,j,k) > 0 )then 
	          Xeta(i,j,k) = Xi(i,j,k)*exp(PHI(i,j,k))/(r(i,j,k)**2)
		  drXi(i,j,k) = dxXi(i,j,k)*dxdr(i,j,k) + dyXi(i,j,k)*dydr(i,j,k) + dzXi(i,j,k)*dzdr(i,j,k)
		  Xidot(i,j,k) = Pi(i,j,k)
		  drPi(i,j,k) = dxPi(i,j,k)*dxdr(i,j,k) + dyPi(i,j,k)*dydr(i,j,k) + dzPi(i,j,k)*dzdr(i,j,k)
		  Hdot(i,j,k) = drPi(i,j,k)
		  drH(i,j,k) = dxH(i,j,k)*dxdr(i,j,k) + dyH(i,j,k)*dydr(i,j,k) + dzH(i,j,k)*dzdr(i,j,k)
		  Pidot(i,j,k) = A(i,j,k)*H(i,j,k) + B(i,j,k)*drH(i,j,k) + C(i,j,k)*Xi(i,j,k)
		else if (grid_r(i,j,k) >= (TOV_surface - 2*rprec) .AND. grid_r(i,j,k) <= (TOV_surface + rprec) )then 
		  Xeta(i,j,k) = Xi(i,j,k)*exp(PHI(i,j,k))/(r(i,j,k)**2)
		  drXi(i,j,k) = 0
		  H(i,j,k) = 0
		  Xidot(i,j,k) = Pi(i,j,k)
		  drPi(i,j,k) = 0
		  Hdot(i,j,k) = drPi(i,j,k)
		  drH(i,j,k) = dxH(i,j,k)*dxdr(i,j,k) + dyH(i,j,k)*dydr(i,j,k) + dzH(i,j,k)*dzdr(i,j,k)
		  Pidot(i,j,k) = B(i,j,k)*drH(i,j,k) + C(i,j,k)*Xi(i,j,k)
		else if (grid_r(i,j,k) > (TOV_surface +rprec) )then 
		  Xi(i,j,k)=0
		  Xeta(i,j,k) = 0
		  drXi(i,j,k) = 0
		  H(i,j,k) = 0 
		  Pi(i,j,k) = 0
		  Xidot(i,j,k) = 0
		  drPi(i,j,k) = 0
		  Hdot(i,j,k) = 0
		  drH(i,j,k) = 0
		  Pidot(i,j,k) = 0 
		end if
         end do
    end do
end do
!$OMP END PARALLEL DO

	deriv_error = drXi - H

deallocate ( dxXi, dyXi, dzXi, dxPi, dyPi, dzPi, dxH, dyH, dzH)  
end subroutine SWTNS_calc_rhs