#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <math.h>
#include <time.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>

#include <ellLib.h>
#include <epicsTypes.h>
#include <epicsTime.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <epicsTimer.h>
#include <epicsMutex.h>
#include <epicsEvent.h>
#include <iocsh.h>

#include <drvSup.h>
#include <epicsExport.h>

#include <trigAlignAsynDriver.h>
#include <asynPortDriver.h>
#include <asynOctetSyncIO.h>

//#include "yamlLoader.h"
//#include "perfMeasure.h"
#include "tprTrigAlign.hh"

using namespace TprTrigAlign;

static const  char *driverName="trigAlignAsynDriver";
static char port_name[32];
static char dev_string[32];
static int once = 0;   /* do not make more thant one instace */
static class trigAlignAsynDriver *pLLDrv = NULL;

/** Constructor for the trigAlignAsynDriver class.
 * Calls constructor for the asynPortDriver base class.
 * \param[in] portName The name of the asyn port driver to be created.
 * \param[in] ipString The IP address for LINAC Locking
 *
 * */

trigAlignAsynDriver::trigAlignAsynDriver(const char *portName, 
                                               const char *devString)
    : asynPortDriver(portName,
                     1, /* number of elements of this device */
#if (ASYN_VERSION <<8 | ASYN_REVISION) < (4<<8 | 32)
                     NUM_LL_PARAMS, /* number of asyn params to be cleared for each device */
#endif /* check asyn version, under 4.32 */
                     asynInt32Mask | asynFloat64Mask | asynDrvUserMask, /* Interface mask */
                     asynInt32Mask | asynFloat64Mask,  /* Interrupt mask */
                     1, /* asynFlags.  This driver does block and it is not multi-device, so flag is 1 */
                     1, /* Autoconnect */
                     0, /* Default priority */
                     0) /* Default stack size*/
{
    if(once) exit(1);
    once = 1;

    plock = new epicsMutex;

    //    initPerfMeasure();

    //  Firmware version readback    
    createParam(fwVersionString,     asynParamInt32,         &p_fwVersion);
    //  Normal conducting timing diagnostics
    createParam(ncLinkUpString,      asynParamInt32,         &p_ncLinkUp);
    createParam(ncFidCntString,      asynParamInt32,         &p_ncFidCnt);
    createParam(ncErrCntString,      asynParamInt32,         &p_ncErrCnt);
    createParam(ncIntvErrString,     asynParamInt32,         &p_ncIntvErr);
    createParam(ncClkFreqString,     asynParamInt32,         &p_ncClkFreq);
    //  Super conducting timing diagnostics
    createParam(scLinkUpString,      asynParamInt32,         &p_scLinkUp);
    createParam(scFidCntString,      asynParamInt32,         &p_scFidCnt);
    createParam(scErrCntString,      asynParamInt32,         &p_scErrCnt);
    createParam(scIntvErrString,     asynParamInt32,         &p_scIntvErr);
    createParam(scClkFreqString,     asynParamInt32,         &p_scClkFreq);

    //  Normal Conducting trigger delay (1, 60, 360 Hz)
    createParam(ncDelayString,       asynParamInt32,         &p_ncDelay);


    //  Normal Conducting / Super Conducting Timing difference measurement
    createParam(refMarkString,       asynParamInt32,         &p_refMarkCnt);
    createParam(testMarkString,      asynParamInt32,         &p_testMarkCnt);
    createParam(tmoCntString,        asynParamInt32,         &p_tmoCnt);
    createParam(clocksString,        asynParamInt32,         &p_clocks);
    createParam(phaseString,         asynParamFloat64,       &p_phase);
    createParam(phaseNString,        asynParamFloat64,       &p_phaseN);

    printf("Using tpr %s\n",devString);

    int fd = open(devString, O_RDWR);
    if (fd < 0) {
      perror("Could not open");
      exit(1);
    }

    void* ptr = mmap(0, sizeof(LockCore), PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
    if (ptr == MAP_FAILED) {
      perror("Failed to map");
      exit(2);
    }

    reg = reinterpret_cast<LockCore*>(ptr);
    printf("FpgaVersion: %08X\n", reg->version.FpgaVersion);
    printf("BuildStamp: %s\n", reg->version.buildStamp().c_str());

    setIntegerParam(p_fwVersion, reg->version.FpgaVersion);

    /*
    const bool loopbackXbar = false;
    const bool loopbackGtx = false;
    const unsigned rxmode = 0;

    reg->app.rxmode = rxmode;

    reg->xbar.setEvr( loopbackXbar ? XBar::LoopIn : XBar::StraightIn );
    reg->xbar.setEvr( XBar::StraightOut);
    reg->app.loopbackNC = loopbackGtx ? 2 : 0;

    reg->coreNC.clkSel(false);
    reg->coreNC.resetRxPll();
    usleep(1000000);
    reg->coreNC.resetRx();
    usleep(1000000);
    */

    reg->coreNC.resetCounts();
    for(unsigned i=0; i<3; i++) {
      usleep(1000000);
      reg->coreNC.dump();
    }

    /*
    reg->xbar.setTpr( loopbackXbar ? XBar::LoopIn : XBar::StraightIn );
    reg->xbar.setTpr( XBar::StraightOut);
    reg->app.loopbackSC = loopbackGtx ? 2 : 0;

    reg->coreSC.clkSel(true);

    reg->coreSC.resetRxPll();
    usleep(1000000);
    reg->coreSC.resetRx();
    usleep(1000000);
    */
    reg->coreSC.resetCounts();

    for(unsigned i=0; i<3; i++) {
      usleep(1000000);
      reg->coreSC.dump();
    }

    reg->xbar.dump();

    callParamCallbacks();    //  Update PVs

    ncCache  = new TprTrigAlign::TprCore(reg->coreNC);
    scCache  = new TprTrigAlign::TprCore(reg->coreSC);
    appCache = new TprTrigAlign::LockApp(reg->app);
}

trigAlignAsynDriver::~trigAlignAsynDriver()
{
}


asynStatus trigAlignAsynDriver::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    const char *functionName = "writeInt32";

    /* set the paramter in the parameter library */
    status = (asynStatus) setIntegerParam(function, value);

    if (function == p_ncDelay) {
        reg->app.ncTrigDelay = value;
    }

    plock->lock();
    /* Do callback so higher layers see any changes */
    status = (asynStatus) callParamCallbacks();
    plock->unlock();


    if(status)
        epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
                      "%s:%s: status=%d, function=%d, value=%d",
                      driverName, functionName, status, function, value);
    else
        asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
                  "%s:%s: function=%d, value=%d\n",
                  driverName, functionName, function, value);

    return status;
}

int trigAlignAsynDriver::report(int i)
{
    printf("trigAlignAsynDriver poll counts:\n");
    printf("    diag poll: %u\n", poll_count.diag);

    return 0;
}

int trigAlignAsynDriver::dump(void)
{
    reg->coreNC.dump(*ncCache);
    reg->coreSC.dump(*scCache);
    reg->app   .dump(*appCache);
    
    return 0;
}

int trigAlignAsynDriver::diagnosticsPolling(void)
{

    while(1) {
        plock->lock();
        poll_count.diag++;
        // update NC link diags
        TprCore nc(reg->coreNC);
        setIntegerParam(p_ncLinkUp , (nc.CSR&2) ? 1:0);
        setIntegerParam(p_ncFidCnt , nc.SOFcounts);
        setIntegerParam(p_ncErrCnt , nc.RxDecErrs+nc.RxDspErrs);

        // update SC link diags
        TprCore sc(reg->coreSC);
        setIntegerParam(p_scLinkUp, (sc.CSR&2) ? 1:0);
        setIntegerParam(p_scFidCnt, sc.SOFcounts);
        setIntegerParam(p_scErrCnt, sc.RxDecErrs+nc.RxDspErrs);

        // update msmt
        LockApp dc(reg->app);
        reg->app.ack();
        //        reg->app.dump(dc);
        setIntegerParam(p_ncIntvErr  , dc.intvErrNC);
        setIntegerParam(p_scIntvErr  , dc.intvErrSC);
        setIntegerParam(p_ncClkFreq  , dc.clkFreqNC);
        setIntegerParam(p_scClkFreq  , dc.clkFreqSC);
        setIntegerParam(p_refMarkCnt , dc.refMarkCnt);
        setIntegerParam(p_testMarkCnt, dc.testMarkCnt);
        setIntegerParam(p_tmoCnt     , dc.tmoCnt);
        setIntegerParam(p_clocks     , dc.clks);

        //        static const double PS_PER_CK = 1./119.; // 8.4e-3;
        static const double PS_PER_PH = 7.e6;    // 1/7 MHz
#define int27(u) double(u&(1<<26) ? u-(1<<27) : u)

        setDoubleParam (p_phase , PS_PER_PH*int27(dc.phase )/double(dc.valid));
        setDoubleParam (p_phaseN, PS_PER_PH*int27(dc.phaseN)/double(dc.valid));
        callParamCallbacks();
        plock->unlock();
        epicsThreadSleep(1.1);
    }

    return 0;

}

extern "C" {

/** EPICS iocsh callable function to call constructor for the cpswAsynDriver class.
 * \param[in] portName The name of the asyn port driver to be created.
 * \param[in] devString Device file for TPR */
int trigAlignAsynDriverConfigure(const char *portName, const char *devString)
{
    pLLDrv = new trigAlignAsynDriver(portName, devString);
    strcpy(port_name, portName);
    strcpy(dev_string, devString);
    return 0;
}

/* EPICS iocsh shell commands */

static const iocshArg initArg0 = { "portName",iocshArgString};
static const iocshArg initArg1 = { "devString",iocshArgString};
static const iocshArg * const initArgs[] = {&initArg0,
                                            &initArg1};
static const iocshFuncDef initFuncDef = {"trigAlignAsynDriverConfigure",2,initArgs};
static void initCallFunc(const iocshArgBuf *args)
{
    if(once) {
        printf("The linac locking driver allows single instance. "
               "Duplicated configuration is ignored.\n");
        return;
    }
    trigAlignAsynDriverConfigure(args[0].sval, args[1].sval);
}

void trigAlignAsynDriverRegister(void)
{
    iocshRegister(&initFuncDef,initCallFunc);
}

epicsExportRegistrar(trigAlignAsynDriverRegister);



/* EPICS driver support for trigAlignAsynDriver */
static int diagnosticsPolling(void)
{
    return pLLDrv->diagnosticsPolling();
}

static int trigAlignAsynDriverReport(int interest);
static int trigAlignAsynDriverInitialize(void);
static struct drvet trigAlignAsynDriver = {
    2,
    (DRVSUPFUN) trigAlignAsynDriverReport,
    (DRVSUPFUN) trigAlignAsynDriverInitialize
};


epicsExportAddress(drvet, trigAlignAsynDriver);



static int trigAlignAsynDriverReport(int interest)
{
    if(!once) return 0;

    printf("LINAC Locking port name: %s, LINAC Locking device filename: %s\n", port_name, dev_string);
    pLLDrv->report(interest);

    if(interest<1) return 0;

    pLLDrv->dump();


    return 0;
}


static int trigAlignAsynDriverInitialize(void)
{

    if(!once) {
        printf ("Driver Initialization is so early.\n");
        return 0;
    }
    else      printf ("Driver Initialization is good.\n");

    epicsThreadCreate("LLDiagnosticsTask", epicsThreadPriorityLow,
                      epicsThreadGetStackSize(epicsThreadStackMedium),
                      (EPICSTHREADFUNC) diagnosticsPolling, 0);

    return 0;
}



}  /* extern C */
