#include "lmath_spectrum_freq.h"
#include "lmath_internal.h"
#include "lmath_phase.h"
#include "lmath_fft.h"
#include <stdlib.h>


t_lmath_errs lmath_spectrum_find_peak_freq(const double *amp_spectrum, int size,
                                           double df,
                                           const t_lmath_window_params *winpar,
                                           int search_idx_min, int search_idx_max,
                                           double *peak_freq, double *peak_pwr) {
    int i, fnd_i = 0;
    double fnd_pwr = 0;
    t_lmath_errs err = LMATH_ERR_OK;
    int freq_span = 0;

    if ((amp_spectrum==NULL) || (winpar == NULL) ||  ((peak_freq==NULL) && (peak_pwr==NULL))) {
        err = LMATH_ERR_INVALID_PARAMS;
    } else {
        freq_span = winpar->freq_span;
        if ((df <= 0) || (freq_span < 1)) {
            err = LMATH_ERR_INVALID_PARAMS;
        }
    }

    if (!err) {
        /* если не задана частота, в окрестности которой нужно найти требуемую,
         * то ищем во всем спектре по максимальной мощьности (исключая пост. составляющую) */
        if ((search_idx_min > 0) && (search_idx_min == search_idx_max)) {
            /* иначе рассчитываем мощность только в окрестности указанной частоты */
            int j;
            fnd_i = search_idx_min;
            if (fnd_i >= (size - freq_span/2))
                fnd_i = size - freq_span/2 - 1;
            if (fnd_i < freq_span)
                fnd_i = freq_span;

            for (fnd_pwr=0, j=fnd_i-freq_span/2; j <= (fnd_i+freq_span/2); j++) {
                fnd_pwr+=(amp_spectrum[j]*amp_spectrum[j]);
            }
        } else {
            /* сложение два раза по половине используется для более корректного
             * соответствия при нечетном freq_span */
            int start_idx =  freq_span/2 + freq_span/2 + 1;
            int end_idx = size - freq_span/2;

            if (search_idx_min > 0) {
                search_idx_min += freq_span/2;
                if (search_idx_min > start_idx) {
                    start_idx = search_idx_min;
                }
            }

            if (search_idx_max > 0) {
                search_idx_max -= freq_span/2;
                if ((search_idx_max > 0) && (search_idx_max < end_idx)) {
                    end_idx = search_idx_max + 1;
                }
            }

            for (fnd_i = -1, i = start_idx; i < end_idx; i++) {
                double pwr=0;
                int j;
                for (j=i-freq_span/2; j <= (i+freq_span/2); j++) {
                    pwr+=(amp_spectrum[j]*amp_spectrum[j]);
                }

                if (pwr > fnd_pwr) {
                    fnd_pwr = pwr;
                    fnd_i = i;
                }
            }

            if (fnd_i < 0)
                err = LMATH_ERR_INTERNAL_CALC;
        }
    }

    if (!err) {
        if (peak_freq!=NULL) {
            double fnd_freq=0;
            int j;
            /* поиск частоты сигнала */
            for (j= fnd_i-freq_span/2; j <= (fnd_i+freq_span/2); j++) {
                fnd_freq+=(amp_spectrum[j]*amp_spectrum[j])*j*df;
            }
            fnd_freq/=fnd_pwr;

            *peak_freq = fnd_freq;
        }

        if (peak_pwr!=NULL) {
            *peak_pwr = fnd_pwr/winpar->enbw;
        }
    }

    return err;
}

#ifdef LMATH_USE_FFT
t_lmath_errs lmath_find_peak_freq(const double *src, int size, double dt, double search_freq_min, double search_freq_max,
                                  t_lmath_wintype wintype, double *peak_freq, double *peak_pwr) {
    t_lmath_errs err = LMATH_ERR_OK;
    double *proc_vals = malloc(sizeof(proc_vals[0])*size);
    t_lmath_window_ctx winctx = lmath_window_ctx_create(wintype, 0, size);
    double df = 0.;


    if ((proc_vals == NULL) || (winctx ==NULL)) {
        err = LMATH_ERR_MEMORY_ALLOC;
    } else {
        /* амплитуду берем только на заданной частоте по спектру */
        err = lmath_window_apply_scaled(src, winctx, proc_vals);
    }

    if (!err) {
        err = lmath_amp_pha_spectrum_peak(proc_vals, size, dt, 0, LMATH_PHASE_MODE_COS, proc_vals, 0, &df);
    }

    if (!err) {
        err = lmath_spectrum_find_peak_freq(proc_vals, size/2+1, df, lmath_window_params(winctx),
                                            (int)(search_freq_min/df + 0.5),
                                            (int)(search_freq_max/df + 0.5),
                                            peak_freq, peak_pwr);
    }

    free(proc_vals);
    lmath_window_ctx_destroy(winctx);

    return err;
}
#endif
