#include "ad9912.h"

 uint32_t ad9912regs[AD9912_COUNT] = {
        0x000018,
        0x000100,
        0x000202,
        0x000309,
        0x000400,
        0x000500,
        0x001010,
        0x001200,
        0x001300,
        0x002012,
        0x002204,
        0x010400,
        0x010500,
        0x010600,
        0x01A685,
        0x01A7EB,
        0x01A851,
        0x01A9B8,
        0x01AA1E,
        0x01AB01,
        0x01AC00,
        0x01AD00,
        0x020005,
        0x020100,
        0x040BFF,
        0x040C01,
        0x040E10,
        0x050000,
        0x050100,
        0x050200,
        0x050300,
        0x050400,
        0x050500,
        0x050600,
        0x050700,
        0x050800,
        0x050900
};

/*-------------------------AD9912 INIT FUNCTION-------------------------*/
void ad9912_init(void *bar1) {
    uint32_t *ptr_rst = bar1 + AD9912_BASE_ADDR;
    *ptr_rst = GPIO_INIT_HEADER;
    //Rst on
    *ptr_rst = AD9912_RST_ON;
    // Rst off
    *ptr_rst = GPIO_REG;
    //Init Header
    uint32_t *ptr = bar1 + AD9912_BASE_ADDR;
    *ptr = InitDDSHeader;
    //Init Data
    for (int k = 0; k < AD9912_COUNT; k++) {
        uint32_t *ptr = bar1 + AD9912_BASE_ADDR;
        *ptr = ad9912regs[k];
    }
}
/*----------------------------------------------------------------------*/

double ad9912_set_main_band(double freq, int lmx_n) {
    double f_pd = freq/lmx_n;

    return f_pd;
}

double ad9912_set_out_of_band(double freq,int lmx_n) {
    double f_pd;

    double f_vco; // VCO frequency
    double vco_div = 7.5e9/freq;
    int chan_div = 2;

    if (vco_div > 2 && vco_div <= 4) {
        chan_div = 4;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 4 && vco_div <= 6) {
        chan_div = 6;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 6 && vco_div <=8) {
        chan_div = 8;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 8 && vco_div <= 12) {
        chan_div = 12;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 12 && vco_div <= 16) {
        chan_div = 16;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 16 && vco_div <= 24) {
        chan_div = 24;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 24 && vco_div <= 32) {
        chan_div = 32;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 32 && vco_div <= 48) {
        chan_div = 48;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 48 && vco_div <= 64) {
        chan_div = 64;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 64 && vco_div <= 72) {
        chan_div = 72;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 72 && vco_div <= 96) {
        chan_div = 96;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 96 && vco_div <= 128) {
        chan_div = 128;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 128 && vco_div <= 192) {
        chan_div = 192;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 192 && vco_div <= 256) {
        chan_div = 256;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 256 && vco_div <= 384) {
        chan_div = 384;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 384 && vco_div <= 512) {
        chan_div = 512;
        f_vco = freq * chan_div;
    }
    else if (vco_div > 512 && vco_div <= 768) {
        chan_div = 768;
        f_vco = freq * chan_div;
    }
}

    f_pd = f_vco/lmx_n;

    return f_pd;


double ad9912_set(void *bar1, double freq) {
    double fs = 1e9;
    int lmx_n = 50;

    double f_pd;
    if (freq >= 7500e6 && freq <= 15000e6) {
        f_pd = ad9912_set_main_band(freq, fs, lmx_n);
    }
    else if (freq >= 10e6 && freq < 7500e6) {
        f_pd = ad9912_set_out_of_band(freq, fs, lmx_n);
    }
    else {
        return -1;
        printf("Frequency is out of range\n");
    }
    printf("f_pd = %f\n", f_pd);
    // FTW word
    double ftw_word = (f_pd*pow(2, 48))/fs;
    // Split the FTW word between 6 registers
    // First one is ftw_word[7:0]
    uint32_t ftw0_7_0 = (uint32_t) ftw_word;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_7_0] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_7_0] & ~BITM_AD9912_FTW0_FREQ_WORD_7_0;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_7_0] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_7_0] | (ftw0_7_0 << BITP_AD9912_FTW0_FREQ_WORD_7_0);
    // Second one is ftw_word[15:8]
    uint32_t ftw0_15_8 = (uint32_t) (ftw_word >> 8);
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_15_8] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_15_8] & ~BITM_AD9912_FTW0_FREQ_WORD_15_8;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_15_8] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_15_8] | (ftw0_15_8 << BITP_AD9912_FTW0_FREQ_WORD_15_8);
    // Third one is ftw_word[23:16]
    uint32_t ftw0_23_16 = (uint32_t) (ftw_word >> 16);
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_23_16] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_23_16] & ~BITM_AD9912_FTW0_FREQ_WORD_23_16;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_23_16] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_23_16] | (ftw0_23_16 << BITP_AD9912_FTW0_FREQ_WORD_23_16);
    // Fourth one is ftw_word[31:24]
    uint32_t ftw0_31_24 = (uint32_t) (ftw_word >> 24);
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_31_24] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_31_24] & ~BITM_AD9912_FTW0_FREQ_WORD_31_24;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_31_24] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_31_24] | (ftw0_31_24 << BITP_AD9912_FTW0_FREQ_WORD_31_24);
    // Fifth one is ftw_word[39:32]
    uint32_t ftw1_39_32 = (uint32_t) (ftw_word >> 32);
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_39_24] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_39_24] & ~BITM_AD9912_FTW0_FREQ_WORD_39_24;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_39_24] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_39_24] | (ftw1_39_32<< BITP_AD9912_FTW0_FREQ_WORD_39_24);
    // Sixth one is ftw_word[47:40]
    uint32_t ftw1_47_40 = (uint32_t) (ftw_word >> 40);
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_47_40] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_47_40] & ~BITM_AD9912_FTW0_FREQ_WORD_47_40;
    ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_47_40] = ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_47_40] | (ftw1_47_40 << BITP_AD9912_FTW0_FREQ_WORD_47_40);
    // Set the frequency

    uint32_t ad9912_ftw_regs[] = {
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_7_0],
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_15_8],
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_23_16],
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_31_24],
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_39_24],
        ad9912regs[REGP_AD9912_FTW0_FREQ_WORD_47_40]
    };

    // Create the appropriate header
    uint32_t *DDS_HEADER = bar1 + AD9912_BASE_ADDR;
    *DDS_HEADER = ((0 << 23) | (DeviceIdDDS << 18) | ((sizeof(ad9912_ftw_regs)/4) << 1) | 1);

    // Write the data
    for (int i = 0; i < sizeof(ad9912_ftw_regs)/4; i++) {
        uint32_t *DDS_DATA = bar1 + AD9912_BASE_ADDR;
        *DDS_DATA = ad9912_ftw_regs[i];
    }

    return f_pd;

}