// serialServer.cc

// Author: Marty Kraimer
// Date: 7/98


#include <vxWorks.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <taskLib.h>
#include <intLib.h>
#include <time.h>

#include "Message.h"
#include "Int32Message.h"
#include "Char8ArrayMessage.h"
#include "SerialConfigMessage.h"
#include "SerialPort.h"
#include "serialServer.h"


class Serial
{
public:
    Serial(const char *name,const char *portName,
        int bufsize, int queueSize,
        const unsigned char *eomstr, int circularBufferMode);
    static void serialServer(Serial *);
    void processConfig(SerialConfigMessage *);
    void processChar8Array(Char8ArrayMessage *);
    void readCircularBuffer(Char8ArrayMessage *,Char8ArrayMessage *);
    bool getStartOK() const {return(startOK);}
private:
    const char *serverName;
    static byteHandlerRC byteHandler(void*,unsigned char);
    MessageServer *pMessageServer;
    SerialPort* pSerialPort;
    bool startOK;
    bool readActive;
    bool gotFirstEom;
    bool circularBuffer;
    serialServerStatus status;
    unsigned char eomString[2];
    int eomLen;
    int readNchars;
    int numberRetrys;
    int nextIn;
    int nextOut;
    int readBufSize;
    int sizeFromExtra;
    unsigned char *readBuf;
    SEM_ID ioComplete;
};

static char taskname[] = "serial";
int initSerialServer(char *serverName,char *portName,int bufSize,
    int queueSize, const unsigned char *eomstr, int circularBufferMode)
{
    if(bufSize<=0) {
        printf("%s bufSize must be >0\n",serverName);
        return(0);
    }
    Serial *pSerial =
        new Serial(serverName,portName,bufSize,queueSize,
            eomstr, circularBufferMode);
    if(!pSerial->getStartOK()) return(0);
    int taskId = taskSpawn(taskname,100,VX_FP_TASK,2000,
        (FUNCPTR)Serial::serialServer,(int)pSerial,0,0,0,0,0,0,0,0,0);
    if(taskId==ERROR)
        printf("%s serialServer taskSpawn Failure\n",serverName);
    return(0);
}

Serial:: Serial(const char *name,const char *portName,
    int bufsize, int queueSize, const unsigned char *eomstr,
    int circularBufferMode)
:   serverName(name), startOK(false), readActive(false), gotFirstEom(false),
    status(serialServerStatusOK), readNchars(0), numberRetrys(0),
    nextIn(0), nextOut(0), readBufSize(bufsize), sizeFromExtra(0), readBuf(0)
{
    if(eomstr==0) {
        eomLen = 0;
    } else switch(strlen((const char  *)eomstr)) {
    case 0:
        eomString[0] = 0; eomLen = 1; break;
    case 1:
        eomString[0] = eomstr[0]; eomLen = 1; break;
    case 2:
        eomString[0] = eomstr[0]; eomString[1] = eomstr[1]; eomLen = 2; break;
    default:
        printf("%s serialServer illegal length eomstr. eomLen set to 0\n",
            serverName);
        eomString[0] = 0; eomLen = 0; break;
    }
    circularBuffer = (circularBufferMode==0) ? false : true;
    if(circularBuffer) readActive = true;
    readBuf = (unsigned char *)(new char[(size_t)readBufSize]);
    ::memset((void *)readBuf,'\0',(size_t)readBufSize);
    pSerialPort = SerialPort::bind(portName,this,byteHandler);
    if(pSerialPort==0) {
        printf("%s serialServer: could not bind to SerialPort\n",serverName);
        return;
    }
    pMessageServer = new MessageServer(name,queueSize);
    ioComplete = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
    startOK = true;
}

byteHandlerRC Serial::byteHandler(void* v, unsigned char data)
{
    Serial *pSerial = (Serial *)v;
    if(!pSerial->readActive) return(byteHandlerError);
    int nextIn = pSerial->nextIn;
    if(pSerial->circularBuffer) {
        if(nextIn+1 == pSerial->nextOut) {//must allow 1 byte margin
            pSerial->status = serialServerStatusOverflow;
        } else {
            pSerial->readBuf[nextIn] = data;
            nextIn = ((nextIn+1)>=pSerial->readBufSize) ? 0 : nextIn+1;
            pSerial->nextIn = nextIn;
            semGive(pSerial->ioComplete);
        }
        return(byteHandlerOK);
    }
    // NOTE that rest of byteHandler only applys to !circularBuffer
    if(nextIn>=pSerial->readBufSize) {
        pSerial->readActive = false;
        pSerial->status = serialServerStatusOverflow;
        return(byteHandlerEndRead);
    }
    pSerial->readBuf[nextIn] = data;
    pSerial->nextIn++;
    if(pSerial->eomLen>0) {
        if(!pSerial->gotFirstEom) {
            if(data==pSerial->eomString[0]) {
                if(pSerial->eomLen==1)  goto goteom;
                pSerial->gotFirstEom = true;
            }
        } else {
            if(data==pSerial->eomString[1]) goto goteom;
            pSerial->gotFirstEom = false;
        }
    }
    ++pSerial->readNchars;
    if((pSerial->sizeFromExtra>0)
    && (pSerial->readNchars >= pSerial->sizeFromExtra) )goto goteom;
    return(byteHandlerOK);
goteom:
    pSerial->gotFirstEom = false;
    pSerial->readNchars  = 0;
    pSerial->readActive = false;
    return(byteHandlerEndRead);
}

void Serial::serialServer(Serial *pSerial)
{
    while(true) {
        pSerial->pMessageServer->waitForMessage();
        Message *message;
        while((message = pSerial->pMessageServer->receive())) {
            switch(message->getType()) {
            case messageTypeSerialConfig:
                pSerial->processConfig((SerialConfigMessage *)message);
                break;
            case messageTypeChar8Array:
                pSerial->processChar8Array((Char8ArrayMessage *)message);
                break;
            default:
                printf("%s serialServer got illegal message type %d\n",
                    pSerial->serverName, message->getType());
                break;
            }
            delete message;
        }
    }
    return;
}
void Serial::processConfig(SerialConfigMessage *pconfig)
{
    Int32Message *preply = (Int32Message *)pMessageServer->
        allocReplyMessage(pconfig,messageTypeInt32);
    bool result = pSerialPort->config(
            (int)pconfig->baud,(int)pconfig->stopBits,
            (int)pconfig->bitsPerChar,(char)pconfig->parity,
            (char)pconfig->flowControl);
    preply->status = (result ? 0 : 1);
    pMessageServer->reply(preply);
}

void Serial::processChar8Array(Char8ArrayMessage *preceive)
{
    int cmd = preceive->cmd;
    serialPortStatus portStatus = serialPortSuccess;
    Char8ArrayMessage *preply = (Char8ArrayMessage *)pMessageServer->
        allocReplyMessage(preceive,messageTypeChar8Array);
    if((cmd&cmdStartCircularBuffer) || (cmd&cmdStopCircularBuffer)) {
        if(cmd&cmdStartCircularBuffer) circularBuffer = true;
        if(cmd&cmdStopCircularBuffer) circularBuffer = false;
        if(circularBuffer) readActive = true;
    }
    sizeFromExtra = preceive->extra;
    if(cmd&cmdSetEom) {
        eomLen = preceive->eomLen;
        eomString[0] = preceive->eomString[0];
        eomString[1] = preceive->eomString[1];
    }
    if(!(cmd&cmdRead)) {
        if(cmd&cmdWrite) {
            portStatus = pSerialPort->write((unsigned char *)preceive->value,
                preceive->getSize(), preceive->timeout);
        }
        preply->status = portStatus;
        pMessageServer->reply(preply);
        return;
    }
    if(circularBuffer) {
        int intKey = intLock();
        if(cmd&cmdFlush) {
            readNchars = nextIn = nextOut = 0;
            status = serialServerStatusOK;
        }
        intUnlock(intKey);
        if(cmd&cmdWrite) {
            int rtnstatus = serialServerStatusOK;
            portStatus = pSerialPort->write((unsigned char *)preceive->value,
                preceive->getSize(), preceive->timeout);
            if(portStatus==serialPortTimeout) {
                rtnstatus = serialServerStatusTimeout;
            } else if(portStatus != serialPortSuccess) {
                rtnstatus = serialServerStatusError;
            }
            if(status!=serialServerStatusOK) {
                preply->status = rtnstatus;
                pMessageServer->reply(preply);
                return;
            }
        }
        readCircularBuffer(preceive,preply);
        return;
    }
    int numberRetrys = preceive->numberRetrys;
    for(int nextTry=0; nextTry<=numberRetrys; nextTry++) {
        int intKey = intLock();
        readNchars = nextIn = nextOut = 0;
        status = serialServerStatusOK;
        readActive = true;
        intUnlock(intKey);
        if((cmd&cmdWriteRead)==cmdWriteRead) {
            portStatus = pSerialPort->writeRead(
                (unsigned char *)preceive->value,
                preceive->getSize(), preceive->timeout);
        } else {
                portStatus = pSerialPort->read(preceive->timeout);
        }
        if(status==serialServerStatusOK) {
            if(portStatus==serialPortTimeout) {
                status = serialServerStatusTimeout;
            } else if(portStatus != serialPortSuccess) {
                status = serialServerStatusError;
            }
        }
        if(status==serialServerStatusOK || status==serialServerStatusOverflow)
        break;
    }
    preply->status = status;
    int len = nextIn;
    preply->allocValue(len);
    ::memcpy((void *)preply->value,(void *)readBuf,len);
    preply->setSize(len);
    pMessageServer->reply(preply);
    return;
}

void Serial::readCircularBuffer(
    Char8ArrayMessage *preceive,Char8ArrayMessage *preply)
{
    int len = 0;
    bool got1stEom = false;
    int firstOut = nextOut;
    int nextOutTry = nextOut;
    int lastOut = nextOut -1;
    while(true) {
        unsigned char data;
        int intKey = intLock(); // Must not allow interrupts
        if(nextOutTry==nextIn) {
            //  Flush any stale semGives
            semTake(ioComplete, NO_WAIT);
            intUnlock(intKey);
            if (semTake(ioComplete, preceive->timeout*CLOCKS_PER_SEC)==ERROR) {
printf("%s serialPortTimeout, len=%d\n", serverName, len);
               status = serialServerStatusTimeout;
               break;
            }
        } else {
            intUnlock(intKey);
        }
        data = readBuf[nextOutTry];
        lastOut = nextOutTry;
        nextOutTry = ((nextOutTry+1) >= readBufSize) ? 0 : nextOutTry + 1;
        ++len;
        if(eomLen>0) {
            if(got1stEom) {
                if(data==eomString[1]) break;
                got1stEom = false;
            } else {
                if(data==eomString[0]) {
                   if(eomLen==1) break;
                   got1stEom = true;
                }
            }
        }
        if( sizeFromExtra>0 && len>=sizeFromExtra) break;
    }
    int lenBeg,lenEnd;
    if(len <= 0) {
        lenBeg = lenEnd = 0;
    } else if(firstOut <= lastOut) {
        lenBeg = 0;
        lenEnd = lastOut - firstOut +1;

    } else {
        lenBeg = lastOut + 1;
        lenEnd = readBufSize - firstOut;
    }
    len = lenBeg + lenEnd;
    preply->allocValue(len);
    if(lenEnd>0)
        ::memcpy((void *)preply->value,(void *)(readBuf + firstOut),lenEnd);
    if(lenBeg>0)
        ::memcpy((void *)(preply->value+lenEnd),(void *)readBuf,lenBeg);
    int intKey = intLock();
    nextOut = nextOutTry;
    preply->status = status;
    status=serialServerStatusOK;
    intUnlock(intKey);
    preply->setSize(len);
    pMessageServer->reply(preply);
    return;
}