//please follow 2/17/19 Alex for newfelix SW

#include "daq_device_felix.h"

#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <time.h>

daq_device_felix::daq_device_felix(const int eventtype,
        const int subeventid,
        const int card_nr,
        const int64_t buffer_size,
        const int dma_index,
        const int interrupt_id,
        const int flag)
{
    _eventType = eventtype;
    _subEvtID = subeventid;
    _buffer_size = buffer_size;
    _maxLength = 5000000;
    _interrupt_id = interrupt_id;

    // set the statuss
    _isRunning = false;
    _irqTrigger = flag & 0b001;
    _fanoutEna = flag & 0b010;
    _externalEmu = flag & 0b100;

    // Allocate the DMA buffer
    _dataBuffer = this->cmemAllocate(_buffer_size);

    // did this get forgotten? check later 10/17/17
    // _dma_index = dma_index

    // Normally this should be in a try block to catch FlxExcption
    // Since there is no clear solution yet about what to do when
    // such exception happens, thus we let it simply die here
    _flx = new FlxCard;
    //_flx->card_open(card_nr);
    //ALEX C
    _flx->card_open(card_nr, 0);

    // Trigger logic
    if(_irqTrigger)
    {
        _th = new felixTriggerHandler(_flx, _interrupt_id);
        registerTriggerHandler(_th);
    }
    else
    {
        //should we generate a deadtime based polling trigger or use system's default one?
        _th = 0;
    }
    //setbuf(stdout,NULL); //for debug - this stops buffering, so stdout prints will make it to the log even if the DAQ crashes immediately
}

daq_device_felix::~daq_device_felix()
{
    if(_isRunning) endrun();

    //close the flxcard
    _flx->card_close();

    //release the cmem buffer
    if(_dataBuffer)
    {
        munmap((void*)_cmemDescriptor.uaddr, _cmemDescriptor.size);
        ioctl(_cmemHandle, CMEM_RCC_FREE, &_cmemDescriptor.handle);
    }
}

int daq_device_felix::put_data(const int etype, int* addr, const int length)
{

    struct timespec  req, rem;
    if(etype != _eventType) return 0;  //not our id

    _event_count++;

    sevt = (subevtdata_ptr)addr;

    sevt->sub_length = SEVTHEADERLENGTH;
    sevt->sub_id = _subEvtID;
    sevt->sub_decoding = IDFELIXV1;
    sevt->sub_type = 4; //this value makes ddump print in units of dwords (4 dwords per line)
    //sevt->sub_type = 1; //this value makes ddump print in units of bytes (16 bytes per line) - as suggested by martin 12/8/17
    sevt->reserved[0] = 0;
    sevt->reserved[1] = 0;

    //7/6/18 the changes with this date are the addiditons of the counter in the firmware.

    //usleep(10);
    /*
       printf("desc_done %d\n", _dmaStat->descriptor_done);
       printf("tlp %d\n", _dmaDesc->tlp);
       int numWaits = 0;

       usleep(10);
    //while (_dmaStat->descriptor_done == 1) {
    for (int i = 0;i<100;i++) {
    numWaits++;
    printf("waiting: desc_done %d\n", _dmaStat->descriptor_done);
    printf("tlp %d\n", _dmaDesc->tlp);
    printf("current_address %x after %d waits\n", _dmaStat->current_address,numWaits);
    //sleep
    usleep(10);
    }

    printf("tlp %d\n", _dmaDesc->tlp);
    printf("desc_done %d after %d waits\n", _dmaStat->descriptor_done, numWaits);
    */

    //Copy data from DMA buffer
    char* data = (char*)&(sevt->data);

    // loop or memcpy here to move data
    _currAddr = _dmaStat->current_address;

    //7/13/18 alex sho removed to test the if 
    while(_currAddr < _startAddr || _currAddr > _endAddr) _currAddr = _dmaStat->current_address;
    //printf("Hello this is before the while\n");

    //int counter=0; //debug

    //7/13/18 alex sho removed to test the if 
    /*while(_currAddr < _startAddr || _currAddr > _endAddr || _currAddr == _prevAddr) 
      {
    //this is a temporary fix to prevent the daq from crashing this is believed to occur due to the fact that the DMA is not putting the data fast enough
    //theres two things _curAddr == _prevAddr to make sure a count present
    //sleep is in the while loop to make sure the daq doesnt crash due to the while loop running too fast

    //req.tv_sec = 0;
    //req.tv_nsec = 48;
    //rem.tv_sec = 0;
    //rem.tv_nsec = 0;

    //nanosleep(&req, &rem);

    //usleep(10); //temporary test with 1 us 7/6/18

    //counter ++;//debug
    //printf("countervalue: %d\n", counter);//debug
    _currAddr = _dmaStat->current_address; //7/6/18
    }*/
    unsigned int total_transferred_length = 0; //total number of bytes written

    bool count_valid = false;
    unsigned int count;//7/6/18 this will hold the 10 bit counter from the count fifo
    while (!count_valid && (_currAddr != _prevAddr)) {//look for the count word
        char linknumber = _dataBuffer[_prevAddr - _startAddr + 31]; //0 for the header word (containing the count FIFO counter), otherwise the GBT link number
        //printf("GBT link number: 0x%hhx\n", linknumber);
        count_valid = (linknumber == 0);
        if (count_valid) {
            memcpy(&count,&_dataBuffer[_prevAddr - _startAddr], 4);
        } else {
            printf("event %d: expected the header word with the count, but didn't see it - skipping this word\n",_event_count);
            for (int i=31;i>=0;i--) {
                printf("%02hhx ",_dataBuffer[_prevAddr - _startAddr + i]);
            }
            printf("\n");
            _prevAddr += 32;//discard this FELIX word since we can't make sense of it; go through this while loop again
        }
    }
    if (count_valid) {
        //printf("event %d: count FIFO count is %d\n",_event_count,count);
        if (count==0) {
            printf("event %d: count FIFO count is 0!\n",_event_count);
        }
        //printf("intial count: 0x%x\n", count);

        //end_of_data is a pointer that shows the end of the data  or data that we need to move. Equivalent of the current addres pointer
        uint64_t end_of_data = (((count+1) * 32) + _prevAddr);
        //printf("end address:%d \n", end_of_data);

        //7/6/18
        if (end_of_data >= _endAddr) 
        {   
            //if we are in this case we are wrapping aound so temp count is updated to point inside the bugger
            //printf("at the beginging of inside the if\n");
            end_of_data = (_startAddr + end_of_data) - _endAddr;
            //printf("In loop _startAddr: %d _endAddr:%d, prevAddr: %d, end_of_data %d\n", _startAddr, _endAddr, _prevAddr, end_of_data); 
            uint64_t len = transfer(data, _prevAddr, _endAddr);
            //printf("length: %d\n",len);//debug
            total_transferred_length += len;
            data += len;

            len = transfer(data, _startAddr, end_of_data);
            //printf("length: %d\n",len);//debug
            total_transferred_length += len;
            data += len;
            //printf("at the end of inside the if\n");
        }  	
        else 
        {
            //printf("at the beginging of inside the else\n");
            //printf("In loop _startAddr: %d _endAddr:%d, prevAddr: %d, end_of_data %d\n", _startAddr, _endAddr, _prevAddr, end_of_data); 
            uint64_t len = transfer(data, _prevAddr, end_of_data);
            //printf("length: %d\n",len);//debug
            total_transferred_length += len;
            data += len;
            //printf("at the end of inside the else\n");
        }


        //7/6/18 the below is commented out since it was used prior to adding a counter to the begining of every packet 
        /*
           if(_currAddr > _prevAddr)
           {
           uint64_t len = transfer(data, _prevAddr, _currAddr);
        //sevt->sub_length += len;
        total_transferred_length += len;
        data += len;
        }
        else if(_currAddr < _prevAddr)  //wrap around
        {
        uint64_t len = transfer(data, _prevAddr, _endAddr);
        //sevt->sub_length += len;
        total_transferred_length += len;
        data += len;

        len = transfer(data, _startAddr, _currAddr);
        //sevt->sub_length += len;
        total_transferred_length += len;
        data += len;
        } 
        */
        //if _currAddr == _prevAddr, do nothing
        //sevt->sub_length += (total_transferred_length + (4-1))/4+5; // number of 4-byte dwords, rounded up
        //_prevAddr = _currAddr;

        //printf("before the prevAddr calculation\n"); 
        //_prevAddr += (((((count +1)))))*32;//7/6/18 rounding up where 8 is the number of felix words transferred by the DMA, multiplied by 32 to put prev address in bytes
        _prevAddr += (((((count +1)+8)-1)/8)*8)*32;//7/6/18 rounding up where 8 is the number of felix words transferred by the DMA, multiplied by 32 to put prev address in bytes
        //7/6/18
        if (_prevAddr >= _endAddr)
        {
            _prevAddr = (_startAddr + _prevAddr) - _endAddr;
        }
        //_dmaDesc->read_ptr = _currAddr; // update the PC read pointer 10/17/17
        _dmaDesc->read_ptr = _prevAddr; // update the PC read pointer 7/6/18

    }//alex sho 7/13/18 end of ifif(_currAddr != prevAddr)
    //else printf("Pointers are equal adding fooo fooo\n");   

    //printf("at the end of prevAddr calculation\n");
    //End-of-packet flag
    const unsigned int eop_marker = 0xf000f000;
    memcpy(data, &eop_marker, 4);
    total_transferred_length+=4;

    //printf("event %d: transferred %d bytes\n",_event_count,total_transferred_length);
    //packet padding to make packets aligned: copied from daq_device_file.cc
    sevt->sub_padding = total_transferred_length % 8;
    if ( sevt->sub_padding) sevt->sub_padding = 8 - sevt->sub_padding;
    sevt->sub_length += (total_transferred_length + sevt->sub_padding)/4;
    return sevt->sub_length;
}

uint64_t daq_device_felix::transfer(char* dest, uint64_t headAddr, uint64_t tailAddr)
{
    uint64_t startIndex = headAddr - _startAddr;
    uint64_t len = tailAddr - headAddr;
    if(len > _maxLength) len = _maxLength;

    if(len > 0) memcpy(dest, &_dataBuffer[startIndex], len);
    return len;
}

int daq_device_felix::init()
{
    //start the DMA
    if(_dataBuffer) this->dmaStart(_cmemPhysAddr, _buffer_size, _dma_index);

    //Start the trigger interface if needed
    if(_irqTrigger)
    {
        _flx->irq_enable(_interrupt_id);
        //_flx->irq_enable(FLX_INT_TEST);
    }

    //start the data flow inside Flx
    if(_fanoutEna)
    {
        setFanOut();
    }
    else
    {
        startDataEmulator();
    }

    _event_count = 0;

    _isRunning = true;
    _currAddr = _startAddr;
    _prevAddr = _startAddr;
    return 0;
}

int daq_device_felix::endrun()
{
    if(_irqTrigger )
    {
        _flx->irq_disable(_interrupt_id);
        _flx->irq_cancel(_interrupt_id);
        //_flx->irq_disable(FLX_INT_TEST);
        //_flx->irq_cancel(FLX_INT_TEST);
    }

    if(!_fanoutEna) stopDataEmulator();
    this->dmaStop(_dma_index);

    _isRunning = false;
    return 0;
}

int daq_device_felix::rearm(const int etype)
{
    return 0;
}

void daq_device_felix::identify(std::ostream& os) const
{
    os << "FELIX Device  Event Type: " << _eventType << " Subevent id: " << _subEvtID
        << " Firmware Ver: " << firmwareVersion();
    if(_irqTrigger) os << " trigger ";
    os << std::endl;
}

int daq_device_felix::max_length(const int etype) const
{
    if(etype != _eventType) return 0;
    return _maxLength;
}

//------------------- Below are private functions directly taken from fdaq code ------------------
void daq_device_felix::setFanOut()
{
    this->stopDataEmulator();

    bool locked = this->fanOutLocked();
    if(!locked)
    {
        /*
           _flx->cfg_set_option(BF_GBT_UPLNK_FO_SEL, 0);
           _flx->cfg_set_option(BF_GBT_DNLNK_FO_SEL, 0);
           */
        //2/17/19 Alex for newfelix SW
        _flx->cfg_set_option( BF_GBT_TOFRONTEND_FANOUT_SEL, 0 );
        _flx->cfg_set_option( BF_GBT_TOHOST_FANOUT_SEL, 0 );
    }
}

void daq_device_felix::startDataEmulator()
{
    if(_fanoutEna) return;
    if(_flx == 0) return;

    bool locked = this->fanOutLocked();
    if(_externalEmu)
    {
        if(!locked)
        {
            /*
            // Select real GBT link for all CentralRouter channels
            _flx->cfg_set_option(BF_GBT_DNLNK_FO_SEL, 0);

            // Enable GBT_EMU for all GBTs ('to frontend')
            _flx->cfg_set_option( BF_GBT_UPLNK_FO_SEL, 0xFFFFFF);
            */

            //2/17/19 Alex for newfelix SW
            // Select real GBT link for all CentralRouter channels
            _flx->cfg_set_option( BF_GBT_TOHOST_FANOUT_SEL, 0 );
            // Enable GBT_EMU for all GBTs ('to frontend')
            _flx->cfg_set_option( BF_GBT_TOFRONTEND_FANOUT_SEL, 0 );

        }

        // 'TOFRONTEND' emulator enable
        _flx->cfg_set_option(BF_GBT_EMU_ENA_TOHOST, 0);
        _flx->cfg_set_option(BF_GBT_EMU_ENA_TOFRONTEND, 1);
        _flx->cfg_set_option(BF_GBT_FM_EMU_ENA_TOHOST, 0); // FULL-mode
    }
    else
    {
        if(!locked)
        {
            /*
            // Enable GBT_EMU for all GBTs ('to host')
            _flx->cfg_set_option(BF_GBT_DNLNK_FO_SEL, 0xFFFFFF);

            // Select CentralRouter data for all GBT links
            _flx->cfg_set_option(BF_GBT_UPLNK_FO_SEL, 0);
            */
            //2/17/19 Alex for newfelix SW      
            // Enable GBT_EMU for all GBTs ('to host')
            _flx->cfg_set_option( BF_GBT_TOHOST_FANOUT_SEL, 0 );
            // Select CentralRouter data for all GBT links
            _flx->cfg_set_option( BF_GBT_TOFRONTEND_FANOUT_SEL, 0 );

        }

        if(_flx->cfg_get_reg(REG_FIRMWARE_MODE) == 1)
        {
            // 'TOHOST' FULL-mode emulator enable
            _flx->cfg_set_option(BF_GBT_FM_EMU_ENA_TOHOST, 1);
        }
        else
        {
            // 'TOHOST' emulator enable
            _flx->cfg_set_option(BF_GBT_EMU_ENA_TOFRONTEND, 0);
            _flx->cfg_set_option(BF_GBT_EMU_ENA_TOHOST, 1);
        }
    }
}

void daq_device_felix::stopDataEmulator()
{
    // Stop the data emulator on the FLX-card
    if(_flx == 0) return;

    // Disable emulators
    _flx->cfg_set_option(BF_GBT_EMU_ENA_TOHOST, 0);
    _flx->cfg_set_option(BF_GBT_EMU_ENA_TOFRONTEND, 0);
    _flx->cfg_set_option(BF_GBT_FM_EMU_ENA_TOHOST, 0); // FULL-mode

    bool locked = this->fanOutLocked();
    if(!locked)
    {
        // Select CentralRouter data for all GBT links
        // ("up/downlink fanout select")
        /*
           _flx->cfg_set_option(BF_GBT_UPLNK_FO_SEL, 0);
           */
        //2/17/19 Alex for newfelix SW
        _flx->cfg_set_option( BF_GBT_TOFRONTEND_FANOUT_SEL, 0xFFFFFF );
        // Slowly crap data enters the CR! (when GTH-links not connected?),
        // so outcommented:
        //_flx->cfg_set_option( BF_GBT_DNLNK_FO_SEL, 0 );
        /*
           _flx->cfg_set_option(BF_GBT_DNLNK_FO_SEL, 0xFFFFFF);
           */
        //2/17/19 Alex for newfelix SW  
        _flx->cfg_set_option( BF_GBT_TOHOST_FANOUT_SEL, 0xFFFFFF );
    }
}

bool daq_device_felix::fanOutLocked()
{
    bool locked = false;
    /*
       if(_flx->cfg_get_reg( REG_GBT_DNLNK_FO_SEL ) & 0x80000000) locked = true;
       if(_flx->cfg_get_reg( REG_GBT_UPLNK_FO_SEL ) & 0x80000000) locked = true;
       */
    //2/17/19 Alex for newfelix SW 
    if( _flx->cfg_get_option( BF_GBT_TOHOST_FANOUT_LOCK ) == 1)
        locked = true;
    if( _flx->cfg_get_option( BF_GBT_TOFRONTEND_FANOUT_LOCK ) == 1 )
        locked = true;  

    return locked;
}

char* daq_device_felix::cmemAllocate(uint64_t buffer_size)
{
    char* buffer = 0;

    // Open CMEM(_RCC) and allocate the requested buffer size
    if((_cmemHandle = open("/dev/cmem_rcc", O_RDWR)) < 0)
    {
        return 0;
    }

    sprintf(_cmemDescriptor.name, "FlxDaq");
    _cmemDescriptor.size  = _buffer_size;
    _cmemDescriptor.order = 0;
    _cmemDescriptor.type  = TYPE_GFPBPA;
    if(CMEM_RCC_SUCCESS == ioctl(_cmemHandle, CMEM_RCC_GET, &_cmemDescriptor))
    {
        int64_t result = (int64_t)mmap(0, buffer_size, PROT_READ | PROT_WRITE, MAP_SHARED, _cmemHandle, (int64_t)_cmemDescriptor.paddr);
        if(result != -1)
        {
            _cmemDescriptor.uaddr = result;
            if(CMEM_RCC_SUCCESS == ioctl(_cmemHandle, CMEM_RCC_SETUADDR, &_cmemDescriptor)) buffer = (char *)_cmemDescriptor.uaddr;

            _cmemPhysAddr = _cmemDescriptor.paddr;
            _cmemVirtAddr = _cmemDescriptor.uaddr;
        }

        if(buffer == 0) return 0;
    }
    else
    {
        return 0;
    }

    _startAddr = _cmemPhysAddr;
    _endAddr   = _startAddr + buffer_size;

    return buffer;
}

void daq_device_felix::dmaStart(uint64_t dst_phys_addr, uint64_t size, int dma_index)
{
    if(!(_flx != 0 && dst_phys_addr != 0 && size > 0)) return;

    // Just in case...
    this->stopDataEmulator();
    this->dmaStop(dma_index);

    uint64_t bar0_addr = _flx->openBackDoor(0);
    volatile flxcard_bar0_regs_t* bar0 = (volatile flxcard_bar0_regs_t*)bar0_addr;
    _dmaDesc = &bar0->DMA_DESC[dma_index];
    _dmaStat = &bar0->DMA_DESC_STATUS[dma_index];

    // Enable data-available interrupt
    _flx->irq_enable(_interrupt_id);
    //_flx->irq_enable(FLX_INT_TEST);

    int tlp = _flx->dma_max_tlp_bytes();
    _tlpSizeMask = ~((uint64_t)tlp - 1);

    _flx->dma_to_host(dma_index, dst_phys_addr, size, FLX_DMA_WRAPAROUND);
    usleep(1000); // If we don't, _readPtr is 0 after the next statement!?
}

void daq_device_felix::dmaStop(int dma_index)
{
    if(_flx == 0) return;
    _flx->dma_stop(dma_index);

    // Reset and flush
    _flx->dma_reset();
    _flx->dma_fifo_flush();
    _flx->soft_reset();
}

std::string daq_device_felix::firmwareVersion() const
{
    if(_flx == 0) return std::string();

    std::ostringstream oss;
    uint64_t ts  = _flx->cfg_get_reg(REG_BOARD_ID_TIMESTAMP);
    uint64_t svn = _flx->cfg_get_reg(REG_BOARD_ID_SVN);
    uint64_t fm  = _flx->cfg_get_reg(REG_FIRMWARE_MODE);
    uint64_t chn = _flx->cfg_get_reg(REG_NUM_OF_CHANNELS);
    uint64_t typ = _flx->cfg_get_reg(REG_CARD_TYPE);

    oss << std::hex << ts << "-" << std::dec << svn;
    if(fm == 1)
    {
        oss << "-FM-";
    }
    else
    {
        oss << "-GBT-";
    }
    oss << chn << "ch-" << typ;

    return oss.str();
}