/*
 * Provide a 'network attached device' for ASYN labs
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <ctype.h>
#include <math.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <osiSock.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

#define BASE_PORT    24742
int debugFlag = 0;

/*
 * BPM/Corrector simulation
 */
#define SamplingFrequency         10.0
#define HorizontalOffset           0.1
#define VerticalOffset            -0.08
#define HorizontalWanderFrequency  0.1
#define HorizontalWanderAmplitude  0.1
#define VerticalWanderFrequency    0.05
#define VerticalWanderAmplitude    0.05
#define CorrectorFullScaleAmps    10.0
#define BeamCurrentFraction       0.8
#define ButtonFullScale           8192

struct BpmCorr {
    int     beamOn;
    int     latched;
    double  time;
    double  hAmps;
    double  vAmps;
    int     b;
    int     t;
    int     l;
    int     r;
};

struct workerParms {
    char          *clientName;
    int            fd;
    FILE          *fp;
    struct BpmCorr bpm;
};

static int
bpmCorrector(char *cp, struct workerParms *p)
{
    struct BpmCorr *bpm = &p->bpm;

    if (debugFlag) printf("CMD: \"%s\"   latched:%d   hAmps:%g,  vAmps:%g\n", cp, bpm->latched, bpm->hAmps, bpm->vAmps);
    if (epicsStrCaseCmp("LATCH", cp) == 0) {
        if (bpm->latched == 0) {
            double position;
            position = (2 * bpm->hAmps / CorrectorFullScaleAmps) + 
                        HorizontalOffset +
                        HorizontalWanderAmplitude * sin(
                            HorizontalWanderFrequency * bpm->time * 2.0 * M_PI /
                                                            SamplingFrequency);
            if (position < -1)
                position = -1;
            else if (position > 1)
                position = 1;
            bpm->l = ButtonFullScale * BeamCurrentFraction * (1 - position) / 2;
            bpm->r = (ButtonFullScale * BeamCurrentFraction) - bpm->l;
            if (debugFlag) printf("position:%g  l:%d   r:%d\n", position, bpm->l, bpm->r);
            position = (2 * bpm->vAmps / CorrectorFullScaleAmps) + 
                        VerticalOffset +
                        VerticalWanderAmplitude * sin(
                            VerticalWanderFrequency * bpm->time * 2.0 * M_PI /
                                                            SamplingFrequency);
            if (position < -1)
                position = -1;
            else if (position > 1)
                position = 1;
            bpm->b = ButtonFullScale * BeamCurrentFraction * (1 - position) / 2;
            bpm->t = (ButtonFullScale * BeamCurrentFraction) - bpm->b;
            if (debugFlag) printf("position:%g  b:%d   t:%d\n", position, bpm->b, bpm->t);
        }
        if (bpm->latched <= 1)
            bpm->latched++;
        bpm->time++;
    }
    else if (epicsStrnCaseCmp("BEAM ", cp, 5) == 0) {
        cp += 5;
        while (isspace(*cp)) {
            if (*cp == '\0')
                return -1;
            cp++;
        }
        if (epicsStrCaseCmp("OFF", cp) == 0) {
            bpm->beamOn = 0;
        }
        else if (epicsStrCaseCmp("ON", cp) == 0) {
            bpm->latched = 0;
            bpm->beamOn = 1;
        }
        else
            return -1;
    }
    else if ((epicsStrCaseCmp("BUTTON-X?", cp) == 0)
         ||  (epicsStrCaseCmp("BUTTON+X?", cp) == 0)
         ||  (epicsStrCaseCmp("BUTTON-Y?", cp) == 0)
         ||  (epicsStrCaseCmp("BUTTON+Y?", cp) == 0)) {
        int val = 0;
        if ((bpm->latched == 1) && bpm->beamOn) {
            if (epicsStrCaseCmp("BUTTON-X?", cp) == 0)   val = bpm->l;
            if (epicsStrCaseCmp("BUTTON+X?", cp) == 0)   val = bpm->r;
            if (epicsStrCaseCmp("BUTTON-Y?", cp) == 0)   val = bpm->b;
            if (epicsStrCaseCmp("BUTTON+Y?", cp) == 0) { val = bpm->t;
                                                         bpm->latched = 0; }
        }
        fprintf(p->fp, "%d\n", val);
        if (debugFlag) printf("Latched:%d beam:%d val:%d\n", bpm->latched, bpm->beamOn, val);
    }
    else if ((epicsStrnCaseCmp("HCORR ", cp, 6) == 0)
          || (epicsStrnCaseCmp("VCORR ", cp, 6) == 0)) {
        double xtmp;
        if (sscanf (cp+6, "%lf", &xtmp) != 1)
            return -1;
        if (xtmp < -CorrectorFullScaleAmps)
            xtmp = -CorrectorFullScaleAmps;
        else if (xtmp > CorrectorFullScaleAmps)
            xtmp = CorrectorFullScaleAmps;
        if ((*cp == 'h') || (*cp == 'H'))
            bpm->hAmps = xtmp;
        else
            bpm->vAmps = xtmp;
    }
    else {
        return -1;
    }
    return 0;
}

static void
workerThread(void *a)
{
    struct workerParms *p = (struct workerParms *)a;
    char cbuf[512];
    int l;
    double volts = 0;

    if ((p->fp = fdopen(p->fd, "r+")) == NULL) {
        fprintf(stderr, "Can't create stdio stream for %s\n", p->clientName);
        return;
    }
    for (;;) {
        fflush(p->fp);
        if (fgets(cbuf, sizeof cbuf, p->fp) == NULL) {
            if (ferror(p->fp))
                fprintf(stderr, "Error reading %s: %s\n", p->clientName, strerror(errno));
            break;
        }
        l = strlen(cbuf);
        if (l <= 1)
            continue;
        if (cbuf[l-1] != '\n') {
            int c;
            fprintf(p->fp, "ERROR -- Command too long.\n");
            while (((c = getc(p->fp)) != '\n') && (c != EOF))
                continue;
            continue;
        }
        cbuf[l-1] = '\0';

        /*
         * *IDN?
         */
        if (epicsStrCaseCmp("*IDN?", cbuf) == 0) {
            fprintf(p->fp, "USPAS-2010 Network attached device.          This is a really long identification string to check for buffer overflow.\n");
        }

        /*
         * LOADAV?
         */
        else if (epicsStrCaseCmp("LOADAV?", cbuf) == 0) {
            double avg[3];
            if(getloadavg(avg, 3) != 3) {
                fprintf(p->fp, "ERROR -- Can't get load average.\n");
            }
            else {
                fprintf(p->fp, "%g %g %g\n", avg[0], avg[1], avg[2]);
            }
        }

        /*
         * CLIENT?
         */
        else if (epicsStrCaseCmp("CLIENT?", cbuf) == 0) {
            fprintf(p->fp, "%s\n", p->clientName);
        }

        /*
         * VOLTAGE?
         */
        else if (epicsStrCaseCmp("VOLTAGE?", cbuf) == 0) {
            fprintf(p->fp, "%g\n", volts);
        }

        /*
         * VOLTAGE
         */
        else if (sscanf(cbuf, "VOLTAGE %lf", &volts) == 1) {
        }

        /*
         * BPM/Corrector command?
         */
        else if (bpmCorrector(cbuf, p) == 0) {
        }

        /*
         * Must be an error
         */
        else {
            fprintf(p->fp, "BAD COMMAND \"%s\"\n", cbuf);
        }
    }
    fclose(p->fp);
    free(a);
}

int
main (int argc, char **argv)
{
    int s, s1;
    struct sockaddr_in myAddr, farAddr;
    socklen_t addrlen;
    epicsThreadId tid;
    int tnum = 0;
    int yes = 1;
    int c;
    int port = BASE_PORT;
    extern char *optarg;

    while ((c = getopt(argc, argv, "dp:")) != EOF) {
        switch(c) {
            case 'd': debugFlag++; break;
            case 'p': port = atoi(optarg); break;
        }
    }
    printf("Port %d\n", port);
    s = socket (AF_INET, SOCK_STREAM, 0);
    if (s < 0) {
        fprintf(stderr, "Can't create socket: %s\n", strerror (errno));
        return 1;
    }
    s1 = setsockopt (s, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof yes);
    if (s1 < 0) {
        fprintf(stderr, "Error from setsockopt: %s\n", strerror (errno));
    }
    myAddr.sin_family = AF_INET;
    myAddr.sin_port = htons (port);
    myAddr.sin_addr.s_addr = INADDR_ANY;
    memset (myAddr.sin_zero, '\0', sizeof myAddr.sin_zero);
    if (bind (s, (struct sockaddr *)&myAddr, sizeof myAddr) < 0) {
        fprintf(stderr, "Can't bind socket: %s\n", strerror (errno));
        return 1;
    }
    if (listen (s, 2) < 0) {
        fprintf(stderr, "Can't listen on socket: %s\n", strerror (errno));
        return 1;
    }
    for (;;) {
        struct workerParms *p;
        char name[1000];
        addrlen = sizeof farAddr;
        s1 = accept (s, (struct sockaddr *)&farAddr, &addrlen);
        if (s1 < 0) {
            fprintf(stderr, "Can't accept connection: %s\n", strerror (errno));
            return 1;
        }
        sockAddrToA((struct sockaddr *)&farAddr, name, sizeof name);
        p = calloc(1, sizeof *p);
        if (p == NULL) {
            fprintf(stderr, "Can't allocate memory: %s\n", strerror (errno));
            return 1;
        }
        p->fd = s1;
        p->clientName = epicsStrDup(name);
        sprintf(name, "Worker %9.9d", ++tnum);
        tid = epicsThreadCreate(name,
                                epicsThreadPriorityMedium,
                                epicsThreadGetStackSize(epicsThreadStackMedium),
                                workerThread,
                                p);
        if (tid == 0) {
            fprintf(stderr, "Can't create worked thread\n");
            close(s1);
        } else {
            printf("Worker #%d created, tid=%p, client:%s\n", tnum, tid, p->clientName);
        }
    }
}

