/**
* @file SWaveformParser.cpp
* @author Nick Ryder & Dan Saunders, on behalf of the SoLid collaboration.
* @date 17 Feb 2016
*/

#include <bitset>

#include "SWaveformParser.h"

//==============================================================================

//! TODO
SWaveformParser::SWaveformParser(SDetector * dtr, SClipboard * cb) :
  ISAlgorithm(dtr, cb, "SWaveformParser"), m_buf(NULL), m_timeblocks(NULL), 
  m_timeblock(NULL), m_cyclestarttime(0), m_triggerrecord(NULL), 
  m_stopnext(false), m_maxhtime(1.0), m_maxgaphtime(0.01)
{
    m_options.push_back(new SOptionDouble("maxcyclehtime", &m_maxhtime));
    m_options.push_back(new SOptionDouble("maxgaphtime", &m_maxgaphtime));

}



//==============================================================================

//! TODO
void SWaveformParser::initialize()
{
    // Get access to SRawBuffer from clipboard
    m_buf = cb()->rawBuffer();
    m_timeblocks = cb()->timeblocks();
}


//==============================================================================

/** Parse triggered data.
 *
 * Get blocks of data from SRawBuffer on clipboard. Parse each
 * block as either STriggerRecord or SWaveformBlock.
 * Create new STimeBlock for each different time block.
 * Add waveforms and trigger records from all planes for
 * a given time block. This assumes that data is time ordered.
 *
 * Will continue until total block time exceeds a threshold or
 * until a gap between time blocks exceeds a threshold. Each of these
 * are set by user in options file.
 */
void SWaveformParser::execute()
{
    if (m_stopnext) {
        setRunStopped(true);
        return;
    }
    // Loop over time blocks:
    //     * Check if timestamp of next block should be included in this
    //       processing cycle
    //     * Parse blocks that have correct timestamps
    //     * Append parsed waveforms into vector on clipboard
    // Do we want the SWaveforms ordered in any specific way?
    bool firstincycle = true;
    bool running = true;
    // Will keep processing until conditions to start a new cycle are met.
    while (running) {
        SRawBlock blk = m_buf->nextblock();
        if (blk.data == NULL) {
            std::cout << "Received empty block, finishing." << std::endl;
            if (m_timeblock != NULL) cb()->addTimeBlock(m_timeblock);
            m_stopnext = true;
            running = false;
            break;
        } else {
            //std::cout << std::hex << *(blk.data + 1) << ": type = " << std::dec << blk.type << std::endl;
            if (blk.type == triggerblock) {
                STriggerRecord * trigrec = parseTriggerBlock(blk);
                if (firstincycle) {
                    firstincycle = false;
                    m_cyclestarttime = trigrec->timestamp();
                }
                if (m_timeblock == NULL) {
                    m_timeblock = new STimeBlock(trigrec->timestamp());
                }
                // Try adding trigger record to current time block. If it's 
                // timestamp is wrong this will return false. That signals
                // that the current time block is complete and a new one
                // should be started. In this case, check whether the conditions
                // have been met to start a new cycle.
                if (!m_timeblock->addTriggerRecord(trigrec)) {
                    // Store the old block and start a new one.
                    cb()->addTimeBlock(m_timeblock);
                    m_timeblock = new STimeBlock(trigrec->timestamp());
                    // This will be a block from the right time by definition.
                    m_timeblock->addTriggerRecord(trigrec);
                    // Check time between new block and the first one in this cycle
                    int ndb = m_timeblocks->size() - 1;
                    uint64_t dt = m_timeblocks->at(ndb)->timestamp();
                    dt -= m_timeblocks->at(0)->timestamp();
                    double dhtime = 25e-9 * (double) dt;
                    if (dhtime > m_maxhtime) running = false;
                    // If there are more than one data blocks then check gap
                    // between the new block and the previous one.
                    if (ndb >= 1) {
                        dt = m_timeblocks->at(ndb)->timestamp();
                        dt -= m_timeblocks->at(ndb - 1)->timestamp();
                        dhtime = 25e-9 * (double) dt;
                        // If the gap is large enough finish this cycle
                        if (dhtime > m_maxgaphtime) running = false;
                    }
                }
            } else if (blk.type == waveformblock) {
                SWaveformBlock * waveformblock = parseWaveformBlock(blk);
                bool added = m_timeblock->addWaveformBlock(waveformblock);
                if (!added) {
                    std::cout << "[Error] waveform block doesn't match time of current time block, dropping data." << std::endl;
                    delete waveformblock;
                }
            }
        }
    }
    // Now all blocks are added, get SClipboard to sort waveforms
    cb()->sortWaveforms();
}


//==============================================================================

//! TODO
void SWaveformParser::finalize()
{
}


//==============================================================================

STriggerRecord * SWaveformParser::parseTriggerBlock(SRawBlock block) {
    uint32_t * data = block.data;
    std::vector<uint64_t> channeltriggers;
    uint16_t triggermask = 0;
    uint16_t boardid = 0;
    uint32_t len = 0;
    uint64_t blocktimestamp;

    // Parse block header
    if ((*data & 0xff000000) != 0xaa000000) {
        std::cout << "[Error] block has invalid header: " << std::hex << *data << std::dec << std::endl;
        return NULL;
    }
    boardid = (uint8_t) ((*data & 0x00ff0000) >>  16);
    len = *data & 0x0000ffff;
    //std::cout << std::hex << *data << ": len = " << std::dec << len << std::endl;
    data++;
    triggermask = (uint16_t) (*data & 0x0000ffff);
    //std::cout << std::hex << *data << ": mask = " << triggermask << std::dec << std::endl;
    data++;
    blocktimestamp = (uint64_t) *data;
    data++;
    blocktimestamp |= ((uint64_t) *data) << 32;
    //blocktimestamp = blocktimestamp << 8;
    data++;
    data++; // There is a word not currently used
    uint64_t * mask = (uint64_t *) data;
    int nmask = ((int) len - 4) / 2;
    for (int i = 0; i < nmask; i++) {
        channeltriggers.push_back(*mask);
        //std::cout << "mask " << i << ": "<< std::hex << *mask << std::dec << std::endl;
        mask++;
    }
    //std::cout << "Trigger parsed. " << triggermask << ": " << nmask << " masks." << std::endl;
    return new STriggerRecord(
            boardid, blocktimestamp, triggermask, channeltriggers);
}

SWaveformBlock * SWaveformParser::parseWaveformBlock(SRawBlock block) {
    // This parses data from FPGA. See 
    // https://bitbucket.org/solidexperiment/readout-software/wiki/TriggeredFirmware
    // for format. Currently assume only event data blocks are received.
    //
    uint32_t * data = block.data;
    uint32_t len = 0;
    uint16_t boardid = 0;
    uint32_t counter = 0;
    uint64_t includedchannels = 0, requestedchannels = 0;
    //uint32_t eventid = 0;
    uint64_t blocktimestamp = 0;
    //uint32_t crc = 0;
    std::vector<uint16_t> wf;
    int nwf = 0;
    uint64_t chanbit = 0x1;

    // Parse block header
    if ((*data & 0xff000000) != 0xaa000000) {
        std::cout << "[Error] block has invalid header: " << std::hex << *data << std::dec << std::endl;
        return NULL;
    }
    boardid = (uint8_t) ((*data & 0x00ff0000) >>  16);
    len = *data & 0x0000ffff;
    data++;
    counter = (*data) & 0x0fffffff;
    // Convert uint48 block timestamp to sample level timestamp by
    // shifting by 8 bits.
    data++;
    blocktimestamp = (uint64_t) *data;
    data++;
    blocktimestamp |= ((uint64_t) *data) << 32;
    //blocktimestamp = blocktimestamp << 8;
    data++;
    includedchannels = (uint64_t) *data;
    data++;
    includedchannels |= ((uint64_t) *data) << 32;
    //std::bitset<64> x(includedchannels);
    //std::cout << "included channels = \t" << x << std::endl;
    data++;
    requestedchannels = (uint64_t) *data;
    data++;
    requestedchannels |= ((uint64_t) *data) << 32;
    //x = requestedchannels;
    //std::cout << "requested channels = \t" << x << std::endl;
    SWaveformBlock * wfb = new SWaveformBlock(boardid, counter, blocktimestamp, includedchannels, requestedchannels);

    // Each sample is packed in a uin16_t. For each channel one extra
    // sample (0x0000) may be added to pad to 32 bits
    data++;
    uint16_t * sampleword = (uint16_t *) data;
    int nwords = (len - 7) * 2;
    uint64_t timestamp = blocktimestamp;
    uint8_t channel = 0;
    /* Finding which channel a waveform corresponds to
     *
     * The header has a uint64 bitflag for which channels
     * are contributing data to the event. bitN is 1 if
     * channel N has data, 0 otherwise.
     *
     * The next channel number (variable channel) is found before
     * creating SWaveform instances. Before looping over the block's
     * data the first channel is found. When the flag is set for the 
     * end of an event on a channel, the next channel number is found.
     *
     * The method of finding a new channel is to use the uint64 chanbit 
     * variable. Initially this is set to 0x1. For each possible channel
     * the chanbit is compared to the includedchannels flag. If the 
     * corresponding bit in includedchannels is 1 then the next channel is
     * found. If not the chanbit is shifted one bit to the left, and the 
     * channel number is incremented. This process continues until an 
     * included channel is found or chanbit has been shifted 64 times and
     * therefore has a value or zero.
     *
     * Once the channel number is found, this and the board ID are used
     * to collect the SChannel pointer from the SDetector.
     *
     * NB, after the last valid channel, a new channel will be searched for
     * and not found, so chan will be NULL. However there should not be
     * any more completed wavforms for this channel to be assigned to.
    */  
    SChannel * chan = NULL;
    while (((includedchannels & chanbit) == 0x0) && (chanbit != 0x0)) {
        channel++;
        chanbit = chanbit << 1;
    }
    //std::cout << "Channel = " << (int) channel << ", chanbit = " << std::hex << chanbit << std::dec <<  std::endl;
    if (chanbit > 0) {
        //std::cout << "Getting channel for board = " << boardid << ", channel = " << (int) channel << std::endl;
        chan = dtr()->channel(boardid, channel);
        if (chan == NULL) std::cout << "[WTF] unexpected NULL channel." << std::endl;
    }
    //std::cout << "Channel " << (int) channel << std::endl;
    // Parse waveforms
    for (int iword = 0; iword < nwords; iword++) {
        // Loop:
        //     * Get next channel number from mask
        //     * Read data from channel until enf of block flag seen
        //     * Per sample:
        //         * b15 is end of block flag
        //         * b14 is ZS flag
        //         * b14 == 0: b13-0 is ADC sample
        //         * b14 == 1: b13-0 is number of sample to skip
        //     * Create one or many SWaveforms for each channel
        uint16_t word = *(sampleword + iword);
        bool endblock = (word & 0x8000) > 0x0;
        bool zs = (word & 0x4000) > 0x0;
        uint16_t val = word & 0x3fff;
        if (zs) {
            if (wf.size() > 0) {
                // Create new waveform from samples
                double htime = (double) 25e-9 * (timestamp - m_cyclestarttime);
                SWaveform * swf = new SWaveform(timestamp, htime, wf, chan);
                wfb->waveforms.push_back(swf);
                nwf++;
                //std::cout << counter << ": " << nwf << "th waveform at " << std::hex << timestamp;
                //std::cout << " with " << std::dec << wf.size() << " samples." << std::endl;
            }
            timestamp += (uint64_t) wf.size();
            timestamp += (uint64_t) val;
            wf.clear();
            //std::cout << "Skip " << val << " samples to " << std::hex << timestamp << std::dec << std::endl;
        } else {
            wf.push_back(val);
        }
        if (endblock) {
            if (wf.size() > 0) {
                // Create new waveform from samples
                double htime = (double) 25e-9 * (timestamp - m_cyclestarttime);
                SWaveform * swf = new SWaveform(timestamp, htime, wf, chan);
                wfb->waveforms.push_back(swf);
                nwf++;
                //std::cout << nwf << "th waveform at " << std::hex << timestamp;
                //std::cout << " with " << std::dec << wf.size() << " samples." << std::endl;
            }
            // Unless it's the first channel do a shift to get from the previous channel
            channel++;
            chanbit = chanbit << 1;
            while (((includedchannels & chanbit) == 0x0) && (chanbit != 0x0)) {
                channel++;
                chanbit = chanbit << 1;
            }
            //std::cout << "Channel = " << (int) channel << ", chanbit = " << std::hex << chanbit << std::dec <<  std::endl;
            if (chanbit != 0x0) {
                //std::cout << "Getting channel for board = " << boardid << ", channel = " << (int) channel << std::endl;
                chan = dtr()->channel(boardid, channel);
                if (chan == NULL) std::cout << "[WTF] unexpected NULL channel." << std::endl;
            } else {
                //std::cout << "[WARNING] zero chanbit, channel will be NULL." << std::endl;
                chan = NULL;
            }
            timestamp = blocktimestamp;
            wf.clear();
        }
    }
    return wfb;
}