grid_3cm_t *allocate_3cmgrid(int nbins, float box_size)
{
#ifdef __MPI
ptrdiff_t alloc_local, local_n0, local_0_start;
#else
ptrdiff_t local_n0;
#endif
grid_3cm_t *this3cmGrid;
this3cmGrid = init3cmgrid();
this3cmGrid->nbins = nbins;
this3cmGrid->box_size = box_size;
this3cmGrid->local_n0 = nbins;
this3cmGrid->local_0_start = 0;
local_n0 = nbins;
#ifdef __MPI
alloc_local = fftw_mpi_local_size_3d(nbins, nbins, nbins, MPI_COMM_WORLD, &local_n0, &local_0_start);
this3cmGrid->local_n0 = local_n0;
this3cmGrid->local_0_start = local_0_start;
this3cmGrid->Ts_inv = fftw_alloc_complex(alloc_local);
this3cmGrid->Tb = fftw_alloc_complex(alloc_local);
#else
this3cmGrid->Ts_inv = (fftw_complex*) fftw_malloc(sizeof(fftw_complex)*local_n0*nbins*nbins);
this3cmGrid->Tb = (fftw_complex*) fftw_malloc(sizeof(fftw_complex)*local_n0*nbins*nbins);
#endif
initialize_3cmgrid(this3cmGrid->Ts_inv, nbins, local_n0, 0.);
initialize_3cmgrid(this3cmGrid->Tb, nbins, local_n0, 0.);
return this3cmGrid;
}
Processor::Processor(const char* path, const bool debug) :
m_file(0),
m_buffer(0),
m_bufsize(0)
{
std::cout << "Reading file at " << path << std::endl;
m_path = path;
DEBUG_ENABLED = debug;
m_frameCount = 0;
m_channelCount = 0;
m_sampleRate = 0;
m_fileInfo.format = 0;
m_fileInfo.frames = 0;
fft_in = fftw_alloc_complex(FFT_SIZE);
fft_out = fftw_alloc_complex(FFT_SIZE);
trans = fftw_plan_dft_1d(FFT_SIZE,fft_in,fft_out,FFTW_FORWARD,FFTW_MEASURE);
m_file = sf_open(path, SFM_READ, &m_fileInfo);
if(!m_file){
std::cerr << "error reading file " << sf_strerror(m_file) << std::endl;
return;
}
if (m_fileInfo.channels > 0) {
m_frameCount = m_fileInfo.frames;
m_channelCount = m_fileInfo.channels;
m_sampleRate = m_fileInfo.samplerate;
}
m_bufsize = FFT_SIZE * m_fileInfo.channels;
m_buffer = new float[m_bufsize];
}
void init_buffers() {
fft_in = (fftw_complex *)fftw_alloc_complex(pul_num_sam);
fft_out = (fftw_complex *)fftw_alloc_complex(pul_num_sam);
signal_cpx = (fftw_complex *)fftw_alloc_complex(frame_size);
fft_plan = fftw_plan_dft_1d(pul_num_sam, fft_in, fft_out, FFTW_FORWARD,
FFTW_MEASURE);
fft_abs = (double *)malloc(pul_num_sam * sizeof(double));
gain = (char *)malloc(num_fra_p_file * num_files * sizeof(char));
data = (char *)malloc(file_lenght * sizeof(char));
pulse_snr = zero_mat_f(num_col, ttl_n_fr);
gps_pos = zero_mat_i(ttl_n_fr, 3);
frame = zero_mat_c(num_fra_p_file, data_frame_size);
in_phase = (double *)malloc(data_frame_size * sizeof(double) / 2);
quadrature = (double *)malloc(data_frame_size * sizeof(double) / 2);
}
fftw_complex *alloc_complex(size_t size) {
fftw_complex *ret = fftw_alloc_complex(size);
if(!ret) abort();
return ret;
}
/*! \memberof splitop
create new operator using values in prefs */
splitop_t * splitop_new(preferences_t * prefs)
{
splitop_t * w = malloc(sizeof(splitop_t)); assert(w);
w->prefs = prefs;
// cache values
double * V = prefs->potential->data;
int bins = prefs->bins;
double dt = prefs->dt;
double dk = prefs->dk;
// allocate all the arrays needed
w->eV = fftw_alloc_complex(bins); assert(w->eV);
w->eVn = fftw_alloc_complex(bins); assert(w->eVn);
w->ehV = fftw_alloc_complex(bins); assert(w->ehV);
w->ehVn = fftw_alloc_complex(bins); assert(w->ehVn);
w->eT = fftw_alloc_complex(bins); assert(w->eT);
w->apsi = fftw_alloc_complex(bins); assert(w->apsi);
w->psi = fftw_alloc_complex(bins); assert(w->psi);
w->psik = fftw_alloc_complex(bins); assert(w->psik);
for (int k = 0; k < bins; k++) {
w->apsi[k] = 0;
}
// calculate position space propagators
for (int n = 0; n < bins; n++) {
w->eV[n] = cexp(-I * V[n] * dt);
w->eVn[n] = w->eV[n] / bins;
w->ehV[n] = cexp(-I * V[n] * dt / 2);
w->ehVn[n] = w->ehV[n] / bins;
}
// calculate momentum space propagator
for (int n = 0; n < bins; n++) {
if (n < bins/2) {
w->eT[n] = cexp(-I * dk * dk * n * n * dt);
} else { // negative frequencies come after bins/2
int m = (n - bins);
w->eT[n] = cexp(-I * dk * dk * m * m * dt);
}
}
// prepare fftw plans for DFT
w->fwd = fftw_plan_dft_1d(bins, w->psi, w->psik, FFTW_FORWARD, FFTW_MEASURE);
w->bwd = fftw_plan_dft_1d(bins, w->psik, w->psi, FFTW_BACKWARD, FFTW_MEASURE);
// setup initial system state wavefunction
for (int k = 0; k < bins; k++) {
w->psi[k] = prefs->psi->data[k];
}
return w;
}
fftw_complex* ds::ReflectionData::fftw_data(int fx, int fy, int fz) const
{
fftw_complex* fftw_data = fftw_alloc_complex(fx*fy*fz);
int size = fx*fy*fz;
//Zero initialization
for(int i=0; i<fx*fy*fz; i++)
{
((fftw_complex*)fftw_data)[i][0] = 0.0;
((fftw_complex*)fftw_data)[i][1] = 0.0;
}
//Iterate over all possible miller indices h, k, l
for(const_iterator ref=this->begin(); ref!=this->end(); ++ref)
{
//Assign the current Miller Index to the array
ds::MillerIndex currentHKL = (*ref).first;
Complex currentComplex = (*ref).second.value();
// Fill in this spot
if(currentHKL.h() >= 0){
int idx = currentHKL.h();
int idy = currentHKL.k();
int idz = currentHKL.l();
if(idy < 0) idy = idy + fy;
if(idz < 0) idz = idz + fz;
int memory_id = idx + (idy*fx) + (idz*fy*fx);
if(memory_id >= size)
{
std::cerr << "Oops! This reflection exceeds limits!! Leaving it!\n";
std::cerr << "Miller index found: " << currentHKL.to_string() << " and HKL Limit: +/-(" << fx-1 << ", " << fy/2 << ", " << fz/2 << ")\n";
}
else
{
((fftw_complex*)fftw_data)[memory_id][0] = currentComplex.real();
((fftw_complex*)fftw_data)[memory_id][1] = currentComplex.imag();
}
}
}
return fftw_data;
}
static void prepare_fftw(struct holder *holder)
{
unsigned int a;
holder->samples = fftw_alloc_real(holder->samples_count);
if (!holder->samples)
errx(3, "cannot allocate input");
holder->output = fftw_alloc_complex(holder->samples_count);
if (!holder->output)
errx(3, "cannot allocate output");
for (a = 0; a < holder->samples_count; a++) {
holder->samples[a] = 0;
holder->output[a] = 0;
}
holder->plan = fftw_plan_dft_r2c_1d(holder->samples_count,
holder->samples, holder->output, 0);
if (!holder->plan)
errx(3, "plan not created");
}
static void prepare_fftw(struct holder *holder)
{
unsigned int a;
// init buffer for samples
holder->samples = fftw_alloc_real(holder->samples_count);
if (!holder->samples)
errx(3, "cannot allocate input");
// init buffer for windowed samples
holder->windowed_samples = fftw_alloc_real(holder->samples_count);
if (!holder->samples)
errx(3, "cannot allocate window space");
// init fft output buffer
holder->output = fftw_alloc_complex(holder->fftout_count);
if (!holder->output)
errx(3, "cannot allocate output");
holder->max = (double *)malloc(holder->fftout_count * sizeof(double));
for(int i = 0; i<holder->fftout_count; i++) holder->max[i] = 0.000000000000001;//;// FLT_MIN;
// // null all
// for (a = 0; a < holder->samples_count; a++) {
// holder->samples[a] = 0;
// holder->windowed_samples[a] = 0;
// }
// for (int i = 0; i < holder->fftout_count; i++) {
// holder->output[i] = 0;
// }
// calculate a blackman window if not already done
holder->blackman_window = (double*)malloc(holder->samples_count * sizeof(double));
for(int i = 0; i < holder->samples_count; i++)
holder->blackman_window[i] = 0.53836 - 0.46164*cos( 2*M_PI * i / ( holder->samples_count-1) );
holder->plan = fftw_plan_dft_r2c_1d(holder->samples_count, holder->windowed_samples, holder->output, 0);
if (!holder->plan)
errx(3, "plan not created");
}
int uint16_1chan(struct core_param o, struct core_return *retstr) {
fftw_plan fftplan;
unsigned long int s_bytes, i_bytes, o_bytes, ret;
unsigned long int i, ffts, iavg, /* iphs,*/ nyq, seeksamp, bps;
long int bl_first, bl_last;
long int power, maxmag = 0, minmag = 0;
off_t dataend, bskip_avg = 0, bskip_fft = 0;
// double cur_mag[o.N], avg_mag[o.N], window[o.N], pow_adj;
double cur_mag[o.N], avg_mag[o.N], pow_adj;
double dataz;
// complex double avg_phs[o.N][N_PHASES];
// complex double nphs;
struct timeval now,then;
// unsigned flags = FFTW_PATIENT|FFTW_DESTROY_INPUT;
unsigned flags = FFTW_MEASURE|FFTW_DESTROY_INPUT;
// unsigned flags = FFTW_ESTIMATE|FFTW_DESTROY_INPUT;
double time, ratio;
struct timespec nsl;
FILE *istream, *ostream, *tstream = NULL;//, *phstream;
/* For timestamp conversion */
double tConv;
struct tm tm_initial;
struct tm tm_info;
int haveTimeStr;
double seconds;
char tBuf[255];
signal(SIGINT,do_depart);
/*
* Complex/Real Input
*/
unsigned short int *samples;
double *input, * window;
fftw_complex *output;
bps = sizeof(unsigned short int); // real, unsigned short int (2-byte) data
s_bytes = bps*o.N;
i_bytes = sizeof(double)*o.N;
o_bytes = sizeof(fftw_complex)*o.N;
nyq = o.N/2;
// complex double datai[nyq];
//printf("bps: %d\n",bps);
/* if there's a start string, use it! */
haveTimeStr = 0;
if( strncmp(o.tStartString,"",19) > 0) {
setenv("TZ", "UTC0", 1); //Set timezone to UTC
printf("startString: %s\n",o.tStartString);
printf("o.time_avg : %.04f\n",o.time_avg);
/* One to keep track of changing time, one to keep start time */
memset(&tm_initial, 0, sizeof(struct tm));
memset(&tm_info, 0, sizeof(struct tm));
strptime(o.tStartString,"%Y-%m-%d/%H:%M:%S",&tm_initial);
strptime(o.tStartString,"%Y-%m-%d/%H:%M:%S",&tm_info);
/* tm_initial.tm_sec = 0; */
/* tm_info.tm_sec = 0; */
/* strftime(tBuf,sizeof(tBuf), "%d %b %Y %H:%M",&tm_info); */
/* puts(tBuf); */
haveTimeStr = 1;
} else {
/* What about time units for x axis? */
tConv = 1;
if( strncmp(o.time_units,"m",1) == 0) {
tConv = 60;
} else if( strncmp(o.time_units,"h",1) == 0) {
tConv = 3600;
} else if( strncmp(o.time_units,"d",1) == 0) {
tConv = 86400;
}
printf("tConv: %.0f (units: %s)\n",tConv,o.time_units);
}
/*
* Memory allocation.
*/
printf("Memory allocation...");
samples = (unsigned short int *) malloc(s_bytes);
if (samples == NULL) fprintf(stderr,"Failed to allocate initial sample memory!\n");
// input = (double *) fftw_malloc(i_bytes);
input = (double *) fftw_alloc_real(i_bytes);
if (input == NULL) fprintf(stderr,"Failed to allocate fft input array!\n");
window = (double *) fftw_alloc_real(i_bytes);
if (input == NULL) fprintf(stderr,"Failed to allocate fft input array!\n");
// output = (fftw_complex *) fftw_malloc(o_bytes);
output = (fftw_complex *) fftw_alloc_complex(o_bytes);
if (output == NULL) fprintf(stderr,"Failed to allocate fft output array!\n");
printf("done. (%liKB)\n",(s_bytes + i_bytes + o_bytes)/1024);
/*
* FFTW Setup. Initialize threads, plan_many.
*/
printf("FFTW initialization and planning...");
gettimeofday(&then,NULL);
if (o.threads > 1) {
if (fftw_init_threads() == 0) {
fprintf(stderr,"Failed to initialize FFTW Threads!\n");
}
fftw_plan_with_nthreads(o.threads);
}
/*
* fftw_plan fftw_plan_dft_1d(int n,
fftw_complex *in, fftw_complex *out,
int sign, unsigned flags);
*/
/*
* fftw_plan fftw_plan_dft_r2c_1d(
* int n, double *in, fftw_complex *out, unsigned flags);
*/
fftplan = fftw_plan_dft_r2c_1d(o.N, input, output, flags);
gettimeofday(&now,NULL);
printf("wewt. (%fs elapsed)\n",(now.tv_sec-then.tv_sec)+(now.tv_usec-then.tv_usec)/1e6);
/*
* File setup
*/
printf("Opening files...");
istream = fopen(o.infile,"r");
if (istream == NULL) {
fprintf(stderr,"Failed to open %s for reading.\n",o.infile);
return(1);
}
if (strcmp(o.timefile,"") != 0) {
tstream = fopen(o.timefile,"r");
if (tstream == NULL) {
fprintf(stderr,"Failed to open %s for reading.\n",o.timefile);
return(1);
}
}
ostream = fopen(o.outfile,"w+");
if (ostream == NULL) {
fprintf(stderr,"Failed to open %s for writing.\n",o.outfile);
return(1);
}
printf("fiddling...");
fseek(istream,0,SEEK_END);
dataend = ftello(istream)-s_bytes;
if (o.newheader == COREH_TAIL) {
dataend -= 8192;
}
rewind(istream);
seeksamp = o.n_startsample*s_bytes;
if (fseeko(istream,seeksamp,SEEK_SET)){
fprintf(stderr,"Failed to seek to starting sample!\n");
}
/*
* Window selection, and setup to adjust for Coherent Gain,
* 2x for real input data, and hz/bin multiplier.
*/
window_sel(window,&o);
pow_adj = window_cog(&o)*2*(o.frequency/o.N);
nsl.tv_sec = 0; nsl.tv_nsec = CORE_RFP;
if (o.bl_last == -1) bl_last = nyq; else bl_last = o.bl_last;
if (o.bl_first == -1) bl_first = 1; else bl_first = o.bl_first;
if (o.skip_avg > 0) bskip_avg = bps*o.skip_avg;
if (o.skip_fft > 0) bskip_fft = bps*o.skip_fft;
ffts = 0;
iavg = 0;
time = o.time_start;
seconds = 0;
printf("done.\n"); fflush(stdout); fflush(stderr);
running = true;
while ((ftello(istream) < dataend) && (running == true)) {
nanosleep(&nsl,NULL);
/*
* Main loop: read in data while data is still readable.
*/
ret = fread(samples,bps,o.N,istream);
if (ret != o.N) {
printf("Failed to read from data file.\n");
}
/*
* Copy channels to input array, switching from row-major to
* column-major ordering to satisfy the (hopefully faster)
* plan_many data format, and casting to proper form as we go.
*/
for (i = 0; i < o.N; i++) {
dataz = (double) samples[i];
if (o.convert) {
dataz = apply_polynomial(&o.poly[0], dataz);
}
input[i] = dataz*window[i];
}
fftw_execute(fftplan); // FOR GLORY!!!
/*
* Get the squared magnitude, adjust powers
*/
for (i = 0; i < nyq; i++) {
// iphs = 0;
cur_mag[i] = pow_adj*pow(cabs(output[i]),2);
/* if (o.phases) {
for (k = j+1; k < o.n_chan; k++) {
if (iavg == 0) avg_phs[i][j][iphs] = 0;
avg_phs[i][j][iphs] += datai[i][j]*conj(datai[i][k]);
iphs++;
}
}*/
}
/*
* Find the AGC ratio, apply to all data,
* and sum into averaging array.
*/
if (o.agc_level != 0) {
ratio = o.agc_level/cur_mag[o.agc_bin-1];
for (i = 0; i < nyq; i++) {
if (iavg == 0) avg_mag[i] = 0; // First average, so initialize
avg_mag[i] += cur_mag[i]*ratio;
}
} else {
for (i = 0; i < nyq; i++) {
if (iavg == 0) avg_mag[i] = 0; // First average, so initialize
avg_mag[i] += cur_mag[i];
}
}
iavg++;
if (iavg == o.avg) {
/*
* We've averaged enough, output to files.
*/
if (tstream != NULL) {
fscanf(tstream,"%lf\n",&time);
}
if (o.verbose) printf("%f\n",time);
if(haveTimeStr > 0){
/* Need to update seconds? */
/* printf("time: %.5f, %8.5f\n",time,time*1000); */
if(seconds > 1){
tm_info.tm_sec += floor(seconds);
seconds -= floor(seconds);
mktime( &tm_info); // normalize tm_info
}
if(haveTimeStr == 1){
strftime(tBuf,sizeof(tBuf),"%m-%d/%H:%M:%S",&tm_info);
haveTimeStr++;
} else{
strftime(tBuf,sizeof(tBuf),"%H:%M:%S",&tm_info);
}
fprintf(ostream,"%*s.%03.0f\n",strlen(tBuf),tBuf,seconds*1000);
/* printf("%s. %3.0f\n",tBuf,roundl(seconds*((double) 1000))); */
/* printf("%*s.%03.0f\n",strlen(tBuf),tBuf,seconds*1000); */
/* printf("%s\n",tBuf); */
} else{
fprintf(ostream,"%8.03f\n",time/tConv); // Timestamps
}
/* if (o.phases) {
fprintf(phstream,"%.8f\n",time);
}
*/
for (i = bl_first-1; i < bl_last; i++) {
if (o.linear == true) {
return(1); //not working
power = avg_mag[i]/o.avg;
} else {
power = lround(10 * log10(avg_mag[i]/o.avg)); // log10(|x|^2)
}
//power &= 65535; // Trim to 16 bits.
if (!o.binary) {
fprintf(ostream,"%li\n", power);
} else {
// Blerg
}
/*
* Find global min & max.
*/
if (ffts == 0) {
minmag = maxmag = power;
} else {
minmag = fmin(power, minmag);
maxmag = fmax(power, maxmag);
}
/* if (o.phases) {
for (k = 0; k < N_PHASES; k++) {
nphs = avg_phs[i][j][k]/( \
avg_mag[i][n_phsmap[k][0]] * \
avg_mag[i][n_phsmap[k][1]] );
if (k > 0) fprintf(phstream," ");
fprintf(phstream,"%2.2f %2.2f",cabs(nphs),carg(nphs));
}
fprintf(phstream,"\n");
}*/
}
/* Add time per avg. */
if (o.time_avg > 0) {
time += o.time_avg;
seconds += o.time_avg;
}
if (o.skip_avg > 0) {
ret = fseeko(istream,bskip_avg,SEEK_CUR);
if (ret != 0) {
fprintf(stderr,"Failed to skip to next sample: %s\n",strerror(errno));
return(1);
}
}
iavg = 0;
}
ffts++;
if (ffts == o.n_ffts) break;
if ((o.time_stop != 0) && (time > o.time_stop)) break;
/*
* This will add o.time_nffts to the timestamp every o.time_fftmod ffts.
* Interaction with o.time_avg must be carefully considered to get
* the correct timestamp advancement. Also note that some programs that
* may end up processing this data (e.g. for ps making) do not like it
* when timestamps increase inconsistently.
*/
if ((o.time_nfft > 0) && (ffts % o.time_fftmod == 0)) {
printf("modding FFT time!\n");
time += o.time_nfft;
seconds += o.time_nfft;
}
if (o.skip_fft > 0) {
ret = fseeko(istream,bskip_fft,SEEK_CUR);
if (ret != 0) {
fprintf(stderr,"Failed to skip to next sample!\n");
return(1);
}
}
}
fclose(istream);
fclose(ostream);
if (tstream != NULL) {
fclose(tstream);
}
//fclose(phstream);
retstr->time_total = time-o.time_start;
retstr->nfft = ffts;
retstr->min[0] = minmag;
retstr->max[0] = maxmag;
return(0);
}
int main(int argc, char*argv[])
{
FILE *in, *out;
char filename[128], *dot;
int i, j, k, value[9], index, chan[3];
int numsamples, numgroups;
double *rawdata, *rawin, *endraw;
double *pass1, *passptr, *pass2;
double bcoef[8][3], acoef[8][3];
double stage[3][8][3]; // curve, harmonic, history (dimension meaning)
fftw_plan plan, invrs;
double *fftin, *mag, phase, *fftrvout, maxmag[32];
fftw_complex *fftout, *fftrvin;
in = fopen(argv[1], "r");
if(!in)
{
printf("can't find file %s\n", argv[1]);
exit(0);
}
strcpy(filename, argv[1]);
dot = strstr( filename, ".data");
strcpy( dot, ".raw");
printf("output filename: %s\n", filename);
out = fopen(filename, "w");
if(!out)
{
printf("can't create %s\n", filename);
exit(0);
}
/* assume we don't take more than 30 minutes worth of data */
rawdata = (double*) malloc(sizeof(double)*3*2000*30*60);
rawin = &rawdata[3*3]; // zero out first sample
/* Each channel uses 3 bytes. There are 3 channels. */
while(!feof(in))
{
for(i=0; i<9; i++) value[i] = fgetc(in);
chan[0] = (value[0] << 16) | (value[1] << 8) | value[2];
if(chan[0] & 0x800000) chan[0] |= 0xff000000;
chan[1] = (value[3] << 16) | (value[4] << 8) | value[5];
if(chan[1] & 0x800000) chan[1] |= 0xff000000;
chan[2] = (value[6] << 16) | (value[7] << 8) | value[8];
if(chan[2] & 0x800000) chan[2] |= 0xff000000;
*rawin = chan[0];
rawin++;
*rawin = chan[1];
rawin++;
*rawin = chan[2];
rawin++;
}
fclose(in);
for(i=0; i<9; i++) // zero fill initial data
rawdata[i] = 0.0;
endraw = rawin - 9; // ignore last values collected
for(i=0; i<9; i++)
endraw[i] = 0.0;
endraw += 9;
printf("data read in\n");
rawin = rawdata;
while (rawin < endraw)
{
fprintf(out, "%lf %lf %lf\n", rawin[0], rawin[1], rawin[2]);
rawin += 3;
}
fclose(out);
numsamples = (endraw - rawdata)/3;
printf("saving %d samples\n", numsamples);
/* compute IIR notch filter
one notch for each odd harmonic of 60 Hz out to 900 Hz */
dot = strstr( filename, ".raw");
strcpy( dot, ".plt_ntch");
printf("output filename: %s\n", filename);
out = fopen(filename, "w");
if(!out)
{
printf("can't create %s\n", filename);
exit(0);
}
pass1 = (double*)malloc(sizeof(double)*(3*numsamples + 6*FILTER_TAP_NUM));
rawin = rawdata + 3*3;
passptr = pass1 + 3*(FILTER_TAP_NUM - 3);
// notch filter
bcoef[0][0] = (1.0 + A12)/2.0;
bcoef[0][1] = A11;
bcoef[0][2] = (1.0 + A12)/2.0;
acoef[0][0] = 1.0;
acoef[0][1] = A11;
acoef[0][2] = A12;
bcoef[1][0] = (1.0 + A32)/2.0;
bcoef[1][1] = A31;
bcoef[1][2] = (1.0 + A32)/2.0;
acoef[1][0] = 1.0;
acoef[1][1] = A31;
acoef[1][2] = A32;
bcoef[2][0] = (1.0 + A52)/2.0;
bcoef[2][1] = A51;
bcoef[2][2] = (1.0 + A52)/2.0;
acoef[2][0] = 1.0;
acoef[2][1] = A51;
acoef[2][2] = A52;
bcoef[3][0] = (1.0 + A72)/2.0;
bcoef[3][1] = A71;
bcoef[3][2] = (1.0 + A72)/2.0;
acoef[3][0] = 1.0;
acoef[3][1] = A71;
acoef[3][2] = A72;
bcoef[4][0] = (1.0 + A92)/2.0;
bcoef[4][1] = A91;
bcoef[4][2] = (1.0 + A92)/2.0;
acoef[4][0] = 1.0;
acoef[4][1] = A91;
acoef[4][2] = A92;
bcoef[5][0] = (1.0 + A112)/2.0;
bcoef[5][1] = A111;
bcoef[5][2] = (1.0 + A112)/2.0;
acoef[5][0] = 1.0;
acoef[5][1] = A111;
acoef[5][2] = A12;
bcoef[6][0] = (1.0 + A132)/2.0;
bcoef[6][1] = A131;
bcoef[6][2] = (1.0 + A132)/2.0;
acoef[6][0] = 1.0;
acoef[6][1] = A131;
acoef[6][2] = A132;
bcoef[7][0] = (1.0 + A152)/2.0;
bcoef[7][1] = A151;
bcoef[7][2] = (1.0 + A152)/2.0;
acoef[7][0] = 1.0;
acoef[7][1] = A151;
acoef[7][2] = A152;
for(k=0; k<3; k++)
for(i=0; i<8; i++)
for(j=0; j<3; j++)
stage[k][i][j] = 0.0;
while(rawin < endraw)
{
for(j=0; j<3; j++) // loop over each curve
{
stage[j][0][2] = 0.0;
for(i=0; i<3; i++)
stage[j][0][2] += rawin[-3*i + j]*bcoef[0][i];
stage[j][0][2] -= acoef[0][1]*stage[j][0][1] + acoef[0][2]*stage[j][0][0];
for(k=1; k<7; k++) // loop over each harmonic
{
stage[j][k][2] = 0.0;
for(i=0; i<3; i++)
stage[j][k][2] += stage[j][k-1][2-i]*bcoef[k][i];
stage[j][k][2] -= acoef[k][1]*stage[j][k][1] + acoef[k][2]*stage[j][k][0];
}
passptr[j] = 0.0;
for(i=0; i<3; i++)
passptr[j] += stage[j][6][2-i]*bcoef[7][i];
passptr[j] -= acoef[7][1]*passptr[j-3] + acoef[7][2]*passptr[j-6];
for(k=0; k<7; k++)
{
stage[j][k][0] = stage[j][k][1];
stage[j][k][1] = stage[j][k][2];
}
/*
passptr[j] = 0.0;
for(i=0; i<3; i++)
passptr[j] += rawin[-3*i + j]*bcoef[0][i];
passptr[j] -= acoef[0][1]*passptr[j-3] + acoef[0][2]*passptr[j-6];
*/
}
passptr += 3;
rawin += 3;
}
printf("done with pass 1\n");
/* perform FIR low pass on remaining data */
endraw = passptr;
pass2 = (double*)malloc(sizeof(double)*(3*numsamples + 3*FILTER_TAP_NUM));
rawin = pass1;
passptr = pass2;
while(rawin < endraw + 3*FILTER_TAP_NUM)
{
for(j=0; j<3; j++)
{
passptr[j] = 0.0;
for(i=0; i<FILTER_TAP_NUM; i++)
passptr[j] += rawin[3*i + j]*filter_taps[i];
}
passptr += 3;
rawin += 3;
}
printf("done with pass 2\n");
/* save data for gnuplot */
rawin = pass2 + 6*FILTER_TAP_NUM;
for(i=0; i<numsamples - 3*FILTER_TAP_NUM; i++)
{
fprintf(out, "%lf %lf %lf\n", rawin[0] , rawin[1], rawin[2]);
rawin += 3;
}
fclose(out);
/* save data for further processing */
dot = strstr( filename, ".plt_ntch");
strcpy( dot, ".bin_ntch");
printf("output filename: %s\n", filename);
out = fopen(filename, "w");
if(!out)
{
printf("can't create %s\n", filename);
exit(0);
}
fwrite(pass2 + 6*FILTER_TAP_NUM, sizeof(double), (numsamples - FILTER_TAP_NUM)*3, out);
fclose(out);
exit(0); // skip FFT's for now
/* compute 1D FFT's on each wave form */
fftin = fftw_alloc_real(FFTSIZE);
fftout = fftw_alloc_complex(FFTSIZE/2);
mag = (double*)malloc(sizeof(double)*FFTSIZE/2);
plan = fftw_plan_dft_r2c_1d(FFTSIZE, fftin, fftout, FFTW_MEASURE);
rawin = pass1;
k = 0;
while(rawin < endraw - 3*FFTSIZE)
{
for(i=0; i<FFTSIZE; i++) fftin[i] = rawin[3*i + 2];
fftw_execute(plan);
strcpy(filename, argv[1]);
dot = strstr( filename, ".data");
sprintf( dot, "_%03d_fft.plt", k);
printf("output filename: %s\n", filename);
out = fopen(filename, "w");
if(!out)
{
printf("can't create %s\n", filename);
exit(0);
}
for(i=0; i<FFTSIZE/2; i++)
{
mag[i] = sqrt(fftout[i][0]*fftout[i][0] + fftout[i][1]*fftout[i][1]);
phase = atan2(fftout[i][1], fftout[i][0]);
fprintf(out, "%lg %lg\n", mag[i], phase);
}
fclose(out);
k++;
rawin += 3*FFTSIZE;
}
fftw_free(fftin);
fftw_free(fftout);
free(mag);
free(pass1);
free(pass2);
free(rawdata);
}
/* --------------------------------------------------------------------------- *
* This overloaded function is an implementation of your tone mapping operator *
* --------------------------------------------------------------------------- */
int TMOZhao10::Transform()
{
double* data;
int size = pSrc->GetHeight()*pSrc->GetWidth();
int spec_size = pSrc->GetHeight()*(pSrc->GetWidth()/2+1);
double *rgb[] = {fftw_alloc_real(size),fftw_alloc_real(size),fftw_alloc_real(size)};
double *lab[] = {fftw_alloc_real(size),fftw_alloc_real(size),fftw_alloc_real(size)};
fftw_complex *spec_rgb[] = {fftw_alloc_complex(spec_size),fftw_alloc_complex(spec_size),fftw_alloc_complex(spec_size)};
fftw_complex *spec_lab[] = {fftw_alloc_complex(spec_size),fftw_alloc_complex(spec_size),fftw_alloc_complex(spec_size)};
double *theta = fftw_alloc_real(spec_size);
double *phi = fftw_alloc_real(spec_size);
fftw_complex *spec_gray = fftw_alloc_complex(spec_size);
double *gray = fftw_alloc_real(size);
fftw_plan p = fftw_plan_dft_r2c_2d(pSrc->GetHeight(), pSrc->GetWidth(),rgb[0], spec_rgb[0], FFTW_ESTIMATE);
//copy data channels to r,g,b arrays
data=pSrc->GetData();
for(int i=0;i<size;++i){
rgb[0][i] = *data++;
rgb[1][i] = *data++;
rgb[2][i] = *data++;
}
//transform to Lab space
pSrc->Convert(TMO_LAB);
pDst->Convert(TMO_LAB);
//copy data channels to l,a,b array
data=pSrc->GetData();
for(int i=0;i<size;++i){
lab[0][i] = *data++/100;
lab[1][i] = *data++/100;
lab[2][i] = *data++/100;
//fprintf(stderr,"%f %f %f\n",lab[0][i],lab[1][i],lab[2][i]);
}
//compute fft of all channels
fftw_execute_dft_r2c(p,rgb[0], spec_rgb[0]);
fftw_execute_dft_r2c(p,rgb[1], spec_rgb[1]);
fftw_execute_dft_r2c(p,rgb[2], spec_rgb[2]);
fftw_execute_dft_r2c(p,lab[0], spec_lab[0]);
fftw_execute_dft_r2c(p,lab[1], spec_lab[1]);
fftw_execute_dft_r2c(p,lab[2], spec_lab[2]);
fftw_destroy_plan(p);
p = fftw_plan_dft_c2r_2d(pSrc->GetHeight(), pSrc->GetWidth(),spec_gray, gray, FFTW_ESTIMATE);
//compute phi and theta coefficient
double thetasum=0;
double phisum=0;
for(int i=0;i<spec_size;++i){
double a2 = spec_lab[1][i][0]*spec_lab[1][i][0]+spec_lab[1][i][1]*spec_lab[1][i][1];
double b2 = spec_lab[2][i][0]*spec_lab[2][i][0]+spec_lab[2][i][1]*spec_lab[2][i][1];
phi[i] = a2/(a2+b2);
phisum += phi[i];
double rr2 = spec_rgb[0][i][0]*spec_rgb[0][i][0]+spec_rgb[0][i][1]*spec_rgb[0][i][1];
double gg2 = spec_rgb[1][i][0]*spec_rgb[1][i][0]+spec_rgb[1][i][1]*spec_rgb[1][i][1];
double bb2 = spec_rgb[2][i][0]*spec_rgb[2][i][0]+spec_rgb[2][i][1]*spec_rgb[2][i][1];
double l2 = spec_lab[0][i][0]*spec_lab[0][i][0]+spec_lab[0][i][1]*spec_lab[0][i][1];
double rgb2 = rr2+gg2+bb2;
theta[i] = (rgb2-l2)/rgb2;
thetasum += theta[i];
}
thetasum=0.6*spec_size;
phisum=0.9*spec_size;
for(int i=0;i<spec_size;++i){
spec_gray[i][0] = ((1-thetasum/spec_size)*spec_lab[0][i][0]+thetasum/spec_size*(phisum/spec_size*spec_lab[1][i][0]+(1-phisum/spec_size)*spec_lab[2][i][0]))/size;
//spec_gray[i][0] = ((1-theta[i])*spec_lab[0][i][0]+theta[i]*(phi[i]*spec_lab[1][i][0]+(1-phi[i])*spec_lab[2][i][0]))/size;
spec_gray[i][1] = ((1-thetasum/spec_size)*spec_lab[0][i][1]+thetasum/spec_size*(phisum/spec_size*spec_lab[1][i][1]+(1-phisum/spec_size)*spec_lab[2][i][1]))/size;
//spec_gray[i][1] = ((1-theta[i])*spec_lab[0][i][1]+theta[i]*(phi[i]*spec_lab[1][i][1]+(1-phi[i])*spec_lab[2][i][1]))/size;
}
fftw_execute(p);
double minimum = 99999999999999999;
double maximum = -99999999999999999;
data=pDst->GetData();
for(int i=0;i<size;++i){
if(gray[i]>maximum) maximum=gray[i];
if(gray[i]<minimum) minimum=gray[i];
*data++ = gray[i]*100;
*data++ = 0;//gray[i];
*data++ = 0;//gray[i];
}
fprintf(stderr,"%f %f %d %d\n",minimum,maximum,size,spec_size);
data=pDst->GetData();
for(int i=0;i<size;++i){
*data = 100*(gray[i]-minimum)/(maximum-minimum);
data += 3;
}
fftw_destroy_plan(p);
fftw_free(gray); fftw_free(spec_gray);
fftw_free(phi); fftw_free(theta);
fftw_free(rgb[0]);fftw_free(rgb[1]);fftw_free(rgb[2]);
fftw_free(lab[0]);fftw_free(lab[1]);fftw_free(lab[2]);
fftw_free(spec_rgb[0]);fftw_free(spec_rgb[1]);fftw_free(spec_rgb[2]);
fftw_free(spec_lab[0]);fftw_free(spec_lab[1]);fftw_free(spec_lab[2]);
pDst->Convert(TMO_RGB);
return 0;
}
/*****************
* Version DOUBLE
*****************/
void Chi2LibFFTW::conv2d_fft(MyMatrix<double> *img, MyMatrix<double> *kernel_img, MyMatrix<double> *output){
MyLogger::log()->debug("[Chi2LibFFTW][conv2d_fft] Generating Convolution using FFTW");
fftw_complex *fft_image, *fft_kernel;
fftw_plan plan_forward_image, plan_forward_kernel, plan_backward;
//auxiliary structures are necessary because fftw3 optimization plan will destroy it!
double *ifft_result, *data, *kernel;
int nwidth = (int)(img->sX()+kernel_img->sX()-1);
int nheight = (int)(img->sY()+kernel_img->sY()-1);
pthread_mutex_lock( &mutex1 );
// FFTW Allocs
size_t size = (size_t)(nwidth * nheight);
//the new size includes zero padding space
data = fftw_alloc_real(size);
kernel = fftw_alloc_real(size);
ifft_result = fftw_alloc_real(size);
//fftw handle real fft avoiding redundancy in the complex plane, therefore the nheight/2
size = (size_t)(nwidth*(floor(nheight/2) + 1));
fft_image = fftw_alloc_complex(size);
fft_kernel = fftw_alloc_complex(size);
plan_forward_image = fftw_plan_dft_r2c_2d( nwidth, nheight, data, fft_image, FFTW_ESTIMATE );
plan_forward_kernel = fftw_plan_dft_r2c_2d( nwidth, nheight, kernel, fft_kernel, FFTW_ESTIMATE );
plan_backward = fftw_plan_dft_c2r_2d( nwidth, nheight, fft_image, ifft_result, FFTW_ESTIMATE );
pthread_mutex_unlock( &mutex1 );
//populate kernel and shift input
for(unsigned int x = 0 ; x < (unsigned int)nwidth ; ++x ){
unsigned int xnw = x*nwidth;
for(unsigned int y=0; y < (unsigned int)nheight; ++y){
if(x < kernel_img->sX() && y < kernel_img->sY())
kernel[xnw+ y] = kernel_img->getValue(x,y);
else
kernel[xnw+ y] = 0;
}
}
for(unsigned int x = 0 ; x < (unsigned int)nwidth ; ++x ){
unsigned int xnw = x*nwidth;
for(unsigned int y=0; y < (unsigned int)nheight; ++y){
if(x < img->sX() && y < img->sY())
data[xnw+ y] = img->getValue(x,y);
else
data[xnw+ y] = 0;
}
}
MyLogger::log()->debug("[Chi2LibFFTW][conv2d_fft] Starting FFTW");
/** FFT Execute */
//fft of image
fftw_execute( plan_forward_image );
//fft of kernel
fftw_execute( plan_forward_kernel );
//convolution in fourier domain
double f1, f2;
double nwnh = (double)(nwidth*nheight);
unsigned int limit = (unsigned int)(nwidth * (floor(nheight/2) + 1));
for(unsigned int i=0; i< limit; ++i){
f1 = fft_image[i][0]*fft_kernel[i][0] - fft_image[i][1]*fft_kernel[i][1];
f2 = fft_image[i][0]*fft_kernel[i][1] + fft_image[i][1]*fft_kernel[i][0];
fft_image[i][0]=f1/nwnh;
fft_image[i][1]=f2/nwnh;
}
//ifft of the product
fftw_execute( plan_backward );
/** FFT Execute */
MyLogger::log()->debug("[Chi2LibFFTW][conv2d_fft] FFTW Finished");
if(output->sX() == (unsigned int)nwidth && output->sY() == (unsigned int)nheight)
for(unsigned int x = 0 ; x < output->sX() ; ++x ){
unsigned int xnw = x*nwidth;
for(unsigned int y = 0 ; y < output->sY() ; ++y ){
output->at(x,y) = ifft_result[xnw+y];
}
}
/* free memory */
fftw_destroy_plan( plan_forward_image );
fftw_destroy_plan( plan_forward_kernel );
fftw_destroy_plan( plan_backward );
fftw_free( data );
fftw_free( kernel );
fftw_free( ifft_result );
fftw_free( fft_image );
fftw_free( fft_kernel );
}
int main(int argc,char *argv[]){
char *filename,*locale;
struct sample *samples = MAP_FAILED;
off_t length;
struct stat statbuf;
double complex *buffer = NULL;
double complex *spectrum = NULL;
double complex cpstep;
double carrier_freq,cstep;
int i,fd,start,nsamples,exitcode;
double complex carrier;
exitcode = 0;
fd = -1;
if((locale = getenv("LANG")) != NULL)
setlocale(LC_ALL,locale);
else
setlocale(LC_ALL,"en_US.utf8"); // The world revolves around the USA...
while((i = getopt(argc,argv,"c:qr:f")) != EOF){
switch(i){
case 'c':
Shift = atof(optarg);
break;
case 'q':
Quiet = 1;
break;
case 'r': // Sample rate
Samprate = atof(optarg);
break;
case 'f':
Flip_samples++;
break;
default:
fprintf(stderr,"%s: unknown option %c\n",argv[0],i);
exit(1);
}
}
filename = argv[optind];
if(lstat(filename,&statbuf) == -1){
fprintf(stderr,"lstat(%s) failed: %s\n",filename,strerror(errno));
exitcode = 1;
goto done;
}
if(!S_ISREG(statbuf.st_mode)){
fprintf(stderr,"%s is not an ordinary file\n",filename);
exitcode = 1;
goto done;
}
length = statbuf.st_size;
if((fd = open(filename,O_RDONLY)) == -1){
fprintf(stderr,"open(%s,readonly) failed; %s\n",filename,strerror(errno));
exitcode = 1;
goto done;
}
if((samples = mmap(NULL,length,PROT_READ,MAP_SHARED,fd,0)) == MAP_FAILED){
fprintf(stderr,"mmap(%s,%lld) failed: %s\n",filename,
(long long)length,strerror(errno));
exitcode = 1;
goto done;
}
nsamples = length / sizeof(struct sample);
if(Flip_samples && !Quiet)
fprintf(stderr,"I & Q samples swapped (spectrum inverted)\n");
if((buffer = fftw_alloc_complex(Fftsize)) == NULL){
fprintf(stderr,"fftw_alloc_complex(%d) failed\n",Fftsize);
exitcode = 2;
goto done;
}
if((spectrum = fftw_alloc_complex(Fftsize)) == NULL){
fprintf(stderr,"fftw_alloc_complex(%d) failed\n",Fftsize);
exitcode = 2;
goto done;
}
if(!Quiet)
fprintf(stderr,"demodulating %s: %'lld bytes, %'lld samples, %'.2lf sec @ %'.1lf Hz\n",
argv[optind],(long long)length,(long long)nsamples,nsamples/Samprate,Samprate);
// For each block of input samples
for(start=0; start < nsamples; start += Fftsize){
if(!Flip_samples){
for(i=0; i<Fftsize; i++)
buffer[i] = samples[start+i].i + Q*samples[start+i].q;
} else {
for(i=0; i<Fftsize; i++)
buffer[i] = samples[start+i].q + Q*samples[start+i].i;
}
carrier_freq = Shift;
// Spin down
cstep = 2*M_PI * carrier_freq/Samprate; // carrier phase step per sample, radians
cpstep = cos(cstep) - Q * sin(cstep); // Complex carrier step per sample
carrier = 1; // Unity-amplitude carrier starts at 0 radians
for(i=0;i<Fftsize;i++){
buffer[i] *= carrier; // Spin down to baseband
carrier *= cpstep; // Increment carrier phase
}
for(i=0; i<Fftsize; i++){
double s;
s = creal(buffer[i]);
fwrite(&s,sizeof(s),1,stdout);
s = cimag(buffer[i]);
fwrite(&s,sizeof(s),1,stdout);
}
}
done:; // Clean up and exit
if(samples != MAP_FAILED && munmap(samples,length) == -1){
fprintf(stderr,"munmap(%p,%lld) failed: %s\n",samples,(long long)length,strerror(errno));
}
if(fd != -1)
close(fd);
if(buffer != NULL)
fftw_free(buffer);
exit(exitcode);
}
// Set up sync vector for search correlator
// Use current estimates of sample rate
void generate_sync(double symrate){
int ind,k;
unsigned char data[10];
unsigned char symbols[2*8*10];
// The last 5 bytes (40 bits) of each 1024-bit minor frame are constants given below
// Run them through the encoder and use the last 34 symbols as the sync vector
// Those are the only invariant symbols from the encoder, after the user data has been flushed
memset(data,0,sizeof(data));
data[0] = 0x12;
data[1] = 0xfc;
data[2] = 0x81;
data[3] = 0x9f;
data[4] = 0xbe;
encode(symbols,data,10,0);
#if 1
for(k=0;k<80;k++)
printf(" %d",symbols[k]);
printf("\n");
#endif
// Update numbers based on current sample rate
Symrate = symrate;
Synclen = SYNCBITS * Symbolsamples + 1; // Fudge to prevent off-by-one overrun
printf("Symbol rate: %'.3lf Hz; samples/sym: %'.3lf; samples/frame: %'.1lf; samples in sync: %'d\n",
symrate,Samprate/symrate,2*FRAMEBITS*Symbolsamples,Synclen);
if(Syncvector != NULL)
free(Syncvector);
if((Syncvector = malloc(sizeof(*Syncvector) * (int)Synclen)) == NULL){
fprintf(stderr,"Can't malloc sync vector\n");
exit(3);
}
// Manchester encode last 34 symbols in sequence
ind = 0;
for(k=0;k<SYNCBITS;k++){
// First half of manchester symbol
for(;ind < (k+0.5) * Symbolsamples;ind++)
Syncvector[ind] = symbols[k+80-SYNCBITS] ? -1 : 1;
// Second half
for(;ind < (k+1) * Symbolsamples;ind++)
Syncvector[ind] = symbols[k+80-SYNCBITS] ? 1 : -1;
}
assert(ind <= Synclen);
#if 0
for(k=0;k<Synclen;k++){
putchar(Syncvector[k] == 1 ? '+':'-');
}
putchar('\n');
printf("sync vector done\n");
#endif
// Set up FFT correlator
Corr_size = 2*Framesamples;
Corr_size = 1 << 20; // hack!! change to round Corr_size up to next power of 2
Corr_input = fftw_alloc_real(Corr_size);
assert(Corr_input != NULL);
Corr_vector = fftw_alloc_real(Corr_size);
assert(Corr_vector != NULL);
Corr_result = fftw_alloc_real(Corr_size);
assert(Corr_result != NULL);
Corr_vector_transform = fftw_alloc_complex(Corr_size);
assert(Corr_vector_transform != NULL);
Corr_data_transform = fftw_alloc_complex(Corr_size);
assert(Corr_data_transform != NULL);
fftw_import_system_wisdom();
Corr_ffr = fftw_plan_dft_c2r_1d(Corr_size,Corr_data_transform,Corr_result,FFTW_ESTIMATE);
assert(Corr_ffr != NULL);
Corr_ff2 = fftw_plan_dft_r2c_1d(Corr_size,Corr_input,Corr_data_transform,FFTW_ESTIMATE);
assert(Corr_ff2 != NULL);
Corr_ff1 = fftw_plan_dft_r2c_1d(Corr_size,Corr_vector,Corr_vector_transform,FFTW_ESTIMATE);
// Load up the sync vector
for(k=0;k<Synclen;k++)
Corr_vector[k] = Syncvector[k];
// Zero pad
for(;k<Corr_size;k++)
Corr_vector[k] = 0;
fftw_execute(Corr_ff1); // Compute transform of sync vector
// Take complex conjugate so we don't have to do it every time
for(k=0;k<Corr_size;k++)
Corr_vector_transform[k] = conj(Corr_vector_transform[k]);
}
/*! \memberof splitop
draw some nice representation of the current state */
void splitop_draw(splitop_t * w, cairo_t * cr, cairo_rectangle_t rect, fftw_complex * psi)
{
cairo_save(cr);
cairo_rectangle(cr, rect.x, rect.y, rect.width, rect.height);
cairo_clip(cr);
cairo_set_source_rgb(cr, 1, 1, 1);
cairo_set_line_width(cr, 2);
cairo_paint(cr);
double y0 = rect.height/2;
//cairo_set_line_width(cr, 10);
cairo_set_source_rgb(cr, 0, 0, 0);
cairo_move_to(cr, rect.x, rect.y + y0);
cairo_line_to(cr, rect.x + rect.width, rect.y + y0);
cairo_stroke(cr);
int bins = w->prefs->bins;
double xscale = rect.width/bins, yscale, max;
double * V = w->prefs->potential->data;
max = 0;
for (int n = 0; n < bins; n++) {
double a = fabs(V[n]); if (max < a) { max = a; }
}
yscale = y0/max;
cairo_set_source_rgb(cr, 0, 0, 0);
cairo_move_to(cr, rect.x, y0);
for (int n = 0; n < bins; n++) { cairo_line_to(cr, rect.x + n * xscale, rect.y + y0 - V[n] * yscale); }
cairo_stroke(cr);
fftw_complex * apsi = w->apsi;
max = 0;
for (int n = 0; n < bins; n++) {
double a = cabs(apsi[n]); if (max < a) { max = a; }
}
yscale = y0/(5*max/4);
cairo_set_line_width(cr, 1);
cairo_set_source_rgb(cr, 0, 0, 1);
cairo_move_to(cr, rect.x, y0);
for (int n = 0; n < bins; n++) { cairo_line_to(cr, rect.x + n * xscale, rect.y + y0 - cabs(apsi[n]) * yscale); }
cairo_stroke(cr);
cairo_set_line_width(cr, 2);
cairo_set_source_rgb(cr, 1, 0, 0);
cairo_move_to(cr, rect.x, y0);
for (int n = 0; n < bins; n++) { cairo_line_to(cr, rect.x + n * xscale, rect.y + y0 - cabs(psi[n]) * yscale); }
cairo_stroke(cr);
fftw_complex * psik = fftw_alloc_complex(bins); assert(psik);
fftw_execute_dft(w->fwd, psi, psik);
max = 0;
for (int n = 0; n < bins; n++) {
double a = cabs(psik[n]); if (max < a) { max = a; }
}
yscale = y0/(5*max/4);
cairo_set_line_width(cr, 1);
cairo_set_source_rgb(cr, 1, .5, 0);
/*cairo_move_to(cr, rect.x, y0);
for (int n = 0; n < bins; n++) {
int l = (n+bins/2)%w->bins;
cairo_line_to(cr, rect.x + (n-bins/3)*4 * xscale, rect.y + 2*y0 - cabs(psik[l]) * yscale);
}*/
cairo_move_to(cr, rect.x, y0);
int dron = 1;
double reg = 1.0 / bins;
for (int k = bins/2; k < bins; k++) {
double x = rect.x + rect.width/2 + (k-bins) * reg * rect.width;
double y = rect.y + 2*y0 - cabs(psik[k]) * yscale;
if (dron) { cairo_move_to(cr, x, y); dron = 0; } else { cairo_line_to(cr, x, y); }
}
for (int k = 0; k < bins/2; k++) {
double x = rect.x + rect.width/2 + k * reg * rect.width;
double y = rect.y + 2*y0 - cabs(psik[k]) * yscale;
cairo_line_to(cr, x, y);
}
cairo_stroke(cr);
fftw_free(psik);
cairo_restore(cr);
}
void init_fft2d_(void )
{
int i,j,k;
double vm2,vm1,v,vp1,vp2,vb;
commx = MPI_Comm_f2c(topo_.commxc);
MPI_Comm_rank(commx, &irankx);
MPI_Comm_size(commx, &isizex);
commyz = MPI_Comm_f2c(topo_.commyzc);
MPI_Comm_rank(commyz, &irankyz);
MPI_Comm_size(commyz, &isizeyz);
fftw_mpi_init();
howmany = topo_.mxlc;
//***********
alloc_ly = fftw_mpi_local_size_2d(my, mz, commx, &ly, &lys);
/* alloc_ly=fftw_mpi_local_size_many(rnk, myz, howmany,
FFTW_MPI_DEFAULT_BLOCK, commx, &ly, &lys);
*/
//***********
if(((ly-topo_.mylc)!=0) || topo_.npzc>1) {
printf("Error,npz should equal to 1, or %d\t%d\n",irankx,ly-topo_.mylc);
MPI_Abort(commx,1);
}
minp = fftw_alloc_complex(alloc_ly);
mout = fftw_alloc_complex(alloc_ly);
if( !(freq = r3tensor(topo_.mxlc, topo_.mylc*topo_.mzlc, 2)) )
printf("Malloc error!\n");
if( !(data = r3tensor(topo_.mxlc, topo_.mylc*topo_.mzlc, 2)) )
printf("Malloc error!\n");
/* if( !(dar = r3tensor(topo_.mxlc, topo_.mylc, topo_.mzlc)) )
printf("Malloc error!\n");
if( !(dai = r3tensor(topo_.mxlc, topo_.mylc, topo_.mzlc)) )
printf("Malloc error!\n");
*/
//***********
mplanF = fftw_mpi_plan_dft_2d(my, mz, minp, mout, commx, FFTW_FORWARD, FFTW_MEASURE);
mplanR = fftw_mpi_plan_dft_2d(my, mz, minp, mout, commx, FFTW_BACKWARD, FFTW_MEASURE);
/* mplanF = fftw_mpi_plan_many_dft(rnk, myz, howmany,
FFTW_MPI_DEFAULT_BLOCK,FFTW_MPI_DEFAULT_BLOCK,
minp, mout, commx, FFTW_FORWARD, FFTW_MEASURE);
mplanR = fftw_mpi_plan_many_dft(rnk, myz, howmany,
FFTW_MPI_DEFAULT_BLOCK,FFTW_MPI_DEFAULT_BLOCK,
minp, mout, commx, FFTW_BACKWARD, FFTW_MEASURE);
*/
//***********
//***** Solver part ******
dxs = topo_.dx0*topo_.dx0;
dys = topo_.dy0*topo_.dy0;
dzs = topo_.dz0*topo_.dz0;
vm2=-1.0/12.0;
vm1=16.0/12.0;
v =-30.0/12.0;
vp1=16.0/12.0;
vp2=-1.0/12.0;
MatCreateMPIAIJ(commyz, PETSC_DECIDE, PETSC_DECIDE, mx, mx, 5, PETSC_NULL, 5, PETSC_NULL, &A);
ierr = MatGetOwnershipRange(A,&Istart,&Iend);
for (Ii=Istart; Ii<Iend; Ii++) {
i = Ii; j = Ii;
if ((i>1)&&(i<mx-2)) {
J = Ii - 2; MatSetValues(A,1,&Ii,1,&J,&vm2,INSERT_VALUES);
J = Ii - 1; ierr = MatSetValues(A,1,&Ii,1,&J,&vm1,INSERT_VALUES);CHKERRQ(ierr);
J = Ii; ierr = MatSetValues(A,1,&Ii,1,&J,&v,INSERT_VALUES);CHKERRQ(ierr);
J = Ii + 1; ierr = MatSetValues(A,1,&Ii,1,&J,&vp1,INSERT_VALUES);CHKERRQ(ierr);
J = Ii + 2; ierr = MatSetValues(A,1,&Ii,1,&J,&vp2,INSERT_VALUES);CHKERRQ(ierr);
}
if (i==0) {
J = Ii; ierr = MatSetValues(A,1,&Ii,1,&J,&v,INSERT_VALUES);CHKERRQ(ierr);}
if (i==1) {
J = Ii - 1; vb = 11.0/12.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);CHKERRQ(ierr);
J = Ii ; vb = -5.0/3.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii + 1; vb = 0.5; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii + 2; vb = 1.0/3.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii + 3; vb = -1.0/12.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
}
if (i==mx-2) {
J = Ii + 1; vb = 11.0/12.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);CHKERRQ(ierr);
J = Ii ; vb = -5.0/3.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii - 1; vb = 0.5; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii - 2; vb = 1.0/3.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
J = Ii - 3; vb = -1.0/12.0; ierr = MatSetValues(A,1,&Ii,1,&J,&vb,INSERT_VALUES);
}
if (i==mx-1) {J = Ii; ierr = MatSetValues(A,1,&Ii,1,&J,&v,INSERT_VALUES);CHKERRQ(ierr);}
}
ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
ierr = VecCreate(commyz,&br);CHKERRQ(ierr);
ierr = VecSetSizes(br,PETSC_DECIDE,mx);CHKERRQ(ierr);
ierr = VecSetFromOptions(br);CHKERRQ(ierr);
ierr = VecDuplicate(br,&xr);CHKERRQ(ierr);
ierr = VecDuplicate(br,&bi);CHKERRQ(ierr);
ierr = VecDuplicate(br,&xi);CHKERRQ(ierr);
ierr = KSPCreate(commyz,&ksp);CHKERRQ(ierr);
ierr = KSPSetOperators(ksp,A,A,SAME_PRECONDITIONER);CHKERRQ(ierr);
ierr = KSPSetFromOptions(ksp);CHKERRQ(ierr);
ierr = KSPGetPC(ksp,&pc);
PCSetType(pc,PCJACOBI);
ierr = KSPSetTolerances(ksp,1.e-7,1.e-50,PETSC_DEFAULT,
PETSC_DEFAULT);CHKERRQ(ierr);
//******* End *******
}
plggdn_complex *_plggdn_complex_alloc_fftw(int N) {
return fftw_alloc_complex(N);//sizeof(plggdn_complex));
}
