float shift_addition_cc(complexf *input, complexf* output, int input_size, shift_addition_data_t d, float starting_phase)
{
//The original idea was taken from wdsp:
//http://svn.tapr.org/repos_sdr_hpsdr/trunk/W5WC/PowerSDR_HPSDR_mRX_PS/Source/wdsp/shift.c
//However, this method introduces noise (from floating point rounding errors), which increases until the end of the buffer.
//fprintf(stderr, "cosd=%g sind=%g\n", d.cosdelta, d.sindelta);
float cosphi=cos(starting_phase);
float sinphi=sin(starting_phase);
float cosphi_last, sinphi_last;
for(int i=0;i<input_size;i++) //@shift_addition_cc: work
{
iof(output,i)=cosphi*iof(input,i)-sinphi*qof(input,i);
qof(output,i)=sinphi*iof(input,i)+cosphi*qof(input,i);
//using the trigonometric addition formulas
//cos(phi+delta)=cos(phi)cos(delta)-sin(phi)*sin(delta)
cosphi_last=cosphi;
sinphi_last=sinphi;
cosphi=cosphi_last*d.cosdelta-sinphi_last*d.sindelta;
sinphi=sinphi_last*d.cosdelta+cosphi_last*d.sindelta;
}
starting_phase+=d.rate*PI*input_size;
while(starting_phase>PI) starting_phase-=2*PI; //@shift_addition_cc: normalize starting_phase
while(starting_phase<-PI) starting_phase+=2*PI;
return starting_phase;
}
void logpower_cf(complexf* input, float* output, int size, float add_db)
{
for(int i=0;i<size;i++) output[i]=iof(input,i)*iof(input,i) + qof(input,i)*qof(input,i); //@logpower_cf: pass 1
for(int i=0;i<size;i++) output[i]=log10(output[i]); //@logpower_cf: pass 2
for(int i=0;i<size;i++) output[i]=10*output[i]+add_db; //@logpower_cf: pass 3
}
void apply_window_c(complexf* input, complexf* output, int size, window_t window)
{
float (*window_function)(float)=firdes_get_window_kernel(window);
for(int i=0;i<size;i++) //@apply_window_c
{
float rate=(float)i/(size-1);
iof(output,i)=iof(input,i)*window_function(2.0*rate+1.0);
qof(output,i)=qof(input,i)*window_function(2.0*rate+1.0);
}
}
void amdemod_cf(complexf* input, float *output, int input_size)
{
//@amdemod: i*i+q*q
for (int i=0; i<input_size; i++)
{
output[i]=iof(input,i)*iof(input,i)+qof(input,i)*qof(input,i);
}
//@amdemod: sqrt
for (int i=0; i<input_size; i++)
{
output[i]=sqrt(output[i]);
}
}
complexf fmdemod_quadri_novect_cf(complexf* input, float* output, int input_size, complexf last_sample)
{
output[0]=fmdemod_quadri_K*(iof(input,0)*(qof(input,0)-last_sample.q)-qof(input,0)*(iof(input,0)-last_sample.i))/(iof(input,0)*iof(input,0)+qof(input,0)*qof(input,0));
for (int i=1; i<input_size; i++) //@fmdemod_quadri_novect_cf
{
float qnow=qof(input,i);
float qlast=qof(input,i-1);
float inow=iof(input,i);
float ilast=iof(input,i-1);
output[i]=fmdemod_quadri_K*(inow*(qnow-qlast)-qnow*(inow-ilast))/(inow*inow+qnow*qnow);
//TODO: expression can be simplified as: (qnow*ilast-inow*qlast)/(inow*inow+qnow*qnow)
}
return input[input_size-1];
}
void amdemod_estimator_cf(complexf* input, float *output, int input_size, float alpha, float beta)
{
//concept is explained here:
//http://www.dspguru.com/dsp/tricks/magnitude-estimator
//default: optimize for min RMS error
if(alpha==0)
{
alpha=0.947543636291;
beta=0.392485425092;
}
//@amdemod_estimator
for (int i=0; i<input_size; i++)
{
float abs_i=iof(input,i);
if(abs_i<0) abs_i=-abs_i;
float abs_q=qof(input,i);
if(abs_q<0) abs_q=-abs_q;
float max_iq=abs_i;
if(abs_q>max_iq) max_iq=abs_q;
float min_iq=abs_i;
if(abs_q<min_iq) min_iq=abs_q;
output[i]=alpha*max_iq+beta*min_iq;
}
}
complexf fmdemod_quadri_cf(complexf* input, float* output, int input_size, float *temp, complexf last_sample)
{
float* temp_dq=temp;
float* temp_di=temp+input_size;
temp_dq[0]=qof(input,0)-last_sample.q;
for (int i=1; i<input_size; i++) //@fmdemod_quadri_cf: dq
{
temp_dq[i]=qof(input,i)-qof(input,i-1);
}
temp_di[0]=iof(input,0)-last_sample.i;
for (int i=1; i<input_size; i++) //@fmdemod_quadri_cf: di
{
temp_di[i]=iof(input,i)-iof(input,i-1);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output numerator
{
output[i]=(iof(input,i)*temp_dq[i]-qof(input,i)*temp_di[i]);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output denomiator
{
temp[i]=iof(input,i)*iof(input,i)+qof(input,i)*qof(input,i);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output division
{
output[i]=fmdemod_quadri_K*output[i]/temp[i];
}
return input[input_size-1];
}
float shift_math_cc(complexf *input, complexf* output, int input_size, float rate, float starting_phase)
{
rate*=2;
//Shifts the complex spectrum. Basically a complex mixer. This version uses cmath.
float phase=starting_phase;
float phase_increment=rate*PI;
float cosval, sinval;
for(int i=0;i<input_size; i++) //@shift_math_cc
{
cosval=cos(phase);
sinval=sin(phase);
//we multiply two complex numbers.
//how? enter this to maxima (software) for explanation:
// (a+b*%i)*(c+d*%i), rectform;
iof(output,i)=cosval*iof(input,i)-sinval*qof(input,i);
qof(output,i)=sinval*iof(input,i)+cosval*qof(input,i);
phase+=phase_increment;
while(phase>2*PI) phase-=2*PI; //@shift_math_cc: normalize phase
while(phase<0) phase+=2*PI;
}
return phase;
}
int fir_decimate_cc(complexf *input, complexf *output, int input_size, int decimation, float *taps, int taps_length)
{
//Theory: http://www.dspguru.com/dsp/faqs/multirate/decimation
//It uses real taps. It returns the number of output samples actually written.
//It needs overlapping input based on its returned value:
//number of processed input samples = returned value * decimation factor
//The output buffer should be at least input_length / 3.
// i: input index | ti: tap index | oi: output index
int oi=0;
for(int i=0; i<input_size; i+=decimation) //@fir_decimate_cc: outer loop
{
if(i+taps_length>input_size) break;
float acci=0;
for(int ti=0; ti<taps_length; ti++) acci += (iof(input,i+ti)) * taps[ti]; //@fir_decimate_cc: i loop
float accq=0;
for(int ti=0; ti<taps_length; ti++) accq += (qof(input,i+ti)) * taps[ti]; //@fir_decimate_cc: q loop
iof(output,oi)=acci;
qof(output,oi)=accq;
oi++;
}
return oi;
}
float shift_table_cc(complexf* input, complexf* output, int input_size, float rate, shift_table_data_t table_data, float starting_phase)
{
//RTODO
rate*=2;
//Shifts the complex spectrum. Basically a complex mixer. This version uses a pre-built sine table.
float phase=starting_phase;
float phase_increment=rate*PI;
float cosval, sinval;
for(int i=0;i<input_size; i++) //@shift_math_cc
{
int sin_index, cos_index, temp_index, sin_sign, cos_sign;
//float vphase=fmodf(phase,PI/2); //between 0 and 90deg
int quadrant=phase/(PI/2); //between 0 and 3
float vphase=phase-quadrant*(PI/2);
sin_index=(vphase/(PI/2))*table_data.table_size;
cos_index=table_data.table_size-1-sin_index;
if(quadrant&1) //in quadrant 1 and 3
{
temp_index=sin_index;
sin_index=cos_index;
cos_index=temp_index;
}
sin_sign=(quadrant>1)?-1:1; //in quadrant 2 and 3
cos_sign=(quadrant&&quadrant<3)?-1:1; //in quadrant 1 and 2
sinval=sin_sign*table_data.table[sin_index];
cosval=cos_sign*table_data.table[cos_index];
//we multiply two complex numbers.
//how? enter this to maxima (software) for explanation:
// (a+b*%i)*(c+d*%i), rectform;
iof(output,i)=cosval*iof(input,i)-sinval*qof(input,i);
qof(output,i)=sinval*iof(input,i)+cosval*qof(input,i);
phase+=phase_increment;
while(phase>2*PI) phase-=2*PI; //@shift_math_cc: normalize phase
while(phase<0) phase+=2*PI;
}
return phase;
}
void apply_fir_fft_cc(FFT_PLAN_T* plan, FFT_PLAN_T* plan_inverse, complexf* taps_fft, complexf* last_overlap, int overlap_size)
{
//use the overlap & add method for filtering
//calculate FFT on input buffer
fft_execute(plan);
//multiply the filter and the input
complexf* in = plan->output;
complexf* out = plan_inverse->input;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: multiplication
{
iof(out,i)=iof(in,i)*iof(taps_fft,i)-qof(in,i)*qof(taps_fft,i);
qof(out,i)=iof(in,i)*qof(taps_fft,i)+qof(in,i)*iof(taps_fft,i);
}
//calculate inverse FFT on multiplied buffer
fft_execute(plan_inverse);
//add the overlap of the previous segment
complexf* result = plan_inverse->output;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: normalize by fft_size
{
iof(result,i)/=plan->size;
qof(result,i)/=plan->size;
}
for(int i=0;i<overlap_size;i++) //@apply_fir_fft_cc: add overlap
{
iof(result,i)=iof(result,i)+iof(last_overlap,i);
qof(result,i)=qof(result,i)+qof(last_overlap,i);
}
}
void fft_swap_sides(complexf* io, int fft_size)
{
int middle=fft_size/2;
complexf temp;
for(int i=0;i<middle;i++)
{
iof(&temp,0)=iof(io,i);
qof(&temp,0)=qof(io,i);
iof(io,i)=iof(io,i+middle);
qof(io,i)=qof(io,i+middle);
iof(io,i+middle)=iof(&temp,0);
qof(io,i+middle)=qof(&temp,0);
}
}
void firdes_bandpass_c(complexf *output, int length, float lowcut, float highcut, window_t window)
{
//To generate a complex filter:
// 1. we generate a real lowpass filter with a bandwidth of highcut-lowcut
// 2. we shift the filter taps spectrally by multiplying with e^(j*w), so we get complex taps
//(tnx HA5FT)
float* realtaps = (float*)malloc(sizeof(float)*length);
firdes_lowpass_f(realtaps, length, (highcut-lowcut)/2, window);
float filter_center=(highcut+lowcut)/2;
float phase=0, sinval, cosval;
for(int i=0; i<length; i++) //@@firdes_bandpass_c
{
cosval=cos(phase);
sinval=sin(phase);
phase+=2*PI*filter_center;
while(phase>2*PI) phase-=2*PI; //@@firdes_bandpass_c
while(phase<0) phase+=2*PI;
iof(output,i)=cosval*realtaps[i];
qof(output,i)=sinval*realtaps[i];
//output[i] := realtaps[i] * e^j*w
}
}
boot get_xmax(const char *path,const int det)
{
FILE *f=open_file(combine("%s/data.raw",path).c_str(),"r");
fread(&nx,sizeof(int),1,f);
fread(&nx_rew,sizeof(int),1,f);
fread(&nazione,sizeof(int),1,f);
fread(&noss,sizeof(int),1,f);
fread(&npow,sizeof(int),1,f);
fread(&nboot,sizeof(int),1,f);
nboot-=1;
x=new double[nazione*nx];
y=new double[nazione*nx*(nboot+1)*noss*npow];
x_rew=new double[nazione*nx_rew];
y_rew=new double[nazione*nx_rew*(nboot+1)*noss*npow];
fread(x,sizeof(double),nazione*nx,f);
fread(y,sizeof(double),nx*(nboot+1)*noss*npow,f);
fread(x_rew,sizeof(double),nazione*nx_rew,f);
fread(y_rew,sizeof(double),nx_rew*(nboot+1)*noss*npow,f);
fclose(f);
bvec da(nx,nboot,njack);
bvec da_rew(nx_rew,nboot,njack);
int ipowo=1,io=3;
for(int is=0;is<nx;is++) da[is].put(y+(nboot+1)*iof(is,io,ipowo));
for(int ip=0;ip<nx_rew;ip++) da_rew[ip].put(y_rew+(nboot+1)*iof(ip,io,ipowo));
da*=vol;
da_rew*=vol;
bvec par_rew=lorentzian_fit(x_rew,da_rew);
//for(int i=0;i<3;i++) cout<<par[i]<<" "<<par[i].err()/par[i].med()<<endl;
//for(int i=0;i<3;i++) cout<<par_rew[i]<<" "<<par_rew[i].err()/par_rew[i].med()<<endl;
//filter
int nrew_filt=0,nfilt=0;
double tresh=par_rew[0].med()/4;
double tresh_rew=par_rew[0].med()*3/4;
for(int i=0;i<da_rew.nel;i++) nrew_filt+=da_rew[i].med() > tresh_rew ? 1 : 0;
for(int i=0;i<da.nel;i++) nfilt+=da[i].med() > tresh ? 1 : 0;
double x_f[nfilt];
double x_rew_f[nrew_filt];
bvec da_f(nfilt,nboot,njack);
bvec da_rew_f(nrew_filt,nboot,njack);
int ifilt=0;
int irew_filt=0;
for(int i=0;i<da_rew.nel;i++)
if(da_rew[i].med()>tresh_rew)
{
x_rew_f[irew_filt]=x_rew[i];
da_rew_f[irew_filt]=da_rew[i];
//cout<<ifilt<<" "<<x_rew_f[irew_filt]<<" "<<1/da_rew_f[irew_filt]<<endl;
irew_filt++;
}
for(int i=0;i<da.nel;i++)
if(da[i].med()>tresh)
{
x_f[ifilt]=x[i];
da_f[ifilt]=da[i];
//cout<<ifilt<<" "<<x_f[ifilt]<<" "<<1/da_f[ifilt]<<endl;
ifilt++;
}
cout<<"Det: "<<det<<endl;
cout<<"NFilt: "<<nfilt<<endl;
bvec par_f=lorentzian_fit(x_f,da_f);
bvec par_rew_f=lorentzian_fit(x_rew_f,da_rew_f);
for(int i=0;i<3;i++)
for(int iboot=0;iboot<nboot;iboot++)
{
if(isnan(par_f[i][iboot]))
{
par_f[i].data[iboot]=par_f[i].data[(iboot-1+nboot)%nboot];
//cout<<"par: "<<par_f[i][iboot]<<" "<<par_f[i].err()/par_f[i].med()<<endl;
}
if(isnan(par_rew_f[i][iboot]))
{
par_rew_f[i].data[iboot]=par_rew_f[i].data[(iboot-1+nboot)%nboot];
//cout<<"par_: "<<par_rew_f[i][iboot]<<" "<<par_rew_f[i].err()/par_rew_f[i].med()<<endl;
}
}
bvec re_f(da_rew_f.nel,nboot,njack);
bvec re_rew_f(da_rew_f.nel,nboot,njack);
for(int i=0;i<da_rew_f.nel;i++) re_f[i]=par_f[0]/(1+sqr((x_rew_f[i]-par_f[1])/par_f[2]));
for(int i=0;i<da_rew_f.nel;i++) re_rew_f[i]=par_rew_f[0]/(1+sqr((x_rew_f[i]-par_rew_f[1])/par_rew_f[2]));
grace out("%s/fit_susc_%d.xmg",path,det);
out.set(2,"none","square");
if(det==0) out.print_graph(x_rew_f,da_rew_f);
if(det==2) out.print_graph(x_f,da_f);
//out.print_graph(x_rew,da_rew);
out.new_set();
out.set(1,"green");
if(det==0) out.polygon(x_rew_f,da_rew_f);
if(det==2) out.polygon(x_f,da_f);
out.new_set();
out.set(1,"blue");
if(det==0) out.polygon(x_rew_f,re_rew_f);
if(det==2) out.polygon(x_rew_f,re_f);
boot xmax_rew=find_xmax(x_rew_f,da_rew_f);
//cout<<xmax<<endl;
switch(det)
{
case 0:return par_rew_f[1];break;
case 1:return xmax_rew;break;
case 2:return par_f[1];break;
}
cerr<<"Unknown value of 'det'"<<endl;
exit(0);
return par_f[1];
}
decimating_shift_addition_status_t fastddc_inv_cc(complexf* input, complexf* output, fastddc_t* ddc, FFT_PLAN_T* plan_inverse, complexf* taps_fft, decimating_shift_addition_status_t shift_stat)
{
//implements DDC by using the overlap & scrap method
//TODO: +/-1s on overlap_size et al
//input shoud have ddc->fft_size number of elements
complexf* inv_input = plan_inverse->input;
complexf* inv_output = plan_inverse->output;
//Initialize buffers for inverse FFT to zero
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)=0;
qof(inv_input,i)=0;
}
//Alias & shift & filter at once
fft_swap_sides(input, ddc->fft_size); //TODO this is not very optimal, but now we stick with this slow solution until we got the algorithm working
//fprintf(stderr, " === fastddc_inv_cc() ===\n");
//The problem is, we have to say that the output_index should be the _center_ of the spectrum when i is at startbin! (startbin is at the _center_ of the input to downconvert, not at its first bin!)
for(int i=0;i<ddc->fft_size;i++)
{
int output_index = (ddc->fft_size+i-ddc->offsetbin+(ddc->fft_inv_size/2))%plan_inverse->size;
int tap_index = i;
//fprintf(stderr, "output_index = %d , tap_index = %d, input index = %d\n", output_index, tap_index, i);
//cmultadd(inv_input+output_index, input+i, taps_fft+tap_index); //cmultadd(output, input1, input2): complex output += complex input1 * complex input 2
// (a+b*i)*(c+d*i) = (ac-bd)+(ad+bc)*i
// a = iof(input,i)
// b = qof(input,i)
// c = iof(taps_fft,i)
// d = qof(taps_fft,i)
iof(inv_input,output_index) += iof(input,i) * iof(taps_fft,i) - qof(input,i) * qof(taps_fft,i);
qof(inv_input,output_index) += iof(input,i) * qof(taps_fft,i) + qof(input,i) * iof(taps_fft,i);
//iof(inv_input,output_index) += iof(input,i); //no filter
//qof(inv_input,output_index) += qof(input,i);
}
//Normalize inv fft bins (now our output level is not higher than the input... but we may optimize this into the later loop when we normalize by size)
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)/=ddc->pre_decimation;
qof(inv_input,i)/=ddc->pre_decimation;
}
fft_swap_sides(inv_input,plan_inverse->size);
fft_execute(plan_inverse);
//Normalize data
for(int i=0;i<plan_inverse->size;i++) //@fastddc_inv_cc: normalize by size
{
iof(inv_output,i)/=plan_inverse->size;
qof(inv_output,i)/=plan_inverse->size;
}
//Overlap is scrapped, not added
//Shift correction
shift_stat=decimating_shift_addition_cc(inv_output+ddc->scrap, output, ddc->post_input_size, ddc->dsadata, ddc->post_decimation, shift_stat);
//shift_stat.output_size = ddc->post_input_size; //bypass shift correction
//memcpy(output, inv_output+ddc->scrap, sizeof(complexf)*ddc->post_input_size);
return shift_stat;
}
