float complex
csqrtf (float complex z)
{
  float re, im;
  float complex v;

  re = REALPART (z);
  im = IMAGPART (z);
  if (im == 0)
    {
      if (re < 0)
        {
          COMPLEX_ASSIGN (v, 0, copysignf (sqrtf (-re), im));
        }
      else
        {
          COMPLEX_ASSIGN (v, fabsf (sqrtf (re)), copysignf (0, im));
        }
    }
  else if (re == 0)
    {
      float r;

      r = sqrtf (0.5 * fabsf (im));

      COMPLEX_ASSIGN (v, r, copysignf (r, im));
    }
  else
    {
      float d, r, s;

      d = hypotf (re, im);
      /* Use the identity   2  Re res  Im res = Im x
         to avoid cancellation error in  d +/- Re x.  */
      if (re > 0)
        {
          r = sqrtf (0.5 * d + 0.5 * re);
          s = (0.5 * im) / r;
        }
      else
        {
          s = sqrtf (0.5 * d - 0.5 * re);
          r = fabsf ((0.5 * im) / s);
        }

      COMPLEX_ASSIGN (v, r, copysignf (s, im));
    }
  return v;
}

float complex
csqrtf(float complex z)
{
	double t;
	float a, b;

	a = creal(z);
	b = cimag(z);

	/* Handle special cases. */
	if (z == 0)
		return (CMPLXF(0, b));
	if (isinf(b))
		return (CMPLXF(INFINITY, b));
	if (isnan(a)) {
		t = (b - b) / (b - b);	/* raise invalid if b is not a NaN */
		return (CMPLXF(a + 0.0L + t, a + 0.0L + t)); /* NaN + NaN i */
	}
	if (isinf(a)) {
		/*
		 * csqrtf(inf + NaN i)  = inf +  NaN i
		 * csqrtf(inf + y i)    = inf +  0 i
		 * csqrtf(-inf + NaN i) = NaN +- inf i
		 * csqrtf(-inf + y i)   = 0   +  inf i
		 */
		if (signbit(a))
			return (CMPLXF(fabsf(b - b), copysignf(a, b)));
		else
			return (CMPLXF(a, copysignf(b - b, b)));
	}
	if (isnan(b)) {
		t = (a - a) / (a - a);	/* raise invalid */
		return (CMPLXF(b + 0.0L + t, b + 0.0L + t)); /* NaN + NaN i */
	}

	/*
	 * We compute t in double precision to avoid overflow and to
	 * provide correct rounding in nearly all cases.
	 * This is Algorithm 312, CACM vol 10, Oct 1967.
	 */
	if (a >= 0) {
		t = sqrt((a + hypot(a, b)) * 0.5);
		return (CMPLXF(t, b / (2 * t)));
	} else {
		t = sqrt((-a + hypot(a, b)) * 0.5);
		return (CMPLXF(fabsf(b) / (2 * t), copysignf(t, b)));
	}
}

void test_copysign()
{
    static_assert((std::is_same<decltype(copysign((double)0, (double)0)), double>::value), "");
    static_assert((std::is_same<decltype(copysignf(0,0)), float>::value), "");
    static_assert((std::is_same<decltype(copysignl(0,0)), long double>::value), "");
    assert(copysign(1,1) == 1);
}

int
gl_signbitf (float arg)
{
#if defined FLT_SIGNBIT_WORD && defined FLT_SIGNBIT_BIT
  /* The use of a union to extract the bits of the representation of a
     'long double' is safe in practice, despite of the "aliasing rules" of
     C99, because the GCC docs say
       "Even with '-fstrict-aliasing', type-punning is allowed, provided the
        memory is accessed through the union type."
     and similarly for other compilers.  */
# define NWORDS \
    ((sizeof (float) + sizeof (unsigned int) - 1) / sizeof (unsigned int))
  union { float value; unsigned int word[NWORDS]; } m;
  m.value = arg;
  return (m.word[FLT_SIGNBIT_WORD] >> FLT_SIGNBIT_BIT) & 1;
#elif HAVE_COPYSIGNF_IN_LIBC
  return copysignf (1.0f, arg) < 0;
#else
  /* This does not do the right thing for NaN, but this is irrelevant for
     most use cases.  */
  if (isnanf (arg))
    return 0;
  if (arg < 0.0f)
    return 1;
  else if (arg == 0.0f)
    {
      /* Distinguish 0.0f and -0.0f.  */
      static float plus_zero = 0.0f;
      float arg_mem = arg;
      return (memcmp (&plus_zero, &arg_mem, SIZEOF_FLT) != 0);
    }
  else
    return 0;
#endif
}

// Given a convex curve segment with the following order-2 tangent function:
//
//                                                       |C2x  C2y|
//     tan = some_scale * |dx/dt  dy/dt| = |t^2  t  1| * |C1x  C1y|
//                                                       |C0x  C0y|
//
// This function finds the T value whose tangent angle is halfway between the tangents at T=0 and
// T=1 (tan0 and tan1).
static inline float find_midtangent(const Sk2f& tan0, const Sk2f& tan1,
                                    const Sk2f& C2, const Sk2f& C1, const Sk2f& C0) {
    // Tangents point in the direction of increasing T, so tan0 and -tan1 both point toward the
    // midtangent. 'n' will therefore bisect tan0 and -tan1, giving us the normal to the midtangent.
    //
    //     n dot midtangent = 0
    //
    Sk2f n = normalize(tan0) - normalize(tan1);

    // Find the T value at the midtangent. This is a simple quadratic equation:
    //
    //     midtangent dot n = 0
    //
    //     (|t^2  t  1| * C) dot n = 0
    //
    //     |t^2  t  1| dot C*n = 0
    //
    // First find coeffs = C*n.
    Sk4f C[2];
    Sk2f::Store4(C, C2, C1, C0, 0);
    Sk4f coeffs = C[0]*n[0] + C[1]*n[1];

    // Now solve the quadratic.
    float a = coeffs[0], b = coeffs[1], c = coeffs[2];
    float discr = b*b - 4*a*c;
    if (discr < 0) {
        return 0; // This will only happen if the curve is a line.
    }

    // The roots are q/a and c/q. Pick the one closer to T=.5.
    float q = -.5f * (b + copysignf(std::sqrt(discr), b));
    float r = .5f*q*a;
    return std::abs(q*q - r) < std::abs(a*c - r) ? q/a : c/q;
}

void
vector_copysign (void)
{
  int i;

  for (i = 0; i < SIZE; i++)
    a[i] = copysignf (b[i], c[i]);
}

float complex
casinhf(float complex z)
{
	float x, y, ax, ay, rx, ry, B, sqrt_A2my2, new_y;
	int B_is_usable;
	float complex w;

	x = crealf(z);
	y = cimagf(z);
	ax = fabsf(x);
	ay = fabsf(y);

	if (isnan(x) || isnan(y)) {
		if (isinf(x))
			return (CMPLXF(x, y + y));
		if (isinf(y))
			return (CMPLXF(y, x + x));
		if (y == 0)
			return (CMPLXF(x + x, y));
		return (CMPLXF(x + 0.0L + (y + 0), x + 0.0L + (y + 0)));
	}

	if (ax > RECIP_EPSILON || ay > RECIP_EPSILON) {
		if (signbit(x) == 0)
			w = clog_for_large_values(z) + m_ln2;
		else
			w = clog_for_large_values(-z) + m_ln2;
		return (CMPLXF(copysignf(crealf(w), x),
		    copysignf(cimagf(w), y)));
	}

	if (x == 0 && y == 0)
		return (z);

	raise_inexact();

	if (ax < SQRT_6_EPSILON / 4 && ay < SQRT_6_EPSILON / 4)
		return (z);

	do_hard_work(ax, ay, &rx, &B_is_usable, &B, &sqrt_A2my2, &new_y);
	if (B_is_usable)
		ry = asinf(B);
	else
		ry = atan2f(new_y, sqrt_A2my2);
	return (CMPLXF(copysignf(rx, x), copysignf(ry, y)));
}

float complex
csqrtf(float complex z)
{
	float a = crealf(z), b = cimagf(z);
	double t;

	/* Handle special cases. */
	if (z == 0)
		return (cpackf(0, b));
	if (isinf(b))
		return (cpackf(INFINITY, b));
	if (isnan(a)) {
		t = (b - b) / (b - b);	/* raise invalid if b is not a NaN */
		return (cpackf(a, t));	/* return NaN + NaN i */
	}
	if (isinf(a)) {
		/*
		 * csqrtf(inf + NaN i)  = inf +  NaN i
		 * csqrtf(inf + y i)    = inf +  0 i
		 * csqrtf(-inf + NaN i) = NaN +- inf i
		 * csqrtf(-inf + y i)   = 0   +  inf i
		 */
		if (signbit(a))
			return (cpackf(fabsf(b - b), copysignf(a, b)));
		else
			return (cpackf(a, copysignf(b - b, b)));
	}
	/*
	 * The remaining special case (b is NaN) is handled just fine by
	 * the normal code path below.
	 */

	/*
	 * We compute t in double precision to avoid overflow and to
	 * provide correct rounding in nearly all cases.
	 * This is Algorithm 312, CACM vol 10, Oct 1967.
	 */
	if (a >= 0) {
		t = sqrt((a + hypot(a, b)) * 0.5);
		return (cpackf(t, b / (2.0 * t)));
	} else {
		t = sqrt((-a + hypot(a, b)) * 0.5);
		return (cpackf(fabsf(b) / (2.0 * t), copysignf(t, b)));
	}
}

 void bounce(const Player &player, float &newXSpeed, float &newYSpeed) {
     const float ballCenterY = _y + (BALL_SIZE / 2);
     const float playerCenterY = player.getCoords().y + HALF_PLAYER_HEIGHT;
     const float normalizedDistance = fmin(1.0f, fabs(playerCenterY - ballCenterY) / HALF_PLAYER_HEIGHT);
     const float speedScaleX = fmax(0.5f, 1.0f - normalizedDistance);
     const float speedScaleY = fmax(0.5f, normalizedDistance);
     newXSpeed = fmin(MAX_BALL_SPEED, MAX_BALL_SPEED * speedScaleX + MAX_BALL_SPEED / 2.0f);
     newYSpeed = copysignf(MAX_BALL_SPEED * speedScaleY, _ySpeed);
 }

DLLEXPORT float complex
cprojf(float complex z)
{

	if (!isinf(crealf(z)) && !isinf(cimagf(z)))
		return (z);
	else
		return (CMPLXF(INFINITY, copysignf(0.0, cimagf(z))));
}

float complex
cprojf(float complex z)
{

    if (!isinf(crealf(z)) && !isinf(cimagf(z)))
        return (z);
    else
        return (cpackf(INFINITY, copysignf(0.0, cimagf(z))));
}

static void
radius_callback (GtkWidget *slider, gpointer user_data)
{
  dt_iop_module_t *self = (dt_iop_module_t *)user_data;
  if(self->dt->gui->reset) return;
  dt_iop_lowpass_params_t *p = (dt_iop_lowpass_params_t *)self->params;
  p->radius = copysignf(dt_bauhaus_slider_get(slider), p->radius);
  dt_dev_add_history_item(darktable.develop, self, TRUE);
}

long int
__lroundf (float x)
{
  float adj;

  adj = 0x1.fffffep-2;		/* nextafterf (0.5f, 0.0f) */
  adj = copysignf (adj, x);
  return x + adj;
}

inline float rangecompress (float x)
{
    // Formula courtesy of Sony Pictures Imageworks
    const float x1 = 1.0, a = 1.2607481479644775391;
    const float b = 0.28785100579261779785, c = -1.4042005538940429688;
    float absx = fabsf(x);
    if (absx <= x1)
        return x;
    return copysignf (a + b * logf(fabsf(c*absx + 1.0f)), x);
}

float erff(float x)
{
  float t;
  float z;

  z = fabsf(x);
  t = 1.0F / (1.0F + P * z);
  t = 1.0F - (((((A5 * t + A4) * t) + A3) * t + A2) * t + A1) * t * expf(-z * z);
  return copysignf(t, x);
}

__attribute__((noinline, noclone)) void
f1 (void)
{
  a[0] = copysignf (b[0], c[0]) + 1.0f + sqrtf (d[0]);
  a[1] = copysignf (b[1], c[1]) + 2.0f + sqrtf (d[1]);
  a[2] = copysignf (b[2], c[2]) + 3.0f + sqrtf (d[2]);
  a[3] = copysignf (b[3], c[3]) + 4.0f + sqrtf (d[3]);
  a[4] = copysignf (b[4], c[4]) + 5.0f + sqrtf (d[4]);
  a[5] = copysignf (b[5], c[5]) + 6.0f + sqrtf (d[5]);
  a[6] = copysignf (b[6], c[6]) + 7.0f + sqrtf (d[6]);
  a[7] = copysignf (b[7], c[7]) + 8.0f + sqrtf (d[7]);
}

void ImuFilter::imuMagCallback(
	const ImuMsg::ConstPtr& imu_msg_raw,
	const MagMsg::ConstPtr& mag_msg)
{
	boost::mutex::scoped_lock(mutex_);

	const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
	const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
	const geometry_msgs::Vector3& mag_fld = mag_msg->vector;

	ros::Time time = imu_msg_raw->header.stamp;
	imu_frame_ = imu_msg_raw->header.frame_id;

	if (!initialized_)
	{
		// initialize roll/pitch orientation from acc. vector    
		double sign = copysignf(1.0, lin_acc.z);
		double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x*lin_acc.x + lin_acc.z*lin_acc.z));
		double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y*lin_acc.y + lin_acc.z*lin_acc.z));
		double yaw = 0.0; // TODO: initialize from magnetic raeding?

		tf::Quaternion init_q = tf::createQuaternionFromRPY(roll, pitch, yaw);

		q1 = init_q.getX();
		q2 = init_q.getY();
		q3 = init_q.getZ();
		q0 = init_q.getW();

		w_bx_ = 0;
		w_by_ = 0;
		w_bz_ = 0;

		last_time_ = time;
		initialized_ = true;
	}

	// determine dt: either constant, or from IMU timestamp
	float dt;
	if (constant_dt_ > 0.0)
		dt = constant_dt_;
	else
		dt = (time - last_time_).toSec();

	last_time_ = time;

	madgwickAHRSupdate(
		ang_vel.x, ang_vel.y, ang_vel.z,
		lin_acc.x, lin_acc.y, lin_acc.z,
		mag_fld.x, mag_fld.y, mag_fld.z,
		dt);

	publishFilteredMsg(imu_msg_raw);
	if (publish_tf_)
		publishTransform(imu_msg_raw);
}

float mb_read_float(const unsigned char *buf)
{
	if(!buf) return(nanf(""));
	// Convert MODBus float (see 2.11.1) into native single-precision float
	/* You are not expected to understand this */
	bool s=buf[2]&0x80;
	int8_t e=(((buf[2]&0x7f)<<1)|((buf[3]>>7)&1))-127;
	uint32_t m=(1<<23)|((buf[3]&0x7f)<<16)|(buf[0]<<8)|buf[1];
	float f=ldexpf(m, e-23);
	return(copysignf(f, s?-1:1));
}

/// Determine whether a sphere is wholly outside a cone.
///
/// This is used to cull disks - the sphere is the bounding sphere of the
/// disk, and the cone is the cone of incoming light of interest.
///
/// Getting an expression which is both correct and efficient (ie, not
/// involving special functions) is tricky.  The simple version of it is when
/// the radius is zero, in which case we just need to compute
///
///     cosConeAngle > dot(p/plen, N)
///
/// In the general case of spheres with nonzero radius the condition to check
/// is that
///
///     coneAngle + boundingSphereAngle < coneNormalToSphereCenterAngle
///
/// After some algebra, this reduces to the expression
///
///     sqrt(dot(p,p) - r*r)*cosConeAngle - r*sinConeAngle > dot(p, n);
///
/// which is valid as long as the sum of the sphere angle and cone angle are
/// less than pi:
///
///     sqrt(1 - r*r/dot(p,p)) < -cosConeAngle
///
/// in which case the sphere must intersect the cone.
///
/// \param p - center of sphere
/// \param plen2 - length of p
/// \param r - sphere radius
/// \param n - cone normal
/// \param cosConeAngle - cos(theta) where theta = cone angle from N
/// \param sinConeAngle - sin(theta)
inline bool sphereOutsideCone(V3f p, float plen2, float r,
                              V3f n, float cosConeAngle, float sinConeAngle)
{
    // The actual expressions used here are an optimized version which does
    // the same thing, but avoids calling sqrt().  This makes a difference
    // when this is the primary culling test and you're iterating over lots of
    // points - you need to reject invalid points as fast as possible.
    float x = plen2 - r*r;
    // special case - if the sphere covers the origin, it must intersect the
    // cone.
    if(x < 0)
        return false;
    // Special case - if sphere angle and cone angle add to >= 180 degrees, the
    // sphere and cone must intersect.
    if(cosConeAngle < 0 && x < plen2*cosConeAngle*cosConeAngle)
        return false;
    // General case
    float lhs = x*cosConeAngle*cosConeAngle;
    float rhs = dot(p, n) + r*sinConeAngle;
    return copysignf(lhs, cosConeAngle) > copysignf(rhs*rhs, rhs);
}

int main()
{
  if (copysign (2.0, 1.0) != 2.0)
    link_error ();
  if (copysign (2.0, -1.0) != -2.0)
    link_error ();
  if (copysign (-2.0, 1.0) != 2.0)
    link_error ();
  if (copysign (-2.0, -1.0) != -2.0)
    link_error ();

  if (copysign (2.0, 1.0) != 2.0)
    link_error ();
  if (copysign (2.0, -1.0) != -2.0)
    link_error ();
  if (copysign (-2.0, 1.0) != 2.0)
    link_error ();
  if (copysign (-2.0, -1.0) != -2.0)
    link_error ();

  if (copysignf (2.0f, 1.0f) != 2.0f)
    link_error ();
  if (copysignf (2.0f, -1.0f) != -2.0f)
    link_error ();
  if (copysignf (-2.0f, 1.0f) != 2.0f)
    link_error ();
  if (copysignf (-2.0f, -1.0f) != -2.0f)
    link_error ();

  if (copysignl (2.0l, 1.0l) != 2.0l)
    link_error ();
  if (copysignl (2.0l, -1.0l) != -2.0l)
    link_error ();
  if (copysignl (-2.0l, 1.0l) != 2.0l)
    link_error ();
  if (copysignl (-2.0l, -1.0l) != -2.0l)
    link_error ();

  return 0;
}