double processing_clarify_peak_frequency_in_range(Processing* p, double range) {
memcpy(p->subSpectrumReal, p->subSignalReal, p->subLength * sizeof(*p->subSignalReal));
memcpy(p->subSpectrumImag, p->subSignalImag, p->subLength * sizeof(*p->subSignalImag));
DSPDoubleSplitComplex subspectrum = {p->subSpectrumReal, p->subSpectrumImag};
vDSP_fft_zipD(p->sfs, &subspectrum, 1, log2(p->subLength), kFFTDirection_Forward);
memset(p->subSpectrum, 0, p->subLength * sizeof(*p->subSpectrum));
vDSP_zaspecD(&subspectrum, p->subSpectrum, p->subLength);
double subPeak = 0;
size_t subPeakIndex = 1;
for(size_t i = 0; i < p->subLength; i ++) {
if (p->subSpectrum[i] > subPeak) {
subPeak = p->subSpectrum[i];
subPeakIndex = i;
}
}
if (subPeakIndex > p->subLength) {
volatile double shift = subPeakIndex;
return range * (shift - p->subLength) / p->subLength;
}
return range * subPeakIndex / p->subLength;
}
void processing_recalculate(Processing* p){
memcpy(p->real, p->signal, p->signalLength * sizeof(*p->signal));
memset(p->imag, 0, p->signalLength* sizeof(*p->signal));
DSPDoubleSplitComplex spectrum = {p->real, p->imag};
vDSP_fft_zipD(p->fs, &spectrum, 1, log2(p->signalLength), kFFTDirection_Forward);
memset(p->spectrum, 0, p->signalLength * sizeof(*p->spectrum));
vDSP_zaspecD(&spectrum, p->spectrum, p->signalLength);
double peak = 0;
double peakFrequency = 0;
size_t peakIndex = 1;
for(size_t i = 1; i < p->signalLength / 2; i ++){
double spectrumValue = p->spectrum[i];
double f = p->fd * i / p->signalLength;
if (p->filter) {
double df = (f - p->targetFrequency) / p->targetFrequency;
if(df < 0){
df = 0;
}
spectrumValue *= exp(- 10.0 * df * df);
}
if (spectrumValue > peak) {
peak = p->spectrum[i];
peakIndex = i;
peakFrequency = f;
}
}
p->peakFrequency = peakFrequency;
if (p->subcounter / 16) {
data_append_and_shift(p->subSignalReal, p->subLength, p->real[peakIndex]);
data_append_and_shift(p->subSignalImag, p->subLength, p->imag[peakIndex]);
p->subcounter = 0;
}else{
p->subSignalReal[p->subLength-1] = p->real[peakIndex];
p->subSignalImag[p->subLength-1] = p->imag[peakIndex];
}
p->subcounter++;
double range = (double)p->fd / p->signalLength;
p->peakSubFrequency = processing_clarify_peak_frequency_in_range(p, range);
}
void computeReferenceD(clFFT_SplitComplexDouble *out, clFFT_Dim3 n,
unsigned int batchSize, clFFT_Dimension dim, clFFT_Direction dir)
{
FFTSetupD plan_vdsp;
DSPDoubleSplitComplex out_vdsp;
FFTDirection dir_vdsp = dir == clFFT_Forward ? FFT_FORWARD : FFT_INVERSE;
unsigned int i, j, k;
unsigned int stride;
unsigned int log2Nx = (int) log2(n.x);
unsigned int log2Ny = (int) log2(n.y);
unsigned int log2Nz = (int) log2(n.z);
unsigned int log2N;
log2N = log2Nx;
log2N = log2N > log2Ny ? log2N : log2Ny;
log2N = log2N > log2Nz ? log2N : log2Nz;
plan_vdsp = vDSP_create_fftsetupD(log2N, 2);
switch(dim)
{
case clFFT_1D:
for(i = 0; i < batchSize; i++)
{
stride = i * n.x;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
break;
case clFFT_2D:
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.y; j++)
{
stride = j * n.x + i * n.x * n.y;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.x; j++)
{
stride = j + i * n.x * n.y;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, n.x, log2Ny, dir_vdsp);
}
}
break;
case clFFT_3D:
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.z; j++)
{
for(k = 0; k < n.y; k++)
{
stride = k * n.x + j * n.x * n.y + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.z; j++)
{
for(k = 0; k < n.x; k++)
{
stride = k + j * n.x * n.y + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, n.x, log2Ny, dir_vdsp);
}
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.y; j++)
{
for(k = 0; k < n.x; k++)
{
stride = k + j * n.x + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zipD(plan_vdsp, &out_vdsp, n.x*n.y, log2Nz, dir_vdsp);
}
}
}
break;
}
vDSP_destroy_fftsetupD(plan_vdsp);
}
