#define _XOPEN_SOURCE 500
#include <stdio.h>
#include <unistd.h>
#include "libwr2rf/board.h"
#include "libwr2rf/api.h"
#include "wr2rf_rfnco_regs.h"
#include "regs.h"

struct pll_reg_cfg {
  unsigned char reg;
  unsigned char val;
};

static const struct pll_reg_cfg pll_cfg[] = {
#include "ltc6950_config.h"
};

static void
libwr2rf_ltc6950_init (struct libwr2rf_dev *dev)
{
        unsigned i;

        /* Full reset ?  */

        /* Set registers.  */
        for (i = 0; i < sizeof(pll_cfg)/sizeof(pll_cfg[0]); i++) {
                const struct pll_reg_cfg *cfg = &pll_cfg[i];
                libwr2rf_pll_write(dev, cfg->reg, cfg->val);
        }

        /* Wait until locked.  */
        while (1) {
                unsigned v = libwr2rf_pll_read(dev, 0);
                if (v & 0x4)
                        break;
        }
}

/* Select the DDS clock as clock source.  */
static void
libwr2rf_clock_select_dds (struct libwr2rf_dev *dev)
{
        unsigned mmcm_locked      = 0;
        unsigned mmcm_retries     = 0;
        unsigned mmcm_max_retries = 100;
        unsigned ctrl;


        // take sys clock mmcm out of reset + then switch bufg mux select from dmtd to ltc6950
        libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_CLOCK_CTRL, 0);

        // Wait for FPGA MMCM to lock...
        while (! mmcm_locked) {
                unsigned mmcm_status = libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_CLOCK_STATUS);
                mmcm_locked = !!(mmcm_status & WR2RF_INIT_REGS_CLOCK_STATUS_MMCM_LOCKED);
                if (mmcm_retries > mmcm_max_retries) {
                        printf("MMCM not locking on LTC6950 FPGA sys clock. \n");
                        return;
                }
                mmcm_retries++;
        }

        libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_CLOCK_CTRL, WR2RF_INIT_REGS_CLOCK_CTRL_CLK_SEL);
        ctrl = libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_CLOCK_CTRL);

        printf("SysClockCtrl=%04x\n", ctrl);
}

static void
libwr2rf_wrcore_init(struct libwr2rf_dev *dev)
{
        /* Take the wrcore module out of reset.
           This is necessary to connect it on the bus.  */
        libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRCORE_CTRL,
                         WR2RF_INIT_REGS_WRCORE_CTRL_RESET_N);
}

void
libwr2rf_iodelay_init(struct libwr2rf_dev *dev)
{
    /* Apply a reset sequence to the iodelayctrl units */
    unsigned iodelay_rst;
    unsigned iodelay_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_IODELAY_CTRL;

    iodelay_rst = libwr2rf_read16(dev, iodelay_addr);
    iodelay_rst = !!(iodelay_rst & WR2RF_INIT_REGS_IODELAY_CTRL_RESET);

    /* Take iodelay out of reset */
    if (iodelay_rst) {
            libwr2rf_write16(dev, iodelay_addr, 0);
    }

    /* Assert reset for > 50 ns, timing here is ~ 0.5 us, 35 cycles x 16 ns  */
    libwr2rf_write16(dev, iodelay_addr, WR2RF_INIT_REGS_WRCORE_CTRL_RESET_N);
    libwr2rf_write16(dev, iodelay_addr, 0);
}

void
libwr2rf_ltc6950_sync (struct libwr2rf_dev *dev)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_PLL_CTRL;
    unsigned val  = libwr2rf_read16(dev, addr);

    // provide a signal > 1ms to the ltc6950
    val |= WR2RF_INIT_REGS_PLL_CTRL_SYNC;
    libwr2rf_write16(dev, addr, val);
    usleep(2000);
    val &= ~WR2RF_INIT_REGS_PLL_CTRL_SYNC;
    libwr2rf_write16(dev, addr, val);
}

void
libwr2rf_clock_init (struct libwr2rf_dev *dev)
{
    unsigned ctrl;
    /* Note: The OCXO must have been enabled.  */

    /* we have to check if the PLL has already been initialised and the clock select set */
    ctrl  = libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_CLOCK_CTRL);
    if ((ctrl & WR2RF_INIT_REGS_CLOCK_CTRL_CLK_SEL) == 0)
    {
        /* 1. the PLL (ltc6950).  */
        /* 1.1 the SPI control for the PLL.  */
        libwr2rf_pll_spi_init(dev);

        /* 1.2 the PLL itself.  */
        libwr2rf_ltc6950_init (dev);
        /* 1.3 sync the PLL */
        libwr2rf_ltc6950_sync (dev);

        /* 2. FPGA clock.  */
        libwr2rf_clock_select_dds (dev);

        /* 5. WR-Core */
        libwr2rf_wrcore_init(dev);

        /* 6. IOdelays */
        libwr2rf_iodelay_init(dev);
    }
}

static int
libwr2rf_nco_wrcfg (struct libwr2rf_dev *dev, unsigned ch)
{
  unsigned base;

  switch (ch) {
  case 1:
    base = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO;
    break;
  case 2:
    base = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO;
    break;
  default:
    return LIBWR2RF_ERROR_BAD_ID;
  }

  uint64_t LO0 = 0x1C9BA5E300000;//223.5 MHz
  uint64_t LO1 = 0x1C9BA5E300000;//223.5 MHz
  unsigned harmonic = 4620;
  unsigned control  = RFNCO_CONTROL_SELFTWH1 | RFNCO_CONTROL_EXTRESETENABLE | RFNCO_CONTROL_SELLOAD | RFNCO_CONTROL_SELWRFTWH1;

  libwr2rf_16x32_write64(dev, base + RFNCO_FTW_LO0, LO0);
  libwr2rf_16x32_write64(dev, base + RFNCO_FTW_LO1, LO1);

  libwr2rf_16x32_write32(dev, base + RFNCO_INPUTS_H, harmonic);
  libwr2rf_16x32_write32(dev, base + RFNCO_CONTROL, control);

  return 0;
}

static int
libwr2rf_rf_init_dds (struct libwr2rf_dev *dev)
{
    int res;

    /* 3. DDS (ad9910).  */
    //libwr2rf_dds_spi_init(dev);
    libwr2rf_dds_reset_pulse(dev);
    res = libwr2rf_dds_init(dev, 1);
    if (res != 0)
      return res;

    usleep(1000);
    libwr2rf_dds_sync_calibrate(dev, 0);
    return 0;
}

static void
libwr2rf_rf_init_dac (struct libwr2rf_dev *dev)
{
  /* 4. RF DAC.  */
  /* 4.1 Init registers.  */
  libwr2rf_dac_init(dev, DAC1_CS);
  libwr2rf_dac_init(dev, DAC2_CS);
  /* 4.2 Set timing for inputs.  */
  libwr2rf_dac_port_timing(dev, DAC1_CS);
  libwr2rf_dac_port_timing(dev, DAC2_CS);

    /* 7.  Configure input of IQ DAC (use rfnco).  */
    libwr2rf_write16
      (dev, WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_CTRL, 0x04);
    libwr2rf_write16
      (dev, WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_CTRL, 0x04);
}

static void
libwr2rf_rf_init_rfnco (struct libwr2rf_dev *dev)
{
    /* Use network as RFNCO source.  */
    libwr2rf_write16
        (dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_NCO_LOC_OR_WRS, 0);

    usleep(100);
    /* Configure rfnco. */
    libwr2rf_nco_wrcfg (dev, 1);
    libwr2rf_nco_wrcfg (dev, 2);
}

int
libwr2rf_rf_init (struct libwr2rf_dev *dev)
{
  int res;

  res = libwr2rf_rf_init_dds (dev);
  if (res != 0)
    return res;
  libwr2rf_rf_init_dac (dev);
  libwr2rf_rf_init_rfnco (dev);
  return 0;
}

int
libwr2rf_api_init (struct libwr2rf_dev *dev)
{
  int res;

  /* Command iodelay-init */
  libwr2rf_iodelay_init(dev);

  /* Command dac-iqsetpoint */
  libwr2rf_dac_reset_phase_amp(dev, 1, 0.5, 0.0);
  libwr2rf_dac_reset_phase_amp(dev, 2, 0.5, 0.0);

  /* Commands:
     dds-init  (TODO: io_update)
     dds-sync-cal
     dac-init
     dac-timing
     dac-iqctrl
     nco-source
     nco-wrcfg
  */
  res = libwr2rf_rf_init(dev);
  if (res != 0)
    return res;

  /* Configure RF outputs.
     Command set-rfout
  */
  libwr2rf_set_rf_out(dev, 1, LIBWR2RF_RF_OUT_MIXER);
  libwr2rf_set_rf_out(dev, 2, LIBWR2RF_RF_OUT_MIXER);

  /* Command wrs-fixedlat */
  libwr2rf_set_wrs_fixed_latency (dev, 1000);

  /* Command wrs-tolat */
  libwr2rf_set_wrs_timeout_latency (dev, 2000);

  /* Command nco-reset-delay */
  libwr2rf_vtu_nco_reset_delay(dev, LIBWR2RF_RF1_CHANNEL_ID, 75, 5, 0xa);
  libwr2rf_vtu_nco_reset_delay(dev, LIBWR2RF_RF2_CHANNEL_ID, 75, 4, 0x14);

  /* Set the rf reset offset delay wrt nco_reset_cdelayed */
  libwr2rf_vtu_rf_reset_offset(dev, LIBWR2RF_RF1_CHANNEL_ID, 10);
  libwr2rf_vtu_rf_reset_offset(dev, LIBWR2RF_RF2_CHANNEL_ID, 10);

  /* Initialise the trigger unit flip-flop delay lines */
  libwr2rf_vtu_output_ff_delay(dev, LIBWR2RF_TRIG_ID_RF_1_TRIG_1, 0);
  libwr2rf_vtu_output_ff_delay(dev, LIBWR2RF_TRIG_ID_RF_1_TRIG_2, 0);
  libwr2rf_vtu_output_ff_delay(dev, LIBWR2RF_TRIG_ID_RF_2_TRIG_1, 0);
  libwr2rf_vtu_output_ff_delay(dev, LIBWR2RF_TRIG_ID_RF_2_TRIG_2, 0);

  return 0;
}

void
libwr2rf_set_wrs_fixed_latency (struct libwr2rf_dev *dev, unsigned wr_cycles)
{
    unsigned cfg5_addr = 0x74; // STREAMERS_RX_CFG5 - fixed latency, # of 8ns clock cycles
    unsigned cfg_addr  = 0x78; // STREAMERS_CFG for accessing Tx/Rx overide

    libwr2rf_be_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_PAGE0, 0x48000);
    libwr2rf_wrc_write32(dev, cfg5_addr, wr_cycles);
    libwr2rf_wrc_write32(dev, cfg_addr, (1 << 21));
}

unsigned
libwr2rf_get_wrs_fixed_latency (struct libwr2rf_dev *dev)
{
    unsigned cfg5_addr = 0x74; // STREAMERS_RX_CFG5 - fixed latency, # of 8ns clock cycles

    libwr2rf_be_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_PAGE0, 0x48000);
    return libwr2rf_wrc_read32(dev, cfg5_addr);
}

void
libwr2rf_set_wrs_timeout_latency (struct libwr2rf_dev *dev, unsigned wr_cycles)
{
    unsigned cfg6_addr = 0xa4; // STREAMERS_RX_CFG6 - fixed latency timeout
    unsigned cfg_addr  = 0x78; // STREAMERS_CFG for accessing Tx/Rx overide

    libwr2rf_be_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_PAGE0, 0x48000);
    libwr2rf_wrc_write32(dev, cfg6_addr, wr_cycles);
    libwr2rf_wrc_write32(dev, cfg_addr, (1 << 21));
}

unsigned
libwr2rf_get_wrs_timeout_latency (struct libwr2rf_dev *dev)
{
    unsigned cfg6_addr = 0xa4; // STREAMERS_RX_CFG6 - fixed latency timeout

    libwr2rf_be_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_PAGE0, 0x48000);
    return libwr2rf_wrc_read32(dev, cfg6_addr);
}
