//Copyright (c) Granite Devices Oy

#include <stdio.h>
#include <string.h>

#include "busdevice.h"
#include "sm485.h"
#include <stdarg.h>


#include "simplemotion_private.h"

SM_STATUS smParseReturnData( smbus handle, smuint8 data );

#define HANDLE_STAT(stat) if(stat!=SM_OK)return (stat);
#define HANDLE_STAT_AND_RET(stat,returndata) { if(returndata==RET_INVALID_CMD||returndata==RET_INVALID_PARAM) return SM_ERR_PARAMETER; if(stat!=SM_OK) return (stat); }

enum RecvState {WaitCmdId,WaitAddr,WaitPayloadSize,WaitPayload,WaitCrcHi,WaitCrcLo};
FILE *smDebugOut=NULL;

//useful macros from extracting/storing multibyte values from/to byte buffer
#define bufput32bit(buf, pos, val) *((smuint32*)(smuint8*)((buf)+(pos)))=((smuint32)(val))
#define bufput16bit(buf, pos, val) *((smuint16*)(smuint8*)((buf)+(pos)))=((smuint16)(val))
#define bufput8bit(buf, pos, val) *((smuint8*)(smuint8*)((buf)+(pos)))=((smuint8)(val))
#define bufget32bit(buf, pos) (*((smuint32*)(smuint8*)((buf)+(pos))))
#define bufget16bit(buf, pos) (*((smuint16*)(smuint8*)((buf)+(pos))))
#define bufget8bit(buf, pos) (*((smuint8*)(smuint8*)((buf)+(pos))))


smbool smIsHandleOpen( const smbus handle );

SM_STATUS smReceiveReturnPacket( smbus bushandle );

typedef struct SM_BUS_
{
    smbusdevicehandle bdHandle;
    smbool opened;

    enum RecvState recv_state,recv_state_next;

    smint16 recv_payloadsize;
    smint16 recv_storepos;
    smuint8 recv_rsbuf[SM485_RSBUFSIZE];
    smuint8 recv_cmdid;
    smuint8 recv_addr;
    smuint16 recv_crc;
    smuint16 recv_read_crc_hi;
    smbool receiveComplete;
    smbool transmitBufFull;//set true if user uploads too much commands in one SM transaction. if true, on execute commands, nothing will be sent to bus to prevent unvanted clipped commands and buffer will be cleared
    char busDeviceName[SM_BUSDEVICENAME_LEN];

    smint16 cmd_send_queue_bytes;//for queued device commands
    smint16 cmd_recv_queue_bytes;//recv_queue_bytes counted upwards at every smGetQueued.. and compared to payload size


    SM_STATUS cumulativeSmStatus;
} SM_BUS;


SM_BUS smBus[SM_MAX_BUSES];
smuint16 readTimeoutMs=SM_READ_TIMEOUT;

//init on first smOpenBus call
smbool smInitialized=smfalse;

//if debug message has priority this or above will be printed to debug stream
smVerbosityLevel smDebugThreshold=Trace;

#ifdef ENABLE_DEBUG_PRINTS
void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...)
{
    va_list fmtargs;
    char buffer[1024];

    if(smDebugOut!=NULL && verbositylevel <= smDebugThreshold )
    {
        va_start(fmtargs,format);
        vsnprintf(buffer,sizeof(buffer)-1,format,fmtargs);
        va_end(fmtargs);
        if(handle>=0)
        {
            if(smIsHandleOpen(handle)==smtrue)
            {
                fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer);
            }
        }
        else
            fprintf(smDebugOut,"SMLib: %s",buffer);//no handle given
    }
}
#else
#define smDebug(...)
#endif

void smResetSM485variables(smbus handle)
{
    smBus[handle].recv_state=WaitCmdId;
    smBus[handle].recv_state_next=WaitCmdId;
    smBus[handle].recv_payloadsize=-1;
    smBus[handle].recv_storepos=0;//number of bytes to expect data in cmd, -1=wait cmd header
    smBus[handle].recv_cmdid=0;// cmdid=0 kun ed komento suoritettu
    smBus[handle].recv_addr=255;
    smBus[handle].recv_crc=SM485_CRCINIT;
    smBus[handle].recv_read_crc_hi=0xffff;//bottom bits will be contains only 1 byte when read
    smBus[handle].receiveComplete=smfalse;
    smBus[handle].transmitBufFull=smfalse;
    smBus[handle].cmd_send_queue_bytes=0;
    smBus[handle].cmd_recv_queue_bytes=0;
}


smuint16 calcCRC16(smuint8 data, smuint16 crc)
{
    unsigned int i; /* will index into CRC lookup */

    i = (crc>>8) ^ data; /* calculate the CRC  */
    crc = (((crc&0xff) ^ table_crc16_hi[i])<<8) | table_crc16_lo[i];

    return crc;
}

smuint16 calcCRC16Buf(const char *buffer, smuint16 buffer_length)
{
    smuint8 crc_hi = 0xFF; /* high CRC byte initialized */
    smuint8 crc_lo = 0xFF; /* low CRC byte initialized */
    unsigned int i; /* will index into CRC lookup */

    /* pass through message buffer */
    while (buffer_length--) {
        i = crc_hi ^ *buffer++; /* calculate the CRC  */
        crc_hi = crc_lo ^ table_crc16_hi[i];
        crc_lo = table_crc16_lo[i];
    }

    return (crc_hi << 8 | crc_lo);
}

smuint8 calcCRC8Buf( smuint8 *buf, int len, int crcinit )
{
    int i;
    smuint8 crc=crcinit;

    for(i=0;i<len;i++)
    {
            crc=table_crc8[crc^buf[i]];
    }

    return crc;
}

SM_STATUS smSetTimeout( smuint16 millsecs )
{
    if(millsecs<=5000 && millsecs>=1)
    {
        readTimeoutMs=millsecs;
        return SM_OK;
    }
    return SM_ERR_PARAMETER;
}

smuint32 smGetVersion()
{
    return SM_VERSION;
}


//init bus struct table
void smBusesInit()
{
    int i;
    for(i=0;i<SM_MAX_BUSES;i++)
    {
        smBus[i].opened=smfalse;;
        smResetSM485variables(i);
    }
    smInitialized=smtrue;
}

smbool smIsHandleOpen( const smbus handle )
{
    if(handle<0) return smfalse;
    if(handle>=SM_MAX_BUSES) return smfalse;
    return smBus[handle].opened;
}


/** Open SM RS485 communication bus. Parameters:
-devicename: on Windows COM port as "COMx" or on unix /dev/ttySx or /dev/ttyUSBx where x=port number
-return value: integer handle to be used with all other commands, -1 if fails
*/
smbus smOpenBus( const char * devicename )
{
    int handle;

    //true on first call
    if(smInitialized==smfalse)
        smBusesInit();

    //find free handle
    for(handle=0;handle<SM_MAX_BUSES;handle++)
    {
        if(smBus[handle].opened==smfalse) break;//choose this
    }
    //all handles in use
    if(handle>=SM_MAX_BUSES) return -1;

    //open bus device
    smBus[handle].bdHandle=smBDOpen(devicename);
    if(smBus[handle].bdHandle==-1) return -1;

    //success
    strncpy( smBus[handle].busDeviceName, devicename, SM_BUSDEVICENAME_LEN );
    smBus[handle].busDeviceName[SM_BUSDEVICENAME_LEN-1]=0;//null terminate string
    smBus[handle].opened=smtrue;
    return handle;
}

/** Change baudrate of SM communication port. This does not affect already opened ports but the next smOpenBus will be opened at the new speed.
    Calling this is optional. By default SM bus and all slave devices operates at 460800 BPS speed.
    Parameters:
    -bps: bus speed in bits per second. for possible choices, see rs232.c (but note that all speeds are not necessarily supported by SM devices)
    Typical usage is:
    - first call smSetParameter(handle,0,SMP_BUS_SPEED,N) to change speed of all connected slaves to N PBS
    - then close port with smCloseBus
    - then call smSetBaudrate(N)
    - then open bus again with smOpenBus

    Note that in upcoming SM device firmware versions, bitrate will be reset to default (460800) if device side SM bus watchdog timer has been enabled, and it timeouts.
    This allows re-establishing connection at defautl speed if connection breaks up and SM bus watchdog timeout gets exceeded. To identify is device supports this,
    read parameter SMP_SM_VERSION. Values above 25 support this feature. Value 25 and below will not reset baudrate.

    Note also that SMP_BUS_SPEED will not be saved in device flash memory - it will reset to default at every reset & power on.
    */
LIB void smSetBaudrate( unsigned long pbs )
{
    SMBusBaudrate=pbs;
}

/** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening.
-return value: a SM_STATUS value, i.e. SM_OK if command succeed
*/
LIB SM_STATUS smCloseBus( const smbus bushandle )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

    smBus[bushandle].opened=smfalse;

    if( smBDClose(smBus[bushandle].bdHandle) == smfalse ) return recordStatus(bushandle,SM_ERR_BUS);

    return SM_OK;
}

char *cmdidToStr(smuint8 cmdid )
{
    char *str;
    switch(cmdid)
    {
    case SMCMD_INSTANT_CMD : str="SMCMD_INSTANT_CMD";break;
    case SMCMD_INSTANT_CMD_RET :str="SMCMD_INSTANT_CMD_RET";break;
    case SMCMD_BUFFERED_CMD :str="SMCMD_BUFFERED_CMD";break;
    case SMCMD_BUFFERED_RETURN_DATA :str="SMCMD_BUFFERED_RETURN_DATA";break;
    case SMCMD_BUFFERED_RETURN_DATA_RET :str="SMCMD_BUFFERED_RETURN_DATA_RET";break;
    case SMCMD_BUFFERED_CMD_RET:str="SMCMD_BUFFERED_CMD_RET";break;
#ifdef PROCESS_IMAGE_SUPPORT
    case SMCMD_PROCESS_IMAGE:str="SMCMD_PROCESS_IMAGE";break;
    case SMCMD_PROCESS_IMAGE_RET: str="SMCMD_PROCESS_IMAGE_RET";break;
#endif
    case SMCMD_GET_CLOCK: str="SMCMD_GET_CLOCK";break;
    case SMCMD_GET_CLOCK_RET :str="SMCMD_GET_CLOCK_RET";break;
    case SMCMD_FAST_UPDATE_CYCLE:str="SMCMD_FAST_UPDATE_CYCLE";break;
    case SMCMD_FAST_UPDATE_CYCLE_RET:str="SMCMD_FAST_UPDATE_CYCLE_RET";break;
    default: str="unknown cmdid";break;
    }
    //puts(str);
    return str;
}

//write one byte to tx buffer
//returns true on success
smbool smWriteByte( const smbus handle, const smuint8 byte, smuint16 *crc )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    smbool success=smBDWrite(smBus[handle].bdHandle,byte);
    if(crc!=NULL)
        *crc = calcCRC16(byte,*crc);

    if(success==smtrue)
        smDebug(handle, High, "  sent byte %02x\n",byte);
    else
        smDebug(handle, High, "  sending byte %02x failed\n",byte);

    return success;
}

//write tx buffer to bus
//returns true on success
smbool smTransmitBuffer( const smbus handle )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    smbool success=smBDTransmit(smBus[handle].bdHandle);
    return success;
}

SM_STATUS smSendSMCMD( smbus handle, smuint8 cmdid, smuint8 addr, smuint8 datalen, smuint8 *cmddata )
{
    int i;
    smuint16 sendcrc;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    sendcrc=SM485_CRCINIT;

    smDebug(handle, Mid, "> %s (id=%d, addr=%d, payload=%d)\n",cmdidToStr(cmdid),cmdid,
            addr,
            datalen);


    smDebug(handle,High,"ID ");
    if( smWriteByte(handle,cmdid, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

    if(cmdid&SMCMD_MASK_N_PARAMS)
    {
        smDebug(handle,High,"Nparams ");
        if( smWriteByte(handle,datalen, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
    }

    smDebug(handle,High,"ADDR ");
    if( smWriteByte(handle,addr, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

    for(i=0;i<datalen;i++)
    {
        smDebug(handle,High,"DATA ");
        if( smWriteByte(handle,cmddata[i], &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
    }
    smDebug(handle,High,"CRC ");
    if( smWriteByte(handle,sendcrc>>8, NULL)  != smtrue ) return recordStatus(handle,SM_ERR_BUS);
    smDebug(handle,High,"CRC ");
    if( smWriteByte(handle,sendcrc&0xff,NULL) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

    //transmit bytes to bus that were written in buffer by smWriteByte calls
    if( smTransmitBuffer(handle) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

    return recordStatus(handle,SM_OK);
}


SM_STATUS smFastUpdateCycle( smbus handle, smuint8 nodeAddress, smuint16 write1, smuint16 write2, smuint16 *read1, smuint16 *read2)
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    smDebug(handle, Mid, "> %s (addr=%d, w1=%d, w2=%d)\n",cmdidToStr(SMCMD_FAST_UPDATE_CYCLE),
            nodeAddress,
            write1,write2);


    //form the tx packet
    smuint8 cmd[8];
    int i;
    cmd[0]=SMCMD_FAST_UPDATE_CYCLE;
    cmd[1]=nodeAddress;
    bufput16bit(cmd,2,write1);
    bufput16bit(cmd,4,write2);
    cmd[6]=calcCRC8Buf(cmd,6,0x52);

    //send
    for(i=0;i<7;i++)
    {
        if( smWriteByte(handle,cmd[i], NULL) != smtrue )
            return recordStatus(handle,SM_ERR_BUS);
    }
    smTransmitBuffer(handle);//this sends the bytes entered with smWriteByte

    smDebug(handle, High, "  Reading reply packet\n");
    for(i=0;i<6;i++)
    {
        smbool success;
        smuint8 rx;
        success=smBDRead(smBus[handle].bdHandle,&rx);
        cmd[i]=rx;
        if(success!=smtrue)
            return recordStatus(handle,SM_ERR_BUS|SM_ERR_LENGTH);//no enough data received
    }

    //parse
    if( cmd[5]!=calcCRC8Buf(cmd,5,0x52) || cmd[0]!=SMCMD_FAST_UPDATE_CYCLE_RET )
    {
        return recordStatus(handle,SM_ERR_COMMUNICATION);//packet error
    }
    if(read1!=NULL)
        *read1=bufget16bit(cmd,1);
    if(read2!=NULL)
        *read2=bufget16bit(cmd,3);

    //return data read complete
    smDebug(handle,Mid, "< %s (id=%d, r1=%d, r2=%d)\n",
            cmdidToStr( cmd[0] ),
            cmd[0],*read1,*read2);

    return recordStatus(handle,SM_OK);
}



SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;


    //empty pending rx buffer to avoid further parse errors
    if(flushrx==smtrue)
    {
        smbool success;
        do{
            smuint8 rx;
            success=smBDRead(smBus[handle].bdHandle,&rx);
        }while(success==smtrue);
    }
    smResetSM485variables(handle);
    smBus[handle].receiveComplete=smtrue;
    return recordStatus(handle,SM_ERR_COMMUNICATION);
}


SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType,smint32 paramvalue  )
{
    int cmdlength;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    switch(smpCmdType)
    {
    case SMPCMD_SETPARAMADDR:
        cmdlength=2;
        break;
    case SMPCMD_24B:
        cmdlength=3;
        break;
    case SMPCMD_32B:
        cmdlength=4;
        break;
    default:
        return recordStatus(handle,SM_ERR_PARAMETER);
        break;
    }

    //check if space if buffer
    if(smBus[handle].cmd_send_queue_bytes>(SM485_MAX_PAYLOAD_BYTES-cmdlength) )
    {
        smBus[handle].transmitBufFull=smtrue; //when set true, smExecute will do nothing but clear transmit buffer. so this prevents any of overflowed commands getting thru
        return recordStatus(handle,SM_ERR_LENGTH); //overflow, too many commands in buffer
    }

    if(smpCmdType==SMPCMD_SETPARAMADDR)
    {
        SMPayloadCommand16 newcmd;
        newcmd.ID=SMPCMD_SETPARAMADDR;
        newcmd.param=paramvalue;
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
    }
    if(smpCmdType==SMPCMD_24B)
    {
        SMPayloadCommand24 newcmd;
        newcmd.ID=SMPCMD_24B;
        newcmd.param=paramvalue;
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
    }
    if(smpCmdType==SMPCMD_32B)
    {
        SMPayloadCommand32 newcmd;
        newcmd.ID=SMPCMD_32B;
        newcmd.param=paramvalue;
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[3]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
    }


    return recordStatus(handle,SM_OK);
}


SMPayloadCommandRet32 smConvertToPayloadRet32_16(SMPayloadCommandRet16 in)
{
    SMPayloadCommandRet32 out;
    out.ID=in.ID;
    out.retData=(long)in.retData; //negative value, extend ones
    return out;
}

//for library internal use only
SM_STATUS smTransmitReceiveCommandQueue( const smbus bushandle, const smaddr targetaddress, smuint8 cmdid )
{
    SM_STATUS stat;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

    if(smBus[bushandle].transmitBufFull!=smtrue) //dont send/receive commands if queue was overflowed by user error
    {
        stat=smSendSMCMD(bushandle,cmdid,targetaddress, smBus[bushandle].cmd_send_queue_bytes, smBus[bushandle].recv_rsbuf ); //send commands to bus
        if(stat!=SM_OK) return recordStatus(bushandle,stat);
    }

    smBus[bushandle].cmd_send_queue_bytes=0;
    smBus[bushandle].cmd_recv_queue_bytes=0;//counted upwards at every smGetQueued.. and compared to payload size

    if(smBus[bushandle].transmitBufFull!=smtrue)//dont send/receive commands if queue was overflowed by user error
    {
        stat=smReceiveReturnPacket(bushandle);//blocking wait & receive return values from bus
        if(stat!=SM_OK) return recordStatus(bushandle,stat); //maybe timeouted
    }

    smBus[bushandle].transmitBufFull=smfalse;//reset overflow status
    return recordStatus(bushandle,SM_OK);
}


SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

    return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_INSTANT_CMD));
}

SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress )
{
    return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_BUFFERED_CMD));
}

//return number of how many bytes waiting to be read with smGetQueuedSMCommandReturnValue
SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer )
{
    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

    smint32 bytes=smBus[bushandle].recv_payloadsize - smBus[bushandle].cmd_recv_queue_bytes;//how many bytes waiting to be read with smGetQueuedSMCommandReturnValue
    *bytesinbuffer=bytes;

    return recordStatus(bushandle,SM_OK);
}

SM_STATUS smGetQueuedSMCommandReturnValue(  const smbus bushandle, smint32 *retValue )
{
    smuint8 rxbyte, rettype;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;


    //if get called so many times that receive queue buffer is already empty, return error
    if(smBus[bushandle].cmd_recv_queue_bytes>=smBus[bushandle].recv_payloadsize)
    {

        smDebug(bushandle,Trace, "Packet receive error, return data coudn't be parsed\n");

        //return 0
        if(retValue!=NULL) *retValue=0;//check every time if retValue is set NULL by caller -> don't store anything to it if its NULL

        return recordStatus(bushandle,SM_ERR_LENGTH);//not a single byte left
    }

    //get first byte to deterime packet length
    rxbyte=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
    rettype=rxbyte>>6; //extract ret packet header 2 bits

    //read rest of data based on packet header:

    if(rettype == SMPRET_16B)
    {
        //extract return packet and convert to 32 bit and return
        SMPayloadCommandRet16 read;
        smuint8 *readBuf=(smuint8*)&read;
        readBuf[1]=rxbyte;
        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        smDebug(bushandle,Trace,"RET16B: %d\n",read.retData);

        if(retValue!=NULL) *retValue=read.retData;
        return recordStatus(bushandle,SM_OK);
    }
    if(rettype == SMPRET_24B)
    {
        //extract return packet and convert to 32 bit and return
        SMPayloadCommandRet24 read;
        smuint8 *readBuf=(smuint8*)&read;
        readBuf[2]=rxbyte;
        readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        smDebug(bushandle,Trace,"RET24B: %d\n",read.retData);

        if(retValue!=NULL) *retValue=read.retData;
        return recordStatus(bushandle,SM_OK);
    }
    if(rettype == SMPRET_32B)
    {
        //extract return packet and convert to 32 bit and return
        SMPayloadCommandRet32 read;
        smuint8 *readBuf=(smuint8*)&read;
        readBuf[3]=rxbyte;
        readBuf[2]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
        smDebug(bushandle,Trace,"RET32B: %d\n",read.retData);

        if(retValue!=NULL) *retValue=read.retData;
        return recordStatus(bushandle,SM_OK);
    }
    if(rettype == SMPRET_OTHER) //8bit
    {
        //extract return packet and convert to 32 bit and return
        SMPayloadCommandRet8 read;
        smuint8 *readBuf=(smuint8*)&read;
        readBuf[0]=rxbyte;
        smDebug(bushandle,Trace,"RET_OTHER: %d\n",read.retData);

        if(retValue!=NULL) *retValue=read.retData;
        return recordStatus(bushandle,SM_OK);
    }

    return recordStatus(bushandle,SM_ERR_PARAMETER); //something went wrong, rettype not known
}


SM_STATUS smReceiveReturnPacket( smbus bushandle )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

    smDebug(bushandle, High, "  Reading reply packet\n");
    do
    {
        smuint8 ret;
        SM_STATUS stat;

        smbool succ=smBDRead(smBus[bushandle].bdHandle,&ret);

        if(succ==smfalse)
        {
            smReceiveErrorHandler(bushandle,smfalse);
            smDebug(bushandle, High, "  smRawCommand RX read byte failed\n");
            return recordStatus(bushandle,SM_ERR_COMMUNICATION);
        }
        else
        {
            smDebug(bushandle, High, "  got byte %02x \n",ret);
        }

        stat=smParseReturnData( bushandle, ret );
        if(stat!=SM_OK) return recordStatus(bushandle,stat);
    } while(smBus[bushandle].receiveComplete==smfalse); //loop until complete packaget has been read

    //return data read complete
    smDebug(bushandle,Mid, "< %s (id=%d, addr=%d, payload=%d)\n",
            cmdidToStr( smBus[bushandle].recv_cmdid ),
            smBus[bushandle].recv_cmdid,
            smBus[bushandle].recv_addr,
            smBus[bushandle].recv_payloadsize);

    return recordStatus(bushandle,SM_OK);
}


/** Set stream where debug output is written. By default nothing is written. */
LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream )
{
    smDebugThreshold=level;
    smDebugOut=stream;
}


//short returndata16=0, payload=0;
//can be called at any frequency
SM_STATUS smParseReturnData( smbus handle, smuint8 data )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    //buffered variable allows placing if's in any order (because recv_state may changes in this function)
    smBus[handle].recv_state=smBus[handle].recv_state_next;
    smBus[handle].receiveComplete=smfalse;//overwritten to true later if complete

    if(smBus[handle].recv_state==WaitPayload)
    {
        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);

        //normal handling for all payload data
        if(smBus[handle].recv_storepos<SM485_MAX_PAYLOAD_BYTES)
            smBus[handle].recv_rsbuf[smBus[handle].recv_storepos++]=data;
        else//rx payload buffer overflow
        {
            return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));
        }

        //all received
        if(smBus[handle].recv_payloadsize<=smBus[handle].recv_storepos)
            smBus[handle].recv_state_next=WaitCrcHi;

        return recordStatus(handle,SM_OK);
    }


    if(smBus[handle].recv_state==WaitCmdId)
    {
        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
        smBus[handle].recv_cmdid=data;
        switch(data&SMCMD_MASK_PARAMS_BITS)//commands with fixed payload size
        {
        case SMCMD_MASK_2_PARAMS: smBus[handle].recv_payloadsize=2; smBus[handle].recv_state_next=WaitAddr; break;
        case SMCMD_MASK_0_PARAMS: smBus[handle].recv_payloadsize=0; smBus[handle].recv_state_next=WaitAddr; break;
        case SMCMD_MASK_N_PARAMS: smBus[handle].recv_payloadsize=-1; smBus[handle].recv_state_next=WaitPayloadSize;break;//-1 = N databytes
        default:
            return recordStatus(handle,(smReceiveErrorHandler(handle, smtrue)));
            break; //error, unsupported command id
        }

        return recordStatus(handle,SM_OK);
    }

    //no data payload size known yet
    if(smBus[handle].recv_state==WaitPayloadSize)
    {
        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
        smBus[handle].recv_payloadsize=data;
        smBus[handle].recv_state_next=WaitAddr;
        return recordStatus(handle,SM_OK);
    }

    if(smBus[handle].recv_state==WaitAddr)
    {
        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
        smBus[handle].recv_addr=data;//can be receiver or sender addr depending on cmd
        if(smBus[handle].recv_payloadsize>smBus[handle].recv_storepos)
            smBus[handle].recv_state_next=WaitPayload;
        else
            smBus[handle].recv_state_next=WaitCrcHi;
        return recordStatus(handle,SM_OK);
    }

    if(smBus[handle].recv_state==WaitCrcHi)
    {
        smBus[handle].recv_read_crc_hi=data;//crc_msb
        smBus[handle].recv_state_next=WaitCrcLo;
        return recordStatus(handle,SM_OK);
    }

    //get crc_lsb, check crc and execute
    if(smBus[handle].recv_state==WaitCrcLo)
    {
        if(((smBus[handle].recv_read_crc_hi<<8)|data)!=smBus[handle].recv_crc)
        {
            //CRC error
            return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));
        }
        else
        {
            //CRC ok
            //if(smBus[handle].recv_addr==config.deviceAddress || smBus[handle].recv_cmdid==SMCMD_GET_CLOCK_RET || smBus[handle].recv_cmdid==SMCMD_PROCESS_IMAGE ) executeSMcmd();
            smBus[handle].receiveComplete=smtrue;
        }

        //smResetSM485variables(handle);
        smBus[handle].recv_storepos=0;
        smBus[handle].recv_crc=SM485_CRCINIT;
        smBus[handle].recv_state_next=WaitCmdId;
        return recordStatus(handle,SM_OK);
    }

    return recordStatus(handle,SM_OK);
}


//reply consumes 16 bytes in payload buf, so max calls per cycle is 7
SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress )
{
    SM_STATUS stat=SM_NONE;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    //possible errors will set bits to stat
    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN ); //2b
    stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, SMPRET_32B );//3b
    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );//2b
    stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, paramAddress );//3b
    //=10 bytes

    return recordStatus(handle,stat);
}

SM_STATUS smGetQueuedGetParamReturnValue(  const smbus bushandle, smint32 *retValue )
{
    smint32 retVal=0;
    SM_STATUS stat=SM_NONE;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

    //must get all inserted commands from buffer
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );//4x4b
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );  //the real return value is here
    if(retValue!=NULL) *retValue=retVal;
    return recordStatus(bushandle,stat);
}

//consumes 6 bytes in payload buf, so max calls per cycle is 20
SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue )
{
    SM_STATUS stat=SM_NONE;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, paramAddress );//2b
    stat|=smAppendSMCommandToQueue( handle, SMPCMD_32B, paramValue );//4b
    return recordStatus(handle,stat);
}


SM_STATUS smGetQueuedSetParamReturnValue(  const smbus bushandle, smint32 *retValue )
{
    smint32 retVal=0;
    SM_STATUS stat=SM_NONE;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

    //must get all inserted commands from buffer
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );  //the real return value is here
    if(retValue!=NULL) *retValue=retVal;

    return recordStatus(bushandle,stat);
}


SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock )
{
    SM_STATUS stat;

    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return recordStatus(handle,SM_ERR_NODEVICE);

    stat=smSendSMCMD(handle, SMCMD_GET_CLOCK ,targetaddr, 0, NULL ); //send get clock commands to bus
    if(stat!=SM_OK) return recordStatus(handle,stat);

    stat=smReceiveReturnPacket(handle);//blocking wait & receive return values from bus
    if(stat!=SM_OK) return recordStatus(handle,stat); //maybe timeouted

    if(clock!=NULL)
	memcpy(clock,smBus[handle].recv_rsbuf,sizeof(smuint16));

    smBus[handle].recv_storepos=0;

    return recordStatus(handle,SM_OK);
}

/** Simple read & write of parameters with internal queueing, so only one call needed.
Use these for non-time critical operations. */
SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 )
{
    SM_STATUS smStat=0;

    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
    smStat|=smExecuteCommandQueue(handle,nodeAddress);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);

    return recordStatus(handle,smStat);
}

SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 )
{
    SM_STATUS smStat=0;

    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
    smStat|=smAppendGetParamCommandToQueue(handle,paramId2);
    smStat|=smExecuteCommandQueue(handle,nodeAddress);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);

    return recordStatus(handle,smStat);
}

SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 )
{
    SM_STATUS smStat=0;

    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
    smStat|=smAppendGetParamCommandToQueue(handle,paramId2);
    smStat|=smAppendGetParamCommandToQueue(handle,paramId3);
    smStat|=smExecuteCommandQueue(handle,nodeAddress);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);
    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal3);

    return recordStatus(handle,smStat);
}

SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal )
{
    smint32 nul;
    SM_STATUS smStat=0;

    smStat|=smAppendSetParamCommandToQueue( handle, paramId, paramVal );
    smStat|=smExecuteCommandQueue(handle,nodeAddress);
    smStat|=smGetQueuedSetParamReturnValue(  handle, &nul );

    return recordStatus(handle,smStat);
}




//accumulates status to internal variable by ORing the bits. returns same value that is fed as paramter
SM_STATUS recordStatus( const smbus handle, const SM_STATUS stat )
{
    //check if bus handle is valid & opened
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    smBus[handle].cumulativeSmStatus|=stat;
    return stat;
}


/** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/
SM_STATUS getCumulativeStatus( const smbus handle )
{
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    return smBus[handle].cumulativeSmStatus;
}

/** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/
SM_STATUS resetCumulativeStatus( const smbus handle )
{
    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

    smBus[handle].cumulativeSmStatus=0;

    return SM_OK;
}


/** Return number of bus devices found. details of each device may be consequently fetched by smGetBusDeviceDetails() */
smint smGetNumberOfDetectedBuses()
{
    return smBDGetNumberOfDetectedBuses();
}

/** Fetch information of detected bus nodes at certain index. Example:

    smint num=smGetNumberOfDetectedBuses();
    for(int i=0;i<num;i++)
    {
        SM_BUS_DEVICE_INFO info;
        SM_STATUS s=smGetBusDeviceDetails(i,&info);
        if(s==SM_OK)
        {
            ...do something with info...
        }
        else
        {
            ...report error...
        }
    }
*/
LIB SM_STATUS smGetBusDeviceDetails( smint index, SM_BUS_DEVICE_INFO *info )
{
    smbool ok=smBDGetBusDeviceDetails(index,info);

    if(ok==smtrue)
        return SM_OK;
    else
        return SM_ERR_NODEVICE;
}