module mod_confmap
   private
   real pi_1
   real pi_2
   real deg
   real rad
   real theta_a
   real phi_a
   real theta_b
   real phi_b
   real di
   real dj
   complex imag
   complex ac
   complex bc
   complex cmna
   complex cmnb
   real mu_s
   real psi_s
   real epsil
   logical mercator

   real lat_a,lon_a
   real lat_b,lon_b
   real wlim,elim
   real slim,nlim
   real mercfac
   integer ires,jres

   public :: initconfmap, oldtonew, pivotp, bilincoeff, &
      ll2gind, gind2ll, newtoold

contains

   
subroutine initconfmap(idm,jdm)
! This routine initialize constants used in the conformal mapping
! and must be called before the routines 'oldtonew' and 'newtoold'
! are called. The arguments of this routine are the locations of
! the two poles in the old coordiante system.
   implicit none
   integer, intent(in) :: idm, jdm


! local variables

   real cx,cy,cz,theta_c,phi_c
   complex c,w
   logical ass,lold

! Read info file
   open(unit=10,file='grid.info',form='formatted')
      read(10,*) lat_a,lon_a
      read(10,*) lat_b,lon_b
      read(10,*) wlim,elim,ires
      read(10,*) slim,nlim,jres
      read(10,*) ass
      read(10,*) ass
      read(10,*) ass
      read(10,*) mercator
      read(10,*) mercfac,lold
   close(10)
   if ((ires /= idm).and.(jres /= jdm)) then
      print *,'initconfmap: WARNING -- the dimensions in grid.info are not'
      print *,'initconfmap: WARNING -- consistent with idm and jdm'
      print *,'initconfmap: WARNING -- IGNORE IF RUNNING CURVIINT'
   endif

! some constants
   pi_1=4.*atan(1.)
   pi_2=.5*pi_1
   deg=180./pi_1
   rad=1.0/deg
   epsil=1.0E-9

   di=(elim-wlim)/float(ires-1)   ! delta lon'
   dj=(nlim-slim)/float(jres-1)   ! delta lat' for spherical grid

   if (mercator) then
      dj=di
      if (lold) then
         print *,'initconfmap: lold'
         slim=-mercfac*jres*dj
      else
         print *,'initconfmap: not lold'
         slim= mercfac
      endif
   endif

! transform to spherical coordinates

   theta_a=lon_a*rad
   phi_a=pi_2-lat_a*rad
   theta_b=lon_b*rad
   phi_b=pi_2-lat_b*rad

! find the angles of a vector pointing at a point located exactly
! between the poles

   cx=cos(theta_a)*sin(phi_a)+cos(theta_b)*sin(phi_b)
   cy=sin(theta_a)*sin(phi_a)+sin(theta_b)*sin(phi_b)
   cz=cos(phi_a)+cos(phi_b)

   theta_c=atan2(cy,cx)
   phi_c=pi_2-atan2(cz,sqrt(cx*cx+cy*cy))

! initialize constants used in the conformal mapping

   imag=(.0,1.)
   ac=tan(.5*phi_a)*exp(imag*theta_a)
   bc=tan(.5*phi_b)*exp(imag*theta_b)
   c=tan(.5*phi_c)*exp(imag*theta_c)
   cmna=c-ac
   cmnb=c-bc

   w=cmnb/cmna
   mu_s=atan2(aimag(w),real(w))
   psi_s=2.*atan(abs(w))

end subroutine initconfmap

subroutine oldtonew(lat_o,lon_o,lat_n,lon_n)

! this routine performes a conformal mapping of the old to the new
! coordinate system

   implicit none

   real, intent(in)  :: lat_o,lon_o
   real, intent(out) :: lat_n,lon_n

! local variables

   real theta,phi,psi,mu
   complex z,w

! transform to spherical coordinates


   theta=mod(lon_o*rad+3.0*pi_1,2.0*pi_1)-pi_1
   phi=pi_2-lat_o*rad

! transform to the new coordinate system

   if (abs(phi-pi_1) < epsil) then
     mu=mu_s
     psi=psi_s
   elseif ((abs(phi-phi_b)<epsil).and.&
           (abs(theta-theta_b)<epsil)) then
     mu=.0
     psi=pi_1
   else
     z=tan(.5*phi)*exp(imag*theta)
     w=(z-ac)*cmnb/((z-bc)*cmna)
     mu=atan2(aimag(w),real(w))
     psi=2.*atan(abs(w))
   endif

! transform to lat/lon coordinates

   lat_n=(pi_2-psi)*deg
   lon_n=mu*deg

end subroutine oldtonew


subroutine pivotp(lon,lat,ipiv,jpiv,shiftin)
! This subroutine computes the pivot point of each of the observations
! in the temporary array tmpobs of type observation. The pivot point
! is the biggest i and the biggest j, (i,j) is the computation points/
! the grid, that is less than the position of the observation.

   implicit none

   real, intent(in) ::  lon,lat
   integer, intent(out) :: ipiv,jpiv
   real, intent(in), optional :: shiftin

   real tmptan,tmp
   real lontmp, shift

   shift=0.
   if (present(shiftin)) shift=shiftin

! fix for wrap-around
! western limit in new coordinates can be east of eastern limit (sigh)....
! in that case di is < 0
   if (lon < wlim .and. di > 0. ) then
      lontmp=lon+360.0
   elseif (lon > wlim .and. di < 0. ) then
         lontmp=lon-360.0
!more fixes ...
   elseif ((lon-wlim)>360.) then
      lontmp=lon
      do while(lontmp-wlim>360.)
         lontmp=lontmp-360.
      end do
   else
      lontmp=lon
   endif

   ipiv=int((lontmp-wlim)/di)+1

   if (mercator) then
      if (abs(lat) < 89.999) then
         tmptan=tan(0.5*rad*lat+0.25*pi_1)
         !jpiv=int( (log(tmptan)-slim*rad)/(rad*dj) ) +1
         jpiv=int( (log(tmptan)-slim*rad)/(rad*dj) +1.0 + shift)
      else
         jpiv=-999
      endif 
   else
      jpiv=int((lat-slim)/dj + 1.0 + shift)
   endif


! Inverse transformation to check pivot point jpiv
!   tmp=slim+(jpiv-1)*dj
!   tmp=(2.*atan(exp(tmp*rad))-pi_1*.5)*deg

end subroutine pivotp



subroutine bilincoeff(glon,glat,nx,ny,lon,lat,ipiv,jpiv,a1,a2,a3,a4)
! This subroutine uses bilinear interpolation to interpolate the field
! computed by the model (MICOM) to the position defined by lon, lat
! The output is the interpolation coeffisients a[1-4]
! NB  NO locations on land.
!TODO: periodic grid support
   implicit none

   integer, intent(in)  :: nx,ny
   real,    intent(in)  :: glon(nx,ny),glat(nx,ny)
   real,    intent(in)  :: lon,lat
   integer, intent(in)  :: ipiv,jpiv
   real,    intent(out) :: a1,a2,a3,a4

   real t,u
   real lat_1,lon_1,lat_2,lon_2,lat_n,lon_n,lat_t,lon_t


   call oldtonew(glat(ipiv,jpiv),glon(ipiv,jpiv),lat_1,lon_1)
   call oldtonew(glat(ipiv+1,jpiv+1),glon(ipiv+1,jpiv+1),lat_2,lon_2)
   call oldtonew(lat,lon,lat_n,lon_n)

!   print *,lat_1,lon_1,lat_2,lon_2,lat_n,lon_n
!   call oldtonew(glat(ipiv+1,jpiv),glon(ipiv+1,jpiv),lat_t,lon_t)
!   print *,lat_t,lon_t,lat_t,lon_t
!
!   call oldtonew(glat(ipiv,jpiv+1),glon(ipiv,jpiv+1),lat_t,lon_t)
!   print *,lat_t,lon_t,lat_t,lon_t
!   print *,'--------------'


   t=(lon_n-lon_1)/(lon_2-lon_1)
   u=(lat_n-lat_1)/(lat_2-lat_1)

   a1=(1-t)*(1-u)
   a2=t*(1-u)
   a3=t*u
   a4=(1-t)*u
end subroutine bilincoeff



subroutine gind2ll(ipiv,jpiv,lon,lat)
! KAL - This routine takes as input floating point ipiv, jpiv 
! KAL - and calculates actual lon,lat positions from that
   implicit none

   real, intent(out) ::  lon,lat
   real, intent(in) :: ipiv,jpiv

   real tmptan,tmp
   real lontmp
   real lon_n,lat_n

   !print '(a,2i5,4f10.2)','pivotp:',ipiv,jpiv,lontmp,lon,wlim,di
   !ipiv=int((lontmp-wlim)/di)+1
   lon_n = (ipiv-1)*di + wlim

!   if (mercator) then
!      if (abs(lat) < 89.999) then
!         tmptan=tan(0.5*rad*lat+0.25*pi_1)
!         jpiv=int( (log(tmptan)-slim*rad)/(rad*dj) ) +1
!      else
!         jpiv=-999
!      endif 
!   else
!      jpiv=int((lat-slim)/dj)+1
!   endif

   if (mercator) then
      tmptan = (jpiv-1) * (rad*dj) + slim*rad
      tmptan = exp(tmptan)
      lat_n=(atan(tmptan)-0.25*pi_1)*2/rad
   else
      lat_n = (jpiv-1) *dj + slim
   endif

   call newtoold(lat_n,lon_n,lat,lon)


! Inverse transformation to check pivot point jpiv
!   tmp=slim+(jpiv-1)*dj
!   tmp=(2.*atan(exp(tmp*rad))-pi_1*.5)*deg

end subroutine gind2ll



subroutine ll2gind(lon_o,lat_o,x,y)
! KAL - This routine takes as input  actual lon, lat
! KAL - and calculates floating point grid index x,y
   implicit none

   real, intent(in) ::  lon_o,lat_o
   real, intent(out) :: x,y

   real tmptan,tmp, lon, lat
   real lontmp

   call oldtonew(lat_o,lon_o,lat,lon)

! fix for wrap-around
! western limit in new coordinates can be east of eastern limit (sigh)....
! in that case di is < 0
   if (lon < wlim .and. di > 0. ) then
      lontmp=lon+360.0
   elseif (lon > wlim .and. di < 0. ) then
         lontmp=lon-360.0
!more fixes ...
   elseif ((lon-wlim)>360.) then
      lontmp=lon
      do while(lontmp-wlim>360.)
         lontmp=lontmp-360.
      end do
   else
      lontmp=lon
   endif

   !print '(a,2i5,4f10.2)','pivotp:',ipiv,jpiv,lontmp,lon,wlim,di
   x=(lontmp-wlim)/di+1

   if (mercator) then
      if (abs(lat) < 89.999) then
         tmptan=tan(0.5*rad*lat+0.25*pi_1)
         y=(log(tmptan)-slim*rad)/(rad*dj) +1
      else
         y=-999
      endif 
   else
      y=(lat-slim)/dj+1
   endif


! Inverse transformation to check pivot point jpiv
   tmp=slim+(y-1)*dj
   tmp=(2.*atan(exp(tmp*rad))-pi_1*.5)*deg
   !print *,'ll2gind',lon_o,lat_o,tmp
end subroutine ll2gind


subroutine newtoold(lat_n,lon_n,lat_o,lon_o)
! this routine performes a conformal mapping of the new to the old 
! coordinate system

   implicit none

   real lat_o,lon_o,lat_n,lon_n

! local variables

   real theta,phi,psi,mu
   complex w,z

! transform to spherical coordinates

   mu=mod(lon_n*rad+3*pi_1,2*pi_1)-pi_1
   psi=abs(pi_2-lat_n*rad)

! transform to the old coordinate system

   if (abs(psi-pi_1) < epsil) then
     theta=theta_b
     phi=phi_b
   elseif ((abs(mu-mu_s) < epsil).and.((psi-psi_s)<epsil)) then
     theta=.0
     phi=pi_1
   else
     w=tan(.5*psi)*exp(imag*mu)
     z=(ac*cmnb-w*bc*cmna)/(cmnb-w*cmna)
     theta=atan2(aimag(z),real(z))
     phi=2.*atan(abs(z))
   endif

! transform to lat/lon coordinates

   lat_o=(pi_2-phi)*deg
   lon_o=theta*deg

end subroutine newtoold



end module mod_confmap