/* moments.c */
#include "raw_proto.h"

/* compute_bin_doppler **************************************************
* INPUT:    ph      pointer to processed data header
*           data    pointer to array to store doppler data
*           spec    pointer to spectral array for all ranges
* OUTPUT:   returns updated data pointer.
* PURPOSE:  To handle doppler computation.  result is independent of
* actual doppler shift, using only bin numbers.
************************************************************************/
float *compute_bin_doppler(ProcHeader *ph, float *data, ComplexFloat *spec)
{
    long int i,rx_skip;

    rx_skip = (ph->tu_ng - ph->last_w - ph->n_tx_smp - ph->n_nx_smp) * ph->fftwdth;

    /* find doppler for transmitter samples if any */
    if(ph->inc_tx)
    {
        for(i=0;i<ph->n_tx_smp;i++)
        {
            *data++ = compute_sp_doppler(spec,ph);
            spec += ph->fftwdth;
        }
    }
    else /* skip over them */
    {
        spec += ph->fftwdth * ph->n_tx_smp;
    }
    
    /* skip to first range gate to write */
    if(ph->first_w)
    {
        spec += ph->first_w * ph->fftwdth;
    }
    
    /* find power in range samples */
    for(i=ph->first_w;i<ph->last_w;i++)
    {
        *data++ = (ph->tu_tau) ?
                    compute_dp_doppler(spec,ph):
                    compute_sp_doppler(spec,ph);
        spec += ph->fftwdth;
    }
    
    /* find power in noise samples if any */
    if(ph->inc_nx)
    {
        /* skip to first noise gate */
        if(rx_skip > 0)
        {
            spec += rx_skip;
        }
        
        for(i=0;i<ph->n_nx_smp;i++)
        {
            *data++ = compute_sp_doppler(spec,ph);
            spec += ph->fftwdth;
        }
    }

    return(data);
}

/* compute_bin_width ************************************************
* INPUT:    spec    pointer to spectral array for all ranges
*           ph      pointer to processed data header
* OUTPUT:   float   bin count
* PURPOSE:
* To determine spectral width for one spectrum.  Determines maximum
* and proceeds to count bins that exceed half of maximum value.
* If max bin signal power is less than 1/4th of bin noise level,
* the data is considered noisy, and width is set to 0.
********************************************************************/
float compute_bin_width(ComplexFloat *spec, ProcHeader *ph)
{
    long int i, count = 0;
    float max = 0.,fcount;
    
    for(i=0;i<ph->fftwdth;i++)
    {
        if(spec[i].r > max)
        {
            max = spec[i].r;
        }
    }
    
    /*
    *  at least one bin must have power level greater than
    *  the noise level in order for there to be signal present
    */
    if(max > ph->bin_noise)
    {
        max = (max + ph->bin_noise)/2;
        
        for(i=0;i<ph->fftwdth;i++)
        {
            if(spec[i].r > max)
            {
                count++;
            }
        }
    }
    
    fcount = count;
    
    return(fcount);
}

/* compute_dp_doppler ***************************************************
* INPUT:    spec    pointer to a spectral array
*           ph      pointer to processed data header
* OUTPUT:   returns adjusted value of mean double pulse doppler bin.
* PURPOSE:
* To handle double pulse doppler computation.  result is
* independent of actual doppler shift, using only bin numbers.
************************************************************************/
float compute_dp_doppler(ComplexFloat *spec, ProcHeader *ph)
{
    long int i,hlfwdth,offset;
    float doppler = 0.0, mag = 0.0;
    
    hlfwdth = ph->fftwdth >> 1;
    
    /* if we assume reasonable spectra will not wrap in a tu_tau interval,
     * the true doppler shift can be computed from the phase of the tu_tau
     * shift and the current bin index.
     */
    for(i=0;i<ph->fftwdth;i++)
    {
        /* Subtract noise power level (ph->bin_noise) from spectra.
         * Make sure ph->bin_noise is correctly modified to reflect
         * processing of range data, eg: double pulse range data vs 
	 * single pulse noise.
         * This should have been done in compute_sp_power().
         */
        if(spec[i].r < ph->bin_noise)
        {
            spec[i].r = 0.0;
        }
        else
        {
            spec[i].r -= ph->bin_noise;
        }
        mag += spec[i].r;
        
        /* find nearest whole integer offset. */
        offset = ((spec[i].i<0) ? spec[i].i - 0.5: spec[i].i + 0.5);
        doppler += spec[i].r * 
                    (((i<hlfwdth) ? i: i - ph->fftwdth) + ph->fftwdth * offset);
    }
    
    /* arbitrary minimum power threshold */
    if(mag < 1.0e-5)
    {
        doppler = 0.;
    }
    else /* scale doppler and return mean bin */
    {
        doppler /= mag;
    }
    
    return(doppler);
}

/* compute_dp_power *****************************************************
* INPUT:    cf      pointer to a spectral array
*           ph      pointer to processed data header
* OUTPUT:   returns adjusted value of mean double pulse power.
* PURPOSE:
* To handle double pulse power computation.
************************************************************************/
float compute_dp_power(ComplexFloat *cf, ProcHeader *ph)
{
    static float two_pi = 3.141592654*2.;
    float power = 0.0,ipp_tau;
    long int i,hlfwdth;

    /* 100 correction factor to adjust tu_ipp to tu_tau units. */
    ipp_tau = 100. * ph->tu_ipp / ph->tu_tau;
    /* correct DC */
    cf[0].r = (cf[1].r + cf[ph->fftwdth-1].r)/2.;
    cf[0].i = (cf[1].i + cf[ph->fftwdth-1].i)/2.;
    
    hlfwdth = ph->fftwdth >> 1;
    
    for(i=0;i<ph->fftwdth;i++)
    {
      RecToPol(&cf[i]);
      /* remember to adjust bin factor in compute_dp_doppler() */
      cf[i].i = (cf[i].i/two_pi) * ipp_tau - 1.0*((i<hlfwdth) ? 
						  i
						  : 
						  i - ph->fftwdth
						  )/ph->fftwdth;
      power += cf[i].r;
    }

    return(power);
}

/* compute_moments ******************************************************
* INPUT:    ph      pointer to processed data header
*           pd      processed data array
*           spec    pointer to spectral array for all ranges
* PURPOSE:
* To handle moments calculations for selected spectral data.
************************************************************************/
void compute_moments(ProcHeader *ph, float *pd, ComplexFloat *spec)
{
    float *pspn, *pdop, *pdev;
    long int i;
    ProcHeader *tmpPh;
    
    tmpPh = (ProcHeader *) pd;
    
    pspn = (float *)((char *)pd + ph->size);

    /* compute power in ranges and return updated processed data pointer */
    pdop = compute_power(ph, pspn, spec);

#if 0
    /* see signal plus noise */
    show_noise_spectra(spec,ph);
#endif
    
    pdev = compute_bin_doppler(ph, pdop, spec);

#if 0
    /* see spectra after subtracting noise level. */
    show_noise_spectra(spec,ph);
#endif
    
    compute_spectral_width(ph, pdev, spec);

    ph->nbytes =    (ph->last_w == -1 ?
                        -1 /* nothing to write out */
                        :
                        (    ph->last_w - ph->first_w
                            +
                            (ph->inc_tx ?
                                ph->n_tx_smp
                                :
                                0
                            )
                            +
                            (ph->inc_nx ?
                                ph->n_nx_smp
                                :
                                0
                            )
                        ) * 3 * sizeof(float)
                    );
    ph->totnbyts = ph->nbytes + ph->size;
    
    /* add processed data header into start of output data record */
    *tmpPh = *ph;
    
#if 0
    /* see RTI of moments and draw line for noise level. */
    show_moments(pspn, pdop, pdev, ph);
#endif
}

/* compute_power ********************************************************
* INPUT:    ph      pointer to processed data header
*           data    pointer to array to store power data
*           spec    pointer to spectral array for all ranges
* OUTPUT:   returns updated data pointer.
* PURPOSE:
* To handle power computation for all ranges.
************************************************************************/
float *compute_power(ProcHeader *ph, float *data, ComplexFloat *spec)
{
    long int i,rx_skip;

    rx_skip = (ph->tu_ng - ph->last_w - ph->n_tx_smp - ph->n_nx_smp) * ph->fftwdth;

    /* find power in transmitter samples if any desired */
    if(ph->inc_tx)
    {
        for(i=0;i<ph->n_tx_smp;i++)
        {
            *data++ = compute_sp_power(spec,ph,0);
            spec += ph->fftwdth;
        }
    }
    else /* just skip over to the range gates */
    {
        spec += ph->fftwdth * ph->n_tx_smp;
    }
    /* skip to first range gate to write */
    if(ph->first_w)
    {
        spec += ph->first_w * ph->fftwdth;
    }
    
    /* find power in range samples */
    for(i=ph->first_w;i<ph->last_w;i++)
    {
        *data++ = (ph->tu_tau) ?
                    compute_dp_power(spec,ph):
                    compute_sp_power(spec,ph,0);
        spec += ph->fftwdth;
    }
    
    /* skip to first noise gate */
    if(rx_skip)
    {
        spec += rx_skip;
    }
    
    /* find power in noise samples and compute bin_noise.
     * bin_noise is used for doppler and spectral width calculations
     * but not for power calculations.  This allows us to see full
     * signal plus noise in RTI if we desire, for further examination.
     * The noise level is stored as part of the processed data header.
     * The noise level is an average of all noise gates.
     */
    ph->bin_noise = 0.0;
    for(i=0;i<ph->n_nx_smp;i++)
    {
        *data = compute_sp_power(spec,ph,1);
        ph->bin_noise += *data;

        /* increment counter only if we wish to save noise data */
        if(ph->inc_nx) data++;
        spec += ph->fftwdth;
    }
    
    ph->bin_noise /= ph->n_nx_smp;
    
    return(data);
}

/* compute_sp_doppler ***************************************************
* INPUT:    spec    pointer to a spectral array
*           ph      pointer to processed data header
* OUTPUT:   returns adjusted value of mean single pulse doppler bin.
* PURPOSE:
* To handle single pulse doppler computation.  result is
* independent of actual doppler shift, using only bin numbers.
************************************************************************/
float compute_sp_doppler(ComplexFloat *spec, ProcHeader *ph)
{
    long int i,hlfwdth;
    float doppler = 0.0, mag = 0.0;
    
    hlfwdth = ph->fftwdth >> 1;
    for(i=0;i<ph->fftwdth;i++)
    {
        /* Subtract noise power level (ph->bin_noise) from spectra.
         * Make sure ph->bin_noise is correctly modified to reflect
         * processing of range data, eg: barker range data vs single pulse noise.
         * This should have been done in compute_sp_power().
         */
        if(spec[i].r < ph->bin_noise)
        {
            spec[i].r = 0.0;
        }
        else
        {
            spec[i].r -= ph->bin_noise;
        }
        mag += spec[i].r;
        
        doppler += ((i<hlfwdth) ? i: i - ph->fftwdth) * spec[i].r;
    }
    
    if(mag < 1.0e-1)
    {
        doppler = 0.0;
    }
    else
    {
        doppler /= mag;
    }
    
    return(doppler);
}

/* compute_sp_power *****************************************************
* INPUT:    cf            pointer to a spectral array
*           ph            pointer to processed data header
*           noise_flag    if 1 then assume data is from noise gates
* OUTPUT:   returns adjusted value of mean double pulse power.
* PURPOSE:
* To handle single pulse power computation, and noise gate mean bin noise
* computation.  A percentile is used specified by the PERCENTILE_NOISE
* constant defined in stcr_proto.h
************************************************************************/
float compute_sp_power(ComplexFloat *cf, ProcHeader *ph, long int noise_flag)
{
    float power = 0.0, nscale;
    long int i;

    /* correct DC */
    cf[0].r = (cf[1].r + cf[ph->fftwdth-1].r)/2.;

    /*
     * we use percentile scheme to calculate mean noise,
     * doing so prevents large spikes from contributing to
     * mean noise value.
     */
    if(!noise_flag)
    {
        for(i=0;i<ph->fftwdth;i++)
        {
            power += cf[i].r;
        }
    }
    else
    {
        power = sort_spectrum(cf,ph);

#ifdef DEBUG_MOM
        printf("moments: compute_sp_power: Correcting Noise Power\n");
#endif /* DEBUG_MOM */

        /* adjust power calculation for double pulse and barker coded data */
        if(ph->tu_tau) power /= sqrt((double) ph->nave);
        if(ph->nbaudp) power *= ph->nbaudp;
    }
    return(power);
}

/* compute_spectral_width *******************************************
* INPUT:    ph      pointer to processed data header
*           pdev    pointer to processed spectral width array
*           spec    pointer to spectral array for all ranges
* PUROSE:
* Compute the full spectral width in bin count at half max power.
********************************************************************/
void compute_spectral_width(ProcHeader *ph, float *pdev, ComplexFloat *spec)
{
    long int i,rx_skip;

    rx_skip = (ph->tu_ng - ph->last_w - ph->n_tx_smp - ph->n_nx_smp) * ph->fftwdth;

    /* find width for transmitter samples if any */
    if(ph->inc_tx)
    {
        for(i=0;i<ph->n_tx_smp;i++)
        {
            *pdev++ = compute_bin_width(spec,ph);
            spec += ph->fftwdth;
        }
    }
    else /* skip over them */
    {
        spec += ph->fftwdth * ph->n_tx_smp;
    }
    
    /* skip to first range gate to write */
    if(ph->first_w)
    {
        spec += ph->first_w * ph->fftwdth;
    }
    
    /* find width in range samples */
    for(i=ph->first_w;i<ph->last_w;i++)
    {
        *pdev++ = compute_bin_width(spec,ph);
        spec += ph->fftwdth;
    }
    
    /* find width in noise samples if any */
    if(ph->inc_nx)
    {
        /* skip to first noise gate */
        if(rx_skip > 0)
        {
            spec += rx_skip;
        }
        
        for(i=0;i<ph->n_nx_smp;i++)
        {
            *pdev++ = compute_bin_width(spec,ph);
            spec += ph->fftwdth;
        }
    }
}

/* RecToPol *****************************************************
* INPUT:    cf    a pointer to a complex number
* PURPOSE:
* Convert complex number from rectangular to Polar coordinates.
* angle is given in radians from -pi to +pi.
****************************************************************/
void RecToPol(ComplexFloat *cf)
{
    double angle,mag;
    
    angle = atan2((double)cf->i,(double)cf->r);
    mag = sqrt((double)(cf->r * cf->r + cf->i * cf->i));
    
    cf->r = mag;
    cf->i = angle;
}

/* sort_spectrum ****************************************************
* INPUT:    cf       pointer to complex spectral array
*           ph       pointer to processed data header
* OUTPUT:   float    returns value of PERCENTILE_NOISE bin in array
* PURPOSE:
* To determine the value of mean bin power.  The PERCENTILE_NOISE is
* assumed a reasonable approximation of the mean noise value.  To 
* determine this value the spectral array is sorted in ascending order
* and the value of the PERCENTILE_NOISE is returned.
* History:
* A similar routine is used by John Sahr in his wenas codes, in that
* code he uses the 75th percentile.  I have decided to use a similar
* routine in order to avoid getting 'signal power' into my estimate
* of the noise.  I tried to use the lowest of averaged noise spectrum
* power but this resulted in noticeable increases in noise level when
* there was signal in the noise gates, as is the case in long range
* reflections.
********************************************************************/
float sort_spectrum(ComplexFloat *cf, ProcHeader *ph)
{
    float tf;
    int i,j,npts,k,m;
    static ComplexFloat pcf[MAX_FFT_SIZE];

    for(i=0;i<ph->fftwdth;i++)
    {
      pcf[i] = cf[i];
    }

    npts = ph->fftwdth >> 1;
    while(npts>0)
    {
        k = ph->fftwdth - npts;
        for(i=0;i<k;i++)
        {
            j = i;
            m = i + npts;
            while(pcf[m].r < pcf[j].r)
            {
                tf = pcf[m].r;
                pcf[m].r = pcf[j].r;
                pcf[j].r = tf;
                j -= npts;
                if(j<0) break;
                m -= npts;
            }
        }
        npts >>= 1;
    }
    /* select percentile for bin noise value. */
    i = ph->fftwdth * PERCENTILE_NOISE;
    return(pcf[i].r);
}