CartesianIntoGeodeticI Subroutine

subroutine CartesianIntoGeodeticI(ax, ay, az, r, latitude, longitude, altitude, error)

    real(wp),intent(in) :: ax, ay, az !! semiaxes of the celestial body: ax>ay>az
    real(wp),dimension(3),intent(in) :: r !! cartesian coordinates of the considered point
                                          !! in the first octant: xG, yG, zG with (xG,yG,zG)<>(0,0,0)
    real(wp),intent(out) :: latitude, longitude, altitude !! geodetic coordinates of the considered point
    real(wp),intent(in) :: error !! Values smaller than error treated as 0.0

    real(wp) :: ax2,ay2,az2,ax4,ay4,az4,b5,b4,b3,b3x,b3y,b3z,&
                b2,b2x,b2y,b2z,b1,b1x,b1y,b1z,b0,b0x,b0y,b0z,eec,exc
    real(wp) :: xg2,yg2,zg2,aux,xG,yG,zG
    real(wp) :: xE,yE,zE,k,B(0:6),BB(0:6)
    logical :: done

    call special_cases(ax,ay,az,r(1),r(2),r(3),latitude,longitude,altitude,done)
    if (done) return

    ! Computations independent of xG,yG,zG. They can be precomputed, if necessary.
    ax2 = ax*ax
    ay2 = ay*ay
    az2 = az*az
    ax4 = ax2*ax2
    ay4 = ay2*ay2
    az4 = az2*az2
    b5  = 2.0_wp*(ax2+ay2+az2)
    b4  = ax4 + 4.0_wp*ax2*ay2 + ay4 + 4.0_wp*ax2*az2 + 4.0_wp*ay2*az2 + az4
    b3  = 2.0_wp*ax4*ay2 + 2.0_wp*ax2*ay4 + 2.0_wp*ax4*az2 + 8.0_wp*ax2*ay2*az2 + 2.0_wp*ay4*az2 + 2.0_wp*ax2*az4 + 2.0_wp*ay2*az4
    b3x = - 2.0_wp*ax2*ay2 - 2.0_wp*ax2*az2
    b3y = - 2.0_wp*ax2*ay2 - 2.0_wp*ay2*az2
    b3z = - 2.0_wp*ay2*az2 - 2.0_wp*ax2*az2
    b2  = 4.0_wp*ax4*ay2*az2 + 4.0_wp*ax2*ay4*az2 + ax4*az4 + 4.0_wp*ax2*ay2*az4 + ax4*ay4 + ay4*az4
    b2x = -ax2*ay4 -4.0_wp*ax2*ay2*az2 -ax2*az4
    b2y = -ax4*ay2 -4.0_wp*ax2*ay2*az2 -ay2*az4
    b2z = -ax4*az2 -4.0_wp*ax2*ay2*az2 -ay4*az2
    b1  = 2.0_wp*ax4*ay4*az2 + 2.0_wp*ax4*ay2*az4 + 2.0_wp*ax2*ay4*az4
    b1x = - 2.0_wp*ax2*ay4*az2 - 2.0_wp*ax2*ay2*az4
    b1y = - 2.0_wp*ax4*ay2*az2 - 2.0_wp*ax2*ay2*az4
    b1z = - 2.0_wp*ax4*ay2*az2 - 2.0_wp*ax2*ay4*az2
    b0  = ax4*ay4*az4
    b0x = - ax2*ay4*az4
    b0y = - ax4*ay2*az4
    b0z = - ax4*ay4*az2
    eec = (ax2-ay2)/ax2
    exc = (ax2-az2)/ax2

    ! Computations dependant of xG, yG, zG
    xG = abs(r(1))
    yG = abs(r(2))
    zG = abs(r(3))
    xg2 = xG*xG
    yg2 = yG*yG
    zg2 = zG*zG
    aux = xg2/ax2+yg2/ay2+zg2/az2

    B = [b0+b0x*xg2+b0y*yg2+b0z*zg2, &
         b1+b1x*xg2+b1y*yg2+b1z*zg2, &
         b2+b2x*xg2+b2y*yg2+b2z*zg2, &
         b3+b3x*xg2+b3y*yg2+b3z*zg2, &
         b4-(ax2*xg2+ay2*yg2+az2*zg2), &
         b5, &
         1.0_wp ]

  if (abs(aux-1.0_wp) < error) then ! The point is on the ellipsoid

    xE = xG
    yE = yG
    zE = zG

  else if (aux > 1.0_wp) then ! The point is outside the ellipsoid

    k = solve_polynomial(B,(xg2+yg2+zg2)/3.0_wp,error)
    xE = ax2*xG/(ax2+k)
    yE = ay2*yG/(ay2+k)
    zE = az2*zG/(az2+k)

  else if (zG > 0.0_wp) then ! The point  is inside the ellipsoid and zG>0

    call horner(B,az2,BB) ! B(x-az2) = BB(x)
    k = solve_polynomial(BB,(xg2+yg2+zg2)/3.0_wp+az2,error) - az2
    xE = ax2*xG/(ax2+k)
    yE = ay2*yG/(ay2+k)
    zE = az2*zG/(az2+k)

  else if (xG > 0.0_wp .and. yG > 0.0_wp) then ! The point is inside the ellipsoid and zG=0, yG > 0, xG > 0

    call horner(B,ay2,BB)
    k = solve_polynomial(BB,(xg2+yg2+zg2)/3.0_wp+ay2,error) - ay2
    xE = ax2*xG/(ax2+k)
    yE = ay2*yG/(ay2+k)
    zE = 0.0_wp

  else if (xG < error .and. yG > 0.0_wp) then

    xE = 0.0_wp
    yE = ay
    zE = 0.0_wp

  else if (xG > 0.0_wp .and. yG < error) then

    xE = ax
    yE = 0.0_wp
    zE = 0.0_wp

  end if

  ! Computing longitude
  if (xG > 0.0_wp) then
    longitude = atan(yE/((1.0_wp-eec)*xE))
  else if (yG > 0.0_wp) then
    longitude = halfpi
  else
    longitude = huge(1.0_wp)  ! undefined
  end if

  ! Computing latitude
  if (xE > 0.0_wp .or. yE > 0.0_wp) then
    latitude = atan((1.0_wp-eec)/(1.0_wp-exc)*zE/norm2([xE*(1.0_wp-eec),yE]))
  else
    latitude = halfpi
  end if

  ! Computing altitude
  if (aux>=1.0_wp) then
    altitude = norm2([xE-xG,yE-yG,zE-zG])
  else
    altitude = -norm2([xE-xG,yE-yG,zE-zG])
  end if

  call philambda_quadrant(r(1), r(2), r(3), latitude, longitude)

  end subroutine CartesianIntoGeodeticI