MODULE etat0_williamson_mod
  USE icosa
  PRIVATE
    REAL(rstd), PARAMETER :: h0=8.E3
    REAL(rstd), PARAMETER :: R0=4
    REAL(rstd), PARAMETER :: K0=7.848E-6
    REAL(rstd), PARAMETER :: omega0=K0
    
    PUBLIC  etat0_williamson, etat0_williamson_new
    
CONTAINS
  
  
  SUBROUTINE etat0_williamson(f_h,f_u)
  USE icosa
  IMPLICIT NONE
    TYPE(t_field),POINTER :: f_h(:)
    TYPE(t_field),POINTER :: f_u(:)
  
    REAL(rstd),POINTER :: h(:)
    REAL(rstd),POINTER :: u(:)
    INTEGER :: ind
    
    DO ind=1,ndomain
      CALL swap_dimensions(ind)
      CALL swap_geometry(ind)
      h=f_h(ind)
      u=f_u(ind)
      CALL compute_etat0_williamson(h, u)
    ENDDO

  END SUBROUTINE etat0_williamson 
  
  SUBROUTINE etat0_williamson_new(f_phis,f_mass,f_theta_rhodz,f_u, f_q)
  USE icosa
  USE mpipara, ONLY : is_mpi_root
  USE disvert_mod, ONLY : caldyn_eta, eta_lag

  IMPLICIT NONE
    TYPE(t_field),POINTER :: f_phis(:)
    TYPE(t_field),POINTER :: f_mass(:)
    TYPE(t_field),POINTER :: f_theta_rhodz(:)
    TYPE(t_field),POINTER :: f_u(:)
    TYPE(t_field),POINTER :: f_q(:)
  
    REAL(rstd),POINTER :: phis(:)
    REAL(rstd),POINTER :: h(:,:)
    REAL(rstd),POINTER :: theta_rhodz(:,:)
    REAL(rstd),POINTER :: u(:,:)
    INTEGER :: ind

    IF(caldyn_eta /= eta_lag) THEN
       IF(is_mpi_root) PRINT *, 'etat0_type=williamson91.5 (Williamson,1991) must be used with caldyn_eta=eta_lag'
       STOP
    END IF

    IF(llm>1) THEN
       IF(is_mpi_root) PRINT *, 'etat0_type=williamson91.5 (Williamson,1991) must be used with llm=1 but llm =',llm
       STOP
    END IF

    DO ind=1,ndomain
      CALL swap_dimensions(ind)
      CALL swap_geometry(ind)
      h=f_mass(ind)
      u=f_u(ind)
      theta_rhodz=f_theta_rhodz(ind)
      phis=f_phis(ind)
      CALL compute_etat0_williamson(h(:,1), u(:,1))
      phis(:)=0.
      theta_rhodz(:,:) = h(:,:)
    ENDDO

  END SUBROUTINE etat0_williamson_new
  
  SUBROUTINE compute_etat0_williamson(hi, ue)
  USE icosa
  IMPLICIT NONE  
    REAL(rstd),INTENT(OUT) :: hi(iim*jjm)
    REAL(rstd),INTENT(OUT) :: ue(3*iim*jjm)
    REAL(rstd) :: lon, lat
    REAL(rstd) :: nx(3),n_norm,Velocity(3)
    REAL(rstd) :: A,B,C
    REAL(rstd) :: v1(3),v2(3),ny(3)
    REAL(rstd) :: de_min
    
    INTEGER :: i,j,n

  
    DO j=jj_begin-1,jj_end+1
      DO i=ii_begin-1,ii_end+1
        n=(j-1)*iim+i
        CALL xyz2lonlat(xyz_i(n,:),lon,lat)
        A= 0.5*omega0*(2*omega+omega0)*cos(lat)**2   &
         + 0.25*K0**2*cos(lat)**(2*R0)*((R0+1)*cos(lat)**2+(2*R0**2-R0-2)-2*R0**2/cos(lat)**2)
        B=2*(omega+omega0)*K0/((R0+1)*(R0+2))*cos(lat)**R0*((R0**2+2*R0+2)-(R0+1)**2*cos(lat)**2)
        C=0.25*K0**2*cos(lat)**(2*R0)*((R0+1)*cos(lat)**2-(R0+2))
        
        hi(n)=(g*h0+radius**2*A+radius**2*B*cos(R0*lon)+radius**2*C*cos(2*R0*lon))/g


        CALL compute_velocity(xyz_e(n+u_right,:),velocity)
        CALL cross_product2(xyz_v(n+z_rdown,:),xyz_v(n+z_rup,:),nx)
        
        ue(n+u_right)=1e-10
        n_norm=sqrt(sum(nx(:)**2))
        IF (n_norm>1e-30) THEN
          nx=-nx/n_norm*ne(n,right)
          ue(n+u_right)=sum(nx(:)*velocity(:))
          IF (ABS(ue(n+u_right))<1e-100) PRINT *,"ue(n+u_right) ==0",i,j,velocity(:)
        ENDIF
        
        
        
        CALL compute_velocity(xyz_e(n+u_lup,:),velocity)
        CALL cross_product2(xyz_v(n+z_up,:),xyz_v(n+z_lup,:),nx)

        ue(n+u_lup)=1e-10
        n_norm=sqrt(sum(nx(:)**2))
        IF (n_norm>1e-30) THEN
          nx=-nx/n_norm*ne(n,lup)
          ue(n+u_lup)=sum(nx(:)*velocity(:))
          IF (ABS(ue(n+u_lup))<1e-100) PRINT *,"ue(n+u_lup) ==0",i,j,velocity(:)
        ENDIF

       
        CALL compute_velocity(xyz_e(n+u_ldown,:),velocity)
        CALL cross_product2(xyz_v(n+z_ldown,:),xyz_v(n+z_down,:),nx)

        ue(n+u_ldown)=1e-10
        n_norm=sqrt(sum(nx(:)**2))
        IF (n_norm>1e-30) THEN
          nx=-nx/n_norm*ne(n,ldown)
          ue(n+u_ldown)=sum(nx(:)*velocity(:))
          IF (ABS(ue(n+u_ldown))<1e-100) PRINT *,"ue(n+u_ldown) ==0",i,j
        ENDIF
        
        
      ENDDO
    ENDDO

  CONTAINS
    
    SUBROUTINE compute_velocity(x,velocity)
      IMPLICIT NONE
      REAL(rstd),INTENT(IN)  :: x(3)
      REAL(rstd),INTENT(OUT) :: velocity(3)
      REAL(rstd)  :: e_lat(3), e_lon(3)
      REAL(rstd) :: lon,lat
      REAL(rstd) :: u,v
      
      CALL xyz2lonlat(x/radius,lon,lat)
      e_lat(1) = -cos(lon)*sin(lat)
      e_lat(2) = -sin(lon)*sin(lat)
      e_lat(3) = cos(lat)
        
      e_lon(1) = -sin(lon)
      e_lon(2) = cos(lon)
      e_lon(3) = 0
        
      u=radius*omega0*cos(lat)+radius*K0*cos(lat)**(R0-1)*(R0*sin(lat)**2-cos(lat)**2)*cos(R0*lon)
      v=-radius*K0*R0*cos(lat)**(R0-1)*sin(lat)*sin(R0*lon)

      Velocity=(u*e_lon+v*e_lat+1e-50)

    END SUBROUTINE compute_velocity
     
  END SUBROUTINE compute_etat0_williamson
  
END MODULE etat0_williamson_mod