#include "RegMap.h"
#include "vna.h"
#include <cmath>
#include <fcntl.h>
#include <iostream>
#include <sys/mman.h>


#define BITM_0_7 0x000000FF
#define BITM_8_15 0x0000FF00
#define BITM_16_23 0x00FF0000
#define BITM_24_31 0xFF000000

#define BITP_MANTISSA 0
#define BITP_EXPONENT 23
#define BITP_SIGN 31

#define BITM_MANTISSA ((1 << 23) - 1) << BITP_MANTISSA
#define BITM_EXPONENT ((1 << 8) - 1) << BITP_EXPONENT
#define BITM_SIGN (1 << BITP_SIGN)

void *bar0;

void vna::cfgRegWrite() {
    // Write to the configuration register
    uint32_t *ptr = (uint32_t *) (bar0) + CFGREG_ADDR;
    *ptr = ENUM_CFGREG_MEASSTART_START;
}

void vna::cfgRegRead() {
    // Read the configuration register
    uint32_t *readValue;
    uint32_t reg = 0;
    while (!(reg & ENUM_CFGREG_MEASSTOP_STOP)) {
        readValue = (uint32_t *) (bar0) + CFGREG_ADDR;
        reg = lsbToMsb(*readValue);
        if (reg & ENUM_CFGREG_MEASSTOP_STOP) {
            std::cout << "End measurement bit is set" << std::endl;
        } else {
            std::cout << "End measurement bit is not set" << std::endl;
        }
    }
}

void vna::adcDataRead() {
    // Read the adc data
    for (int i = 0; i < 8; i++) {
        uint32_t *data = (uint32_t *) (bar0) + 0x02;
        adcData[i] = lsbToMsb(*data);
        (void) printf("adcData[%d] = %0x\n", i, adcData[i]);
    }
    // Convert the read adc data to fp32 1.8.23 form
    for (int i = 0; i < 8; i++) {
        int sign = (adcData[i] & BITM_SIGN) ? -1 : 1;
        int exponent = ((adcData[i] & BITM_EXPONENT) >> 23);
        float mantissa = (adcData[i] & BITM_MANTISSA) / (float) pow(2, 23);
        float adcFloat = sign * (mantissa + 1) * pow(2, (exponent - 127));
        (void) printf("adcDataFloat[%d] = %.12f\n", i, adcFloat);
    }
}

uint32_t vna::lsbToMsb(uint32_t reg) {
    auto result = static_cast<uint32_t>((reg & BITM_24_31) >> 24);
    result |= static_cast<uint32_t>((reg & BITM_16_23) >> 8);
    result |= static_cast<uint32_t>((reg & BITM_8_15) << 8);
    result |= static_cast<uint32_t>((reg & BITM_0_7) << 24);
    return result;
}

void vna::sendSettings() {
    /*Send the PGMODE reg*/
    uint32_t *data = (uint32_t *) (bar0) + PGMODE0_ADDR;
    *data = 0x49209U;
    /*Send the MEASCTRL reg*/
    data = (uint32_t *) (bar0) + MEAS_CTRL_ADDR;
    *data = 0x3f6000U;
    /*Send the ADCCTRL reg*/
    data = (uint32_t *) (bar0) + ADC_CTRL_ADDR;
    *data = 0x2U;
    /*Send the CORR_COEF_H reg*/
    data = (uint32_t *) (bar0) + CORR_COEF_H_ADDR;
    *data = 0x3fU;
    /*Send the PGMODE1 reg*/
    data = (uint32_t *) (bar0) + PGMODE1_ADDR;
    *data = 0x1000U;
    /*Send the MUXCTRL2 reg*/
    data = (uint32_t *) (bar0) + MUX_CTRL2_ADDR;
    *data = 0x1a3U;
    /*Send the MUXCTRL3 reg*/
    data = (uint32_t *) (bar0) + MUX_CTRL3_ADDR;
    *data = 0x13c2eU;
    /*Send the MUXCTRL4 reg*/
    data = (uint32_t *) (bar0) + MUX_CTRL4_ADDR;
    *data = 0x8421U;
    /*Send the PG7P1W reg*/
    data = (uint32_t *) (bar0) + PG7P1W_ADDR;
    *data = 0x1U;
    /*Send the PG1P1W reg*/
    data = (uint32_t *) (bar0) + PG1P1W_ADDR;
    *data = 0x1U;
    /*Send the PG2P1W reg*/
    data = (uint32_t *) (bar0) + PG2P1W_ADDR;
    *data = 0x1U;
    /*Send the PG3P1W reg*/
    data = (uint32_t *) (bar0) + PG3P1W_ADDR;
    *data = 0x1U;
    /*Send the PG4P1W reg*/
    data = (uint32_t *) (bar0) + PG4P1W_ADDR;
    *data = 0x1U;
    /*Send the PG5P1W reg*/
    data = (uint32_t *) (bar0) + PG5P1W_ADDR;
    *data = 0x1U;
    /*Send the PG6P1W reg*/
    data = (uint32_t *) (bar0) + PG6P1W_ADDR;
    *data = 0x1U;
    /*Send the MEASNUM reg*/
    data = (uint32_t *) (bar0) + MEASNUM_23_0_ADDR;
    *data = 0x1U;
}

int main() {

    const char *filename = "/dev/MyDmaModule";
    int pciFd;// File descriptor

    // Open the device file
    pciFd = open(filename, O_RDWR | O_SYNC);
    if (pciFd < 0) {
        std::cout << "Error: Cannot open device file" << std::endl;
        return -1;
    }
    bar0 = mmap(nullptr, 0x1000U, PROT_READ | PROT_WRITE, MAP_SHARED, pciFd, 0);

    if (bar0 == MAP_FAILED) {
        std::cout << "Error: Cannot map the device file" << std::endl;
        (void) close(pciFd);
        return -1;
    }
    // Create an object of the vna class
    vna tmsg;
    /* Send the settings */
    tmsg.sendSettings();
    // Call the cfgRegWrite method
    tmsg.cfgRegWrite();
    // Call the cfgRegRead method
    vna::cfgRegRead();
    // Call the adcDataRead method
    tmsg.adcDataRead();
    // Close the device file
    (void) close(pciFd);
    // Unmap the device file
    (void) munmap(bar0, 0x1000U);
    return 0;
}