/*
 * This module contains the main() function for fc-rfcal-vcxo.
 */

#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <endian.h>
#include <rvinterf/l1tm.h>
#include <rvinterf/exitcodes.h>
#include "afcparams.h"
#include "stdband.h"

#define	VCXOCAL_BAND		900
#define	VCXOCAL_BAND_RFPW	RFPW_STD_BAND(6, 0)
#define	VCXOCAL_ARFCN		40

extern float vcxo_freq_meas();

static float freq_max_neg, freq_max_pos;
static float lin_a, lin_b, lin_a2, lin_b2;
static int zero_search_dac1, zero_search_dac2, zero_search_incr;
static float zero_search_freq1, zero_search_freq2, zero_check_freq;
static float dac_min, dac_max, dac_init;
static int dac_init_int;
static float Psi_sta, Psi_st;

struct afcparams afcparams_host, afcparams_arm;

prepare_rf_test_system()
{
	char cmd[80];

	printf("Preparing RF test system for VCXO calibration\n");
	sprintf(cmd, "vcxo-cal-setup %d %d\n", VCXOCAL_BAND, VCXOCAL_ARFCN);
	tsid_command(cmd);
	return(0);
}

main(argc, argv)
	char **argv;
{
	socket_pathname_options(argc, argv);
	connect_rvinterf_socket();
	connect_tsid_socket();
	setlinebuf(stdout);	/* to allow logging with tee */
	prepare_rf_test_system();

	printf("Putting the DUT into Test Mode\n");
	do_tms(1);
	do_rfpw(STD_BAND_FLAG, VCXOCAL_BAND_RFPW);
	do_rfpw(TCH_ARFCN, VCXOCAL_ARFCN);
	do_rfpw(AFC_ENA_FLAG, 0);
	do_txpw(TX_PWR_LEVEL, 12);
	printf("Starting RF Tx on the DUT\n");
	do_rfe(RX_TX_TCH);

	/* initial measurements at the DAC extremes */
	freq_max_neg = vcxo_freq_meas(-2048, "max-neg");
	freq_max_pos = vcxo_freq_meas(2048, "max-pos");
	lin_a = (freq_max_pos - freq_max_neg) / 4096.0f;
	lin_b = freq_max_pos - lin_a * 2048.0f;
	zero_search_dac1 = -lin_b / lin_a;
	zero_search_freq1 = vcxo_freq_meas(zero_search_dac1, "zero-search");

	/* search for zero crossing */
	if (zero_search_freq1 < 0)
		zero_search_incr = 100;
	else
		zero_search_incr = -100;
	for (;;) {
		zero_search_dac2 = zero_search_dac1 + zero_search_incr;
		zero_search_freq2 = vcxo_freq_meas(zero_search_dac2,
						   "zero-search");
		if (zero_search_incr > 0 && zero_search_freq2 >= 0)
			break;
		if (zero_search_incr < 0 && zero_search_freq2 < 0)
			break;
		zero_search_dac1 = zero_search_dac2;
		zero_search_freq1 = zero_search_freq2;
	}

	/* second linear approximation */
	lin_a2 = (zero_search_freq2 - zero_search_freq1) /
		(float)(zero_search_dac2 - zero_search_dac1);
	lin_b2 = zero_search_freq2 - lin_a2 * zero_search_dac2;

	/* DAC settings */
	dac_min = (-13500.0f - lin_b) / lin_a;
	dac_max = (13500.0f - lin_b) / lin_a;
	dac_init = -lin_b2 / lin_a2;
	dac_init_int = (int) dac_init;

	/* check the frequency offset at the final DAC value */
	zero_check_freq = vcxo_freq_meas(dac_init_int, "zero-check");

	/* done with the measurements and the Tx */
	printf("Stopping RF Tx on the DUT\n");
	do_rfe(STOP_ALL);

	/* sanity check */
	if (zero_check_freq < -70.0f || zero_check_freq > 70.0f) {
		printf("Error: final freq offset out of range, aborting\n");
		exit(ERROR_RFFAIL);
	}

	/* Psi computations */
	Psi_sta = 2.0f * (float)M_PI * lin_a / 270833.0f;
	Psi_st = Psi_sta * 0.8f;

	/* compute and fill afcparams */
	afcparams_host.psi_sta_inv = (unsigned)(1.0f / Psi_sta);
	afcparams_host.psi_st = (unsigned)(Psi_st * 65536.0f);
	afcparams_host.psi_st_32 = (unsigned)(Psi_st * 65536.0f * 65536.0f);
	afcparams_host.psi_st_inv = (unsigned)(1.0f / Psi_st);
	afcparams_host.dac_center = (int)(dac_init * 8.0f);
	afcparams_host.dac_min = (int)(dac_min * 8.0f);
	afcparams_host.dac_max = (int)(dac_max * 8.0f);
	afcparams_host.snr_thr = 2560;

	/* print them out */
	printf("afcparams Psi_sta_inv: %u\n", afcparams_host.psi_sta_inv);
	printf("afcparams Psi_st: %u\n", afcparams_host.psi_st);
	printf("afcparams Psi_st_32: %u\n", afcparams_host.psi_st_32);
	printf("afcparams Psi_st_inv: %u\n", afcparams_host.psi_st_inv);
	printf("afcparams DAC_INIT*8: %d\n", afcparams_host.dac_center);
	printf("afcparams DAC_MIN*8: %d\n", afcparams_host.dac_min);
	printf("afcparams DAC_MAX*8: %d\n", afcparams_host.dac_max);
	printf("afcparams snr_thr: %d\n", afcparams_host.snr_thr);

	/* convert to LE for upload to the DUT */
	afcparams_arm.psi_sta_inv = htole32(afcparams_host.psi_sta_inv);
	afcparams_arm.psi_st      = htole32(afcparams_host.psi_st);
	afcparams_arm.psi_st_32   = htole32(afcparams_host.psi_st_32);
	afcparams_arm.psi_st_inv  = htole32(afcparams_host.psi_st_inv);
	afcparams_arm.dac_center  = htole16(afcparams_host.dac_center);
	afcparams_arm.dac_min     = htole16(afcparams_host.dac_min);
	afcparams_arm.dac_max     = htole16(afcparams_host.dac_max);
	afcparams_arm.snr_thr     = htole16(afcparams_host.snr_thr);

	/* send them up */
	printf("Uploading afcparams and INITIAL_AFC_DAC to the DUT\n");
	do_rftw(AFC_PARAMS, &afcparams_arm, sizeof(struct afcparams));
	do_rfpw(INITIAL_AFC_DAC, dac_init_int);
	printf("Saving rf-cal and rf-cfg in FFS\n");
	misc_enable(CFG_WRITE_RF_CAL);
	misc_enable(CFG_WRITE_RF_CFG);

	exit(0);
}