/*
 * biss.c
 *
 *  Created on: 26 δεκ. 2023 γ.
 *      Author: seklyuts
 */
#include "f28x_project.h"
#include "CLB_init.h"
#include "math.h"
#include "biss.h"
#include "CLB/board.h"
#include "crc.h"


#define DEVICE_SYSCLK_FREQ  200000000
#define CLB_CLOCK_FREQ (DEVICE_SYSCLK_FREQ / 2)



#define CLKGEN_MIN_FREQ         ((float) 1.0)
#define CLKGEN_MAX_FREQ         ((float) 10e6)

#define CLKGEN_LOAD_GP_FLAG     ((uint32_t) 0x0001)
#define CLKGEN_RUN_GP_FLAG      ((uint32_t) 0x0002)

uint16_t biss_on = 0;
uint16_t biss_auto = 1;
uint32_t BissBrr = BISS_BR;

void BissInit(void)
{
    EALLOW;

    CpuSysRegs.PCLKCR17.bit.CLB1 = 1;
    CpuSysRegs.PCLKCR17.bit.CLB2 = 1;


    ClkCfgRegs.CLBCLKCTL.bit.CLBCLKDIV = 1;
    ClkCfgRegs.CLBCLKCTL.bit.CLKMODECLB1 = 0;
    ClkCfgRegs.CLBCLKCTL.bit.CLKMODECLB2 = 0;
    ClkCfgRegs.CLBCLKCTL.bit.TILECLKDIV = 0;

//    Clb1LogicCtrlRegs.CLB_OUT_EN |= 1UL << 17UL;
//    Clb1LogicCfgRegs.CLB_MISC_ACCESS_CTRL.bit.BLKEN = 0;
//    Clb1LogicCfgRegs.CLB_MISC_ACCESS_CTRL.bit.SPIEN = 0;
//    Clb1LogicCfgRegs.CLB_SPI_DATA_CTRL_HI.bit.STRB = 0;
//    Clb1LogicCfgRegs.CLB_SPI_DATA_CTRL_HI.bit.SHIFT = 0;

    Board_init();

//      clkgen_setup(16*BISS_C_BITS, 16*BISS_C_BITS, BissBrr, 0);

    BissClkgenSetup(16*BISS_C_BITS, 16*BISS_C_BITS, BissBrr, 0);


//    InputXbarRegs.INPUT1SELECT = 100;
//    ClbInputXbarRegs.INPUT1SELECT =


    EDIS;
}





void BissGpioInit(void)
{
    EALLOW;

    GpioCtrlRegs.GPDMUX1.bit.GPIO96 = 2;//14 = 1110
    GpioCtrlRegs.GPDGMUX1.bit.GPIO96 = 3;
    GpioCtrlRegs.GPDDIR.bit.GPIO96 = 1;
    GpioDataRegs.GPDDAT.bit.GPIO96 = 0;

    GpioCtrlRegs.GPDMUX1.bit.GPIO97 = 2;//14 = 1110
    GpioCtrlRegs.GPDGMUX1.bit.GPIO97 = 3;
    GpioCtrlRegs.GPDDIR.bit.GPIO97 = 1;
    GpioDataRegs.GPDDAT.bit.GPIO97 = 0;

    EDIS;
}


void BissClkgenSetup(unsigned int bits_num_m,
                  unsigned int bits_num_s,
                  float freq_hz,
                  unsigned int phase_us)
{
    const uint32_t half_period_num_m = bits_num_m * 2;
    const uint32_t half_period_num_s = bits_num_s * 2;
    const uint32_t half_period_clb_clocks = ceilf(((float) CLB_CLOCK_FREQ / freq_hz) / 2.0);
    const uint32_t phase_clb_clocks = 1 + ceilf((float) CLB_CLOCK_FREQ * ((float) phase_us * 1e-6));

    const uint32_t push_data_m[4] =
    {
        half_period_clb_clocks - 1,
        half_period_num_m - 1,
        phase_clb_clocks,
        0,
    };

    CLB_writeFIFOs1(push_data_m);
    Clb1LogicCtrlRegs.CLB_GP_REG.all = CLKGEN_LOAD_GP_FLAG;
    DELAY_US(10);
    Clb1LogicCtrlRegs.CLB_GP_REG.all = 0;


    while  (((Clb1LogicCtrlRegs.CLB_BUF_PTR.all >> 16) & 0x03) == 0);


    CLB_clearFIFOs1();

    const uint32_t push_data_s[4] = {
        half_period_clb_clocks - 1,
        half_period_num_s - 1,
        0,
        0
    };

    CLB_writeFIFOs2(push_data_s);

    Clb2LogicCtrlRegs.CLB_GP_REG.all = CLKGEN_LOAD_GP_FLAG;
    DELAY_US(10);
    Clb2LogicCtrlRegs.CLB_GP_REG.all = 0;

    while  (((Clb2LogicCtrlRegs.CLB_BUF_PTR.all >> 16) & 0x03) == 0);

    CLB_clearFIFOs2();
}

uint16_t bissReInit = 0;


void BissClkgenRun(void) {

    if(biss_on)
    {

            biss_on = 0;
            Clb1LogicCtrlRegs.CLB_GP_REG.all = CLKGEN_RUN_GP_FLAG;
            DELAY_US(10);
            Clb1LogicCtrlRegs.CLB_GP_REG.all = 0;

            while ((((Clb1LogicCtrlRegs.CLB_BUF_PTR.all >> 16) & 0x03) == 0) && (((Clb2LogicCtrlRegs.CLB_BUF_PTR.all >> 16) & 0x03) == 0));

            CLB_clearFIFOs1();
            CLB_clearFIFOs2();
    }
}



void BissStartSet(void)
{
    if(biss_auto == 1) {biss_on = 1;}
}

uint64_t BiSStheta = 0;
uint16_t BissCRC = 0, BissCRC_ = 0, BissCRC2_ = 0;
uint16_t BissErr = 0;
uint16_t BissWarn = 0;
uint32_t Bissth1 = 0;
uint16_t Bissth2 = 0;
uint64_t BissAllCRC;

uint16_t BissTestShift1[16];
uint16_t BissErrData = 0;
uint64_t BissAll = 0;
uint32_t WrongCS =0, RightCS = 0;


void BissCalc(uint64_t BissData)
{
    uint16_t BissEmptyBits = 0;

    BissData = BissData << 2;

    while(((BissData & 0xC000000000000000) != 0x8000000000000000) && (BissEmptyBits < 15))
    {
            BissData = BissData << 1;
            BissEmptyBits++;
    }

    BissTestShift1[BissEmptyBits]++;

    if(BissEmptyBits >= 15)
    {
        BissErrData++;
    }
    else
    {
        BissAll = BissData << 2;

        BiSStheta = (BissAll & 0xFFFFFFFFF0000000) >> 28;
        Bissth1 = (BiSStheta & 0x7FFFF);
        Bissth2 = Bissth1 >> 3;
        BissCRC =  (BissAll & 0x0000000003F00000) >> 20;
        BissAllCRC = (BissAll & 0xFFFFFFFFFC000000) >> 26; //38 bit
        BissCRC2_ = crcbitbybitfast(BissAllCRC);
        if(BissCRC2_ != BissCRC)
        {
            WrongCS++;
        }
        else
        {
            RightCS++;
        }
        BissErr =  (BissAll & 0x0000000008000000) >> 27;
        BissWarn = (BissAll & 0x0000000004000000) >> 26;
    }

}