/* pythsum.c Pythagorean Sum Algorithm */

/* These routines find the Pythagorean sum of x and y,
    namely sqrt(x^2 + y^2)

    float ApproxPythSum ( float x, float y : real ) ;
    Crude approximation (choose K according to desired approximation criterion)
    Inputs:  x, y

    float PythSum ( float x, float y : real ) ;
    Uses Moler-Morrison algorithm
    Inputs:  x, y

    These routines can be used for AM demodulation.

    For further details see DSP-CSP Section 16.4   */

/* Jonathan (Y) Stein */


#include <math.h>
#include <stdlib.h>
#define sqr(x) x*x

/* old crude approximation */
float ApproxPythSum ( float x, float y ) {
float K = 0.300585 ; /* minimum variance */
/* float K = 0.267304 ; unbiased */
    return max(fabs(x),fabs(y))  +  K * min(fabs(x),fabs(y)) ;
}



/* Moler and Morrison's Pythagorean sum routine */
float PythSum ( float x, float y ) {
float  p, q, r, s, resolution=0.00001 ;
    x = fabs(x) ;
    y = fabs(y) ;
    p = max(x,y) ;
    q = min(x,y) ;
    while (q > resolution) {
        r =  sqr(q/p) ;
        s = r / (4+r) ;
        p +=  2 * s * p ;
        q = s * q ;
    }
    return p ;
}

#ifdef TEST
/* to compile for test:   cc -DTEST pythsum.c -o pythtest.exe  */


#include <stdio.h>

void main ( void ) {
int i ;
float x, y, p, a, m ;

    printf ( "Pythagorean Sum test\n" ) ;
    printf ( "    x        y       check    approx   Moler\n"  ) ;
    for (i=0; i<=20; i++) {
        x = 2*((float)rand()/RAND_MAX)-1 ;
        y = 2*((float)rand()/RAND_MAX)-1 ;
        p = sqrt(x*x + y*y) ;
        a = ApproxPythSum ( x, y ) ;
        m = PythSum ( x, y ) ;
        printf ( "%8.4f %8.4f  %8.4f %8.4f %8.4f\n",
                  x,    y,     p,    a,    m  ) ;
    }
}
#endif