#include <stdint.h>
#include <QString>
#include <QDebug>
#include <QDateTime>
#include "tslib.h"
#include "sendWRcmd.h"
#include "shared_mem_buffer.h"


void indat_PrnPwr(void);


void sendWRcmd_INI(void)
{

    //sendWRcmd_clrCmdStack();
    //sendWRcmd_clrCmd4Stack();
    sendFDcmd_clrStack();

}

// Command Stack for commands without parameters


//static uint16_t nextAsynchsendCmd0[CMDSTACKDEPTH];
//static uint8_t  nrOfCmdsInQueue;

/* convention: use simple (not rotating) FIFO Stack:
Example: nrOfCmdsInQueue=4 then
    nextAsynchsendCmd0[0]=cmd1      // was stored as first
    nextAsynchsendCmd0[1]=cmd2
    nextAsynchsendCmd0[2]=cmd3
    nextAsynchsendCmd0[3]=cmd4      // came in as last

    Send: [0] first, then move buffer 1 down:
    nextAsynchsendCmd0[0]=cmd2
    nextAsynchsendCmd0[1]=cmd3
    nextAsynchsendCmd0[2]=cmd4
    nextAsynchsendCmd0[3]=0;
    nrOfCmdsInQueue=3 now
*/

void sendWRcmd_clrCmdStack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMDSTACKDEPTH; nn++)
        SharedMem::write()->nextAsynchsendCmd0[nn]=0;
    SharedMem::write()->nrOfCmdsInQueue=0;
}

bool sendWRcmd_setSendCommand0(uint16_t nextCmd)
{
    // write Command to memory, wait for transport
    uint8_t ciq=SharedMem::read()->nrOfCmdsInQueue;
    if (ciq>=CMDSTACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }

    SharedMem::write()->nextAsynchsendCmd0[ciq++]=nextCmd;
    SharedMem::write()->nrOfCmdsInQueue=ciq;

    //qDebug() << "PI cmd queued:"<< nextCmd << ", saved, pp=" << nrOfCmdsInQueue;
    return true;                // ok, will be sent
}

uint16_t sendWRcmd_getSendCommand0(void)
{
    uint16_t nxtAsynchCmd, data;
    uint8_t nn;
    uint8_t ciq=SharedMem::read()->nrOfCmdsInQueue;

    if (ciq==0 || ciq>CMDSTACKDEPTH)
        return 0;   // error
    nxtAsynchCmd=SharedMem::read()->nextAsynchsendCmd0[0];

    // move Puffer down by one element
    if (ciq>0) ciq--;
    for (nn=0; nn<ciq; nn++)
    {
        data=SharedMem::read()->nextAsynchsendCmd0[nn+1];
        SharedMem::write()->nextAsynchsendCmd0[nn]=data;
    }
    SharedMem::write()->nrOfCmdsInQueue=ciq;
    //qDebug() << "PI cmd queued:"<< nxtAsynchCmd << ", restored, pp now =" << nrOfCmdsInQueue;
    return nxtAsynchCmd;
}

//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
// Command Stack for commands with 4 parameters


/*
static uint16_t nextAsynchsendCmd4[CMD4STACKDEPTH];
static uint8_t  nextCmd4para1[CMD4STACKDEPTH];
static uint8_t  nextCmd4para2[CMD4STACKDEPTH];
static uint8_t  nextCmd4para3[CMD4STACKDEPTH];
static uint8_t  nextCmd4para4[CMD4STACKDEPTH];
static uint8_t  nrOfCmds4InQueue;
*/

/*
void sendWRcmd_clrCmd4Stack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMD4STACKDEPTH; nn++)
    {
        SharedMem::write()->nextAsynchsendCmd4[nn]=0;
        SharedMem::write()->nextCmd4para1[nn]=0;
        SharedMem::write()->nextCmd4para2[nn]=0;
        SharedMem::write()->nextCmd4para3[nn]=0;
        SharedMem::write()->nextCmd4para4[nn]=0;
    }
    SharedMem::write()->nrOfCmds4InQueue=0;
}


bool sendWRcmd_setSendCommand4(uint16_t nextCmd, uint8_t dat1, uint8_t dat2, uint8_t dat3, uint8_t dat4)
{
    // write Command to memory, wait for transport
    uint8_t ciq;
    ciq=SharedMem::read()->nrOfCmds4InQueue;

    if (ciq>=CMD4STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }

    SharedMem::write()->nextAsynchsendCmd4[ciq]=nextCmd;
    SharedMem::write()->nextCmd4para1[ciq]=dat1;
    SharedMem::write()->nextCmd4para2[ciq]=dat2;
    SharedMem::write()->nextCmd4para3[ciq]=dat3;
    SharedMem::write()->nextCmd4para4[ciq]=dat4;
    ciq++;
    SharedMem::write()->nrOfCmds4InQueue=ciq;

    //qDebug() << QDateTime::currentDateTime().time()
    //         << "sendWRcmd 4 byte saved, pp=" << nextCmd
    //         << " para: " << SharedMem::getDataConst()->nextCmd4para1[pp];

    return true;                // ok, will be sent
}
*/
/*
uint16_t sendWRcmd_getSendCommand4(uint8_t *dat1, uint8_t *dat2, uint8_t *dat3, uint8_t *dat4)
{
    uint16_t nxtAsynchCmd;
    uint8_t nn, ll;

    if (nrOfCmds4InQueue==0 || nrOfCmds4InQueue>CMD4STACKDEPTH)
        return 0;   // error
    nxtAsynchCmd=nextAsynchsendCmd4[0];
    *dat1=nextCmd4para1[0];
    *dat2=nextCmd4para2[0];
    *dat3=nextCmd4para3[0];
    *dat4=nextCmd4para4[0];
    //qDebug() << "cmd4  restored to send from [0];  pp=" << nrOfCmds4InQueue;
    //qDebug() << " data1: " << nextCmd4para1[0] << " data2: " << nextCmd4para2[0] <<
    //            " data3: " << nextCmd4para3[0] << " data4: " << nextCmd4para4[0];

    // move Puffer down by one element
        if (CMD4STACKDEPTH>0)
        ll=CMD4STACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
    {
        nextAsynchsendCmd4[nn]=nextAsynchsendCmd4[nn+1];
        nextCmd4para1[nn]=nextCmd4para1[nn+1];
        nextCmd4para2[nn]=nextCmd4para2[nn+1];
        nextCmd4para3[nn]=nextCmd4para3[nn+1];
        nextCmd4para4[nn]=nextCmd4para4[nn+1];
    }
    if (nrOfCmds4InQueue>0)
        nrOfCmds4InQueue--;
    //qDebug() << "cmd4  after push down: pp=" << nrOfCmds4InQueue;
    return nxtAsynchCmd;
}


*/
/*
uint16_t sendWRcmd_getSendCommand4(uint8_t *dat1, uint8_t *dat2, uint8_t *dat3, uint8_t *dat4)
{
    uint16_t nxtAsynchCmd, data;
    uint8_t nn;
    uint8_t ciq=SharedMem::read()->nrOfCmds4InQueue;

    if (ciq==0 || ciq > CMD4STACKDEPTH)
        return 0;   // error

    nxtAsynchCmd=SharedMem::read()->nextAsynchsendCmd4[0];
    *dat1=SharedMem::read()->nextCmd4para1[0];
    *dat2=SharedMem::read()->nextCmd4para2[0];
    *dat3=SharedMem::read()->nextCmd4para3[0];
    *dat4=SharedMem::read()->nextCmd4para4[0];
    //qDebug() << "cmd4  restored to send from [0];  pp=" << nrOfCmds4InQueue;
    //qDebug() << " data1: " << nextCmd4para1[0] << " data2: " << nextCmd4para2[0] <<
    //            " data3: " << nextCmd4para3[0] << " data4: " << nextCmd4para4[0];

    // move Puffer down by one element
    if (ciq>0) ciq--;

    for (nn=0; nn<ciq; nn++)
    {
        data=SharedMem::read()->nextAsynchsendCmd4[nn+1];
        SharedMem::write()->nextAsynchsendCmd4[nn]=data;
        data=SharedMem::read()->nextCmd4para1[nn+1];
        SharedMem::write()->nextCmd4para1[nn]=data;
        data=SharedMem::read()->nextCmd4para2[nn+1];
        SharedMem::write()->nextCmd4para2[nn]=data;
        data=SharedMem::read()->nextCmd4para3[nn+1];
        SharedMem::write()->nextCmd4para3[nn]=data;
        data=SharedMem::read()->nextCmd4para4[nn+1];
        SharedMem::write()->nextCmd4para4[nn]=data;
    }

    SharedMem::write()->nrOfCmds4InQueue=ciq;
    return nxtAsynchCmd;
}
*/


/*
void sendWRcmd_clrCmd8Stack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMD8STACKDEPTH; nn++)
    {
        SharedMem::write()->nextAsynchsendCmd8[nn]=0;
        SharedMem::write()->nextCmd8para1[nn]=0;
        SharedMem::write()->nextCmd8para2[nn]=0;
        SharedMem::write()->nextCmd8para3[nn]=0;
        SharedMem::write()->nextCmd8para4[nn]=0;
    }
    SharedMem::write()->nrOfCmds8InQueue=0;
}

bool sendWRcmd_setSendCommand8(uint16_t nextCmd, uint8_t dat1, uint8_t dat2, uint16_t dat3, uint32_t dat4)
{
    // write Command to memory, wait for transport
    uint8_t ciq;
    ciq=SharedMem::read()->nrOfCmds8InQueue;

    if (ciq>=CMD8STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    SharedMem::write()->nextAsynchsendCmd8[ciq]=nextCmd;
    SharedMem::write()->nextCmd8para1[ciq]=dat1;
    SharedMem::write()->nextCmd8para2[ciq]=dat2;
    SharedMem::write()->nextCmd8para3[ciq]=dat3;
    SharedMem::write()->nextCmd8para4[ciq]=dat4;
    ciq++;
    SharedMem::write()->nrOfCmds8InQueue=ciq;
    return true;                // ok, will be sent
}

uint16_t sendWRcmd_getSendCommand8(uint8_t *dat1, uint8_t *dat2, uint16_t *dat3, uint32_t *dat4)
{
    uint16_t nxtAsynchCmd, data;
    uint8_t nn;

    uint8_t ciq=SharedMem::read()->nrOfCmds8InQueue;

    if (ciq==0 || ciq > CMD8STACKDEPTH)
        return 0;   // error

    nxtAsynchCmd=SharedMem::read()->nextAsynchsendCmd8[0];
    *dat1=SharedMem::read()->nextCmd8para1[0];
    *dat2=SharedMem::read()->nextCmd8para2[0];
    *dat3=SharedMem::read()->nextCmd8para3[0];
    *dat4=SharedMem::read()->nextCmd8para4[0];

    // move buffer down by one element       
    if (ciq>0) ciq--;

    for (nn=0; nn<ciq; nn++)
    {
        data=SharedMem::read()->nextAsynchsendCmd8[nn+1];
        SharedMem::write()->nextAsynchsendCmd8[nn]=data;
        data=SharedMem::read()->nextCmd8para1[nn+1];
        SharedMem::write()->nextCmd8para1[nn]=data;
        data=SharedMem::read()->nextCmd8para2[nn+1];
        SharedMem::write()->nextCmd8para2[nn]=data;
        data=SharedMem::read()->nextCmd8para3[nn+1];
        SharedMem::write()->nextCmd8para3[nn]=data;
        data=SharedMem::read()->nextCmd8para4[nn+1];
        SharedMem::write()->nextCmd8para4[nn]=data;
    }
    SharedMem::write()->nrOfCmds8InQueue=ciq;
    return nxtAsynchCmd;
}
*/


//static uint8_t sendAsynchDataBuf[160]; // no stack, only ONE buffer
//static uint8_t sendAsyDatLen;

bool sendWRcmd_setSendBlock160(uint8_t leng, uint8_t *buf)
{
    if (leng>SENDASYDAT_BUFFSIZE) leng=SENDASYDAT_BUFFSIZE;
    SharedMem::write()->sendAsyDatLen=leng;
    tslib_strclr(SharedMem::write()->sendAsynchDataBuf, 0, SENDASYDAT_BUFFSIZE);
    for (uint8_t nn=0; nn<leng; nn++)
        SharedMem::write()->sendAsynchDataBuf[nn]=buf[nn];
    return true;                // ok, will be sent
}

uint8_t sendWRcmd_getSendBlock160(uint8_t *leng, uint8_t *buf)
{    

    uint8_t  dl=SharedMem::read()->sendAsyDatLen;
    *leng=dl;
    for (uint8_t nn=0; nn<dl; nn++)
        buf[nn]=SharedMem::read()->sendAsynchDataBuf[nn];
    SharedMem::write()->sendAsyDatLen=0;
    //tslib_strclr(SharedMem::write()->sendAsynchDataBuf, 0, SENDASYDAT_BUFFSIZE);
    return *leng;
}


// ------------------------------------------------------------------------------------
// MDB Sendind Data  are store here for next transport to DC (Device Controller)
// Transport to Slave runs every 100ms, answer from mdb-slave (e.g. coin changer) comes rigth
// with next slave answer

// start with: SENDDIRCMD_EXCHGMDB,
    // send crude data from here to DC, DC to mdb slaves, mdb answer, return here within 50ms


//static uint8_t Sdata_mdbSendBuffer[MDBSEND_BUFFSIZE];
//static uint8_t Sdata_mdbSendLen;
/*
uint8_t epi_store64ByteSendData(uint8_t length, uint8_t *buf)
{
    if (length>MDBSEND_BUFFSIZE) length=MDBSEND_BUFFSIZE;

    // HWapi writes data to be forwarded to DC and further to mdb-device
    for (uint8_t nn=0; nn<length; nn++)
        SharedMem::write()->Sdata_mdbSendBuffer[nn]=buf[nn];
    SharedMem::write()->Sdata_mdbSendLen=length;
    return 0;
}

uint8_t gpi_restore64ByteSendData(uint8_t *length, uint8_t *buf)
{
    // datif reads data to forward to dc
    uint8_t  dl=SharedMem::read()->Sdata_mdbSendLen;

    for (uint8_t nn=0; nn<dl; nn++)
        buf[nn]=SharedMem::read()->Sdata_mdbSendBuffer[nn];
    *length=dl;
    SharedMem::write()->Sdata_mdbSendLen=0;
    return 0;
}
*/



//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
//---------------------------------------- Printer Text Fifo -------------------------
//static uint8_t prnDataParameters[];
//static uint8_t prnDataBufferUser;

/*
void epi_storeUserOfSendingTextBuffer(uint8_t user, uint8_t para1, uint8_t para2, uint8_t para3, uint8_t para4 )
{
    // user=1: Text-Print is using this buffer
    //      2: QR-code-Printer is using this buffer
    SharedMem::write()->prnDataBufferUser=user;
    SharedMem::write()->prnDataParameters[0]=para1;
    SharedMem::write()->prnDataParameters[1]=para2;
    SharedMem::write()->prnDataParameters[2]=para3;
    SharedMem::write()->prnDataParameters[3]=para4;

//    qDebug() << "new user stored: " << user;

}

uint8_t gpi_getUserOfSendingTextBuffer(uint8_t *para1, uint8_t *para2, uint8_t *para3, uint8_t *para4)
{
    // user=1: Text-Print is using this buffer
    //      2: QR-code-Printer is using this buffer
    //qDebug() << "returning user "<< prnDataBufferUser;

    *para1=SharedMem::read()->prnDataParameters[0];
    *para2=SharedMem::read()->prnDataParameters[1];
    *para3=SharedMem::read()->prnDataParameters[2];
    *para4=SharedMem::read()->prnDataParameters[3];
    return SharedMem::read()->prnDataBufferUser;

}
*/






    // Sending Text Fifo
// ONE printer doc consists of 20 x 64 byte
// #define MAXNROF_PRNBYTES   64
// #define MAXNROF_PRNBLOCKS  20
//static char Sdata_PRN_TEXT[MAXNROF_PRNBLOCKS][MAXNROF_PRNBYTES];
//static uint8_t pPrnDataBuff;        // points to next PRINTER_BLOCK
//static uint8_t pPrnDataBuff;        // points to next waiting printer text
//  defined above, needed if more then one text is stored (before sent)
// every block will be sent after 100ms, if 8 blocks are stored within this 100ms
// then pointer goes up to 8. Important: FIFO!!!!!!!!
/*
void epi_resetPrinterStack(void)
{
    SharedMem::write()->pPrnDataBuff=0;
}

uint8_t epi_storePrnText(char *buf, uint8_t leng)
{
    // store text from Gui in next higher free memory 0....9

    uint16_t len;
    uint8_t  pp, nn;

    pp=SharedMem::read()->pPrnDataBuff;    // next free memory block with 64byte each
    if (pp>=MAXNROF_PRNBLOCKS)
        return 1;   // not possible, no free mem

    //len=tslib_strlen(buf);    // kennt keine Binärzeichen!!!!!!
    len=leng;
    if (len>MAXNROF_PRNBYTES)
        len=MAXNROF_PRNBYTES;

    tslib_strclr(SharedMem::write()->Sdata_PRN_TEXT[pp], 0, MAXNROF_PRNBYTES);

    for (nn=0; nn<len; nn++)
        SharedMem::write()->Sdata_PRN_TEXT[pp][nn]=buf[nn];     // copy new text into buffer

    if (SharedMem::read()->pPrnDataBuff<MAXNROF_PRNBLOCKS)
        SharedMem::write()->pPrnDataBuff++;                     // inc pointer if end not yet reached
    return 0;       // OK
}



uint8_t gpi_restorePrnText(uint8_t *retbuf)
{
    // read printer text and send to slave, size of retbuf == 64

    // always read from [0] because this is the oldest (Fifo)
    // then move all text lines down by one and dec pointer

    uint8_t  nn, mm, pp=SharedMem::read()->pPrnDataBuff;
    char     buf[MAXNROF_PRNBYTES];

    if (pp==0)    // next free memory block with 64byte each
        return 1;           // no text in buffer

    // example: pp=5: then buffers [0...4] are occupied

    for (nn=0; nn<MAXNROF_PRNBYTES; nn++)
        retbuf[nn] = uint8_t (SharedMem::read()->Sdata_PRN_TEXT[0][nn]);     // restore oldest text

    // now copy textline [1] to [0], then
    // copy textline [2] to [1], then
    // copy textline [3] to [2] .... upto [pp-1] to [pp-2]
    // hint: copying from 9....0 would delete all strings!!!!!!
    for (nn=0; nn<(pp-1); nn++)
    {
        for (mm=0; mm<MAXNROF_PRNBYTES; mm++)
            buf[mm]=SharedMem::read()->Sdata_PRN_TEXT[nn+1][mm];
        for (mm=0; mm<MAXNROF_PRNBYTES; mm++)
            SharedMem::write()->Sdata_PRN_TEXT[nn][mm]=buf[mm];
    }
    if (pp>0) pp--;
        SharedMem::write()->pPrnDataBuff=pp;

    // example: pp=4: then buffers [0...3] are still occupied, pp=0: all buffers empty

    // now clear highest copyed line (which became free now)
    tslib_strclr(SharedMem::write()->Sdata_PRN_TEXT[pp], 0, MAXNROF_PRNBYTES);

    // optionally: clear all remaining higher lines:
    for (nn=(pp+1); nn<MAXNROF_PRNBLOCKS; nn++)
         tslib_strclr(SharedMem::write()->Sdata_PRN_TEXT[nn], 0, MAXNROF_PRNBYTES);

    return 0;
}

uint8_t gpi_chk4remainingText(void)
{
    // retval: 0: no more textline left (to send)  >0: nr of 64byte-blocks
    return (SharedMem::read()->pPrnDataBuff);
}
*/









// ---------------------------------------------------------------------------------
//   11.4.23 neu, Kommando direkt an "FastDevice"-protokoll senden, nicht mehr umsetzen
// ---------------------------------------------------------------------------------
/*
// header
static uint8_t nextFDwrCmd[FDCMD_STACKDEPTH];
static uint8_t nextFDrdCmd[FDCMD_STACKDEPTH];
static uint8_t nextFDblkNr[FDCMD_STACKDEPTH];
static uint8_t nextFDshort[FDCMD_STACKDEPTH];

// short data
static uint8_t  nextFDpara1[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara2[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara3[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara4[FDCMD_STACKDEPTH];
// long data:
static uint8_t longFDlength[FDCMD_STACKDEPTH];
static uint8_t longFDpara[FDCMD_STACKDEPTH][64];

static uint8_t  p_nextFDcmdsInQueue;
*/

/* convention: use simple (not rotating) FIFO Stack:
Example: nrOfCmdsInQueue=4 then
    nextAsynchsendCmd0[0]=cmd1      // was stored as first
    nextAsynchsendCmd0[1]=cmd2
    nextAsynchsendCmd0[2]=cmd3
    nextAsynchsendCmd0[3]=cmd4      // came in as last

    Send: [0] first, then move buffer 1 down:
    nextAsynchsendCmd0[0]=cmd2
    nextAsynchsendCmd0[1]=cmd3
    nextAsynchsendCmd0[2]=cmd4
    nextAsynchsendCmd0[3]=0;
    nrOfCmdsInQueue=3 now
*/


void sendFDcmd_clrStack(void)
{
    uint8_t nn;
    for (nn=0; nn<FDCMD_STACKDEPTH; nn++)
    {
        SharedMem::write()->nextFDwrCmd[nn]=0;
        SharedMem::write()->nextFDrdCmd[nn]=0;
        SharedMem::write()->nextFDblkNr[nn]=0;
        SharedMem::write()->nextFDshort[nn]=0;
        SharedMem::write()->nextFDpara1[nn]=0;
        SharedMem::write()->nextFDpara2[nn]=0;
        SharedMem::write()->nextFDpara3[nn]=0;
        SharedMem::write()->nextFDpara4[nn]=0;
        SharedMem::write()->longFDlength[nn]=0;
        memset(&SharedMem::write()->longFDpara[nn][0],0,64);
    }

    SharedMem::write()->p_nextFDcmdsInQueue=0;
}

bool sendFDcmd_set(uint8_t nextWrCmd, uint8_t nextRdCmd, uint8_t blockNum, uint8_t dat1, uint8_t dat2, uint8_t dat3, uint8_t dat4)
{

    // write Command to memory, wait for transport
    uint8_t pFDcmd=SharedMem::read()->p_nextFDcmdsInQueue;

    if (pFDcmd >=FDCMD_STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    SharedMem::write()->nextFDwrCmd[pFDcmd]=nextWrCmd;
    SharedMem::write()->nextFDrdCmd[pFDcmd]=nextRdCmd;
    SharedMem::write()->nextFDblkNr[pFDcmd]=blockNum;
    SharedMem::write()->nextFDpara1[pFDcmd]=dat1;
    SharedMem::write()->nextFDpara2[pFDcmd]=dat2;
    SharedMem::write()->nextFDpara3[pFDcmd]=dat3;
    SharedMem::write()->nextFDpara4[pFDcmd]=dat4;
    //qDebug() << "data with 4 data byte saved, pp=" << pFDcmd;
    //qDebug() << " dat1=" << nextCmd4para1[pFDcmd] << " dat2=" << nextCmd4para2[pFDcmd]
    //             << " dat3=" << nextCmd4para3[pFDcmd]  << " dat4=" << nextCmd4para4[pFDcmd];
    SharedMem::write()->nextFDshort[pFDcmd]=1; // 1=short
    pFDcmd++;
    SharedMem::write()->p_nextFDcmdsInQueue=pFDcmd;

    return true;                // ok, will be sent
}


bool longFDcmd_set(uint8_t nextWrCmd, uint8_t nextRdCmd, uint8_t blockNum, uint8_t length, uint8_t *data)
{
    // write Command to memory, wait for transport
    // data buffer size always 64! data[64], padded with 0
    uint8_t nn;
    uint8_t pFDcmd=SharedMem::read()->p_nextFDcmdsInQueue;

    if (pFDcmd>=FDCMD_STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    SharedMem::write()->nextFDwrCmd[pFDcmd]=nextWrCmd;
    SharedMem::write()->nextFDrdCmd[pFDcmd]=nextRdCmd;
    SharedMem::write()->nextFDblkNr[pFDcmd]=blockNum;

    SharedMem::write()->longFDlength[pFDcmd]=length;

    for (nn=0; nn<64; nn++)
        SharedMem::write()->longFDpara[pFDcmd][nn]=data[nn];

    SharedMem::write()->nextFDshort[pFDcmd]=2;
    pFDcmd++;
    SharedMem::write()->p_nextFDcmdsInQueue=pFDcmd;
    return true;                // ok, will be sent
}


bool longFDcmd_set(uint8_t nextWrCmd, uint8_t nextRdCmd, uint8_t blockNum, uint8_t length, QByteArray *data)
{
    // write Command to memory, wait for transport
    // data buffer size always 64! data[64], padded with 0
    uint8_t nn;
    uint8_t pFDcmd=SharedMem::read()->p_nextFDcmdsInQueue;
    char    ctmp;

    if (pFDcmd>=FDCMD_STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    SharedMem::write()->nextFDwrCmd[pFDcmd]=nextWrCmd;
    SharedMem::write()->nextFDrdCmd[pFDcmd]=nextRdCmd;
    SharedMem::write()->nextFDblkNr[pFDcmd]=blockNum;

    SharedMem::write()->longFDlength[pFDcmd]=length;

    for (nn=0; nn<64; nn++)
    {
        ctmp=data->at(nn);
        SharedMem::write()->longFDpara[pFDcmd][nn]=ctmp;
    }

    SharedMem::write()->nextFDshort[pFDcmd]=2;
    pFDcmd++;
    SharedMem::write()->p_nextFDcmdsInQueue=pFDcmd;
    return true;                // ok, will be sent
}


bool sendFDcmd_get(uint8_t *nextWrCmd, uint8_t *nextRdCmd, uint8_t *blockNum, uint8_t *dat1, uint8_t *dat2, uint8_t *dat3, uint8_t *dat4)
{
    uint8_t nn, mm, data;  // ll
    uint8_t pFDcmd=SharedMem::read()->p_nextFDcmdsInQueue;

    if (pFDcmd==0 || pFDcmd>FDCMD_STACKDEPTH)
        return false;           // not possible

    *nextWrCmd=SharedMem::read()->nextFDwrCmd[0];
    *nextRdCmd=SharedMem::read()->nextFDrdCmd[0];
    *blockNum=SharedMem::read()->nextFDblkNr[0];
    *dat1=SharedMem::read()->nextFDpara1[0];
    *dat2=SharedMem::read()->nextFDpara2[0];
    *dat3=SharedMem::read()->nextFDpara3[0];
    *dat4=SharedMem::read()->nextFDpara4[0];
    //qDebug() << "sendFDcmd_get [0];  pp=" << SharedMem::read()->p_nextFDcmdsInQueue;
    //qDebug() << " data1: " << SharedMem::read()->nextCmd4para1[0] << " data2: " << SharedMem::read()->nextCmd4para2[0] <<
    //            " data3: " << SharedMem::read()->nextCmd4para3[0] << " data4: " << SharedMem::read()->nextCmd4para4[0];

    // move Puffer down by one element
    //if (FDCMD_STACKDEPTH>0)
    //    ll=FDCMD_STACKDEPTH-1;
    //else
    //    ll=0;

    if (pFDcmd>0) pFDcmd--; else pFDcmd=0;

    //for (nn=0; nn<ll; nn++)
    for (nn=0; nn<pFDcmd; nn++)
    {
        data=SharedMem::read()->nextFDwrCmd[nn+1];
        SharedMem::write()->nextFDwrCmd[nn]=data;
        data=SharedMem::read()->nextFDrdCmd[nn+1];
        SharedMem::write()->nextFDrdCmd[nn]=data;
        data=SharedMem::read()->nextFDblkNr[nn+1];
        SharedMem::write()->nextFDblkNr[nn]=data;
        data=SharedMem::read()->nextFDpara1[nn+1];
        SharedMem::write()->nextFDpara1[nn]=data;
        data=SharedMem::read()->nextFDpara2[nn+1];
        SharedMem::write()->nextFDpara2[nn]=data;
        data=SharedMem::read()->nextFDpara3[nn+1];
        SharedMem::write()->nextFDpara3[nn]=data;
        data=SharedMem::read()->nextFDpara4[nn+1];
        SharedMem::write()->nextFDpara4[nn]=data;

        data=SharedMem::read()->nextFDshort[nn+1];
        SharedMem::write()->nextFDshort[nn] = data;

        data=SharedMem::read()->longFDlength[nn+1];
        SharedMem::write()->longFDlength[nn] = data;

        for (mm=0; mm<64; mm++)
        {
            SharedMem::write()->longFDpara[nn][mm] = SharedMem::read()->longFDpara[nn+1][mm];
        }

    }

    SharedMem::write()->p_nextFDcmdsInQueue=pFDcmd;
    //qDebug() << "cmd4  after push down: pp=" << nrOfCmds4InQueue;

    // clear released buffer:
    //for (nn=p_nextFDcmdsInQueue; nn<FDCMD_STACKDEPTH; nn++)
    //{
        SharedMem::write()->nextFDwrCmd[pFDcmd]=0;
        SharedMem::write()->nextFDrdCmd[pFDcmd]=0;
        SharedMem::write()->nextFDblkNr[pFDcmd]=0;

        SharedMem::write()->nextFDpara1[pFDcmd]=0;
        SharedMem::write()->nextFDpara2[pFDcmd]=0;
        SharedMem::write()->nextFDpara3[pFDcmd]=0;
        SharedMem::write()->nextFDpara4[pFDcmd]=0;
        SharedMem::write()->nextFDshort[pFDcmd]=0;
        SharedMem::write()->longFDlength[pFDcmd]=0;
        for (mm=0; mm<64; mm++)
            SharedMem::write()->longFDpara[pFDcmd][mm] = 0;
    //}

/*
    qDebug() << "sendFDcmd_set, stack now: " << p_nextFDcmdsInQueue;
    for (nn=0; nn<16; nn++)
    {
        qDebug() << "header: " << nextFDwrCmd[nn] << " / " << nextFDrdCmd[nn] << " / "<< nextFDblkNr[nn] << " / " << nextFDshort[nn];

        qDebug() << "    short data: " << nextFDpara1[nn] << " / "<< nextFDpara2[nn] << " / "<< nextFDpara3[nn]<< " / "<< nextFDpara4[nn];

        qDebug() << "    long data: " << longFDlength[nn] << " / "<< longFDpara[nn][0] << " / "<< longFDpara[nn][1]
                 << " / "<< longFDpara[nn][2] << " / "<< longFDpara[nn][3] << " / "<< longFDpara[nn][4];
    }
*/
    return true;                // ok, will be sent
}

uint8_t checkNextFDcmd(void)
{
    // return 0: no command waiting
//          1: short cmd
//          2: long cmd
    //qDebug() << "chk nxt fd cmd: "<<p_nextFDcmdsInQueue<<" "<<nextFDshort[0]<<" "<<nextFDshort[1]<<" "<<nextFDshort[2]<<" "<<nextFDshort[3];
    if (SharedMem::read()->p_nextFDcmdsInQueue==0)
        return 0;
    if (SharedMem::read()->nextFDshort[0]==1)
        return 1;
    return 2;
}

uint8_t check4usedFDstack(void)
{
    // returns number of waiting command, max FDCMD_STACKDEPTH
    return SharedMem::read()->p_nextFDcmdsInQueue;
}


uint8_t check4freeFDstack(void)
{
    // returns number of free places in command stack
    return FDCMD_STACKDEPTH - SharedMem::read()->p_nextFDcmdsInQueue;
}

bool longFDcmd_get(uint8_t *nextWrCmd, uint8_t *nextRdCmd, uint8_t *blockNum, uint8_t *length, uint8_t *data)
{
    uint8_t nn, mm, uctmp;
    uint8_t pFDcmd=SharedMem::read()->p_nextFDcmdsInQueue;

    if (pFDcmd==0 || pFDcmd>FDCMD_STACKDEPTH)
        return false;           // not possible

    *nextWrCmd=SharedMem::read()->nextFDwrCmd[0];
    *nextRdCmd=SharedMem::read()->nextFDrdCmd[0];
    *blockNum=SharedMem::read()->nextFDblkNr[0];
    *length   = SharedMem::read()->longFDlength[0];
    for (mm=0; mm<64; mm++)
        data[mm] = SharedMem::read()->longFDpara[0][mm];

    if (pFDcmd>0) pFDcmd--; else pFDcmd=0;

    //for (nn=0; nn<ll; nn++)
    for (nn=0; nn<pFDcmd; nn++)
    {
        uctmp=SharedMem::read()->nextFDwrCmd[nn+1];
        SharedMem::write()->nextFDwrCmd[nn]=uctmp;
        uctmp=SharedMem::read()->nextFDrdCmd[nn+1];
        SharedMem::write()->nextFDrdCmd[nn]=uctmp;
        uctmp=SharedMem::read()->nextFDblkNr[nn+1];
        SharedMem::write()->nextFDblkNr[nn]=uctmp;

        uctmp=SharedMem::read()->nextFDpara1[nn+1];
        SharedMem::write()->nextFDpara1[nn]=uctmp;
        uctmp=SharedMem::read()->nextFDpara2[nn+1];
        SharedMem::write()->nextFDpara2[nn]=uctmp;
        uctmp=SharedMem::read()->nextFDpara3[nn+1];
        SharedMem::write()->nextFDpara3[nn]=uctmp;
        uctmp=SharedMem::read()->nextFDpara4[nn+1];
        SharedMem::write()->nextFDpara4[nn]=uctmp;

        uctmp=SharedMem::read()->nextFDshort[nn+1];
        SharedMem::write()->nextFDshort[nn]=uctmp;
        uctmp=SharedMem::read()->longFDlength[nn+1];
        SharedMem::write()->longFDlength[nn]=uctmp;

        for (mm=0; mm<64; mm++)
        {
            SharedMem::write()->longFDpara[nn][mm] = SharedMem::read()->longFDpara[nn+1][mm];
        }

    }

    SharedMem::write()->p_nextFDcmdsInQueue=pFDcmd;
    // clear released buffer:
    SharedMem::write()->nextFDwrCmd[pFDcmd]=0;
    SharedMem::write()->nextFDrdCmd[pFDcmd]=0;
    SharedMem::write()->nextFDblkNr[pFDcmd]=0;

    SharedMem::write()->nextFDpara1[pFDcmd]=0;
    SharedMem::write()->nextFDpara2[pFDcmd]=0;
    SharedMem::write()->nextFDpara3[pFDcmd]=0;
    SharedMem::write()->nextFDpara4[pFDcmd]=0;
    SharedMem::write()->nextFDshort[pFDcmd]=0;
    SharedMem::write()->longFDlength[pFDcmd]=0;
    for (mm=0; mm<64; mm++)
        SharedMem::write()->longFDpara[pFDcmd][mm] = 0;

    return true;                // ok, will be sent
}