#include "raw_proto.h"

/* bitrev ***********************************************
* INPUT:    r       array to store bit reversed indeces *
*           npts    how many points in FFT              *
* PURPOSE:                                              *
* compute the bit reversed indeces to unravel           *
* decimation in frequency scheme.                       *
********************************************************/
void bitrev(int *r, long int npts)
{
    int i,bit,n,shift,e;
    
    frexp((double)npts,&e);
    e--;
    
    for(i=0;i<npts;i++)
    {
        bit = 1; shift = npts/2; *r = 0;
        for(n=0;n<e;n++)
        {
            if(i&bit) *r += shift;
            bit <<= 1; shift >>= 1;
        }
        r++;
    }
}

/* cadd *****************************************************************
* INPUT:    cf1             a pointer to first complex number struct    *
*           cf2             a pointer to second complex number struct   *
* OUTPUT:   ComplexFloat    A complex number struct.                    *
* PURPOSE:                                                              *
* adds cf1 and cf2, returning the complex number struct.                *
************************************************************************/
ComplexFloat cadd(ComplexFloat *cf1, ComplexFloat *cf2)
{
    ComplexFloat cfadd;

    cfadd.r = cf1->r + cf2->r;
    cfadd.i = cf1->i + cf2->i;

    return(cfadd);
}

/* cmag *****************************************************************
* INPUT:    cf1             a pointer to first complex number struct    *
*           cf2             a pointer to second complex number struct   *
* OUTPUT:   ComplexFloat    A complex number struct.                    *
* PURPOSE:                                                              *
* multiplies the conjugate of cf1 with cf2, returns the complex         *
* number struct.                                                        *
************************************************************************/
ComplexFloat cmag(ComplexFloat *cf1, ComplexFloat *cf2)
{
    ComplexFloat cfmag;

    cfmag.r = cf1->r * cf2->r + cf1->i * cf2->i;
    cfmag.i = cf1->r * cf2->i - cf1->i * cf2->r;

#if 0
    printf("r %f i %f\n",cfmag.r,cfmag.i);
#endif

    return(cfmag);
}

/* cphase ***************************************************************
* INPUT:    cf1             a pointer to first complex number struct    *
*           cf2             a pointer to second complex number struct   *
* OUTPUT:   ComplexFloat    A complex number struct.                    *
* PURPOSE:                                                              *
* multiplies the conjugate of cf1 with cf2, returns the complex phase   *
* number struct.                                                        *
************************************************************************/
ComplexFloat cphase(ComplexFloat *cf1, ComplexFloat *cf2)
{
    ComplexFloat cfmag;
    float mag;

    cfmag.r = cf1->r * cf2->r + cf1->i * cf2->i;
    cfmag.i = cf1->r * cf2->i - cf1->i * cf2->r;
    mag = sqrt(cfmag.r * cfmag.r + cfmag.i * cfmag.i);
    cfmag.r /= mag;
    cfmag.i /= mag;

#if 0
    printf("r %f i %f\n",cfmag.r,cfmag.i);
#endif

    return(cfmag);
}

/* cmult ****************************************************************
* INPUT:    cf1             a pointer to first complex number struct    *
*           cf2             a pointer to second complex number struct   *
* OUTPUT:   ComplexFloat    A complex number struct.                    *
* PURPOSE:                                                              *
* multiplies cf1 and cf2, returning the complex number struct.          *
************************************************************************/
ComplexFloat cmult(ComplexFloat *cf1, ComplexFloat *cf2)
{
    ComplexFloat cfmul;

    cfmul.r = cf1->r * cf2->r - cf1->i * cf2->i;
    cfmul.i = cf1->r * cf2->i + cf1->i * cf2->r;
    
    return(cfmul);
}

/* csub *****************************************************************
* INPUT:    cf1             a pointer to first complex number struct    *
*           cf2             a pointer to second complex number struct   *
* OUTPUT:   ComplexFloat    A complex number struct.                    *
* PURPOSE:                                                              *
* subtracts cf2 from cf1, returning the complex number struct.          *
************************************************************************/
ComplexFloat csub(ComplexFloat *cf1, ComplexFloat *cf2)
{
    ComplexFloat cfsub;

    cfsub.r = cf1->r - cf2->r;
    cfsub.i = cf1->i - cf2->i;

    return(cfsub);
}

/* df_fft ***********************************************************
* INPUT:    n1      an array of complex numbers                     *
*           npts    how many numbers/points in FFT                  *
*           e       power of 2 exponent of npts                     *
*           wi      array of complex wings used in FFT              *
*           r       array of bit reversed indeces                   *
*           phase   a 1 indicates FFT, a -1 indicates IFFT.         *
* PURPOSE:                                                          *
* takes FFT of a single sequence of npts numbers, npts must be a    *
* power of 2 (ie: e is an int), this routine should be fast since   *
* it already has wings computed, as well as bit reversed sequence   *
********************************************************************/
void df_fft(ComplexFloat *n1, int npts, int e, ComplexFloat *wi, int *r, int phase)
{
    int m,i,pos1,pos2,inipos2,wp,skip;
    ComplexFloat ctmp;

    skip =1;
    phase++;
    m = npts/2;
    for(i=0;i<e;i++)
    {
        pos1 = 0;
        pos2 = m;
        inipos2 = pos2;
        wp = 0;
        while(pos2<npts)
        {
            /* get corresponding wing terms */
            ctmp = wi[wp]; if(!phase) ctmp.i = -ctmp.i;
            /* do 2-point butterfly */
            df_fft_cell(n1 + pos1, n1 + pos2, &ctmp);
            pos1++; pos2++; wp += skip;
            if(pos1 == inipos2)
            {
                pos1 = pos2;
                inipos2 = pos2 = pos1 + m;
                wp = 0;
            }
        }
        skip *= 2;
        m /= 2;
    }
    
    for(i=0;i<npts;i++)
    {
        if(i < *r)
        {
            ctmp = *n1;
            *n1 = n1[*r - i];
            n1[*r - i] = ctmp;
        }
        n1++;
        r++;
    }
}

/* df_fft_cell ******************************************
* INPUT:    a1       a pointer to first complex number  *
*           a2       a pointer to second complex number *
*           cm       a pointer to current wing in FFT   *
* PURPOSE:                                              *
* computes a simple 2-point FFT (butterfly)             *
********************************************************/
void df_fft_cell(ComplexFloat *a1, ComplexFloat *a2, ComplexFloat *cm)
{
    ComplexFloat tmp;

    tmp = csub(a1,a2);
    *a1 = cadd(a1,a2);
    *a2 = cmult(&tmp,cm);
}

/* df_fft_data ******************************************************
* INPUT:    pvrx    a pointer an array of complex time sequences    *
*           ch      a pointer to the current Harris raw data Header *
*           wi      pointer to complex FFT wings array              *
*           br      pointer to array of bit reversed indeces        *
*           phase   a 1 forces FFT, a -1 forces IFFT                *
* PURPOSE:                                                          *
* to compute FFT on an array of time sequences for different ranges *
********************************************************************/
void df_fft_data(ComplexFloat *pvrx, HarrisHeader *ch, InputParms *ip, ComplexFloat *wi, int *br, int phase)
{
    ComplexFloat *pv,*pv_last;
    long int last_pos;
    int npts,e;
    
    npts = ip->fftwdth;
    frexp((double)npts,&e);
    e--;
    
    if(ip->pm == 3 || ip->pm == 4)
    {
        last_pos = ch->nchanls/2 * ch->tu_ng * npts;

#ifdef DEBUG_FFT
        printf("df_fft: df_fft_data: Computing %d-point fft for %ld receivers\n",
            npts,ch->nchanls/2);
#endif /* DEBUG_FFT */

    }
    else
    {
        last_pos = ch->tu_ng * npts;

#ifdef DEBUG_FFT
        printf("df_fft: df_fft_data: Computing %d-point fft for 1 receiver\n",npts);
#endif /* DEBUG_FFT */

    }
    
    for(pv_last = pvrx + last_pos; pvrx < pv_last; pvrx += npts)
        df_fft(pvrx, npts, e, wi, br, phase);
}

/* wings ********************************************************
* INPUT:    w        an array to store the complex wing terms   *
*           max      number of points of FFT                    *
* PURPOSE:                                                      *
* compute complex wing terms used in FFT                        *
****************************************************************/
void wings(ComplexFloat *w, long int max)
{
    ComplexFloat *last_w;
    double theta,del_theta;
    int n;

    theta = 0.0;
    del_theta = 6.283185307 / max;

    for(last_w = w + max;w<last_w;w++)
    {
        w->r = cos(theta);
        w->i = sin(theta);
        theta += del_theta;
    }
}

/* unwrap ************************************************************
* INPUT:    c           complex number struct
*           last_angle  float pointer to value of previous angle
* OUTPUT:   float       phase angle in radians
* PURPOSE:
* Returns adjusted phase angle for given complex number assuming
* variation of phase angle progresses in jumps less than 180 degrees
* in phase from one sample to the next.
*********************************************************************/
float unwrap(ComplexFloat *c,float *last_angle)
{
    float angle,tmp;
    long int step;
    
    if(c->i != 0.0 && c->r != 0.0)
      angle = atan2(c->i,c->r);
    else
      angle = 0.0;
    tmp = (*last_angle - angle) / PI;
    step = (tmp + (*last_angle > angle ? 1:-1))/2;
    
    angle += step * PI2;
    *last_angle = angle;

#if 0
    printf("cr %f ci %f angle %f\n",c->r,c->i,angle);
#endif

    return(angle);
}