/*
 * th_set_freq
 * Assumes rig!=NULL
 */
int
th_set_freq(RIG *rig, vfo_t vfo, freq_t freq)
{
	char buf[20];
	int step;
	freq_t freq5,freq625,freq_sent;

	rig_debug(RIG_DEBUG_TRACE, "%s: called\n", __func__);

	if (vfo != RIG_VFO_CURR && vfo != rig->state.current_vfo)
		return kenwood_wrong_vfo(__func__, vfo);

	freq5=round(freq/5000)*5000;
	freq625=round(freq/6250)*6250;
	if (abs(freq5-freq)<abs(freq625-freq)) {
	  step=0;
	  freq_sent=freq5;
	}
	else {
	  step=1;
	  freq_sent=freq625;
	}
	/* Step needs to be at least 10kHz on higher band, otherwise 5 kHz */
	step = freq_sent >= MHz(470) ? 4 : step;
	freq_sent = freq_sent >= MHz(470) ? (round(freq_sent/10000)*10000) : freq_sent;
	sprintf(buf, "FQ %011"PRIll",%X", (int64_t) freq_sent, step);

	return kenwood_cmd(rig, buf);
}

//
//	The filter bank for the dongle is the first
//	one
static inline
int16_t	bankFor (int32_t freq) {
	if (freq < 60 * MHz (1))
	   return -1;
	if (freq < 120 * MHz (1))
	   return 1;
	if (freq < 245 * MHz (1))
	   return 2;
	if (freq < 420 * MHz (1))
	   return -1;
	if (freq < 1000 * MHz (1))
	   return 3;
	return -1;
}

//
//	The actual thread does not do much more than reading the
//	values made available through the Mirics API implementation
//	and passing the values on
//
void	sdrplayWorker:: run (void) {
int16_t		*localBuf 	=
	                      (int16_t *)alloca (2 * sps * sizeof (int16_t));
int16_t		*xi		= (int16_t *)alloca (sps * sizeof (int16_t));
int16_t		*xq		= (int16_t *)alloca (sps * sizeof (int16_t));
uint32_t	fs;
int16_t		i;
int32_t		grc, rfc, fsc;
int	err;

	functions -> my_mir_sdr_SetSyncUpdatePeriod ((int)(deviceRate * MHz (1) / 2));
	functions -> my_mir_sdr_SetSyncUpdateSampleNum (sps);
	functions	-> my_mir_sdr_SetParam (102, 1);
	functions	-> my_mir_sdr_SetParam (105, 0);

	while (runnable) {
	   err =  functions ->
	       my_mir_sdr_ReadPacket (&xi [0], & xq [0], &fs, &grc, &rfc, &fsc);

//	currently, we are not interested in the results other than the actual
//	data
	   for (i = 0; i < sps; i ++) {
	      localBuf [2 * i] 		= xi [i];
	      localBuf [2 * i + 1] 	= xq [i];
	   }
	   _I_Buffer	-> putDataIntoBuffer (localBuf, 2 * sps);

	   if (fsc != 0 || rfc != 0 ||grc != 0)
	      fprintf (stderr, "fsc = %d, rfc = %d, grc = %d\n",
	                                        fsc, rfc, grc);
//	OK, data is now stored, now checking for updates
	   while (anyChange != NO_CHANGE) {
	      if (anyChange & FREQ_CHANGE) {
	         functions -> my_mir_sdr_SetRf (lastFrequency, 1, 0);
	         anyChange &= ~FREQ_CHANGE;
	      }
	      if (anyChange & GAIN_CHANGE) {
	         functions -> my_mir_sdr_SetGr (lastGain, 1, 0);
	         anyChange &= ~GAIN_CHANGE;
	      }
	      if (anyChange & RATE_CHANGE) {
	        int r =  functions -> my_mir_sdr_SetFs (deltaRate, 1, 1, 0);
	        if (r == 0)
	           deviceRate = deltaRate / MHz (1);
	         anyChange &= ~RATE_CHANGE;
	      }
	   }
	}
//	fprintf (stderr, "sdrplay worker now stopped\n");
}

	sdrplayWorker::sdrplayWorker (int32_t	deviceRate,
	                              int32_t	bandWidth,
	                              int32_t	defaultFreq,
	                              sdrplayLoader	*f,
	                              RingBuffer<int16_t> *buf,
	                              bool	*OK) {
int	err;

	this	-> deviceRate	= float(deviceRate) / MHz (1);
	this	-> bandWidth	= bandWidth / MHz (1);
	this	-> defaultFreq	= float(defaultFreq) / MHz (1);
	this	-> functions	= f;
	_I_Buffer		= buf;
	*OK			= false;	// just the default

	mir_sdr_Bw_MHzT aa = 
	        bandWidth <= 200 * KHz (1) ? mir_sdr_BW_0_200 :
	        bandWidth <= 300 * KHz (1) ? mir_sdr_BW_0_300 :
	        bandWidth <= 600 * KHz (1) ? mir_sdr_BW_0_600 :
	        bandWidth <= 1536 * KHz (1) ? mir_sdr_BW_1_536 :
	        bandWidth <= 5000 * KHz (1) ? mir_sdr_BW_5_000 :
	        bandWidth <= 6000 * KHz (1) ? mir_sdr_BW_6_000 :
	        bandWidth <= 7000 * KHz (1) ? mir_sdr_BW_7_000 :
	        bandWidth <= 8000 * KHz (1) ? mir_sdr_BW_8_000 :
	              mir_sdr_BW_8_000;
	        
//	Note: the "API check" has been done by the owner of this thread
	err			= functions -> my_mir_sdr_Init (40,
	                                            this -> deviceRate,
	                                            this -> defaultFreq,
	                                            aa,
	                                            mir_sdr_IF_Zero,
	                                            &sps);
	if (err != 0) {
	   fprintf (stderr, "Probleem init\n");
	   return;
	}

	err			= functions -> my_mir_sdr_SetDcMode (4, 1);
	err			= functions -> my_mir_sdr_SetDcTrackTime (63);
//
//	some defaults:
	lastFrequency		= defaultFreq;	// the parameter!!!!
	lastGain		= 25;		// dummy
	runnable		= true;
	anyChange		= 0;
	start ();
	*OK			= true;
}

/* --------------------------------------------------------------------- */
int thg71_open(RIG *rig)
{
    char ackbuf[ACKBUF_LEN],*strl,*stru;
    int retval,i;
    const freq_range_t frend=RIG_FRNG_END;

    /* this will check the model id */
    retval = kenwood_open(rig);
    if (retval != RIG_OK)
        return retval;

	/* fill state.rx/tx range_list */
    retval = kenwood_transaction(rig, "FL", ackbuf, sizeof (ackbuf));
        if (retval != RIG_OK)
        	return retval;

    strl=strtok(ackbuf," ");
    for(i=0;i<FRQRANGESIZ;i++) {
		freq_range_t frng;

	    	strl=strtok(NULL,",");
	    	stru=strtok(NULL,",");
		if(strl==NULL && stru==NULL)
			break;
		frng.start=MHz(atoi(strl));
		frng.end=MHz(atoi(stru));
		frng.vfo=RIG_VFO_A;
		frng.ant=0;
		if(frng.end<=MHz(135))
			frng.modes=RIG_MODE_AM;
		else
			frng.modes=RIG_MODE_FM;

		frng.high_power=-1;
		frng.low_power=-1;
		rig->state.rx_range_list[i]=frng;
		if(frng.start> MHz(200))
			frng.high_power=5.5;
		else
			frng.high_power=6;
		frng.low_power=mW(50);
		rig->state.tx_range_list[i]=frng;
	}
	rig->state.rx_range_list[i]= frend;
	rig->state.tx_range_list[i]= frend;
	rig->state.vfo_list=RIG_VFO_A | RIG_VFO_MEM ;

    return RIG_OK;
}

/*
 * ar3k_set_freq
 * Assumes rig!=NULL
 */
int ar3k_set_freq(RIG *rig, vfo_t vfo, freq_t freq)
{
	char freqbuf[BUFSZ];
	int freq_len, retval;
	unsigned lowhz;

	/*
	 * actually, frequency must be like nnnn.nnnnm,
	 * where m must be 0 or 5 (for 50Hz).
	 */
	lowhz = ((unsigned)freq) % 100;
	freq /= 100;
	if (lowhz < 25)
		lowhz = 0;
	else if (lowhz < 75)
		lowhz = 50;
	else
		lowhz = 100;
	freq = freq*100 + lowhz;

	freq_len = sprintf(freqbuf,"%04.5f" EOM, ((double)freq)/MHz(1));

	retval = ar3k_transaction (rig, freqbuf, freq_len, NULL, NULL);
	if (retval != RIG_OK)
		return retval;

	return RIG_OK;
}

/*
 * tentec_init:
 * Basically, it just sets up *priv
 */
int tentec_init(RIG *rig)
{
	struct tentec_priv_data *priv;

	priv = (struct tentec_priv_data*)malloc(sizeof(struct tentec_priv_data));

	if (!priv) {
				/* whoops! memory shortage! */
		return -RIG_ENOMEM;
	}

	memset(priv, 0, sizeof(struct tentec_priv_data));

	/*
	 * set arbitrary initial status
	 */
	priv->freq = MHz(10);
	priv->mode = RIG_MODE_AM;
	priv->width = kHz(6);
	priv->pbt = 0;
	priv->cwbfo = 1000;
	priv->agc = RIG_AGC_MEDIUM;	/* medium */
	priv->lnvol = priv->spkvol = 0.0;	/* mute */

	rig->state.priv = (rig_ptr_t)priv;

	/* tentec_tuning_factor_calc needs rig->state.priv */
	tentec_tuning_factor_calc(rig);

	return RIG_OK;
}

/* --------------------------------------------------------------------- */
int thg71_get_mode (RIG *rig, vfo_t vfo, rmode_t *mode, pbwidth_t *width)
{
    char ackbuf[ACKBUF_LEN];
    int retval;
    int step;
    freq_t freq;

    rig_debug(RIG_DEBUG_TRACE, "%s: called\n", __func__);

	switch (vfo) {
      case RIG_VFO_CURR: break;
      case RIG_VFO_A: break;
	  default:
        rig_debug(RIG_DEBUG_ERR, "%s: Unsupported VFO %d\n", __func__, vfo);
		return -RIG_EVFO;
	}

	/* try to guess from frequency */
  retval = kenwood_transaction(rig, "FQ", ackbuf, sizeof (ackbuf));
        if (retval != RIG_OK)
        return retval;

    sscanf(ackbuf,"FQ %"SCNfreq",%d",&freq,&step);

    if(freq <MHz(136) )  {
		*mode=RIG_MODE_AM;
		*width=kHz(9);
	} else {
		*mode=RIG_MODE_FM;
		*width=kHz(12);
	}

	return RIG_OK;
}

/*
 * th_set_rptr_offs
 * Assumes rig!=NULL
 */
int
tmd710_set_rptr_offs(RIG *rig, vfo_t vfo, shortfreq_t freq)
{
  int retval;
  tmd710_fo fo_struct;
  long freq5,freq625,freq_sent;
  
  rig_debug(RIG_DEBUG_TRACE, "%s: called\n", __func__);
  
  // if (vfo != RIG_VFO_CURR && vfo != rig->state.current_vfo)
  //   return kenwood_wrong_vfo(__func__, vfo);
  
  retval = tmd710_pull_fo (rig,vfo,&fo_struct);
  if (retval != RIG_OK){
     return retval;
  }

  freq5=round(freq/5000)*5000;
  freq625=round(freq/6250)*6250;
  if (abs(freq5-freq)<abs(freq625-freq)) {
    freq_sent=freq5;
  }
  else {
    freq_sent=freq625;
  }
  /* Step needs to be at least 10kHz on higher band, otherwise 5 kHz */
  fo_struct.offset = freq_sent >= MHz(470) ? (round(freq_sent/10000)*10000) : freq_sent;
  
  retval=tmd710_push_fo (rig, vfo, &fo_struct);
  
  return retval;
}

int icmarine_set_tx_freq(RIG *rig, vfo_t vfo, freq_t freq)
{
	char freqbuf[BUFSZ];

	sprintf(freqbuf, "%.6f", freq/MHz(1));

	return icmarine_transaction (rig, CMD_TXFREQ, freqbuf, NULL);
}

static void set_memory_clock(unsigned int frequency)
{
	unsigned int reg, divisor;

	/*
	 * Cheok_0509: For SM750LE, the memory clock is fixed.
	 * Nothing to set.
	 */
	if (sm750_get_chip_type() == SM750LE)
		return;

	if (frequency) {
		/*
		 * Set the frequency to the maximum frequency
		 * that the DDR Memory can take which is 336MHz.
		 */
		if (frequency > MHz(336))
			frequency = MHz(336);

		/* Calculate the divisor */
		divisor = DIV_ROUND_CLOSEST(get_mxclk_freq(), frequency);

		/* Set the corresponding divisor in the register. */
		reg = peek32(CURRENT_GATE) & ~CURRENT_GATE_M2XCLK_MASK;
		switch (divisor) {
		default:
		case 1:
			reg |= CURRENT_GATE_M2XCLK_DIV_1;
			break;
		case 2:
			reg |= CURRENT_GATE_M2XCLK_DIV_2;
			break;
		case 3:
			reg |= CURRENT_GATE_M2XCLK_DIV_3;
			break;
		case 4:
			reg |= CURRENT_GATE_M2XCLK_DIV_4;
			break;
		}

		sm750_set_current_gate(reg);
	}
}

unsigned int getChipClock()
{
    pll_value_t pll;
#if 1
	if(getChipType() == SM750LE)
		return MHz(130);
#endif

    return getPllValue(MXCLK_PLL, &pll);
}

/*
 * racal_set_freq
 * Assumes rig!=NULL
 */
int racal_set_freq(RIG *rig, vfo_t vfo, freq_t freq)
{
	char freqbuf[BUFSZ];
	int freq_len;

	freq_len = sprintf(freqbuf, "F%0g", (double)(freq/MHz(1)));
	if (freq_len < 0)
		return -RIG_ETRUNC;

	return racal_transaction (rig, freqbuf, NULL, NULL);
}

/*
 * This function set up the master clock (MCLK).
 *
 * Input: Frequency to be set.
 *
 * NOTE:
 *      The maximum frequency the engine can run is 168MHz.
 */
void setMasterClock(unsigned int frequency)
{
    unsigned int ulReg, divisor;
  #if 1
	/* Cheok_0509: For SM750LE, the memory clock is fixed. Nothing to set. */
	if (getChipType() == SM750LE)
		return;
#endif
    if (frequency != 0)
    {
        /* Set the frequency to the maximum frequency that the SM750 engine can
           run, which is about 190 MHz. */
        if (frequency > MHz(190))
            frequency = MHz(190);

        /* Calculate the divisor */
        divisor = (unsigned int) roundedDiv(getChipClock(), frequency);

        /* Set the corresponding divisor in the register. */
        ulReg = PEEK32(CURRENT_GATE);
        switch(divisor)
        {
            default:
            case 3:
                ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_3);
                break;
            case 4:
                ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_4);
                break;
            case 6:
                ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_6);
                break;
            case 8:
                ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_8);
                break;
        }

        setCurrentGate(ulReg);
    }
}

static void setMemoryClock(unsigned int frequency)
{
	unsigned int reg, divisor;

	/* Cheok_0509: For SM750LE, the memory clock is fixed. Nothing to set. */
	if (getChipType() == SM750LE)
		return;

	if (frequency) {
		/* Set the frequency to the maximum frequency that the DDR Memory can take
		which is 336MHz. */
		if (frequency > MHz(336))
			frequency = MHz(336);

		/* Calculate the divisor */
		divisor = roundedDiv(get_mxclk_freq(), frequency);

		/* Set the corresponding divisor in the register. */
		reg = PEEK32(CURRENT_GATE) & ~CURRENT_GATE_M2XCLK_MASK;
		switch (divisor) {
		default:
		case 1:
			reg |= CURRENT_GATE_M2XCLK_DIV_1;
			break;
		case 2:
			reg |= CURRENT_GATE_M2XCLK_DIV_2;
			break;
		case 3:
			reg |= CURRENT_GATE_M2XCLK_DIV_3;
			break;
		case 4:
			reg |= CURRENT_GATE_M2XCLK_DIV_4;
			break;
		}

		setCurrentGate(reg);
	}
}

/*
 * This function set up the master clock (MCLK).
 *
 * Input: Frequency to be set.
 *
 * NOTE:
 *      The maximum frequency the engine can run is 168MHz.
 */
static void setMasterClock(unsigned int frequency)
{
	unsigned int reg, divisor;

	/* Cheok_0509: For SM750LE, the memory clock is fixed. Nothing to set. */
	if (getChipType() == SM750LE)
		return;

	if (frequency) {
		/* Set the frequency to the maximum frequency that the SM750 engine can
		run, which is about 190 MHz. */
		if (frequency > MHz(190))
			frequency = MHz(190);

		/* Calculate the divisor */
		divisor = roundedDiv(get_mxclk_freq(), frequency);

		/* Set the corresponding divisor in the register. */
		reg = PEEK32(CURRENT_GATE) & ~CURRENT_GATE_MCLK_MASK;
		switch (divisor) {
		default:
		case 3:
			reg |= CURRENT_GATE_MCLK_DIV_3;
			break;
		case 4:
			reg |= CURRENT_GATE_MCLK_DIV_4;
			break;
		case 6:
			reg |= CURRENT_GATE_MCLK_DIV_6;
			break;
		case 8:
			reg |= CURRENT_GATE_MCLK_DIV_8;
			break;
		}

		setCurrentGate(reg);
		}
}

int icmarine_set_freq(RIG *rig, vfo_t vfo, freq_t freq)
{
	char freqbuf[BUFSZ];
	struct icmarine_priv_data *priv;

	priv = (struct icmarine_priv_data*)rig->state.priv;

	sprintf(freqbuf, "%.6f", freq/MHz(1));

	/* no error reporting upon TXFREQ failure */
	if (RIG_SPLIT_OFF == priv->split)
		icmarine_transaction (rig, CMD_TXFREQ, freqbuf, NULL);

	return icmarine_transaction (rig, CMD_RXFREQ, freqbuf, NULL);
}

MHz calculate_cpu_frequency(){
  
  // We expect the cpu_sampling_irq_handler to push in samples;
  while (_cpu_timestamps.size() < do_samples_)
    OS::halt();
      
  debug("_cpu_sampling_freq_divider_ : %i \n",_cpu_sampling_freq_divider_);
  
  #ifdef DEBUG
  for (auto t : _cpu_timestamps) debug("%lu \n",(uint32_t)t);
  #endif
  
  // Subtract the time it takes to measure time :-)
  auto t1 = OS::cycles_since_boot();
  OS::cycles_since_boot();
  auto t3 = OS::cycles_since_boot();
  auto overhead = (t3 - t1) * 2;
  
  debug ("Overhead: %lu \n", (uint32_t)overhead);
  
  for (size_t i = 1; i < _cpu_timestamps.size(); i++){
    // Compute delta in cycles
    auto cycles = _cpu_timestamps[i] - _cpu_timestamps[i-1] + overhead;
    // Cycles pr. second == Hertz
    auto freq = cycles / (1 / test_frequency().count());
    _cpu_freq_samples.push_back(freq);    
    debug("%lu - %lu = Delta: %lu Current PIT-Freq: %f Hz CPU Freq: %f MHz \n",
	  (uint32_t)_cpu_timestamps[i], (uint32_t)_cpu_timestamps[i-1],
	  (uint32_t)cycles, Hz(test_frequency()), freq);        
  }
  
  
#ifdef DEBUG
  double sum = 0;  
  for (auto freq : _cpu_freq_samples)
    sum += freq;  
  double mean = sum / _cpu_freq_samples.size();
#endif
  
  std::sort(_cpu_freq_samples.begin(), _cpu_freq_samples.end());
  double median = _cpu_freq_samples[_cpu_freq_samples.size() / 2];
  
  debug("<cpu_freq> MEAN: %f MEDIAN: %f \n",mean, median);
  _CPUFreq_ = median;
  
  return MHz(median);
  
}

static unsigned int get_mxclk_freq(void)
{
	unsigned int pll_reg;
	unsigned int M, N, OD, POD;

	if (getChipType() == SM750LE)
		return MHz(130);

	pll_reg = PEEK32(MXCLK_PLL_CTRL);
	M = (pll_reg & PLL_CTRL_M_MASK) >> PLL_CTRL_M_SHIFT;
	N = (pll_reg & PLL_CTRL_N_MASK) >> PLL_CTRL_M_SHIFT;
	OD = (pll_reg & PLL_CTRL_OD_MASK) >> PLL_CTRL_OD_SHIFT;
	POD = (pll_reg & PLL_CTRL_POD_MASK) >> PLL_CTRL_POD_SHIFT;

	return DEFAULT_INPUT_CLOCK * M / N / (1 << OD) / (1 << POD);
}

int racal_get_freq(RIG *rig, vfo_t vfo, freq_t *freq)
{
	char freqbuf[BUFSZ];
	int retval, len;
	double f;

	retval = racal_transaction (rig, "TF", freqbuf, &len);
	if (retval < RIG_OK)
		return retval;

	if (len < 2 || freqbuf[0] != 'F')
		return -RIG_EPROTO;

	sscanf(freqbuf+1, "%lf", &f);
	*freq = (freq_t)f * MHz(1);

	return RIG_OK;
}