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

subroutine SWTNS_init (CCTK_ARGUMENTS)
  implicit none
  DECLARE_CCTK_ARGUMENTS
  DECLARE_CCTK_FUNCTIONS
  DECLARE_CCTK_PARAMETERS
  integer :: i, j, k
  !CCTK_REAL :: TOV_R global
  CCTK_REAL :: pio, helper    
  pio = 3.14159265359
  TOV_surface = TOV_R
  write(*,*)"Check if TOV_R is global..."
  write(*,*)"Radius = ", TOV_R
  write(*,*) "Surface = " , TOV_surface
  write(*,*) "precission limit for grid points = " , rprec
  write(*,*) "grid boundary = " , gridboundary
  write(*,*)"start initial"

!$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)
		grid_r(i,j,k) = r(i,j,k)
	       end do
	end do
  end do
!$OMP END PARALLEL DO

!$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( Seve_in_cube(i,j,k) == 1 ) then !true for cube
		   if (x(i,j,k) == 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		!negative direction
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		!combinations
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) =  r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		 ! more combinations
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   else if (x(i,j,k) > 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) < 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = -r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) == 0 .AND. y(i,j,k) < 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = 0
		   dydr(i,j,k) = -r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) == 0 .AND. z(i,j,k) > 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = 0
		   dzdr(i,j,k) = r(i,j,k)/sqrt(z(i,j,k)**2)
		   else if (x(i,j,k) < 0 .AND. y(i,j,k) > 0 .AND. z(i,j,k) == 0 ) then
		   dxdr(i,j,k) = -r(i,j,k)/sqrt(x(i,j,k)**2)
		   dydr(i,j,k) = r(i,j,k)/sqrt(y(i,j,k)**2)
		   dzdr(i,j,k) = 0
		   end if
	        else if( Seve_in_cube(i,j,k) == 0 ) then
		dxdr(i,j,k) = iJ13(i,j,k)
		dydr(i,j,k) = iJ23(i,j,k)
		dzdr(i,j,k) = iJ33(i,j,k)
		end if	
	   end do
	end do
  end do
!$OMP END PARALLEL DO
	
write(*,*) "Coordinate transformation factors for spatial derivatives have been prepared."

!$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)
		!grid_r = r(i,j,k)
		  if(grid_r(i,j,k) == 0) then
		  PHI(i,j,k)  = TOV_PHI(i,j,k)
		  drPHI(i,j,k) = 0
		  LAMBDA(i,j,k) = 0 
		  gappa(i,j,k)  = (rho(i,j,k)+press(i,j,k))*TOV_Gamma
		  P(i,j,k) = 0
		  Qr(i,j,k) = 0
		  else 
		  PHI(i,j,k)  = TOV_PHI(i,j,k)
		  drPHI(i,j,k) = (TOV_mr(i,j,k) + 4*pio*r(i,j,k)**3 * press(i,j,k))/(r(i,j,k)**2 *(1-2*TOV_mr(i,j,k)/r(i,j,k)))
		  LAMBDA(i,j,k) = -0.5*log(1-2*TOV_mr(i,j,k)/r(i,j,k))
  		  gappa(i,j,k)  = (rho(i,j,k)+press(i,j,k))*TOV_Gamma
		  P(i,j,k) = (gappa(i,j,k) * press(i,j,k) * EXP(LAMBDA(i,j,k) + 3*PHI(i,j,k)))/(r(i,j,k)**2)
		  Qr(i,j,k) = 1/(r(i,j,k)**2)*EXP(LAMBDA(i,j,k) + 3*PHI(i,j,k))*(rho(i,j,k) + press(i,j,k))*(drPHI(i,j,k)**2 &
				 + 4*drPHI(i,j,k)/r(i,j,k) -8*pio*EXP(2*LAMBDA(i,j,k))*press(i,j,k))
		  end if
	       end do
	end do
  end do
!$OMP END PARALLEL DO
write(*,*) "Calculated metric potentials, Qr and P"

!$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)
		helper= (rho(i,j,k) + press(i,j,k)) *EXP(3*LAMBDA(i,j,k) + PHI(i,j,k))
		 if ( helper == 0 ) then
		Wrinv(i,j,k) = 0
                else 
 		Wrinv(i,j,k) = (r(i,j,k)**2)/( (rho(i,j,k) + press(i,j,k)) *EXP(3*LAMBDA(i,j,k) + PHI(i,j,k)) )
		end if
	end do
    end do
end do
!$OMP END PARALLEL DO
write(*,*) "Calculated Wrinv"

call SWTNS_drP(CCTK_ARGUMENTS)

!$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)
		  !grid_r = r(i,j,k)
		  if(grid_r(i,j,k) == 0) then
		  Xi(i,j,k)  = (sin(pio*r(i,j,k)/TOV_surface)*cos(pio*r(i,j,k)/TOV_surface)-pio*r(i,j,k)/TOV_surface)*SWTNS_amplitude
		  Xeta(i,j,k) = 0
		  drXi(i,j,k) = pio * SWTNS_amplitude *(cos(2*pio*r(i,j,k)/TOV_surface)-1)/TOV_surface 
		  drrXi(i,j,k) = -(2*pio**2*SWTNS_amplitude*sin(2*pio*r(i,j,k)/TOV_surface))/(TOV_surface**2)
		  Pi(i,j,k)  = 0
		  Xidot(i,j,k) = 0
   		  Pidot(i,j,k)  = 0
		  else if (grid_r(i,j,k) <= TOV_surface .AND. grid_r(i,j,k) >0 ) then
		  Xi(i,j,k)  = (sin(pio*r(i,j,k)/TOV_surface)*cos(pio*r(i,j,k)/TOV_surface)-pio*r(i,j,k)/TOV_surface)*SWTNS_amplitude
		  Xeta(i,j,k) = Xi(i,j,k)/(r(i,j,k)**2) * exp(PHI(i,j,k))
		  drXi(i,j,k) = pio * SWTNS_amplitude *(cos(2*pio*r(i,j,k)/TOV_surface)-1)/TOV_surface 
		  drrXi(i,j,k) = -(2*pio**2*SWTNS_amplitude*sin(2*pio*r(i,j,k)/TOV_surface))/(TOV_surface**2)
   		  Pi(i,j,k)  = 0
		  Xidot(i,j,k) = 0
   		  Pidot(i,j,k)  = 0
		  else if (grid_r(i,j,k) > TOV_surface) then
		  Xi(i,j,k)  = 0
		  Xeta(i,j,k) = 0
		  drXi(i,j,k) = 0
		  drrXi(i,j,k) = 0
   		  Pi(i,j,k)  = 0
                  Xidot(i,j,k) = 0
   		  Pidot(i,j,k)  = 0
               	  end if
	       end do
	end do
  end do
!$OMP END PARALLEL DO

write(*,*)"calculated initial fluid displacemnts, initial amplitude of Xi is: Xi = " , SWTNS_amplitude
 
  write(*,*)"done initial!"
 	  
end subroutine SWTNS_init 

subroutine SWTNS_drP (CCTK_ARGUMENTS)
implicit none
  DECLARE_CCTK_ARGUMENTS
  DECLARE_CCTK_FUNCTIONS
  DECLARE_CCTK_PARAMETERS
  
   CCTK_INT, parameter :: izero = 0
   CCTK_INT :: i, j, k
       
   integer, parameter :: ik = kind (izero)
   
   integer :: na, nb, nc
   
   CCTK_REAL, dimension(:,:,:), allocatable &
        :: dxP, dyP, dzP, param_dx, param_dy, param_dz

   na = cctk_lsh(1); nb = cctk_lsh(2); nc = cctk_lsh(3)

 allocate( dxP(na, nb, nc), &
           dyP(na, nb, nc), &
           dzP(na, nb, nc), &
	   param_dx(na,nb,nc), &
	   param_dy(na,nb,nc), &
	   param_dz(na,nb,nc) )

             call globalDiff_gv (cctkGH, 0_ik, P, dxP, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33,-1_ik)
             call globalDiff_gv (cctkGH, 1_ik, P, dyP, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)
             call globalDiff_gv (cctkGH, 2_ik, P, dzP, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)
                             
            ! drP = dxP*dxdr + dyP*dydr + dzP*dzdr
		drP =dxP*dxdr +dyP*dydr + dzP*dzdr


	!check derivative algorithem with drPHI
	
	    call globalDiff_gv (cctkGH, 0_ik, PHI, param_dx, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33,-1_ik)

             call globalDiff_gv (cctkGH, 1_ik, PHI, param_dy, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)

             call globalDiff_gv (cctkGH, 2_ik, PHI, param_dz, J11, J21, J31, &
                                  J12, J22, J32, J13, J23, J33, -1_ik)

		param_dr =param_dx*dxdr +param_dy*dydr + param_dz*dzdr

deallocate ( dxP, dyP, dzP,param_dx, param_dy,param_dz)
  write(*,*) "Calculated drP"
end subroutine SWTNS_drP