/*
    Tucnak - VHF contest log
    Copyright (C) 2013-2021 Ladislav Vaiz <ok1zia@nagano.cz>

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    version 2 as published by the Free Software Foundation.

*/
/* TODO
 I + Q = USB
 I - Q = LSB
 SQRT (I * I + Q * Q) = AM
 FM = atan(Q * I_old-I * Q_old, I * I_old+ Q * Q_old)
*/

#include "header.h"


#ifdef USE_SDR

#include "sdr.h"

#include "button.h"
#include "dsp.h"
#include "fifo.h"
#include "kbd.h"
#include "language2.h"
#include "main.h"
#include "rc.h"
#include "trig.h"
#include "tsdl.h"

#define sqr(x) ((x)*(x))       

#define F2PX(hz) ((((hz) + gsdr->samplerate / 2) * (gsdr->screen->w - 1)) / gsdr->samplerate)
#define PX2F(x) (((x) * gsdr->samplerate) / (gsdr->screen->w - 1) - gsdr->samplerate / 2)

#define DX2DF(x) ((double)((dx) * gsdr->samplerate) / (gsdr->screen->w - 1))

#define F2BIN(hz) ((((hz) * gsdr->n) / gsdr->samplerate + gsdr->n) % gsdr->n)
#define BIN2F(bin) ((((bin) < gsdr->n / 2 ? (bin) : (bin - gsdr->n)) * gsdr->samplerate) / gsdr->n)

#define F2SBIN(hz) (((hz) * gsdr->n) / gsdr->samplerate)

#define SBIN2BIN(sbin) (((sbin) < 0 ? (sbin + gsdr->n) : (sbin)))

struct sdr *gsdr;

/*static inline void zcplx_mul(fftw_complex *out, fftw_complex x, fftw_complex y)
{
    (*out)[0] = x[0] * y[0] - x[1] * y[1];
    (*out)[1] = x[1] * y[0] + x[0] * y[1];
} */


int sw_sdr_kbd_func(struct subwin *sw, struct event *ev, int fw){

	if (!gsdr) return 0;
    //if (!sdl) return 0;
	switch (ev->x){
		case 'c':
		case 'C':
			sdr_cw(sw);
			break;
		case 'l':
		case 'L':
			sdr_lsb(sw);
			break;
		case 's':
		case 'S':
			sdr_ssb(sw);
			break;
		case 'u':
		case 'U':
			sdr_usb(sw);
		case 'q':
		case 'Q':
			sdr_iqcomp(sw);
			break;
	}
    return 0;
}																	 


int sw_sdr_mouse_func(struct subwin *sw, struct event *ev, int fw){
	int step = 100;
	if (!gsdr) return 0;
    //if (!sdl) return 0;
#ifdef Z_HAVE_SDL
	sw->mx = ev->mx - sw->x * FONT_W;
    sw->my = ev->my - sw->y * FONT_H;
	zchart_mouse(sw->chart, sw->mx, sw->my);
	/*{ // TODO
		sw->gdirty = 1;
		redraw_later();
	} */

	/*f (ev->b & B_MOVE) {
		int f, b, sbin, bin2;
		f = PX2F(sw->mx);
		b = F2BIN(f);
		sbin = F2SBIN(f);
		bin2 = SBIN2BIN(sbin);
		//dbg("f=%6d   b=%5d  sbin=%6d  bin2=%5d\n", f, b, sbin, bin2);
		return 0;
	} */

	//dbg("ev->b = 0x%04x\n", ev->b);
	if ((ev->b & B_DRAG) != 0  &&  (ev->b & BM_EBUTT) == B_RIGHT){
		//if (sw->my < sw->fft_wf_y + sw->fft_wf_h){
		if (sw->my < sw->fft_sp_y + sw->fft_sp_h){
			int dx = ev->mx - sw->olddragx;
			if (dx != 0){
				SDL_Rect r;

				double df = DX2DF(dx);
				//dbg("sdr dx=%d  = %f hz\n", dx, df);
#ifdef HAVE_HAMLIB
				trigs_set_qrg(gtrigs, gtrigs->qrg - df);
#endif

				sdr_filter_tune(gsdr, gsdr->filter->zero + df);

				MUTEX_LOCK(sw->screen);
				r.y = sw->fft_wf_y;
				r.h = sw->fft_wf_h;
				r.x = -dx;
				r.w = sw->screen->w;
				SDL_BlitSurface(sw->screen, &r, sw->screen, NULL);
				if (dx > 0){
					r.x = 0;
					r.w = dx;
				}else{
					r.x = r.w + dx;
					r.w = dx;
				}
				SDL_FillRect(sw->screen, &r, 0);
				MUTEX_UNLOCK(sw->screen);
			}
		}
	}


    if (SDL_GetModState() & KMOD_SHIFT) step = 1000;
    if (SDL_GetModState() & KMOD_CTRL) step = 10000;

	switch (ev->b & BM_EBUTT){
		case B_WHUP:
			sdr_filter_tune(gsdr, gsdr->filter->zero + step);
			sw->gdirty = 1;
			break;
		case B_WHDOWN:
			sdr_filter_tune(gsdr, gsdr->filter->zero - step);
			sw->gdirty = 1;
			break;
		case B_RIGHT:
			sw->olddragx = ev->mx;
			break;
		case B_LEFT:
			if (/*sw->my >= sw->fft_sp_y &&*/ sw->my < sw->fft_sp_y + sw->fft_sp_h){
				double freq = PX2F(sw->mx);
				freq -= (gsdr->filter->low + gsdr->filter->high) / 2;
				sdr_filter_tune(gsdr, freq);
			}else{
				buttons_mouse(sw->buttons, ev->b, sw->mx, sw->my);
			}
			sw->gdirty = 1;
			break;

	}
	sw_sdr_check_bounds(sw);
#endif
	return 0;
}

void sw_sdr_check_bounds(struct subwin *sw){

//    if (!sdl) return;
	if (!gsdr) return;
	if (!gsdr->filter) return;
}


void sw_sdr_raise(struct subwin *sw){
#ifdef Z_HAVE_SDL    
    sw->gdirty=1;
#endif    
}



struct sdr *init_sdr(void){
	int i, j = 0, r, f;
    struct sdr *sdr;
   
    //dbg("init_sdr  enable=%d\n", cfg->sdr_enable);
	if (!cfg->sdr_enable) return NULL;
    //if (!sdl) return NULL;

    sdr = g_new0(struct sdr, 1);

	sdr->n = cfg->sdr_block;
    sdr->frames = sdr->n / 2;

    sdr->samples = sdr->frames * 2;
	
	sdr->bfo_phase = 0.0;
	sdr->load = -1;
#ifdef Z_HAVE_SDL    
    if (zsdl){
    	sdr->butbg = z_makecol(0, 128, 128);
    }

    MUTEX_INIT(sdr->screen);
#endif
	MUTEX_INIT(sdr->filter);

#ifdef Z_HAVE_SDL    
    if (sdl){
        for (i = 128; i < 256; i++){
            r = (i-128)*2;
            f = 255 - r;
            sdr->pal[j++] = z_makecol(0, 0, r);
        } 
        for (i = 256; i < 512; i++){
            r = i - 256;
            f = 255 - r;
            sdr->pal[j++] = z_makecol(0, r, 255);
        }
        for (i = 512; i < 768; i++){
            r = i - 512;
            f = 255 - r;
            sdr->pal[j++] = z_makecol(r, 255, f);
        }
        for (i = 768; i < 1024-32; i++){
            r = i - 768;
            f = 255 - r;
            sdr->pal[j++] = z_makecol(255, f, 0);
        }

	
    	for (i = 0; i < FFT_COLORS; i++){
	        int val = (z_r(sdl->screen, sdr->pal[i]) * 30 + z_g(sdl->screen, sdr->pal[i]) * 59 + z_b(sdl->screen, sdr->pal[i]) * 11) / 100;
		    sdr->grpal[i] = z_makecol(val, val, val);
	    }
    }
#endif

	// initial filter, rewritten after source DSP open
	sdr->samplerate = cfg->sdr_speed ? cfg->sdr_speed : 48000;
	sdr->filter = init_sdr_filter(sdr, cfg->sdr_zero, 1000, 2000, sdr->samplerate , sdr->n / 4 + 1);
	sdr->agc_gain = 200.0; // experimental, was 1.0;
	
    
    sdr->iqdsp = init_dsp((enum dsp_type)cfg->sdr_rec_dsp_type, DCG_SDR);
	sdr->iqdsp->set_sdr_format(sdr->iqdsp, sdr->frames, cfg->sdr_speed, 1);
	
    sdr->afdsp = init_dsp((enum dsp_type)cfg->sdr_play_dsp_type, DCG_SDR);
	sdr->afdsp->set_sdr_format(sdr->afdsp, sdr->frames, cfg->sdr_af_speed, 0); 

    sdr->iq = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * sdr->n); 
    sdr->bins = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * sdr->n);
    sdr->iout = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * sdr->n); 
    sdr->qout = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * sdr->n); 

	sdr->plan_f = fftw_plan_dft_1d(sdr->n, sdr->iq, sdr->bins, FFTW_FORWARD, FFTW_ESTIMATE);
	sdr->plan_bi = fftw_plan_dft_1d(sdr->n, sdr->bins, sdr->iout, FFTW_BACKWARD, FFTW_ESTIMATE);
	sdr->plan_bq = fftw_plan_dft_1d(sdr->n, sdr->bins, sdr->qout, FFTW_BACKWARD, FFTW_ESTIMATE);

	sdr->amplen = sdr->n; 
	sdr->amp = g_new0(double, sdr->amplen);
	sdr->nea_amp = g_new0(double, sdr->amplen);
	sdr->sndbuf = g_new0(short, sdr->frames * 2);

	sdr->bal_gain = 1.0;
	sdr->bal_phase = 0.0;
    sdr->bal_iq = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * SDR_BAL_N);
    sdr->bal_bins = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * SDR_BAL_N);
	sdr->bal_plan = fftw_plan_dft_1d(SDR_BAL_N, sdr->bal_bins, sdr->bal_bins, FFTW_FORWARD, FFTW_ESTIMATE);

	dbg("init_sdr=%p\n", sdr);

	return sdr;
}

void free_sdr(struct sdr *sdr){
	dbg("free_sdr(%p)\n", sdr);

    if (!sdr) return;


	sdr_stop(sdr);
    free_dsp(sdr->iqdsp);
    free_dsp(sdr->afdsp);
	
    g_free(sdr->amp);
	g_free(sdr->nea_amp);
	g_free(sdr->sndbuf);
	
    fftw_destroy_plan(sdr->plan_f);
	fftw_destroy_plan(sdr->plan_bi);
	fftw_destroy_plan(sdr->plan_bq);

	fftw_free(sdr->iq);
	fftw_free(sdr->bins);
	fftw_free(sdr->iout);
    sdr->iout = NULL;
	fftw_free(sdr->qout);

	g_free(sdr->wfvec);
	free_sdr_filter(sdr->filter);
    MUTEX_FREE(sdr->screen);
    MUTEX_FREE(sdr->filter);
    g_free(sdr);
}

struct sdr *sdr_start(struct sdr *sdr){
	//dbg("sdr_start(%p)\n", sdr);

    if (!sdr) return sdr;
	if (sdr->thread) return sdr;


	sdr->thread_break = 0;
	sdr->thread = g_thread_try_new("Tucnak SDR", sdr_thread_func, (gpointer)sdr, NULL);
    if (!sdr->thread){
        log_addf(VTEXT(T_CANT_CREATE_SDR_THR));
        free_sdr(sdr);
        return NULL;
    }
	return sdr;

}

void sdr_stop(struct sdr *sdr){
	dbg("sdr_stop(%p)\n", sdr);
	if (!sdr) return;

	if (sdr->thread){
        progress(VTEXT(T_WAIT_FOR_SDR_THR));
        sdr->thread_break = 1;
        dbg("join sdr...\n");
        g_thread_join(sdr->thread);
        dbg("done\n");
        sdr->thread = NULL;
    }
}






void sdr_read_handler(struct sdr *sdr, gchar *cmd, gchar *arg){
    /*dbg("sdr_read_handler\n");*/
    
    switch(cmd[0]){
		case 'D':
#ifdef Z_HAVE_SDL
			if (gses && gses->ontop->type == SWT_SDR){
				gses->ontop->gdirty = 1;
			}
#endif
            redraw_later();
		case '!':
			log_adds(arg);
            redraw_later();
			break;
	}
}

struct sdr_filter *init_sdr_filter(struct sdr *sdr, double zero, double low, double high, double samplerate, int size){
	int i, midpoint;
	double fc, ff;
	struct sdr_filter *filter;
	fftw_plan plan;
	fftw_complex /* *iq */ *h;

	filter = g_new0(struct sdr_filter, 1);
	filter->zero = zero;
	filter->low = low;
	filter->high = high;
	filter->window_size = size;
	if (gsdr && gsdr->samplerate > 0) filter->zero_bin = F2BIN((int)filter->zero);

	if ((size & 1) == 0) size++;
	midpoint = size / 2 + 1;
	low += zero;
	high += zero;
	low /= samplerate;
	high /= samplerate;
	fc = (high - low) / 2.0;
	ff = (low + high) * M_PI;
			
	filter->fbins = (fftw_complex*)fftw_malloc(sdr->n * sizeof(fftw_complex));
	
	filter->window = g_new0(double, size);
	filter->window_iq = (fftw_complex*)fftw_malloc(sdr->n * sizeof(fftw_complex));
	memset(filter->window_iq, 0, sdr->n * sizeof(fftw_complex));

	for (i = 0; i < size; i++){
	    filter->window[i] = 
			+ 0.35875
			- 0.48829 * cos(2.0 * M_PI * (i + 0.5) / (size - 1)) 
			+ 0.14128 * cos(4.0 * M_PI * (i + 0.5) / (size - 1))
			- 0.01168 * cos(6.0 * M_PI * (i + 0.5) / (size - 1));
	}

	h = filter->window_iq + sdr->n - size; // RHS
	for (i = 1; i <= size; i++){           // windowed sinc filter
		int j = i - 1;
		int k = i - midpoint;
		double tmp, ph = ff * k;

		if (k == 0){
			tmp = 2.0 * fc;	
		}else{
			tmp = filter->window[j] * sin(2 * M_PI * k * fc) / (M_PI * k);
		}
		tmp *= 2.0;
		h[j][0] = tmp * cos(ph);
		h[j][1] = tmp * sin(ph);
	}

	plan = fftw_plan_dft_1d(sdr->n, filter->window_iq, filter->fbins, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(plan);

	fftw_destroy_plan(plan);	

	if (filter->zero != 0.0) sdr->bfo_inc = 2 * M_PI * filter->zero / samplerate;

	return filter;
}

void free_sdr_filter(struct sdr_filter *filter){
	if (!filter) return;

	fftw_free(filter->window_iq);
	g_free(filter->window);

	fftw_free(filter->fbins);
	g_free(filter);
}

void sdr_new_filter(struct sdr *sdr, double zero, double cw_low, double cw_high, double ssb_low, double ssb_high){
	struct sdr_filter *oldfilter, *newfilter;
	double low, high;

	if (cfg->sdr_cw){
		low = cw_low;
		high = cw_high;
	}else{
		low = ssb_low;
		high = ssb_high;
	}
	
	newfilter = init_sdr_filter(sdr, zero, low, high, sdr->samplerate, sdr->filter->window_size);
	MUTEX_LOCK(sdr->filter);
	oldfilter = sdr->filter;
	sdr->filter = newfilter;
	MUTEX_UNLOCK(sdr->filter);
	free_sdr_filter(oldfilter);

	cfg->sdr_zero = zero;
	cfg->sdr_cw_low = cw_low;
	cfg->sdr_cw_high = cw_high;
	cfg->sdr_ssb_low = ssb_low;
	cfg->sdr_ssb_high = ssb_high;
}

void sdr_filter_tune(struct sdr *sdr, double zero){
	
	if (zero + sdr->filter->low < -sdr->samplerate / 2)
		zero = -sdr->samplerate / 2 - sdr->filter->low;

	if (zero + sdr->filter->high > sdr->samplerate / 2)
		zero = sdr->samplerate / 2 - sdr->filter->high;

	sdr_new_filter(sdr, zero, cfg->sdr_cw_low, cfg->sdr_cw_high, cfg->sdr_ssb_low, cfg->sdr_ssb_high);
	
}

void sdr_ssb(void *xxx){
	struct subwin *sw = (struct subwin *)xxx;
	dbg("sdr_ssb\n");
	cfg->sdr_cw = 0;
	sw->
        sdr_ssb->
        bcolor = 
        gsdr->
        butbg;
	sw->sdr_cw->bcolor = 0;

	sdr_new_filter(gsdr, gsdr->filter->zero, cfg->sdr_cw_low, cfg->sdr_cw_high, cfg->sdr_ssb_low, cfg->sdr_ssb_high);
	redraw_later();
}
  
void sdr_cw(void *xxx){
	struct subwin *sw = (struct subwin *)xxx;
	dbg("sdr_cw\n");
	cfg->sdr_cw = 1;
	sw->sdr_cw->bcolor = gsdr->butbg;
	sw->sdr_ssb->bcolor = 0;

	sdr_new_filter(gsdr, gsdr->filter->zero, cfg->sdr_cw_low, cfg->sdr_cw_high, cfg->sdr_ssb_low, cfg->sdr_ssb_high);
	redraw_later();
}

void sdr_usb(void *xxx){
	struct subwin *sw = (struct subwin *)xxx;
	dbg("sdr_usb\n");
	if (cfg->sdr_lsb == 1){
		int newzero = gsdr->filter->zero + gsdr->filter->high + gsdr->filter->low ;
		sdr_new_filter(gsdr, newzero, -cfg->sdr_cw_high, -cfg->sdr_cw_low, -cfg->sdr_ssb_high, -cfg->sdr_ssb_low);
	}
	cfg->sdr_lsb = 0;
	sw->sdr_usb->bcolor = gsdr->butbg;
	sw->sdr_lsb->bcolor = 0;
	redraw_later();
}

void sdr_iqcomp(void *xxx){
	struct subwin *sw = (struct subwin *)xxx;
	dbg("sdr_iqcomp\n");

	cfg->sdr_iqcomp = !cfg->sdr_iqcomp;
	sw->sdr_iqcomp->bcolor = cfg->sdr_iqcomp ? gsdr->butbg : 0; 

	redraw_later();
}

void sdr_lsb(void *xxx){
	struct subwin *sw = (struct subwin *)xxx;
	dbg("sdr_lsb\n");
	if (cfg->sdr_lsb == 0){
		int newzero = gsdr->filter->zero + gsdr->filter->high + gsdr->filter->low ;
		sdr_new_filter(gsdr, newzero, -cfg->sdr_cw_high, -cfg->sdr_cw_low, -cfg->sdr_ssb_high, -cfg->sdr_ssb_low);
	}
	cfg->sdr_lsb = 1;
	sw->sdr_lsb->bcolor = gsdr->butbg;
	sw->sdr_usb->bcolor = 0;
	redraw_later();
}

void sdr_resize(struct sdr *sdr, struct subwin *sw){
#ifdef Z_HAVE_SDL
    //dbg("sdr_resize\n");
    SDL_Surface *newscr, *oldscr;
	SDL_Rect r;
	int minh;

	if (!sdr) return;
    minh = sw->h * FONT_H / 2;
    sw->fft_sp_h = 210;
    if (sw->fft_sp_h > minh) sw->fft_sp_h = minh;
    sw->fft_sp_y = (sw->h - 1) * FONT_H - sw->fft_sp_h - 4;
    sw->fft_wf_y = 0;
    sw->fft_wf_h = sw->fft_sp_y - FONT_H - 10;
    //sw->fft_sp_x2 = fft->wf_x2;
    //sw->fft_wf_x2 = fft->wf_x2;

    newscr = SDL_CreateRGBSurface(SDL_SWSURFACE, sw->w*FONT_W , sw->h*FONT_H, sdl->bpp, 
        sdl->screen->format->Rmask, sdl->screen->format->Gmask, sdl->screen->format->Bmask, 0);
    MUTEX_LOCK(sdr->screen);
    oldscr = sdr->screen;
    sdr->screen = newscr;
    sdr->screeny = 0;
    MUTEX_UNLOCK(sdr->screen);
    if (oldscr) SDL_FreeSurface(oldscr);

	r.x = 0;
	r.y = sw->fft_sp_y;
	r.w = sw->screen->w;
	r.h = sw->fft_sp_h;

	if (sw->chart) zchart_free(sw->chart);
	sw->chart = zchart_init(sw->screen, &r, z_makecol(30, 30, 0));
	zchart_add_set(sw->chart, "window ", sdl->gr[15]);
	zchart_add_set(sw->chart, "winIQ.Re ", sdl->yellow);
	zchart_add_set(sw->chart, "winIQ.Im ", z_makecol(0, 255, 255));
	zchart_add_set(sw->chart, "fbin.abs", z_makecol(0, 255, 255));


	buttons_clear(sw->buttons);
	sw->sdr_by = sw->fft_sp_y + sw->fft_sp_h - 1 + 2;
	sw->sdr_ssb = init_button(sw->screen, 0, sw->sdr_by, sdr_ssb, sw);
	button_text(sw->sdr_ssb, zsdl->font_h, sdl->gr[14], cfg->sdr_cw ? 0 : sdr->butbg, " SSB ");
	g_ptr_array_add(sw->buttons, sw->sdr_ssb);

	sw->sdr_cw = init_button(sw->screen, sw->sdr_ssb->x + sw->sdr_ssb->w + zsdl->font_w, sw->sdr_by, sdr_cw, sw);
	button_text(sw->sdr_cw, zsdl->font_h, sdl->gr[14], cfg->sdr_cw ? sdr->butbg : 0, " CW ");
	g_ptr_array_add(sw->buttons, sw->sdr_cw);

	sw->sdr_usb = init_button(sw->screen, sw->sdr_cw->x + sw->sdr_cw->w + zsdl->font_w, sw->sdr_by, sdr_usb, sw);
	button_text(sw->sdr_usb, zsdl->font_h, sdl->gr[14], cfg->sdr_lsb ? 0 : sdr->butbg, " USB ");
	g_ptr_array_add(sw->buttons, sw->sdr_usb);

	sw->sdr_lsb = init_button(sw->screen, sw->sdr_usb->x + sw->sdr_usb->w + zsdl->font_w, sw->sdr_by, sdr_lsb, sw);
	button_text(sw->sdr_lsb, zsdl->font_h, sdl->gr[14], cfg->sdr_lsb ? sdr->butbg : 0, " LSB ");
	g_ptr_array_add(sw->buttons, sw->sdr_lsb);

	sw->sdr_iqcomp = init_button(sw->screen, sw->sdr_lsb->x + sw->sdr_lsb->w + zsdl->font_w, sw->sdr_by, sdr_iqcomp, sw);
	button_text(sw->sdr_iqcomp, zsdl->font_h, sdl->gr[14], cfg->sdr_iqcomp ? sdr->butbg : 0, " IQcomp ");
	g_ptr_array_add(sw->buttons, sw->sdr_iqcomp);

	g_free(sdr->wfvec);
	sdr->wfvec = g_new0(int, sw->screen->w);
	sdr->wfveclen = sw->screen->w;
#endif
}


void sw_sdr_redraw(struct subwin *sw, struct band *band, int flags){
#ifdef Z_HAVE_SDL    
	SDL_Rect src, dst, filter_r;
	int i, y, x = 0, sp_y, oldx, oldy = 0, amp, tw, j = 1;
	int bfo_px;//, filter1_px, filter2_px;
	char s[256];
	double qrg;
	int steps[] = {100, 50, 20, 10, 5, 2, 1, 0}; // kHz
			  
	if (!gsdr) return;
    if (!sdl) return;
	
	fill_area(sw->x, sw->y, sw->w, sw->h, 0);

	if (!sw->gdirty) return;
	sw->gdirty=0;
	if (gsdr->samplerate <= 0) return;
	if (gsdr->amp == NULL) return;

	/*area.x = 0;
	area.y = 0;
	area.w = sw->screen->w;
	area.h = sw->screen->h;
    SDL_FillRect(sw->screen, &area, z_makecol(40, ((ssr++) % 8) * 32, 0));
	*/


    MUTEX_LOCK(gsdr->screen);
	if (!gsdr->screen){
		MUTEX_UNLOCK(gsdr->screen);
		return;
	}
	MUTEX_LOCK(sw->screen);
    if (gsdr->screeny > 0){
		//dbg("gsdr->screeny = %d\n", gsdr->screeny);
		y = gsdr->screeny;
		gsdr->screeny = 0;


		// move old screen
		src.x = 0;
		src.y = sw->fft_wf_y + y;
		src.w = sw->screen->w;
		src.h = sw->fft_wf_h - y;
		dst.x = 0;
		dst.y = sw->fft_wf_y;
		dst.w = sw->screen->w;
		dst.h = sw->fft_wf_h - y;
		SDL_BlitSurface(sw->screen, &src, sw->screen, &dst);
    
		// add new data
		src.x = 0;
		src.y = 0;
		src.w = gsdr->screen->w;
		src.h = y;
		dst.x = 0;
		dst.y = sw->fft_wf_y + sw->fft_wf_h - y;
		dst.w = sw->screen->w;
		dst.h = y;
		SDL_BlitSurface(gsdr->screen, &src, sw->screen, &dst);
		//z_rect2(sw->screen, &dst, sdl->gr[15]);
	}


	// clear background under frequency axis
	dst.x = 0;
	dst.y = sw->fft_wf_h + 1;
	dst.w = sw->screen->w;
	dst.h = sw->fft_sp_y - dst.y;
	SDL_FillRect(sw->screen, &dst, z_makecol(0, 0, 0));


	qrg = gsdr->samplerate / 2;
#ifdef HAVE_HAMLIB
	qrg += gtrigs->qrg;
#endif
	z_qrg_format(s, sizeof(s), qrg / 1000);
	tw = strlen(s) * zsdl->font_w;
	for (i = 0; ; i++){
		int px;
		if (steps[i] == 0) break;
		j = steps[i];
		px = F2PX(j * 1000) - F2PX(0);
		if (px < tw * 3) break;
	}

	qrg = -gsdr->samplerate / 2;
#ifdef HAVE_HAMLIB
	qrg += gtrigs->qrg;
#endif
	modf(qrg / (j * 1000.0), &qrg);
	qrg *= j; 
#ifdef HAVE_HAMLIB
	i = (int)(qrg * 1000 - gtrigs->qrg);
#else
	i = -gsdr->samplerate / 2;
#endif

	for (;; qrg += j, i += j * 1000){
		//int flg = ZFONT_CENTERX;
//		char *fmt = "%dk";

		x = F2PX(i);
		if (x > sw->w * FONT_W - 1) break;
		z_line(sw->screen, x, sw->fft_wf_h + 1, x, sw->fft_sp_y - FONT_H , sdl->gr[10]);
		//if (x <= 0) flg = 0;
		//if (x >= sw->screen->w - 1)	flg = ZFONT_RIGHT;
		//if (i == 0) fmt = "VFO";
		z_qrg_format(s, sizeof(s), qrg);
		zsdl_printf(sw->screen, x, sw->fft_sp_y - FONT_H, sdl->gr[10], 0, ZFONT_CENTERX, "%s", s);
		z_line(sw->screen, x, sw->fft_wf_h + 2, x, sw->fft_wf_h + 5, sdl->gr[10]);
	}
	z_line(sw->screen, F2PX(-gsdr->samplerate / 2), sw->fft_wf_h + 1, F2PX(gsdr->samplerate / 2), sw->fft_wf_h + 1, sdl->gr[10]); 

	// clear background under spectral lines
	dst.x = 0;
	dst.w = sw->screen->w;
	dst.y = sw->fft_sp_y;
	dst.h = sw->fft_sp_h;
	SDL_FillRect(sw->screen, &dst, z_makecol(20, 20, 0));

	
	// draw filter and BFO
	sp_y = sw->fft_sp_y + sw->fft_sp_h - 1;
	filter_r.x = F2PX(gsdr->filter->zero + gsdr->filter->low);
	filter_r.y = sp_y - sw->fft_sp_h + 1;
	filter_r.w = F2PX(gsdr->filter->zero + gsdr->filter->high) - filter_r.x + 1;
	filter_r.h = sw->fft_sp_h;
	SDL_FillRect(sw->screen, &filter_r, sdl->green);

	bfo_px = F2PX(gsdr->filter->zero);
	z_line(sw->screen, bfo_px, sp_y, bfo_px, sp_y - sw->fft_sp_h + 1, sdl->red);

#if 1
	// draw spetral lines
	oldx = -1;
	for (i=0; i <= gsdr->amplen; i++){
		x = (i * (gsdr->screen->w - 1)) / gsdr->n;
		amp = gsdr->nea_amp[i] * (sw->fft_sp_h - 1);
		for (; oldx <= x; oldx++)
			z_line(sw->screen, oldx, sp_y, oldx, sp_y - amp, sdl->gr[9]);
	}
	oldx = -1;
	for (i=0; i <= gsdr->amplen; i++){
		x = (i * (gsdr->screen->w - 1)) / gsdr->n;
		amp = gsdr->nea_amp[i] * (sw->fft_sp_h - 1);
		if (i > 0){
			z_line(sw->screen, oldx, oldy, x, sp_y - amp, sdl->gr[12]);
		}
		oldx = x;
		oldy = sp_y - amp;
	} 

	for (i = 0; i < FFT_COLORS; i++){
        z_line(sw->screen, i, 0, i, 3, gsdr->pal[i]);
    }
#endif

#if 0
	zchart_clear(sw->chart);
	for (i = 0; i < gsdr->filter->window_size; i++){
		zchart_add(sw->chart, 0, i, gsdr->filter->window[i]);
	}
	
	
	for (i = 0; i < gsdr->n; i++) {
		int b = (i + gsdr->n / 2) % gsdr->n;
		zchart_add(sw->chart, 1, i, gsdr->filter->window_iq[i][0]);
		zchart_add(sw->chart, 2, i, gsdr->filter->window_iq[i][1]);
		zchart_add(sw->chart, 3, i, Z_CABS(gsdr->filter->fbins[b]));
	}
	zchart_redraw(sw->chart);
	dbg("step=%f\n", gsdr->bfo_inc);
#endif

	// clear background under status line
	dst.x = 0;
	dst.w = sw->screen->w;
	dst.y = sw->sdr_by;
	dst.h = zsdl->font_h;
	SDL_FillRect(sw->screen, &dst, z_makecol(0, 0, 0));

	buttons_redraw(sw->buttons, sw->screen);
	qrg = gsdr->filter->zero;
#ifdef HAVE_HAMLIB
	if (gtrigs) qrg += gtrigs->qrg;
#endif
	z_qrg_format(s, sizeof(s), qrg);
	zsdl_printf(sw->screen, sw->sdr_iqcomp->x + sw->sdr_iqcomp->w + 3 * zsdl->font_w, sw->sdr_by, sdl->gr[14], 0, 0, "%s", s);

	if (gsdr->load > -1){
		zsdl_printf(sw->screen, sw->screen->w - 1, sw->sdr_by, sdl->gr[14], 0, ZFONT_RIGHT,  " CPU %d%%", gsdr->load);
	}
    
    MUTEX_UNLOCK(sw->screen);
    MUTEX_UNLOCK(gsdr->screen);
#endif
}


gpointer sdr_thread_func(gpointer data){
    int total = 0, i;
    struct sdr *sdr = (struct sdr *)data;
	struct timeval tv1, tv2, stop, tvstart;
	long oldsec = 0;

	double totsum = 0.0, cpusum = 0.0, tot;
	int loadn = 0;
	short *buf, *b;
	double *pd, avgi = 0.0, avgq = 0.0;
	int playspeed, playfactor;
	int use_pipe = 0;

    dbg("sdr_thread_func\n");

	zg_thread_set_name("Tucnak SDR");
	zg_thread_set_priority(2);

#ifdef HAVE_LIBRTLSDR
#ifdef HAVE_HAMLIB
	sdr->iqdsp->rtlsdr_frequency = gtrigs->qrg < 24000000 ? 144000000 : gtrigs->qrg;
#endif
#endif

	if (sdr->iqdsp->open(sdr->iqdsp, 1) < 0) {
		zselect_msg_send(zsel, "SDR;!;%s %s", VTEXT(T_CANT_RECORD_FROM_SDR_DSP), sdr->iqdsp->name);
		return NULL;
	}
	
    if (sdr->afdsp->open(sdr->afdsp, 0) < 0) {
		zselect_msg_send(zsel, "SDR;!;%s %s", VTEXT(T_CANT_PLAY_TO_SDR_DSP), sdr->afdsp->name);
        sdr->iqdsp->close2(sdr->iqdsp);
        return NULL;
    };


	playspeed = cfg->sdr_af_speed;
	playfactor = sdr->iqdsp->speed / playspeed;
	//for (playspeed = sdr->iqdsp->speed; playspeed > 48000; playspeed /= 2) playfactor *= 2;


#ifdef HAVE_SNDFILE
	if (gssbd && gssbd->dsp && gssbd->dsp->type == DSPT_SNDPIPE && gssbd->dsp->pipe_opened){	
		gssbd->dsp->set_sdr_format(gssbd->dsp, sdr->frames, playspeed, 0);
		if (gssbd->dsp->open(gssbd->dsp, 0)){
			zselect_msg_send(zsel, "SDR;!;%s %s", VTEXT(T_CANT_OPEN_PLAY_SDR_SOUNDPIPE), sdr->afdsp->name);
			return NULL;
		}
		use_pipe = 1;
	}
#endif
		
#ifdef Z_HAVE_SDL
	sdr->screeny = 0;
#endif
	sdr->samplerate = sdr->iqdsp->speed;
	
	sdr_new_filter(sdr,cfg->sdr_zero, cfg->sdr_cw_low, cfg->sdr_cw_high, cfg->sdr_ssb_low, cfg->sdr_ssb_high);   

	sdr->agc_period_ms = 1;
	sdr->agc_n = sdr->samplerate * sdr->agc_period_ms / 1000.0;
	sdr->agc_ign = sdr->samplerate * 100 / ( 1000.0 * sdr->agc_n); 
	buf = g_new0(short, sdr->frames * sdr->iqdsp->channels); // nemá být sdr->samples?

	memset(sdr->iq, 0, sizeof(fftw_complex) * sdr->n);
	gettimeofday(&tvstart, NULL);
	gettimeofday(&tv1, NULL);
	oldsec = tv1.tv_sec;
     
#ifdef Z_HAVE_SDL    
	MUTEX_LOCK(sdr->screen);
    if (sdr->screen) {
		for (i = 0; i < sdr->wfveclen; i += 3) z_putpixel(sdr->screen, i, sdr->screeny, sdl->gr[12]);
		sdr->screeny++;
	}
	MUTEX_UNLOCK(sdr->screen);
#endif


    while(!sdr->thread_break){
        int oldx, x, col, frames;
        double d, e, cpu, dmax, maxgain, mingain;
		short *sb;
		fftw_complex *iq;
        //dbg("sdr_thread_func while\n");

		memmove(sdr->iq, sdr->iq + sdr->frames, sizeof(fftw_complex) * (sdr->n - sdr->frames)); 

        //ST_START();
		frames = sdr->iqdsp->read(sdr->iqdsp, buf, sdr->frames);
		//ST_STOP("dsp read");
		if (frames != sdr->frames){
            dbg("Can't read from SDR IQDSP %s (%d != %d)\n", sdr->iqdsp->name, frames, sdr->frames);
			zselect_msg_send(zsel, "SDR;!;%s SDR IQDSP %s (%d != %d)", VTEXT(T_CANT_READ_FROM), sdr->iqdsp->name, frames, sdr->frames);
            break;
        }

		gettimeofday(&tv2, NULL);

        total += frames;
        
		b = buf;
		pd = (double*)(sdr->iq + (sdr->n - sdr->frames));
		for (i = 0; i < sdr->frames; i++){
			*pd = *b / 32767.0;
			if (cfg->sdr_iqcomp){
				avgi += 0.00001 * (*pd - avgi);
				*pd -= avgi;
			}
			pd++, b++;
			*pd = *b / 32767.0;
			if (cfg->sdr_iqcomp){
				avgq += 0.00001 * (*pd - avgq);
				*pd -= avgq;
			}
			pd++, b++;
		}

		sdr_bal_adjust(sdr, sdr->iq, sdr->frames, sdr->bal_gain, sdr->bal_phase);
        
		fftw_execute(sdr->plan_f);
    
#ifdef Z_HAVE_SDL        
        MUTEX_LOCK(sdr->screen);
        if (sdr->screen){
            oldx = -1;
            dmax = 0.0;
            maxgain = -1000000000000.0;
            mingain = 100000000000000.0;
            memset(sdr->wfvec, 0, sdr->wfveclen * sizeof(int));
            
            for (i = 0; i < sdr->n; i++){
                int j = (i + sdr->n / 2) % sdr->n;
                x = (i * (sdr->screen->w - 1)) / sdr->n;
                if (x >= sdr->screen->w) break;
                                
                d = sqrt(sqr(sdr->bins[j][0])+sqr(sdr->bins[j][1]));
                //d = sqr(sdr->bins[j][0]) + sqr(sdr->bins[j][1]);
                d = d/sdr->n;
                d /= M_SQRT2; 
                if (d > dmax) dmax = d;
                // d is epsilon to 1.0
                e = log10(d);
                e -= log10(1.0/130072.0);
                e /= -log10(1.0/130072.0);
                //e -= log10(1.0/32768.0);
                //e /= -log10(1.0/32768.0);
                if (e<0) e = 0;  // slabsi signaly nez 1 bit
                if (i < sdr->amplen) {
                    double gain = (1 - exp(-(0.2 * e)));
                    if (maxgain < gain) maxgain = gain;
                    if (mingain > gain) mingain = gain;
                    if (gain < 0.03) gain = 0.03;
                    if (i == 0) gain = 1.0;

                    sdr->amp[i] = e;
                    sdr->nea_amp[i] = sdr->nea_amp[i] * (1-gain) + e * gain;
                }
                col = (int)(e * (FFT_COLORS - 1.0));
                //col = (int)(sdr->nea_amp[i] * (FFT_COLORS - 1.0));
                if (oldx > x) if (col > sdr->wfvec[x]) sdr->wfvec[x] = col;
            
                for (; oldx <= x; oldx++){
                    if (oldx >= sdr->wfveclen) break;
                    if (col > sdr->wfvec[oldx]) sdr->wfvec[oldx] = col;
                }

                    //z_putpixel(sdr->screen, oldx, sdr->screeny, sdr->pal[col]);
            }   
            for (i = 0; i < sdr->wfveclen; i++) z_putpixel(sdr->screen, i, sdr->screeny, sdr->pal[sdr->wfvec[i]]);
            
            //dbg("maxgain=%f  mingain=%f\n", maxgain, mingain);
            if (tv2.tv_sec != oldsec){ // draw time marks
                int x2 = 3 + (tv2.tv_sec % 10 == 0) * 4;
                oldsec = tv2.tv_sec;
                for (i = 0; i <= x2; i++) {
                    z_putpixel(sdr->screen, i, sdr->screeny, sdl->gr[14]);
                    z_putpixel(sdr->screen, sdr->screen->w - 1 - i, sdr->screeny, sdl->gr[14]);
                }
            }

            sdr->screeny++;
            if (sdr->screeny == sdr->screen->h) sdr->screeny = 0;
        }
        MUTEX_UNLOCK(sdr->screen);
#endif        

		MUTEX_LOCK(sdr->filter);
		for (i = 0; i < sdr->n; i++){
			Z_CMUL(sdr->bins[i], sdr->bins[i], sdr->filter->fbins[i]);
		} 	  
		MUTEX_UNLOCK(sdr->filter);

        fftw_execute(sdr->plan_bi);  // convert I back to time domain

		for (i = 0; i < sdr->n; i++){  // rotate frequencies 90 degrees
			double a = sdr->bins[i][0];
			sdr->bins[i][0] = sdr->bins[i][1];
			sdr->bins[i][1] = -a;
		}
        fftw_execute(sdr->plan_bq);  // convert Q back to time domain

		for (i = 0; i < sdr->n; i++){ // normalize FFT output because FFTW generates unnormalized FFT
			sdr->iout[i][0] /= sdr->n;
			sdr->iout[i][1] /= sdr->n;
			sdr->qout[i][0] /= sdr->n;
			sdr->qout[i][1] /= sdr->n;
		}

		for (i = 0; i < sdr->frames; i++){ // mixer to baseband
			//sdr->iqout[i][0] *= sin(sdr->bfo_phase);
			//sdr->iqout[i][1] *= cos(sdr->bfo_phase);
			sdr->iout[i][0] = sdr->iout[i][0] * cos(sdr->bfo_phase) + sdr->qout[i][0] * sin(sdr->bfo_phase);
			sdr->bfo_phase += sdr->bfo_inc;
		} 

        
		/*oldx = -1;
        for (i = 0; i < sdr->n; i++){
			int j = (i + sdr->n / 2) % sdr->n;
            x = (i * sdr->screen->w) / sdr->n;
            if (x > sdr->screen->w) break;
                            
            d = fabs(sqrt(sqr(sdr->bins[j][0])+sqr(sdr->bins[j][1])));
            d = d/sdr->n;  
            // d is epsilon to 1.0
            e = log10(d);
            e -= log10(1.0/32768.0);
            e /= -log10(1.0/32768.0);
            if (e<0) e = 0;  // slabsi signaly nez 1 bit
			if (i < sdr->amplen) sdr->amp[i] = e;
            col = (int)(e * (FFT_COLORS - 1.0));
            for (; oldx <= x; oldx++)
                z_putpixel(sdr->screen, oldx, sdr->screeny, sdr->pal[col]);

			//if (j == bfob) z_putpixel(sdr->screen, x, sdr->screeny, sdl->red);
        } */   


		// AGC
		for (i = 0; i < sdr->frames; i += sdr->agc_n){
			double max = 0.0, oldgain;
			int n;
			double g; // requested gain
			for (n = 0; n < sdr->agc_n; n++){
				int idx = i + n;
				if (idx >= sdr->frames) break;
				max = Z_MAX(max, fabs(sdr->iout[idx][0]));
			}
			//max = max; why?
			g = 0.5 / max;
			oldgain = sdr->agc_gain;
			if (g < sdr->agc_gain){ // decrease gain
				sdr->agc_gain = g;
				sdr->agc_ign_i = 0;
			}else{				    // increase gain
				if (sdr->agc_ign_i < sdr->agc_ign){
					sdr->agc_ign_i++;
					//dbg("agc_ign_i = %d / %d\n", sdr->agc_ign_i, sdr->agc_ign);
				}else{
					sdr->agc_gain *= 1.01; 
					if (sdr->agc_gain > 64000) sdr->agc_gain = 64000; 
				}	
			}
			
			//dbg("sdr->agc_gain = %6.3f\n", sdr->agc_gain);
			for (n = 0; n < sdr->agc_n; n++){
				int idx = i + n;
				if (idx >= sdr->frames) break;
				g = oldgain + ((double)n / (sdr->agc_n - 1)) * (sdr->agc_gain - oldgain);
				sdr->iout[idx][0] *= g;
				if (sdr->iout[idx][0] > 1.0) sdr->iout[idx][0] = 1.0;
				if (sdr->iout[idx][0] < -1.0) sdr->iout[idx][0] = -1.0;
					//sdr->iout[idx][1] = g / 2;
			}	
			
		}
        
        //dbg("iout\n");
		sb = sdr->sndbuf;
		iq = sdr->iout;// + (sdr->n - sdr->frames);
		for (i = 0; i < sdr->frames / playfactor; i++){
			int k;
			//short val = (short)(32767.0 * sqrt(sqr((*iq)[0] / sdr->n)+sqr((*iq)[1] / sdr->n)));
			short val = (short)(32767.0 * (*iq)[0]);
			//val = (short)(32767.0 * sdr->iq[sdr->n - sdr->frames + i][0]);
			*sb++ = val;
			
			//*sb++ = 32767.0 * sin(sdr->filter->bfo_phase); sdr->filter->bfo_phase += sdr->filter->bfo_inc;
			//*sb++ = (i * 32767) / sdr->frames; // saw
			//*sb++ = (short)((*iq)[1]);
			
			*sb++ = (short)(32767.0 * (*iq)[0]);
			for (k = 0; k < playfactor; k++) iq++;

		}
		//zst_stop(stopky, "sdr_thread_func");

		gettimeofday(&stop, NULL);
		tot = z_difftimeval_double(&stop, &tv1);
		cpu = z_difftimeval_double(&stop, &tv2);
		if (tot > 0.0){
			if (++loadn == 40){
				sdr->load = cpusum * 100.0 / totsum;
	            dbg("sdr->load = %d%%\n", sdr->load);	
				totsum = 0.0;
				cpusum = 0.0;
				loadn = 0;
			}else{
				totsum += tot;
				cpusum += cpu;
			}
		}

		gettimeofday(&tv1, NULL);
		sdr->afdsp->write(sdr->afdsp, sdr->sndbuf, sdr->frames / playfactor);
#ifdef HAVE_SNDFILE
		if (use_pipe){
			gssbd->dsp->write(gssbd->dsp, sdr->sndbuf, sdr->frames / playfactor);
		}
#endif
		zselect_msg_send(zsel, "SDR;D");
    }
    gettimeofday(&stop, NULL);
    tot = z_difftimeval_double(&stop, &tvstart);
    dbg("sdr_thread_func done %5.2f seconds\n", tot);
	sdr->iqdsp->close2(sdr->iqdsp);
	sdr->afdsp->close2(sdr->afdsp);
#ifdef HAVE_SNDFILE
	if (use_pipe) gssbd->dsp->close2(gssbd->dsp);
#endif
	return NULL;
}

void sdr_balance(struct sdr *sdr){
	//if (!sdr->iqcomp) return;

	memcpy(sdr->bal_iq, sdr->iq, sizeof(fftw_complex) * SDR_BAL_N);
	fftw_execute(sdr->bal_plan);

} 

void sdr_bal_adjust(struct sdr *sdr, fftw_complex *iq, int len, double gain, double phase){
	int i;

	for (i = 0; i < len; i++){
		iq[i][0] = iq[i][0] * gain + iq[i][1] * phase;
	}

}

#endif // USE_SDR
