#define _XOPEN_SOURCE 500
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "libwr2rf/board.h"
#include "libwr2rf/api.h"
#include "wr2rf_vme_regs.h"
#include "wr2rf_init_regs.h"

#define AD9910_REF_FREQ ( 1000000000ULL ) // 1 GHz

struct ad9910_config_reg {
    int addr;
    uint64_t value;
    int nbits;
};

static struct ad9910_config_reg ad9910_default_config[] = {
//    {0, 0x0200 0002 | (1<<13), 32},              // CFR1, unidir mode for SDIO, autoclear phase accumulator on IOUPDATE
    {0, 0x02000002 | (1<<13), 32},              // CFR1, unidir mode for SDIO, autoclear phase accumulator on IOUPDATE
    {1, 0x00000800, 32},              // CFR2, TW - enabled sync pulse timing validation
    {2, 0x1f3fc000, 32},              // CFR3: no PLL
//    {3, 0x00007f64, 32},              // Aux DAC control: DAC Full scale current
    //{3, 0x11, 32}, // we take the default fsc setting
    {4, 0xffffffff, 32},              // IO update rate
    {7, 0x00000000, 32},              // default FTW
    {8, 0x0000, 16},                  // default phase offset
    {9, 0x00000000, 32},              // amplitude scale factors
    {0xa, 0x00000000, 32},            // multichip sync
//    {0xe, 0x08b5000039374bc6ULL, 64}, // profile 0: 223.5 MHz, 0 deg phi, ASF 0.13
    {0xe, 0x08b50000028F5C28ULL, 64}, // profile 0: 10 MHz, 0 deg phi, ASF 0.13
    {-1, 0, 0}};


#define AD9910_REG_CFR2 1
#define AD9910_DEFAULT_CFR2  0x400820

static int
ad9910_probe(struct libwr2rf_dev *dev)
{
        libwr2rf_dds_write(dev, 0, 0x0, 32); // unidir mode for SDIO
        libwr2rf_dds_write(dev, 0, 0x02000002, 32); // unidir mode for SDIO
        libwr2rf_dds_ioupdate(dev);

        uint32_t cfr1 = libwr2rf_dds_read(dev, 0, 32);
        printf("AD9910: cfr1=0x%x \n", cfr1);

        uint32_t id = libwr2rf_dds_read(dev, AD9910_REG_CFR2, 32);
        printf("AD9910: 0x%x (expected 0x%x)\n", id, AD9910_DEFAULT_CFR2);

        if (id != AD9910_DEFAULT_CFR2) {
                printf ("AD9910: Error: DDS not found\n");
                return LIBWR2RF_ERROR_HW;
        }
        return 0;
}

static uint64_t ad9910_frequency_to_ftw(uint64_t freq_hz )
{
        // formula (2) from AD9910 datasheet, page 22
        return  (1ULL << 32) * freq_hz / AD9910_REF_FREQ;
}


static void ad9910_program(struct libwr2rf_dev *dev, uint64_t ftw, int phase, int fs_current, unsigned io_update)
{
    int i;
    uint64_t prof0_cr = ftw | (0x8b5ULL << 48);

    printf("ad9910 FTW = %d\n", (uint32_t) ftw);

//    dev_dbg("ad9910_program [%08x%08x] asf %d!\n", (uint32_t)(prof0_cr >> 32), (uint32_t)prof0_cr, asf );

    for(i = 0; ad9910_default_config[i].addr >= 0; i++)
    {
        struct ad9910_config_reg r = ad9910_default_config[i];

        if(r.addr == 3)
        {
            r.value &= 0xffffff00;
            r.value |= fs_current;     // Aux DAC control: DAC Full scale current
  //          pp_printf("auxdac %08x\n", (uint32_t) r.value );
        } else
        if( r.addr == 0xe ) // profile 0
            r.value = prof0_cr;

        libwr2rf_dds_write(dev, r.addr, r.value, r.nbits);
    }

    if (io_update)
        libwr2rf_dds_ioupdate(dev);
}

void libwr2rf_dds_configure_sync(struct libwr2rf_dev *dev, int sync_receiver_enable,
                                  int sync_receiver_delay, int sync_validation_delay)
{
    uint32_t r10 = ((sync_receiver_delay & 0x1f) << 3)
                  | (sync_receiver_enable ? ( 1<<27) : 0)
                  | (sync_validation_delay << 28) | (0<<26);

    libwr2rf_dds_write(dev, 0xa, 0, 32);
    libwr2rf_dds_write(dev, 0x1, 0x00000820, 32);      // CFR2, TW - disable sync pulse timing validation
    libwr2rf_dds_ioupdate(dev);

    libwr2rf_dds_write(dev, 0xa, r10 , 32);
    libwr2rf_dds_ioupdate(dev);
    libwr2rf_dds_write(dev, 0x1, 0x00000800, 32);      // CFR2, TW - enabled sync pulse timing validation
    libwr2rf_dds_ioupdate(dev);
}

int
libwr2rf_dds_init(struct libwr2rf_dev *dev, unsigned io_update)
{
  int res;

  /* Probe.  */
  res = ad9910_probe(dev);
  if (res != 0)
    return res;

  /* Configure sync.  */
  /* TODO.  */
  libwr2rf_dds_configure_sync(dev, 0, 0, 0);

  /* Program.  */
  uint64_t ftw = ad9910_frequency_to_ftw(223500000ULL);
  ad9910_program(dev, ftw, 0, 0x0, io_update);

  return 0;
}

int
libwr2rf_dds_freq_init(struct libwr2rf_dev *dev,
                       uint64_t freq, unsigned io_update)
{
  int res;

  /* Probe.  */
  res = ad9910_probe(dev);
  if (res != 0)
    return res;

  /* Configure sync.  */
  /* TODO.  */
  libwr2rf_dds_configure_sync(dev, 0, 0, 0);

  /* Program.  */
  uint64_t ftw = ad9910_frequency_to_ftw(freq);
  ad9910_program(dev, ftw, 0, 0x0, io_update);

  return 0;
}

void
libwr2rf_dds_sync_calibrate (struct libwr2rf_dev *dev, int verbose)
{
    unsigned rx_del;
    unsigned val_del;
    unsigned dds_status_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_DDS_STATUS;
    unsigned sync_err[32][16] = {{0}};

    for (rx_del = 0; rx_del < 32; rx_del++) {
        for (val_del = 0; val_del < 16; val_del++) {
            libwr2rf_dds_configure_sync(dev, 1, rx_del, val_del);
            usleep (10000);
            sync_err[rx_del][val_del] = libwr2rf_read16(dev, dds_status_addr);
        }
    }

    if (verbose) {
        printf ("Complete dds sync error timing matrix error=1, rows=receiver delay, cols=validation delay\n");
        for (rx_del = 0; rx_del < 32; rx_del++) {
            printf("%02d  ", rx_del);
            for (val_del = 0; val_del < 16; val_del++)
                printf("%01d  ", sync_err[rx_del][val_del]);
            printf("\n");
        }
    }


    if (sync_err[10][2] == 0) { // try to use preset
        libwr2rf_dds_configure_sync(dev, 1, 10, 4);
        if (verbose)
            printf("Using preset DDS Sync calibration delays (10, 4)\n");
    } else {
        // Find the best receiver delay
        // Two windows are normally found. We want all our ad9910s to use the same window
        // and don't want small changes in the size of the window to cause this algorithm
        // to oscillate between windows. Therefore we use the first window that meets our
        // criteria
        unsigned win_start      = 0;
        unsigned best_win_len   = 0;
        unsigned best_win_start = 0;
        unsigned win_len        = 0;
        unsigned in_win         = 0;
        unsigned best_rx_del    = -1;
        unsigned best_val_del   = -1;

        for (rx_del = 0; (rx_del < 20); rx_del++) {
            if (rx_del > 0 && rx_del < 31) {
                if (sync_err[rx_del][0] == 0) {
                    if (! in_win) {
                        in_win = 1;
                        win_start = rx_del;
                    }
                    win_len++;
                } else {
                    if (in_win) {
                        in_win = 0;
                        if (win_len > best_win_len) {
                            best_win_start = win_start;
                            best_win_len   = win_len;
                        }
                        win_len = 0;
                    }
                }
            }
        }

        if (best_win_len >= 5) {
            best_rx_del  = best_win_start + (best_win_len >> 1);
            best_val_del = 4; // recommended value pg45, last para

            if (verbose) {
                printf("Best DDS SYNC calibration delays are: \n");
                printf("  receiver delay:   %02d \n", best_rx_del);
                printf("  validation delay: %02d \n", best_val_del);
            }

            libwr2rf_dds_configure_sync(dev, 1, best_rx_del, best_val_del);
        } else {
            if (verbose)
                printf("WARNING: very small sync receiver window, SET Manually\n");
        }
    }
}
