#include "si5351.h"

uint32_t si5351_freq_a;
uint32_t si5351_freq_b;

void si5351_init(uint32_t freq_xtal, uint32_t freq_a, uint32_t freq_b) {
    si5351_write8(CLK_ENABLE_CONTROL, 0x00); // Enable all outputs
    si5351_write8(XTAL_LOAD_CAP, 0xD2);     //crystal load capacitor = 10pF

    si5351_write8(CLK0_CONTROL, 0x0F);      // PLLA to CLK0, 8 mA output
    si5351_write8(PLL_RESET, 0xA0);         // Reset both PLLs

    si5351_freq_a = freq_a;
    si5351_freq_b = freq_b;

    double fdiv = (double) si5351_freq_a / (double) freq_xtal;

    //TODO: Find better way to determine c and b
    uint32_t c = 0x0FFFFF;
    uint32_t a = (uint32_t) fdiv;
    double rm = fdiv - a;
    uint32_t b = rm * c;
    uint32_t p1  = 128 * a + (128 * b / c) - 512;
    uint32_t p2 = 128 * b - c * (128 * b / c);
    uint32_t p3 = c;
    si5351_write_params(PLL_A, p1, p2, p3);


    fdiv = (double) freq_xtal / (double) si5351_freq_a;

    //TODO: Find better way to determine c and b
    c = 0x0FFFFF;
    a = (uint32_t) fdiv;
    rm = fdiv - a;
    b = rm * c;
    p1  = 128 * a + (128 * b / c) - 512;
    p2 = 128 * b - c * (128 * b / c);
    p3 = c;
    si5351_write_params(PLL_B, p1, p2, p3);
}

void si5351_set_freq(enum si5351_param_base base, uint32_t freq) {
    //TODO: Pick freq by clock source
    double fdiv = (double) freq / (double) si5351_freq_a;

    //TODO: Find better way to determine c and b
    uint32_t c = 0x0FFFFF;
    uint32_t a = (uint32_t) fdiv;
    double rm = fdiv - a;
    uint32_t b = rm * c;
    uint32_t p1  = 128 * a + (128 * b / c) - 512;
    uint32_t p2 = 128 * b - c * (128 * b / c);
    uint32_t p3 = c;
    si5351_write_params(base, p1, p2, p3);
}



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