#include "main.h"
#include "stm32g4xx_hal.h"
#include <math.h>
#include "si5351.h"

uint8_t oeb;

void si5351_write8 (uint8_t reg, uint8_t value){
	while (HAL_I2C_IsDeviceReady(&hi2c1, (uint16_t)(SI5351_ADDRESS<<1), 3, 100) != HAL_OK) { }
	HAL_I2C_Mem_Write(&hi2c1, (uint8_t)(SI5351_ADDRESS<<1), (uint8_t)reg, I2C_MEMADD_SIZE_8BIT, (uint8_t*)(&value), 1, 100);
}

uint8_t si5351_read8(uint8_t reg, uint8_t *value){
	HAL_StatusTypeDef status = HAL_OK;
	while (HAL_I2C_IsDeviceReady(&hi2c1, (uint16_t)(SI5351_ADDRESS<<1), 3, 100) != HAL_OK) { }
	status = HAL_I2C_Mem_Read(&hi2c1,							// i2c handle
    						  (uint8_t)(SI5351_ADDRESS<<1),		// i2c address, left aligned
							  (uint8_t)reg,						// register address
							  I2C_MEMADD_SIZE_8BIT,				// si5351 uses 8bit register addresses
							  (uint8_t*)(&value),				// write returned data to this variable
							  1,								// how many bytes to expect returned
							  100);								// timeout

  return status;
}


void CalcRegisters(uint32_t fout, uint8_t *regs){
//    uint32_t fref = SI5351_CRYSTAL_FREQ;                        // The reference frequency
 
    // Calc Output Multisynth Divider and R with e = 0 and f = 1 => msx_p2 = 0 and msx_p3 = 1
    uint32_t d = 4;
    uint32_t msx_p1 = 0;                         // If fout > 150 MHz then MSx_P1 = 0 and MSx_DIVBY4 = 0xC0, see datasheet 4.1.3
    int msx_divby4 = 0;
    int rx_div = 0;
    int r = 1;
 
    if (fout > 150e6)
        msx_divby4 = 0x0C;                       // MSx_DIVBY4[1:0] = 0b11, see datasheet 4.1.3
    else if (fout < 292969UL)                    // If fout < 500 kHz then use R divider, see datasheet 4.2.2. In reality this means > 292 968,75 Hz when d = 2048
    {
        int rd = 0;
        while ((r < 128) && (r * fout < 292969UL))
        {
            r <<= 1;
            rd++;
        }
        rx_div = rd << 4;

        d = 600e6 / (r * fout);                  // Use lowest VCO frequency but handle d minimum
        if (d % 2)                               // Make d even to reduce spurious and phase noise/jitter, see datasheet 4.1.2.1.
            d++;

        if (d * r * fout < 600e6)                // VCO frequency to low check and maintain an even d value
            d += 2;
    }
    else                                         // 292968 Hz <= fout <= 150 MHz
    {
        d = 600e6 / fout;                        // Use lowest VCO frequency but handle d minimum
        if (d < 6)
            d = 6;
        else if (d % 2)                          // Make d even to reduce phase noise/jitter, see datasheet 4.1.2.1.
           d++;

        if (d * fout < 600e6)                    // VCO frequency to low check and maintain an even d value
            d += 2;
    }
    msx_p1 = 128 * d - 512;

    uint32_t fvco = (uint32_t) d * r * fout;

    // Calc Feedback Multisynth Divider
    double fmd = (double)fvco / SI5351_CRYSTAL_FREQ;            // The FMD value has been found
    int a = fmd;                                 // a is the integer part of the FMD value
 
    double b_c = (double)fmd - a;                // Get b/c
    uint32_t c = 1048575UL;
    uint32_t b = (double)b_c * c;
    if (b > 0)
    {
        c = (double)b / b_c + 0.5;               // Improves frequency precision in some cases
        if (c > 1048575UL)
            c = 1048575UL;
    }

    uint32_t msnx_p1 = 128 * a + 128 * b / c - 512;   // See datasheet 3.2
    uint32_t msnx_p2 = 128 * b - c * (128 * b / c);
    uint32_t msnx_p3 = c;
 
    // Feedback Multisynth Divider registers
    regs[0] = (msnx_p3 >> 8) & 0xFF;
    regs[1] = msnx_p3 & 0xFF;
    regs[2] = (msnx_p1 >> 16) & 0x03;
    regs[3] = (msnx_p1 >> 8) & 0xFF;
    regs[4] = msnx_p1 & 0xFF;
    regs[5] = ((msnx_p3 >> 12) & 0xF0) + ((msnx_p2 >> 16) & 0x0F);
    regs[6] = (msnx_p2 >> 8) & 0xFF;
    regs[7] = msnx_p2 & 0xFF;
 
    // Output Multisynth Divider registers 
    regs[8] = 0;                                  // (msx_p3 >> 8) & 0xFF
    regs[9] = 1;                                  // msx_p3 & 0xFF
    regs[10] = rx_div + msx_divby4 + ((msx_p1 >> 16) & 0x03);
    regs[11] = (msx_p1 >> 8) & 0xFF;
    regs[12] = msx_p1 & 0xFF;
    regs[13] = 0;                                 // ((msx_p3 >> 12) & 0xF0) + (msx_p2 >> 16) & 0x0F
    regs[14] = 0;                                 // (msx_p2 >> 8) & 0xFF
    regs[15] = 0;                                 // msx_p2 & 0xFF

//	HAL_I2C_Master_Transmit(&hi2c2, Si5351_ConfigStruct->HW_I2C_Address, reg_data, sizeof(reg_data), HAL_MAX_DELAY); 
    return;
}

void si5351_initialize(){
	uint8_t dummy;
	// Initialize Si5351A
	while (si5351_read8(0,dummy) & 0x80);    // Wait for Si5351A to initialize
	oeb = 0xFF;

    si5351_write8(SI5351_OUT_ENABLE, oeb);            // Output Enable Control, disable all

    si5351_write8(SI5351_INPUT_SOURCE, 0x00);           // PLL Input Source, select the XTAL input as the reference clock for PLLA and PLLB
    si5351_write8(SI5351_OUT_DIS_STATE, 0x00);		// stato bassa Z giu se disabilitati

    // Output MultisynthN, e = 0, f = 1, MS0_P2 and MSO_P3
    si5351_write8(SI5351_MULTISYNTH0, 0x00);
    si5351_write8(SI5351_MULTISYNTH0+1, 0x01);
    si5351_write8(SI5351_MULTISYNTH0+5, 0x00);
    si5351_write8(SI5351_MULTISYNTH0+6, 0x00);
    si5351_write8(SI5351_MULTISYNTH0+7, 0x00);

    si5351_write8(SI5351_MULTISYNTH1, 0x00);
    si5351_write8(SI5351_MULTISYNTH1+1, 0x01);
    si5351_write8(SI5351_MULTISYNTH1+5, 0x00);
    si5351_write8(SI5351_MULTISYNTH1+6, 0x00);
    si5351_write8(SI5351_MULTISYNTH1+7, 0x00);

    si5351_write8(SI5351_MULTISYNTH2, 0x00);
    si5351_write8(SI5351_MULTISYNTH2+1, 0x01);
    si5351_write8(SI5351_MULTISYNTH2+5, 0x00);
    si5351_write8(SI5351_MULTISYNTH2+6, 0x00);
    si5351_write8(SI5351_MULTISYNTH2+7, 0x00);

    si5351_write8(SI5351_CLK0_CONTROL, 0x4F);   // Power up CLK0, PLLA, MS0 operates in integer mode, Output Clock 0 is not inverted, Select MultiSynth 0 as the source for CLK0 and 8 mA
    si5351_write8(SI5351_CLK1_CONTROL, 0x5F);   // Power up CLK1, PLLA, MS0 operates in integer mode, Output Clock 1 is inverted, Select MultiSynth 1 as the source for CLK1 and 8 mA
    si5351_write8(SI5351_CLK2_CONTROL, 0x6F);   // Power up CLK2, PLLB, int, non inv, multisynth 2, 8 ma

    // Reference load configuration
    si5351_write8(SI5351_CRYSTAL_LOAD, 0x12);          // Set reference load C: 6 pF = 0x12, 8 pF = 0x92, 10 pF = 0xD2
}

void si5351_set_frequency(uint32_t freq, uint8_t pll){
	uint8_t regs[16];
	CalcRegisters(freq, regs);

    // Load Output Multisynth0 with d (e and f already set during init. and never changed)
	if(pll == 0){
		for (int i = 0; i < 8; i++)
			si5351_write8(SI5351_PLLA + i, regs[i]);
		for (int i = 10; i < 13; i++)
	        	si5351_write8(34 + i, regs[i]);
	} else if(pll == 1){
		for (int i = 0; i < 8; i++)
			si5351_write8(SI5351_PLLB + i, regs[i]);
		for (int i = 10; i < 13; i++)
        		si5351_write8(42 + i, regs[i]);
	}

    // Reset PLLA
    // delayMicroseconds(500);            // Allow registers to settle before resetting the PLL
    si5351_write8(SI5351_RESET, 0x20);
}

void si5351_off_clk(uint8_t clk){
	oeb |= 1U << clk;
	si5351_write8(SI5351_OUT_ENABLE, oeb);
}

void si5351_on_clk(uint8_t clk){
	oeb &= ~(1U << clk);
	si5351_write8(SI5351_OUT_ENABLE, oeb);
}
