//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only.  Not for
// government, commercial, or other organizational use.
// File: XYZ2ELL.cpp
//
// MATLAB Coder version            : 5.3
// C/C++ source code generated on  : 04-Apr-2023 15:28:59
//

// Include Files
#include "XYZ2ELL.h"
#include "rt_nonfinite.h"
#include "rt_defines.h"
#include <cmath>

// Function Declarations
static double rt_atan2d_snf(double u0, double u1);

// Function Definitions
//
// Arguments    : double u0
//                double u1
// Return Type  : double
//
static double rt_atan2d_snf(double u0, double u1)
{
  double y;
  if (std::isnan(u0) || std::isnan(u1)) {
    y = rtNaN;
  } else if (std::isinf(u0) && std::isinf(u1)) {
    int b_u0;
    int b_u1;
    if (u0 > 0.0) {
      b_u0 = 1;
    } else {
      b_u0 = -1;
    }
    if (u1 > 0.0) {
      b_u1 = 1;
    } else {
      b_u1 = -1;
    }
    y = std::atan2(static_cast<double>(b_u0), static_cast<double>(b_u1));
  } else if (u1 == 0.0) {
    if (u0 > 0.0) {
      y = RT_PI / 2.0;
    } else if (u0 < 0.0) {
      y = -(RT_PI / 2.0);
    } else {
      y = 0.0;
    }
  } else {
    y = std::atan2(u0, u1);
  }
  return y;
}

//
// choose reference ellipsoid
//        1 ...... tide free
//        2 ...... GRS80
//        3 ...... WGS84
//
// Arguments    : const double pos[3]
//                double *lat
//                double *lon
//                double *h
// Return Type  : void
//
void XYZ2ELL(const double pos[3], double *lat, double *lon, double *h)
{
  double N;
  double N_tmp;
  double h_tmp;
  double lat_tmp_im;
  double lat_tmp_re;
  //  ************************************************************************
  //    Description:
  //    Transformation from Cartesian coordinates X,Y,Z to ellipsoidal
  //    coordinates lam,phi,elh. based on Occam subroutine transf.
  //
  //    Input:
  //       pos = [x,y,z]                 [m,m,m]
  //                can be a matrix: number of rows = number of stations
  //
  //    Output:
  //       coor_ell = [lat,lon,h]      [deg,deg,m]
  //
  //    External calls:
  //       global   a_...              Equatorial radius of the Earth [m]
  //                f_...              Flattening factor of the Earth
  //
  //    Coded for VieVS:
  //    17 Dec 2008 by Lucia Plank
  //
  //    Revision:
  //    11 Nov 2022 by Haobin Luo
  //  *************************************************************************
  //  WGS84
  lat_tmp_re = pos[2] * 0.0;
  lat_tmp_im = pos[2];
  *lat = rt_atan2d_snf(pos[2], std::sqrt(pos[0] * pos[0] + pos[1] * pos[1]) +
                                   lat_tmp_re);
  N_tmp = pos[0];
  N = pos[1];
  h_tmp = std::sqrt(N_tmp * N_tmp + N * N);
  for (int j{0}; j < 10; j++) {
    N_tmp = std::sin(*lat);
    N = 6.378137E+6 / std::sqrt(1.0 - 0.0066943799902085387 * N_tmp * N_tmp);
    *h = h_tmp / std::cos(*lat) - N;
    N_tmp = N + *h;
    *lat = rt_atan2d_snf(lat_tmp_im * N_tmp,
                         h_tmp * (0.9933056200097915 * N + *h) +
                             lat_tmp_re * N_tmp);
  }
  *lat *= 57.295779513082323;
  *lon = 57.295779513082323 * rt_atan2d_snf(pos[1], pos[0]);
  // lat=cart2phigd(pos);
}

//
// File trailer for XYZ2ELL.cpp
//
// [EOF]
//