#include "complex_impl.h"
#include <fenv.h>

/* cssqsl() and csqrtl() taken from
 * Kahan, W. (1987). Branch cuts for complex elementary functions.
 */
static inline long double complex _cssqsl(long double complex z) {
#pragma STDC FENV_ACCESS ON
  fenv_t env;
  unsigned k = 0;
  long double x, y, r;
  int set_excepts;

  feholdexcept(&env);
  x = creal(z);
  y = cimag(z);
  r = x * x + y * y;
  if ((isinf(x) || isinf(y)) && (isnan(r) || isinf(r))) {
    r = INFINITY;
  } else {
    set_excepts = fetestexcept(FE_OVERFLOW | FE_UNDERFLOW);
    if ((set_excepts & FE_OVERFLOW) ||
        ((set_excepts & FE_UNDERFLOW) && isless(r, LDBL_MIN / LDBL_EPSILON))) {
      k = logbl(fmaxl(fabsl(x), fabsl(y)));
      x = scalbnl(x, -k);
      y = scalbnl(y, -k);
      r = x * x + y * y;
    }
  }
  feupdateenv(&env);
  return CMPLXL(r, k);
}

long double complex csqrtl(long double complex z) {
  long double x, y, r, xi, eta;
  unsigned k;

  x = creal(z);
  y = cimag(z);
  z = _cssqsl(z);
  r = creal(z);
  k = cimag(z);
  if (!isnan(x)) {
    r = scalbnl(fabsl(x), -k) + sqrtl(r);
  }
  if (k & 1) {
    k = (k - 1) / 2;
  } else {
    k = k / 2 - 1;
    r *= 2;
  }
  r = scalbnl(sqrtl(r), k);
  xi = r;
  eta = y;
  if (r != 0) {
    if (!isinf(eta)) {
      // TODO if eta underflowed, signal it
      eta = (eta / r) / 2;
    }
    if (isless(x, 0)) {
      xi = fabsl(eta);
      eta = copysignl(r, y);
    }
  }
  return CMPLX(xi, eta);
}