#include "si5351.h"

uint32_t si5351_pll_freq[2];
uint8_t si5351_multisynth_pll[3];
uint8_t si5351_outputs;

void si5351_init(uint32_t freq_xtal, uint32_t freq_a, uint32_t freq_b) {
    si5351_pll_freq[SI5351_PLLA] = freq_a;
    si5351_pll_freq[SI5351_PLLB] = freq_b;

    si5351_outputs = 0xFF;
    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs); // Disable all outputs
    si5351_write8(XTAL_LOAD_CAP, 0xD2);     //crystal load capacitor = 10pF

    si5351_write8(PLL_RESET, 0xA0);         // Reset both PLLs

    for(uint8_t i = 0; i < 3; i++) {
        si5351_multisynth_pll[i] = 0; // == SI5351_PLLA
        si5351_write8(SI5351_CLK_CTRL[i], 0x0F);  // MSi as Source, PLLA to MSi, 8 mA output
    }

    for(uint8_t i = 0; i < 2; i++) {
        uint32_t fdiv =  si5351_pll_freq[i] /  freq_xtal;
        uint32_t rm =  si5351_pll_freq[i] %  freq_xtal;

        //TODO: Find better way to determine c and b
        uint32_t c = 0x0FFFFF;
        uint32_t a = (uint32_t) fdiv;
        uint32_t b = (uint32_t) ((uint64_t) rm * (uint64_t) c / (uint64_t) freq_xtal);

        struct si5351_params params = {
            128 * a + (128 * b / c) - 512,
            128 * b - c * (128 * b / c),
            c
        };
        si5351_write_params(SI5351_PLL_BASE_ADDR[i], params);
    }
}

void si5351_ms_enable_output(enum si5351_multisynth synth) {
    si5351_outputs &= ~(1 << synth);
    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs);
}

void si5351_ms_disable_output(enum si5351_multisynth synth) {
    si5351_outputs |= (1 << synth);
    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs);
}

void si5351_ms_set_source(enum si5351_multisynth synth, enum si5351_pll pll) {
    uint8_t value = 0x0F;  // MS as Source, PLLA to MS, 8 mA output
    si5351_multisynth_pll[synth] = 0; // == PLLA

    if(pll == SI5351_PLLB) {
        value |= (1 << 5); // Bit 5 set PLLB to MS
        si5351_multisynth_pll[synth] = 1; // == PLLB
    }
    si5351_write8(SI5351_CLK_CTRL[synth], value);
}

void si5351_ms_set_freq(enum si5351_multisynth synth, uint32_t freq) {
    uint32_t fdiv = si5351_pll_freq[si5351_multisynth_pll[synth]] / freq;
    uint32_t rm = si5351_pll_freq[si5351_multisynth_pll[synth]] % freq;

    //TODO: Find better way to determine c and b
    uint32_t c = 0x0FFFFF;
    uint32_t a = fdiv;
    uint32_t b = (uint32_t) ((uint64_t) rm * (uint64_t) c /  freq);

    struct si5351_params params = {
        128 * a + (128 * b / c) - 512,
        128 * b - c * (128 * b / c),
        c
    };
    si5351_write_params(SI5351_MULTISYNTH_BASE[synth], params);
}



uint8_t si5351_write_params(uint8_t base, struct si5351_params params) {
    uint8_t data[9] = {
                    base,
                    (params.p3 & 0x00FF00) >> 8,
                    params.p3 & 0x0000FF,
                    (params.p1 & 0x030000) >> 16,
                    (params.p1 & 0x00FF00) >> 8,
                    params.p1 & 0x0000FF,
                    ((params.p3 & 0x0F0000) >> 12) | ((params.p2 & 0x0F0000) >> 16),
                    (params.p2 & 0x00FF00) >> 8,
                    params.p2 & 0x0000FF
                };
    return twi_write(SI5351_ADDRESS, data, 9);
}