#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 <lloAsynDriver.h>
#include <asynPortDriver.h>
#include <asynOctetSyncIO.h>

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

using namespace TprLLO;

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

/** Constructor for the lloAsynDriver 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
 *
 * */

lloAsynDriver::lloAsynDriver(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);
    //  Super conducting timing diagnostics
    createParam(scLinkUpString,      asynParamInt32,         &p_scLinkUp);
    createParam(scFidCntString,      asynParamInt32,         &p_scFidCnt);
    createParam(scErrCntString,      asynParamInt32,         &p_scErrCnt);

    //  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(100000);
    reg->coreNC.resetRx();

    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(100000);
    reg->coreSC.resetRx();
    usleep(100000);

    reg->xbar.dump();

    callParamCallbacks();    //  Update PVs

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

lloAsynDriver::~lloAsynDriver()
{
}


asynStatus lloAsynDriver::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 lloAsynDriver::report(int i)
{
    printf("lloAsynDriver poll counts:\n");
    printf("    diag poll: %u\n", poll_count.diag);

    return 0;
}

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

int lloAsynDriver::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.dump(dc);
        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.);
    }

    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 lloAsynDriverConfigure(const char *portName, const char *devString)
{
    pLLDrv = new lloAsynDriver(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 = {"lloAsynDriverConfigure",2,initArgs};
static void initCallFunc(const iocshArgBuf *args)
{
    if(once) {
        printf("The linac locking driver allows single instance. "
               "Duplicated configuration is ignored.\n");
        return;
    }
    lloAsynDriverConfigure(args[0].sval, args[1].sval);
}

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

epicsExportRegistrar(lloAsynDriverRegister);



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

static int lloAsynDriverReport(int interest);
static int lloAsynDriverInitialize(void);
static struct drvet lloAsynDriver = {
    2,
    (DRVSUPFUN) lloAsynDriverReport,
    (DRVSUPFUN) lloAsynDriverInitialize
};


epicsExportAddress(drvet, lloAsynDriver);



static int lloAsynDriverReport(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 lloAsynDriverInitialize(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 */
