#include "lmx2594.h"
#include <math.h>

const uint32_t lmx2594_rst[] = {
		0x002516,
		0x002514
};
uint32_t lmx2594regs[LMX_COUNT] = {
		0x700000,
		0x6F0000,
		0x6E0000,
		0x6D0000,
		0x6C0000,
		0x6B0000,
		0x6A0000,
		0x690021,
		0x680000,
		0x670000,
		0x660000,
		0x650011,
		0x640000,
		0x630000,
		0x620000,
		0x610888,
		0x600000,
		0x5F0000,
		0x5E0000,
		0x5D0000,
		0x5C0000,
		0x5B0000,
		0x5A0000,
		0x590000,
		0x580000,
		0x570000,
		0x560000,
		0x550000,
		0x540000,
		0x530000,
		0x520000,
		0x510000,
		0x500000,
		0x4F0000,
		0x4E0105,
		0x4D0000,
		0x4C000C,
		0x4B0C40,
		0x4A0000,
		0x49003F,
		0x480001,
		0x470081,
		0x46C350,
		0x450000,
		0x4403E8,
		0x430000,
		0x4201F4,
		0x410000,
		0x401388,
		0x3F0000,
		0x3E0322,
		0x3D00A8,
		0x3C03E8,
		0x3B0001,
		0x3A9001,
		0x390020,
		0x380000,
		0x370000,
		0x360000,
		0x350000,
		0x340820,
		0x330080,
		0x320000,
		0x314180,
		0x300300,
		0x2F0300,
		0x2E07FD,
		0x2DC8DF,
		0x2C1F20,
		0x2B0000,
		0x2A0000,
		0x290000,
		0x280000,
		0x2703E8,
		0x260000,
		0x250104,
		0x240032,
		0x230004,
		0x220000,
		0x211E21,
		0x200393,
		0x1F43EC,
		0x1E318C,
		0x1D318C,
		0x1C0488,
		0x1B0002,
		0x1A0DB0,
		0x190C2B,
		0x18071A,
		0x17007C,
		0x160001,
		0x150401,
		0x14D848,
		0x1327B7,
		0x120064,
		0x110130,
		0x100080,
		0x0F064F,
		0x0E1E40,
		0x0D4000,
		0x0C5001,
		0x0B0018,
		0x0A10D8,
		0x090604,
		0x082000,
		0x0740B2,
		0x06C802,
		0x0500C8,
		0x041443,
		0x030642,
		0x020500,
		0x01080B,
		0x00251C
};

uint32_t lmx_change_freq_regs[12];

struct vco_params calculate_vco_params (double lmx_freq, double f_pd) {
	struct vco_params params;

	if (lmx_freq < 7500e6) {
		params.f_vco = 2 * lmx_freq;
		params.chan_div = 2;
		params.ch_div_reg = 0;
		double vco_div = 7.5e9 / lmx_freq;

		if (params.f_vco < 7.5e9) {
			if (vco_div > 2 && vco_div <= 4) {
				params.chan_div = 4;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 4 && vco_div <= 6) {
				params.chan_div = 6;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 6 && vco_div <= 8) {
				params.chan_div = 8;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 8 && vco_div <= 12) {
				params.chan_div = 12;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 12 && vco_div <= 16) {
				params.chan_div = 16;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 16 && vco_div <= 24) {
				params.chan_div = 24;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 24 && vco_div <= 32) {
				params.chan_div = 32;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 32 && vco_div <= 48) {
				params.chan_div = 48;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 48 && vco_div <= 64) {
				params.chan_div = 64;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 64 && vco_div <= 96) {
				params.chan_div = 96;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 96 && vco_div <= 128) {
				params.chan_div = 128;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 128 && vco_div <= 192) {
				params.chan_div = 192;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 192 && vco_div <= 256) {
				params.chan_div = 256;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 256 && vco_div <= 384) {
				params.chan_div = 384;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 384 && vco_div <= 512) {
				params.chan_div = 512;
				params.f_vco = lmx_freq * params.chan_div;
			}
			else if (vco_div > 512 && vco_div <= 768) {
				params.chan_div = 768;
				params.f_vco = lmx_freq * params.chan_div;
			}
			switch(params.chan_div) {
				case 2:
					params.ch_div_reg = 0;
					break;
				case 4:
					params.ch_div_reg = 1;
					break;
				case 6:
					params.ch_div_reg = 2;
					break;
				case 8:
					params.ch_div_reg = 3;
					break;
				case 12:
					params.ch_div_reg = 4;
					break;
				case 16:
					params.ch_div_reg = 5;
					break;
				case 24:
					params.ch_div_reg = 6;
					break;
				case 32:
					params.ch_div_reg = 7;
					break;
				case 48:
					params.ch_div_reg = 8;
					break;
				case 64:
					params.ch_div_reg = 9;
					break;
				case 96:
					params.ch_div_reg = 10;
					break;
				case 128:
					params.ch_div_reg = 11;
					break;
				case 192:
					params.ch_div_reg = 12;
					break;
				case 256:
					params.ch_div_reg = 13;
					break;
				case 384:
					params.ch_div_reg = 14;
					break;
				case 512:
					params.ch_div_reg = 15;
					break;
				case 768:
					params.ch_div_reg = 16;
					break;
			}
		}
		else {
			params.ch_div_reg = 0;
			params.f_vco = lmx_freq*2;
		}
	}
	else {
		params.f_vco = lmx_freq; 
	}

	  if (params.f_vco >= 7500e6 && params.f_vco <= 8600e6) {
		params.vco_core = 1;
		params.f_coremin = 7500e6;
		params.f_coremax = 8600e6;
		params.c_core_min = 164;
		params.c_core_max = 12;
		params.a_core_min = 299;
		params.a_core_max = 240;
	}
	else if (params.f_vco > 8600e6 && params.f_vco < 9800e6) {
		params.vco_core = 2;
		params.f_coremin = 8600e6;
		params.f_coremax = 9800e6;
		params.c_core_min = 165;
		params.c_core_max = 16;
		params.a_core_min = 356;
		params.a_core_max = 247;
	}
	else if (params.f_vco >= 9800e6 && params.f_vco <= 10800e6) {
		params.vco_core = 3;
		params.f_coremin = 9800e6;
		params.f_coremax = 10800e6;
		params.c_core_min = 158;
		params.c_core_max = 19;
		params.a_core_min = 324;
		params.a_core_max = 224;
	}
	else if (params.f_vco > 10800e6 && params.f_vco <= 12000e6) {
		params.vco_core = 4;
		params.f_coremin = 10800e6;
		params.f_coremax = 12000e6;
		params.c_core_min = 140;
		params.c_core_max = 0;
		params.a_core_min = 383;
		params.a_core_max = 244;
	}
	else if (params.f_vco > 12000e6 && params.f_vco <= 12900e6) {
		params.vco_core = 5;
		params.f_coremin = 12000e6;
		params.f_coremax = 12900e6;
		params.c_core_min = 183;
		params.c_core_max = 36;
		params.a_core_min = 205;
		params.a_core_max = 146;
	}
	else if (params.f_vco > 12900e6 && params.f_vco <= 13900e6) {
		params.vco_core = 6;
		params.f_coremin = 12900e6;
		params.f_coremax = 13900e6;
		params.c_core_min = 155;
		params.c_core_max = 6;
		params.a_core_min = 242;
		params.a_core_max = 163;
	}
	else if (params.f_vco > 13900e6 && params.f_vco <= 15000e6) {
		params.vco_core = 7;
		params.f_coremin = 13900e6;
		params.f_coremax = 15000e6;
		params.c_core_min = 175;
		params.c_core_max = 19;
		params.a_core_min = 323;
		params.a_core_max = 244;
	}

	if (params.f_vco >=11900e6 && params.f_vco <=12100e6) {
		params.vco_daciset_strt = 300;
		params.vco_core = 4;
		params.vco_cap_ctrl_strt = 1;
	}

	params.vco_cap_ctrl_strt = round(params.c_core_min - (params.c_core_min - params.c_core_max) * (params.f_vco - params.f_coremin) / (params.f_coremax - params.f_coremin));
	params.vco_daciset_strt = round(params.a_core_min + (params.a_core_min - params.a_core_max) * (params.f_vco - params.f_coremin) / (params.f_coremax - params.f_coremin));

	if (params.f_vco <= 12500e6) {
		params.pfd_dly_sel = 1;
	}
	else if (params.f_vco > 12500e6) {
		params.pfd_dly_sel = 2;
	}

	if (f_pd <= 100e6) {
		params.fcal_hpfd_adj = ENUM_LMX2594_R0_FCAL_HPFD_ADJ_LESS100MHZ;
	}
	else if (f_pd > 100e6 && f_pd <= 150e6) {
		params.fcal_hpfd_adj = ENUM_LMX2594_R0_FCAL_HPFD_ADJ_100_150MHZ;
	}
	else if (f_pd > 150e6 && f_pd <= 200e6) {
		params.fcal_hpfd_adj = ENUM_LMX2594_R0_FCAL_HPFD_ADJ_150_200MHZ;
	}
	else if (f_pd > 200e6) {
		params.fcal_hpfd_adj = ENUM_LMX2594_R0_FCAL_HPFD_ADJ_MORE200MHZ;
	}

	// SET the CAL_CLK_DIV value
	if (f_pd <= 200e6 ) {
		params.cal_clk_div = ENUM_LMX2594_R1_CAL_CLK_DIV1;
	}
	else if (f_pd > 200e6 && f_pd <= 400e6) {
		params.cal_clk_div = ENUM_LMX2594_R1_CAL_CLK_DIV2;
	}
	else if (f_pd > 400e6 && f_pd < 800e6) {
		params.cal_clk_div = ENUM_LMX2594_R1_CAL_CLK_DIV4;
	}

	// Calculate the ACAL_CMP_DLY
	double fsm_clk = f_pd/(pow(2,params.cal_clk_div));
	params.acal_cmp_dly = (uint8_t) ((uint64_t)round((fsm_clk)/10e6));

	// Calculate the N_div
	params.N_div = round(params.f_vco/f_pd);
	params.N = (uint32_t) params.N_div;
	if (params.f_vco <= 12500e6) {
		if (params.N < 28) {
			params.N = 28;
		}
	}
	else if (params.f_vco > 12500e6) {
		if (params.N < 32) {
			params.N = 32;
		}
	}
    printf("N in lmx: %u\n", params.N);
	return params;
}

void set_vco_params (struct vco_params *params) {
	// Set the VCO_CORE
	lmx2594regs[112 - VCO_SEL] = lmx2594regs[112 - VCO_SEL] & (~BITM_LMX2594_R20_VCO_SEL);
	lmx2594regs[112 - VCO_SEL] = lmx2594regs[112 - VCO_SEL] | (params->vco_core << BITP_LMX2594_R20_VCO_SEL);
	// Set the VCO_CAP_CTRL_START
	lmx2594regs[112 - CAP_CTRL_START] = lmx2594regs[112 - CAP_CTRL_START] & (~BITM_LMX2594_R78_VCO_CAP_CTRL_START);
	lmx2594regs[112 - CAP_CTRL_START] = lmx2594regs[112 - CAP_CTRL_START] | (params->vco_cap_ctrl_strt << BITP_LMX2594_R78_VCO_CAP_CTRL_START);
	// Set the VCO_DACISET
	lmx2594regs[112 - VCO_DACISET] = lmx2594regs[112 - VCO_DACISET] & (~BITM_LMX2594_R17_VCO_DACISET);
	lmx2594regs[112 - VCO_DACISET] = lmx2594regs[112 - VCO_DACISET] | (params->vco_daciset_strt << BITP_LMX2594_R17_VCO_DACISET);
	
	// Set PFD_DLY_SEL 
	lmx2594regs[112-PFD_DLY_SEL] = lmx2594regs[112-PFD_DLY_SEL] & (~BITM_LMX2594_R37_PFD_DLY_SEL);
	lmx2594regs[112-PFD_DLY_SEL] = lmx2594regs[112-PFD_DLY_SEL] | (params->pfd_dly_sel << BITP_LMX2594_R37_PFD_DLY_SEL);

	// Set the FCAL_HPFD_ADJ
	lmx2594regs[112-FCAL_ADDR] = lmx2594regs[112-FCAL_ADDR] & (~BITM_LMX2594_RO_FCAL_HPFD_ADJ);
	lmx2594regs[112-FCAL_ADDR] = lmx2594regs[112-FCAL_ADDR] | params->fcal_hpfd_adj;

	// SET the CAL_CLK_DIV value
	lmx2594regs[112-R1_ADDR] = lmx2594regs[112-R1_ADDR] & (~BITM_LMX2594_R1_CAL_CLK_DIV);
	lmx2594regs[112-R1_ADDR] = lmx2594regs[112-R1_ADDR] | params->cal_clk_div;

	// Set the ACAL_CMP_DLY value
	lmx2594regs[112-R4_ADDR] = lmx2594regs[112-R4_ADDR] & (~BITM_LMX2594_R4_ACAL_CMP_DLY);
	lmx2594regs[112-R4_ADDR] = lmx2594regs[112-R4_ADDR] | (params->acal_cmp_dly << BITP_LMX2594_R4_ACAL_CMP_DLY);

	// Set the N Value
	lmx2594regs[112-PLL_N_S] = lmx2594regs[112-PLL_N_S] & (~0xFFFF);
	lmx2594regs[112-PLL_N_S] = lmx2594regs[112-PLL_N_S] | (params->N >> 16);
	lmx2594regs[112-PLL_N_M] = lmx2594regs[112-PLL_N_M] & (~0xFFFF);
	lmx2594regs[112-PLL_N_M] = lmx2594regs[112-PLL_N_M] | (params->N & 0xFFFF);

	// Set the CHDIV value
	lmx2594regs[112-CHDIV] = lmx2594regs[112-CHDIV] & (~BITM_LMX2594_R75_CHDIV);
	lmx2594regs[112-CHDIV] = lmx2594regs[112-CHDIV] | (params->ch_div_reg << BITP_LMX2594_R75_CHDIV);
	if (params->chan_div > 2) {
		lmx2594regs[112-CHDIV_DIV2] = lmx2594regs[112-CHDIV_DIV2] | BITM_LMX2594_R31_CHDIV_DIV2;
	}
	else {
		lmx2594regs[112-CHDIV_DIV2] = lmx2594regs[112-CHDIV_DIV2] & (~BITM_LMX2594_R31_CHDIV_DIV2);
	}

}
void auto_cal(reg_addr_pci* pci_bar_1) {
	lmx2594regs[112-FCAL_ADDR] = lmx2594regs[112-FCAL_ADDR] & (~BITM_LMX2594_R0_FCAL);
	lmx2594regs[112-FCAL_ADDR] = lmx2594regs[112-FCAL_ADDR] | LMX2594_R0_FCAL_EN;
	
	/* Header for LMX 1MOSI */
	pci_bar_1->sbtmsg_addr = (ENUM_SPIMODE_1MOSI | (DeviceIdLmx2594 << SB_HEADER_1MOSI_DEVICE_ID_BITP) | (0x1 << SB_HEADER_1MOSI_WORD_NUM_BITP) | SB_HEADER_TERM_BIT_1);
	/* Data for LMX 1MOSI */
	pci_bar_1->sbtmsg_addr = lmx2594regs[112 - FCAL_ADDR];
}

void lmx2594_init(reg_addr_pci* pci_bar_1) {
	// Header for LMX Reset
	pci_bar_1->sbtmsg_addr = LMX2594_RST_HEADER;
	// Reset Data
	for (int m = 0; m < (sizeof(lmx2594_rst))/4; m++) {
		pci_bar_1->sbtmsg_addr = lmx2594_rst[m];
	}
	// Header for init data
	pci_bar_1->sbtmsg_addr = INIT_LMX2594_HEADER;
	// Init data
	for (int i = 0; i < LMX_COUNT; i++) {
		pci_bar_1->sbtmsg_addr = lmx2594regs[i];
	}
}

/*-------------------------LMX2594 Frequency Set-------------------------*/
int lmx_freq_set_main_band_int_mode(double lmx_freq, double f_pd) {

	// Partial assist for the calibration
	struct vco_params params = calculate_vco_params(lmx_freq, f_pd);

	// Set the vco params
	set_vco_params(&params);
	// Clear the SEG1_EN bit
	lmx2594regs[112 - CHDIV_DIV2] = lmx2594regs[112 - CHDIV_DIV2] & (~BITM_LMX2594_R31_CHDIV_DIV2);
	// Set the OUTA_MUX to channel divider R45[12:11]; 0 - Channel divider, 1 - VCO;
	lmx2594regs[112 - OUTA_MUX] = lmx2594regs[112 - OUTA_MUX] | ENUM_LMX2594_R45_OUTA_MUX_VCO;
	   // Program the FCAL_EN bit
	lmx2594regs[112 - FCAL_ADDR] = lmx2594regs[112 - FCAL_ADDR] & (~BITM_LMX2594_R0_FCAL);   
	lmx2594regs[112 - FCAL_ADDR] = lmx2594regs[112 - FCAL_ADDR] | (LMX2594_R0_FCAL_EN);

	lmx_change_freq_regs[0] = lmx2594regs[112 - VCO_SEL];
	lmx_change_freq_regs[1] = lmx2594regs[112 - CAP_CTRL_START];
	lmx_change_freq_regs[2] = lmx2594regs[112 - VCO_DACISET];
	lmx_change_freq_regs[3] = lmx2594regs[112 - PFD_DLY_SEL];
	lmx_change_freq_regs[4] = lmx2594regs[112 - R4_ADDR];
	lmx_change_freq_regs[5] = lmx2594regs[112 - R1_ADDR];
	lmx_change_freq_regs[6] = lmx2594regs[112 - PLL_N_S];
	lmx_change_freq_regs[7] = lmx2594regs[112 - PLL_N_M];
	lmx_change_freq_regs[8] = lmx2594regs[112 - CHDIV];
	lmx_change_freq_regs[9] = lmx2594regs[112 - CHDIV_DIV2];
	lmx_change_freq_regs[10] = lmx2594regs[112 - OUTA_MUX];
	lmx_change_freq_regs[11] = lmx2594regs[112 - FCAL_ADDR];

	return 0;
}

int lmx_freq_set_out_of_band_int_mode(double lmx_freq, double f_pd) {
	// Partial assist for the calibration

	struct vco_params params = calculate_vco_params(lmx_freq, f_pd);
	// Set the vco params
	set_vco_params(&params);
	// Set the OUTA_MUX to channel divider R45[12:11]; 0 - Channel divider, 1 - VCO;
	lmx2594regs[112 - OUTA_MUX] = lmx2594regs[112 - OUTA_MUX] & (~BITM_LMX2594_R45_OUTA_MUX);
	lmx2594regs[112 - OUTA_MUX] = lmx2594regs[112 - OUTA_MUX] | ENUM_LMX2594_R45_OUTA_MUX_CH_DIV;

	// Program the FCAL_EN bit
	lmx2594regs[112 - FCAL_ADDR] = lmx2594regs[112 - FCAL_ADDR] & (~LMX2594_R0_FCAL_EN);
	lmx2594regs[112 - FCAL_ADDR] = lmx2594regs[112 - FCAL_ADDR] | (LMX2594_R0_FCAL_EN);

	lmx_change_freq_regs[0] = lmx2594regs[112 - VCO_SEL];
	lmx_change_freq_regs[1] = lmx2594regs[112 - CAP_CTRL_START];
	lmx_change_freq_regs[2] = lmx2594regs[112 - VCO_DACISET];
	lmx_change_freq_regs[3] = lmx2594regs[112 - PFD_DLY_SEL];
	lmx_change_freq_regs[4] = lmx2594regs[112 - R4_ADDR];
	lmx_change_freq_regs[5] = lmx2594regs[112 - R1_ADDR];
	lmx_change_freq_regs[6] = lmx2594regs[112 - PLL_N_S];
	lmx_change_freq_regs[7] = lmx2594regs[112 - PLL_N_M];
	lmx_change_freq_regs[8] = lmx2594regs[112 - CHDIV];
	lmx_change_freq_regs[9] = lmx2594regs[112 - CHDIV_DIV2];
	lmx_change_freq_regs[10] = lmx2594regs[112 - OUTA_MUX];
	lmx_change_freq_regs[11] = lmx2594regs[112 - FCAL_ADDR];

	return 0;
}

double lmx_get_freq(double freq) {
	if (freq < 100e3 || freq > 45e9) {
		printf("Frequency range is 100 kHz to 45 GHz\n");
		return -1;
	}
	if (freq >= 100e3 && freq <= 1000e6) {
		double f_max2870 = 4e9;
		double lmx_freq = f_max2870-freq; // 4 GHz - freq
		return lmx_freq;
	}
	else if (freq > 1000e6 && freq <= 15e9) {
		return freq;
	}
	else if (freq > 15e9 && freq <=30e9) {
		return freq / 2;
	}
	else if (freq > 30e9 && freq <= 45e9) {
		return freq / 4;
	}
	return 0;
}

int lmx_freq_set(reg_addr_pci* pci_bar_1, double lmx_freq,double f_pd) {
	// Set the 4 Mosi mode
	// if the frequency is in the main band - 7.5 GHz to 15 GHz
	if (lmx_freq >= 7.5e9 && lmx_freq <= 15e9) {
		lmx_freq_set_main_band_int_mode(lmx_freq, f_pd);
	}
	else if (lmx_freq < 7.5e9) {
		lmx_freq_set_out_of_band_int_mode(lmx_freq, f_pd);
	}
	return 0;
}

uint32_t lmx_ld_status(reg_addr_pci* pci_bar_1) {	 
	uint32_t read_value = pci_bar_1->sbtmsg_ld_status_addr;    
	return read_value;
}