#include "mythen3.h"
#include "clogger.h"
#include "common.h"
#include "sls/ansi.h"
#include "sls/sls_detector_defs.h"
#include "slsDetectorServer_defs.h"

#include <string.h>

/*
// Common C/C++ structure to handle pattern data
typedef struct __attribute__((packed)) {
    uint64_t word[MAX_PATTERN_LENGTH];
    uint64_t ioctrl;
    uint32_t limits[2];
    // loop0 start, loop0 stop .. loop2 start, loop2 stop
    uint32_t loop[6];
    uint32_t nloop[3];
    uint32_t wait[3];
    uint64_t waittime[3];
} patternParameters;
*/

int chipStatusRegister = 0;

int setBit(int ibit, int patword) { return patword |= (1 << ibit); }

int clearBit(int ibit, int patword) { return patword &= ~(1 << ibit); }

int getChipStatusRegister() { return chipStatusRegister; }

int gainCapsToCsr(int caps) {
    // Translates bit representation
    int csr = 0;
    if (!(caps & M3_C10pre))
        csr |= 1 << _CSR_C10pre;
    if (caps & M3_C15sh)
        csr |= 1 << CSR_C15sh;
    if (caps & M3_C30sh)
        csr |= 1 << CSR_C30sh;
    if (caps & M3_C50sh)
        csr |= 1 << CSR_C50sh;
    if (caps & M3_C225ACsh)
        csr |= 1 << CSR_C225ACsh;
    if (!(caps & M3_C15pre))
        csr |= 1 << _CSR_C15pre;

    return csr;
}

int csrToGainCaps(int csr) {
    // Translates bit representation
    int caps = 0;
    if (!(csr & (1 << _CSR_C10pre)))
        caps |= M3_C10pre;
    if (csr & (1 << CSR_C15sh))
        caps |= M3_C15sh;
    if (csr & (1 << CSR_C30sh))
        caps |= M3_C30sh;
    if (csr & (1 << CSR_C50sh))
        caps |= M3_C50sh;
    if (csr & (1 << CSR_C225ACsh))
        caps |= M3_C225ACsh;
    if (!(csr & (1 << _CSR_C15pre)))
        caps |= M3_C15pre;

    return caps;
}

patternParameters *setChipStatusRegisterPattern(int csr) {
    int iaddr = 0;
    int nbits = 18;
    int error = 0;
    // int start=0, stop=MAX_PATTERN_LENGTH, loop=0;
    int patword = 0;

    patternParameters *pat = malloc(sizeof(patternParameters));
    memset(pat, 0, sizeof(patternParameters));

    patword = setBit(SIGNAL_STATLOAD, patword);
    for (int i = 0; i < 2; i++)
        pat->word[iaddr++] = patword;
    patword = setBit(SIGNAL_resStorage, patword);
    patword = setBit(SIGNAL_resCounter, patword);
    for (int i = 0; i < 8; i++)
        pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_resStorage, patword);
    patword = clearBit(SIGNAL_resCounter, patword);
    for (int i = 0; i < 8; i++)
        pat->word[iaddr++] = patword;
    //#This version of the serializer pushes in the MSB first (compatible with
    //the CSR bit numbering)
    for (int ib = nbits - 1; ib >= 0; ib--) {
        if (csr & (1 << ib))
            patword = setBit(SIGNAL_serialIN, patword);
        else
            patword = clearBit(SIGNAL_serialIN, patword);
        for (int i = 0; i < 4; i++)
            pat->word[iaddr++] = patword;
        patword = setBit(SIGNAL_CHSclk, patword);
        pat->word[iaddr++] = patword;
        patword = clearBit(SIGNAL_CHSclk, patword);
        pat->word[iaddr++] = patword;
    }

    patword = clearBit(SIGNAL_serialIN, patword);
    for (int i = 0; i < 2; i++)
        pat->word[iaddr++] = patword;
    patword = setBit(SIGNAL_STO, patword);
    for (int i = 0; i < 5; i++)
        pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_STO, patword);
    for (int i = 0; i < 5; i++)
        pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_STATLOAD, patword);
    for (int i = 0; i < 5; i++)
        pat->word[iaddr++] = patword;

    if (iaddr >= MAX_PATTERN_LENGTH) {
        LOG(logERROR, ("Addr 0x%x is past max_address_length 0x%x!\n", iaddr,
                       MAX_PATTERN_LENGTH));
        error = 1;
    }
    // set pattern wait address
    for (int i = 0; i <= 2; i++)
        pat->wait[i] = MAX_PATTERN_LENGTH - 1;
    // pattern loop
    for (int i = 0; i <= 2; i++) {
        // int stop = MAX_PATTERN_LENGTH - 1, nloop = 0;
        pat->loop[i * 2 + 0] = MAX_PATTERN_LENGTH - 1;
        pat->loop[i * 2 + 1] = MAX_PATTERN_LENGTH - 1;
        pat->nloop[i] = 0;
    }

    // pattern limits
    {
        pat->limits[0] = 0;
        pat->limits[1] = iaddr;
    }

    if (error != 0) {
        free(pat);
        return NULL;
    }
    chipStatusRegister = csr;
    return pat;
}

patternParameters *setInterpolation(int mask) {
    int csr;
    if (mask)
        csr = chipStatusRegister | (1 << CSR_interp);
    else
        csr = chipStatusRegister & ~(1 << CSR_interp);

    return setChipStatusRegisterPattern(csr);
}

patternParameters *setPumpProbe(int mask) {
    int csr;
    if (mask)
        csr = chipStatusRegister | (1 << CSR_pumprobe);
    else
        csr = chipStatusRegister & ~(1 << CSR_pumprobe);

    return setChipStatusRegisterPattern(csr);
}
patternParameters *setDigitalPulsing(int mask) {

    int csr;
    if (mask)
        csr = chipStatusRegister | (1 << CSR_dpulse);
    else
        csr = chipStatusRegister & ~(1 << CSR_dpulse);

    return setChipStatusRegisterPattern(csr);
}
patternParameters *setAnalogPulsing(int mask) {

    int csr;
    if (mask)
        csr = chipStatusRegister | (1 << CSR_apulse);
    else
        csr = chipStatusRegister & ~(1 << CSR_apulse);

    return setChipStatusRegisterPattern(csr);
}
patternParameters *setNegativePolarity(int mask) {

    int csr;
    if (mask)
        csr = chipStatusRegister | (1 << CSR_invpol);
    else
        csr = chipStatusRegister & ~(1 << CSR_invpol);

    return setChipStatusRegisterPattern(csr);
}

patternParameters *setChannelRegisterChip(int ichip, int *mask, int *trimbits) {

    patternParameters *pat = malloc(sizeof(patternParameters));
    memset(pat, 0, sizeof(patternParameters));

    // validate
    for (int ichan = ichip * NCHAN_1_COUNTER * NCOUNTERS;
         ichan <
         ichip * NCHAN_1_COUNTER * NCOUNTERS + NCHAN_1_COUNTER * NCOUNTERS;
         ichan++) {
        if (trimbits[ichan] < 0) {
            LOG(logERROR, ("Trimbit value (%d) for channel %d is invalid - "
                           "setting it to 0\n",
                           trimbits[ichan], ichan));
            trimbits[ichan] = 0;
        }
        if (trimbits[ichan] > 63) {
            LOG(logERROR, ("Trimbit value (%d) for channel %d is invalid - "
                           "settings it to 63\n",
                           trimbits[ichan], ichan));
            trimbits[ichan] = 63;
        }
    }
    LOG(logINFO, ("Trimbits validated\n"));

    // trimming
    int error = 0;
    uint64_t patword = 0;
    int iaddr = 0;

    LOG(logDEBUG1, (" Chip %d\n", ichip));
    iaddr = 0;
    patword = 0;
    pat->word[iaddr++] = patword;

    // chip select
    patword = setBit(SIGNAL_TBLoad_1 + ichip, patword);
    pat->word[iaddr++] = patword;

    // reset trimbits
    patword = setBit(SIGNAL_resStorage, patword);
    patword = setBit(SIGNAL_resCounter, patword);
    pat->word[iaddr++] = patword;
    pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_resStorage, patword);
    patword = clearBit(SIGNAL_resCounter, patword);
    pat->word[iaddr++] = patword;
    pat->word[iaddr++] = patword;

    // select first channel
    patword = setBit(SIGNAL_CHSserialIN, patword);
    pat->word[iaddr++] = patword;
    // 1 clk pulse
    patword = setBit(SIGNAL_CHSclk, patword);
    pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_CHSclk, patword);
    // clear 1st channel
    pat->word[iaddr++] = patword;
    patword = clearBit(SIGNAL_CHSserialIN, patword);
    // 2 clk pulses
    for (int i = 0; i < 2; i++) {
        patword = setBit(SIGNAL_CHSclk, patword);
        pat->word[iaddr++] = patword;
        patword = clearBit(SIGNAL_CHSclk, patword);
        pat->word[iaddr++] = patword;
    }

    // for each channel (all chips)
    for (int ich = 0; ich < NCHAN_1_COUNTER; ich++) {
        LOG(logDEBUG1, (" Chip %d, Channel %d\n", ichip, ich));
        int val =
            trimbits[ichip * NCHAN_1_COUNTER * NCOUNTERS + NCOUNTERS * ich] +
            trimbits[ichip * NCHAN_1_COUNTER * NCOUNTERS + NCOUNTERS * ich +
                     1] *
                64 +
            trimbits[ichip * NCHAN_1_COUNTER * NCOUNTERS + NCOUNTERS * ich +
                     2] *
                64 * 64;

        // push 6 0 bits
        for (int i = 0; i < 3; i++) {
            patword = clearBit(SIGNAL_serialIN, patword);
            patword = clearBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;
            patword = setBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;
        }

        for (int i = 0; i < 3; i++) {
            if (mask[i])
                patword = setBit(SIGNAL_serialIN, patword);
            else
                patword = clearBit(SIGNAL_serialIN, patword);
            patword = clearBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;
            patword = setBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;
        }

        // deserialize
        for (int i = 0; i < 18; i++) {
            if (val & (1 << i)) {
                patword = setBit(SIGNAL_serialIN, patword);
            } else {
                patword = clearBit(SIGNAL_serialIN, patword);
            }
            patword = clearBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;

            patword = setBit(SIGNAL_clk, patword);
            pat->word[iaddr++] = patword;
        }
        pat->word[iaddr++] = patword;
        pat->word[iaddr++] = patword;

        // move to next channel
        for (int i = 0; i < 3; i++) {
            patword = setBit(SIGNAL_CHSclk, patword);
            pat->word[iaddr++] = patword;
            patword = clearBit(SIGNAL_CHSclk, patword);
            pat->word[iaddr++] = patword;
        }
    }
    // chip unselect
    patword = clearBit(SIGNAL_TBLoad_1 + ichip, patword);
    pat->word[iaddr++] = patword;

    // last iaddr check
    if (iaddr >= MAX_PATTERN_LENGTH) {
        LOG(logERROR, ("Addr 0x%x is past max_address_length 0x%x!\n", iaddr,
                       MAX_PATTERN_LENGTH));
        error = 1;
    }

    if (iaddr >= MAX_PATTERN_LENGTH) {
        LOG(logERROR, ("Addr 0x%x is past max_address_length 0x%x!\n", iaddr,
                       MAX_PATTERN_LENGTH));
        error = 1;
    }
    // set pattern wait address
    for (int i = 0; i <= 2; i++)
        pat->wait[i] = MAX_PATTERN_LENGTH - 1;
    // pattern loop
    for (int i = 0; i <= 2; i++) {
        // int stop = MAX_PATTERN_LENGTH - 1, nloop = 0;
        pat->loop[i * 2 + 0] = MAX_PATTERN_LENGTH - 1;
        pat->loop[i * 2 + 1] = MAX_PATTERN_LENGTH - 1;
        pat->nloop[i] = 0;
    }

    // pattern limits
    {
        pat->limits[0] = 0;
        pat->limits[1] = iaddr;
    }

    if (error == 0) {

        LOG(logINFO, ("All trimbits have been loaded\n"));
    } else {
        free(pat);
        return NULL;
    }
    return pat;
}