ZPS3_4

#include <dsplib.h>
#include <testsignal.h>
#include <okno_hamminga.h>
#include <stdio.h>

#define NUM_SAMPLES 2048

int klarnet[NUM_SAMPLES];
int testowa[NUM_SAMPLES]; //potem usunąć
int okno_h[NUM_SAMPLES];

int i;
int j = 0;
int roznica = 0;
int znalezione = 0;
int maks = 0;

const long f_s = 48000;
const int N = 2048;

void maksimum(int* tablica, int podloga, int number){

    for(i = 0; i < number; i++){

            if(tablica[i] > podloga){
                roznica = tablica[i] - tablica[i - 1];
                if(roznica > 0){
                    maks = i;
                    znalezione = 1;
                    printf("%d \n", maks);
                }
                else{
                    if(znalezione==1 && (roznica < 0)){
                        break;
                    }
                    else{continue;}
                }
            }
            else{continue;}
    }

}

void czestotliwosc(int p){
    int df = f_s / N;
    df = df << 15;
    int f_maks = _smpy(p, df);              //format Q30

    f_maks = _sround(f_maks) >> 15;

    printf("%d \n", f_maks);

}


void main(void) {
    int x = okno_hamminga[0];


    for (i = 0; i < NUM_SAMPLES; i++){
        klarnet[i] = testsignal[i];
        //klarnet[i] = _smpy(okno_hamminga[i], testsignal[i]);
        testowa[i] = testsignal[i];
    }

    rfft((DATA*)klarnet, NUM_SAMPLES, SCALE);                  // do 2.

    for (i = 0; i < NUM_SAMPLES; i++){
        klarnet[i] = _smpy(klarnet[i], klarnet[i]);          //do 2.
    }

    for(i = 2; i < NUM_SAMPLES; i = i + 2){
        klarnet[j] = klarnet[i] + klarnet[i + 1];
        j++;
    }

    sqrt_16((DATA*)klarnet, (DATA*)klarnet, 512);

    maksimum(klarnet, 2800, NUM_SAMPLES);
    czestotliwosc(maks);

    while (1); // do not exit
}