/* hypot.c for Thrust by Frank Wille <frank@phoenix.owl.de> */
/* BASED ON: */

/* @(#)e_hypot.c 5.1 93/09/24 */
/*
 * ====================================================
 * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
 *
 * Developed at SunPro, a Sun Microsystems, Inc. business.
 * Permission to use, copy, modify, and distribute this
 * software is freely granted, provided that this notice 
 * is preserved.
 * ====================================================
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <math.h>


#ifdef __VBCC__
#ifdef __PPC__
unsigned long __get_high_word(__reg("r3")double *d) = 
    "\tlwz\t3,0(3)";
unsigned long __get_low_word(__reg("r3")double *d) = 
    "\tlwz\t3,4(3)";
void __set_high_word(__reg("r3")double *d,__reg("r4")unsigned long ix) =
    "\tstw\t4,0(3)";
void __set_low_word(__reg("r3")double *d,__reg("r4")unsigned long ix) =
    "\tstw\t4,4(3)";
#else /* M68k */
unsigned long __get_high_word(__reg("a0")double *d) = 
    "\tmove.l\t(a0),d0";
unsigned long __get_low_word(__reg("a0")double *d) = 
    "\tmove.l\t4(a0),d0";
void __set_high_word(__reg("a0")double *d,__reg("d0")unsigned long ix) =
    "\tmove.l\td0,(a0)";
void __set_low_word(__reg("a0")double *d,__reg("d0")unsigned long ix) =
    "\tmove.l\td0,4(a0)";
#endif

#else
unsigned long __get_high_word(double *d)
{
  return (*(unsigned long *)d);
}
unsigned long __get_low_word(double *d)
{
  return (*((unsigned long *)d+1));
}
void __set_high_word(double *d,unsigned long ix)
{
  *(unsigned long *)d = ix;
}
void __set_low_word(double *d,unsigned long ix)
{
  *((unsigned long *)d+1) = ix;
}
#endif

#define GET_HIGH_WORD(w,d)    w=__get_high_word(&d)
#define GET_LOW_WORD(w,d)     w=__get_low_word(&d)
#define SET_HIGH_WORD(d,w)    __set_high_word(&d,w)
#define SET_LOW_WORD(d,w)     __set_low_word(&d,w)


double hypot(double x, double y)
{
  double a=x,b=y,t1,t2,y1,y2,w;
  long j,k,ha,hb;

  GET_HIGH_WORD(ha,x);
  ha &= 0x7fffffff;
  GET_HIGH_WORD(hb,y);
  hb &= 0x7fffffff;
  if(hb > ha) {a=y;b=x;j=ha; ha=hb;hb=j;} else {a=x;b=y;}
  SET_HIGH_WORD(a,ha);  /* a <- |a| */
  SET_HIGH_WORD(b,hb);  /* b <- |b| */
  if((ha-hb)>0x3c00000) {return a+b;} /* x/y > 2**60 */
  k=0;
  if(ha > 0x5f300000) { /* a>2**500 */
     if(ha >= 0x7ff00000) { /* Inf or NaN */
         unsigned long low;
         w = a+b;     /* for sNaN */
         GET_LOW_WORD(low,a);
         if(((ha&0xfffff)|low)==0) w = a;
         GET_LOW_WORD(low,b);
         if(((hb^0x7ff00000)|low)==0) w = b;
         return w;
     }
     /* scale a and b by 2**-600 */
     ha -= 0x25800000; hb -= 0x25800000;  k += 600;
     SET_HIGH_WORD(a,ha);
     SET_HIGH_WORD(b,hb);
  }
  if(hb < 0x20b00000) { /* b < 2**-500 */
      if(hb <= 0x000fffff) {  /* subnormal b or 0 */  
          unsigned long low;
    GET_LOW_WORD(low,b);
    if((hb|low)==0) return a;
    t1=0;
    SET_HIGH_WORD(t1,0x7fd00000); /* t1=2^1022 */
    b *= t1;
    a *= t1;
    k -= 1022;
      } else {    /* scale a and b by 2^600 */
          ha += 0x25800000;   /* a *= 2^600 */
    hb += 0x25800000; /* b *= 2^600 */
    k -= 600;
    SET_HIGH_WORD(a,ha);
    SET_HIGH_WORD(b,hb);
      }
  }
    /* medium size a and b */
  w = a-b;
  if (w>b) {
      t1 = 0;
      SET_HIGH_WORD(t1,ha);
      t2 = a-t1;
      w  = sqrt(t1*t1-(b*(-b)-t2*(a+t1)));
  } else {
      a  = a+a;
      y1 = 0;
      SET_HIGH_WORD(y1,hb);
      y2 = b - y1;
      t1 = 0;
      SET_HIGH_WORD(t1,ha+0x00100000);
      t2 = a - t1;
      w  = sqrt(t1*y1-(w*(-w)-(t1*y2+t2*b)));
  }
  if(k!=0) {
      unsigned long high;
      t1 = 1.0;
      GET_HIGH_WORD(high,t1);
      SET_HIGH_WORD(t1,high+(k<<20));
      return t1*w;
  } else return w;
}