MODULE spherical_geom_mod
USE genmod



CONTAINS




SUBROUTINE lonlat2xyz(lon,lat,xyz)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: lon
  REAL(rstd),INTENT(IN) :: lat
  REAL(rstd),INTENT(OUT) :: xyz(3)
  
  xyz(1)=cos(lon)*cos(lat)
  xyz(2)=sin(lon)*cos(lat)
  xyz(3)=sin(lat)

END SUBROUTINE lonlat2xyz


SUBROUTINE xyz2lonlat(xyz,lon,lat)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: xyz(3)
  REAL(rstd),INTENT(OUT) :: lon
  REAL(rstd),INTENT(OUT) :: lat
  
  REAL(rstd) :: xyzn(3)
  
  xyzn(:)=xyz(:)/sqrt(sum(xyz(:)**2))
  
  lat=asin(xyzn(3))
  lon=atan2(xyzn(2),xyzn(1))
END SUBROUTINE xyz2lonlat

! lat/lon with respect to a displaced pole (rotated basis) :
!  ( cos(lon0)*sin(lat0), sin(lon0)*sin(lat0), -cos(lat0)) 
!  (-sin(lon0),           cos(lon0),           0)
!  ( cos(lon0)*cos(lat0), sin(lon0)*cos(lat0), sin(lat0))

SUBROUTINE lonlat2xyz_relative(lon,lat,lon0,lat0, xyz)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: lon0, lat0, lon,lat
  REAL(rstd),INTENT(OUT) :: xyz(3)
  REAL(rstd) :: xx,yy,zz
  xx = cos(lon)*cos(lat)
  yy = sin(lon)*cos(lat)
  zz = sin(lat)
  xyz(1) = cos(lon0)*(sin(lat0)*xx+cos(lat0)*zz)-sin(lon0)*yy
  xyz(2) = sin(lon0)*(sin(lat0)*xx+cos(lat0)*zz)+cos(lon0)*yy
  xyz(3) = sin(lat0)*zz-cos(lat0)*xx
END SUBROUTINE lonlat2xyz_relative

SUBROUTINE xyz2lonlat_relative(xyz,lon0,lat0, lon,lat)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: xyz(3), lon0, lat0
  REAL(rstd),INTENT(OUT) :: lon,lat
  REAL(rstd) :: xx,yy,zz
  xx = sin(lat0)*(xyz(1)*cos(lon0)+xyz(2)*sin(lon0))-cos(lat0)*xyz(3)
  yy = xyz(2)*cos(lon0)-xyz(1)*sin(lon0)
  zz = cos(lat0)*(xyz(1)*cos(lon0)+xyz(2)*sin(lon0))+sin(lat0)*xyz(3)
  lon = atan2(yy,xx)
  lat = asin(zz)
END SUBROUTINE xyz2lonlat_relative

SUBROUTINE schmidt_transform(xyz,cc, lon0, lat0)
  ! Based on formula (12) from Guo & Drake, JCP 2005
  IMPLICIT NONE
  REAL(rstd),INTENT(INOUT) :: xyz(3)
  REAL(rstd), INTENT(IN) :: cc, lon0, lat0  ! stretching factor>0, lon/lat of zoomed area
  REAL(rstd) :: lat,lon,mu
  CALL xyz2lonlat_relative(xyz,lon0,lat0, lon,lat)
  mu = sin(lat)
  mu = ((cc-1)+mu*(cc+1)) / ((cc+1)+mu*(cc-1))
  lat = asin(mu)
  CALL lonlat2xyz_relative(lon,lat, lon0,lat0, xyz)
END SUBROUTINE schmidt_transform

SUBROUTINE dist_cart(A,B,d)
USE vector
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(OUT) :: d
  
   REAL(rstd)  :: n(3)
   CALL cross_product2(A,B,n)
   d=asin(sqrt(sum(n**2)))

END SUBROUTINE dist_cart


SUBROUTINE dist_lonlat(lonA,latA,lonB,latB,d)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: lonA
  REAL(rstd),INTENT(IN)  :: latA
  REAL(rstd),INTENT(IN)  :: lonB
  REAL(rstd),INTENT(IN)  :: latB
  REAL(rstd),INTENT(OUT) :: d
  
  d=acos(MAX(MIN(sin(latA)*sin(latB)+cos(latA)*cos(latB)*cos(lonA-lonB),1.),-1.))
 
END SUBROUTINE dist_lonlat

SUBROUTINE surf_triangle(A,B,C,surf)
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: C(3)
  REAL(rstd),INTENT(OUT) :: Surf

  REAL(rstd)  :: AB,AC,BC
  REAL(rstd)  :: s,x
  
  CALL dist_cart(A,B,AB)
  CALL dist_cart(A,C,AC)
  CALL dist_cart(B,C,BC)
  
  s=(AB+AC+BC)/2
  x=tan(s/2) * tan((s-AB)/2)  * tan((s-AC)/2) * tan((s-BC)/2)
  IF (x<0) x=0.
  surf=4*atan(sqrt( x))
  
END SUBROUTINE surf_triangle 


SUBROUTINE div_arc(A,B,frac,C)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: frac
  REAL(rstd),INTENT(OUT)  :: C(3)
  
  REAL(rstd) :: d
  REAL(rstd) :: M(3,3)
  REAL(rstd) :: xa,xb,xc
  REAL(rstd) :: ya,yb,yc
  REAL(rstd) :: za,zb,zc
  
  
  xa=A(1) ; ya=A(2) ; za=A(3)
  xb=B(1) ; yb=B(2) ; zb=B(3)

  CALL dist_cart(A,B,d)

  C(1)=cos(frac*d) 
  C(2)=cos((1-frac)*d)
  C(3)=0.

  xc=ya*zb-yb*za ; yc=-(xa*zb-xb*za) ; zc=xa*yb-xb*ya
  
  M(1,1)=xa ; M(1,2)=ya ; M(1,3)=za
  M(2,1)=xb ; M(2,2)=yb ; M(2,3)=zb
  M(3,1)=xc ; M(3,2)=yc ; M(3,3)=zc
  stop 'STOP'
!  CALL DGESV(3,1,M,3,IPIV,C,3,info)
  
END SUBROUTINE div_arc

SUBROUTINE div_arc_bis(A,B,frac,C)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: frac
  REAL(rstd),INTENT(OUT)  :: C(3)
  
   C=A*(1-frac)+B*frac  
   C=C/sqrt(sum(C**2))
END SUBROUTINE div_arc_bis


  SUBROUTINE circumcenter(A0,B0,C0,center)
  USE vector
  IMPLICIT NONE
    REAL(rstd), INTENT(IN)  :: A0(3),B0(3),C0(3)
    REAL(rstd), INTENT(OUT) :: Center(3)
    
    REAL(rstd)  :: a(3),b(3),c(3), ac(3), ab(3), p1(3), q(3), p2(3)
    
    a=A0/sqrt(sum(A0**2))
    b=B0/sqrt(sum(B0**2))
    c=C0/sqrt(sum(C0**2))
    
    ab=b-a
    ac=c-a
    CALL Cross_product2(ab,ac,p1)
    IF(.FALSE.) THEN ! Direct solution, round-off error
       center=p1/norm(p1)
    ELSE ! Two-step solution, stable
       q = SUM(ac**2)*ab-SUM(ab**2)*ac
       CALL Cross_product2(p1,q,p2)
       p2 = a + p2/(2.*SUM(p1**2))
       center = p2/norm(p2)
    END IF
  END SUBROUTINE circumcenter
    

  SUBROUTINE compute_centroid(points,n,centr)
  USE vector
  IMPLICIT NONE
    INTEGER :: n
    REAL(rstd), INTENT(IN)  :: points(3,n)
    REAL(rstd), INTENT(OUT) :: Centr(3)
    
    REAL(rstd) :: p1(3),p2(3),cross(3), cc(3)
    REAL(rstd) :: norm_cross, area
    INTEGER :: i,j

    centr(:)=0
    IF(.FALSE.) THEN 
       ! Gauss formula (subject to round-off error)
       DO i=1,n
          j=MOD(i,n)+1
          p1=points(:,i)/norm(points(:,i))
          p2=points(:,j)/norm(points(:,j))
          CALL cross_product2(p1,p2,cross)
          norm_cross=norm(cross)
          if (norm_cross<1e-10) CYCLE 
          centr(:)=centr(:)+asin(norm_cross)*cross(:)/norm_cross
       ENDDO       
    ELSE
       ! Simple area-weighted average (second-order accurate, stable)
       cc=SUM(points,2) ! arithmetic average used as center
       cc=cc/norm(cc)
       DO i=1,n
          j=MOD(i,n)+1
          p1=points(:,i)/norm(points(:,i))
          p2=points(:,j)/norm(points(:,j))
          CALL surf_triangle(cc,p1,p2,area)
          centr(:)=centr(:)+area*(p1+p2+cc)
       ENDDO
    END IF
    
    centr(:)=centr(:)/norm(centr(:))

  END SUBROUTINE compute_centroid

END MODULE spherical_geom_mod