/** Writes a string to the controller.
  * \param[in] output The string to be written.
  * \param[in] timeout Timeout before returning an error.*/
asynStatus AG_CONEXController::writeCONEX(const char *output, double timeout)
{
  size_t nwrite;
  asynStatus status;
  // const char *functionName="writeCONEX";
  
  status = pasynOctetSyncIO->write(pasynUserController_, output,
                                   strlen(output), timeout, &nwrite);
                                   
  // On Linux it seems to be necessary to delay a short time between writes
  #ifdef linux
  epicsThreadSleep(LINUX_WRITE_DELAY);
  #endif
                                  
  return status ;
}

int main(int argc,char *argv[])
{
	int i, j;
	extern short DEBUG;
	short daemon;
	char stcmd[256];

	/* Need to catch hang up signal to make sure semaphores are
	   cleaned up properly */
	SetSigShutdownHandler();

	daemon = 0;
	if(argc>=2) {    
		snprintf( stcmd, 255, "%s", argv[1]);
		/* parse command line args for crate and IP */
		j = 0;
		for( i = 2; i < argc; i++) {
		  if( strcmp( argv[i], "-D") == 0) {
		    i++;
		    DEBUG = atoi( argv[i]);
		  } else if( strcmp( argv[i], "-d") == 0) {
		    daemon = 1;
		  }
		}
	}
	else
	{
		printf( "Syntax: hvcontrol <st.cmd> [-c <name>@<hostname>[:<slotlist>]] [-d] [-D <debuglevel>]\n");
		printf( "        where <slotlist> = comma and dash separated list of slots.\n");
		printf( "              -d = run in daemon mode\n");
		printf( "              <debuglevel> = 0 no messages\n");
		printf( "              <debuglevel> = 10 all available messages\n");
	}

	if (strlen(stcmd)>0)	iocsh(stcmd);

	if (daemon)	{
		for(;;)
			epicsThreadSleep(1.0);
	} else {
		iocsh(NULL);
	}
	Shutdown();

	return(0);
}

static void controlThreadFunc(void *param)
{

	drvM6802_taskConfig *ptaskConfig = (drvM6802_taskConfig*) param;
	drvM6802_controlThreadConfig *pcontrolThreadConfig;
	controlThreadQueueData		queueData;

	while( !ptaskConfig->pcontrolThreadConfig ) epicsThreadSleep(.1);
	pcontrolThreadConfig = (drvM6802_controlThreadConfig*) ptaskConfig->pcontrolThreadConfig;


	epicsPrintf("task launching: %s thread for %s task\n",pcontrolThreadConfig->threadName, ptaskConfig->taskName);

	while(TRUE) {
		EXECFUNCQUEUE            pFunc;
		execParam                *pexecParam;
		struct dbCommon          *precord;
		struct rset              *prset;

		drvM6802_taskConfig    *ptaskConfig;

		epicsMessageQueueReceive(pcontrolThreadConfig->threadQueueId,
				         (void*) &queueData,
					 sizeof(controlThreadQueueData));

		pFunc      = queueData.pFunc;
	        pexecParam = &queueData.param;	
		precord    = (struct dbCommon*) pexecParam->precord;
		prset      = (struct rset*) precord->rset;
		ptaskConfig = (drvM6802_taskConfig *) pexecParam->ptaskConfig;

		if(!pFunc) continue;
		else pFunc(pexecParam);

		if(!precord) continue;


		dbScanLock(precord);
		(*prset->process)(precord); 
		dbScanUnlock(precord);
	}


	return;
}

/** Get Acquition Status */ 
epicsInt32 mythen::getStatus()
{
    epicsInt32 detStatus;
    int aux;

    strcpy(outString_, "-get status");
    writeReadMeter();
    aux = stringToInt32(this->inString_);
    int m_status =  aux & 1;          // Acquire running status (non-zero)
    int t_status = aux & (1<<3);      // Waiting for trigger (non-zero)        
    int d_status = aux & (1<<16);     // No Data Available when not zero
    int triggerWaitCnt=0;
    double triggerWait;
    
    //printf("Mythen Status - M:%d\tT:%d\tD:%d\n",m_status,t_status, d_status);
    
    if (m_status || !d_status ) { 
      detStatus = ADStatusAcquire;
      
      triggerWaitCnt=0;
      //Waits for Trigger for increaseing amount of time for a total of almost 1 minute
      while ((t_status ) && (triggerWaitCnt<MAX_TRIGGER_TIMEOUT_COUNT)) {
        triggerWait = 0.0001*pow(10.0,((double)(triggerWaitCnt/10)+1));
        //Wait
        epicsThreadSleep(triggerWait);
        //Check again
        strcpy(outString_, "-get status");
        writeReadMeter();
        aux = stringToInt32(this->inString_);
        t_status = aux & (1<<3);       
        d_status = aux & (1<<16);     
        triggerWaitCnt++;
        
      }
      
      if (!d_status)
        detStatus = ADStatusReadout;
      if (triggerWaitCnt==MAX_TRIGGER_TIMEOUT_COUNT)
        detStatus = ADStatusError;
    }
    else
      detStatus = ADStatusIdle;
    
    return detStatus;
}

asynStatus AG_CONEXAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
  asynStatus status;
  //static const char *functionName = "AG_CONEXAxis::home";

  // Must go to unreferenced state to home
  sprintf(pC_->outString_, "%dRS", pC_->controllerID_);
  status = pC_->writeCONEX();
  epicsThreadSleep(1.0);

  // The CONEX-CC supports home velocity, but only by going to Configuration state (PW1) 
  // and writing to non-volatile memory with the OH command.
  // This is time-consuming and can only be done a limited number of times so we don't do it here.

  sprintf(pC_->outString_, "%dOR", pC_->controllerID_);
  status = pC_->writeCONEX();
  return status;
}

void
vxStats_busyloop(unsigned busyperc)
{
    epicsTimeStamp then, now;
    double         fac = vxStats_busyloop_period/(double)100.0;

    if ( busyperc > 100 )
        busyperc = 100;

    while ( vxStats_busyloop_run ) {
        epicsTimeGetCurrent(&then);
        do {
            epicsTimeGetCurrent(&now);
        } while ( epicsTimeDiffInSeconds(&now,&then) < (double)busyperc*fac );

        epicsThreadSleep((double)(100-busyperc)*fac);
    }
}

static
void testcancel(void)
{
    epicsJob *job[2];
    epicsThreadPool *pool;
    testOk1((pool=epicsThreadPoolCreate(NULL))!=NULL);
    if(!pool)
        return;

    cancel[0]=epicsEventCreate(epicsEventEmpty);
    cancel[1]=epicsEventCreate(epicsEventEmpty);

    testOk1((job[0]=epicsJobCreate(pool, &neverrun, EPICSJOB_SELF))!=NULL);
    testOk1((job[1]=epicsJobCreate(pool, &toolate, EPICSJOB_SELF))!=NULL);

    /* freeze */
    epicsThreadPoolControl(pool, epicsThreadPoolQueueRun, 0);

    testOk1(epicsJobUnqueue(job[0])==S_pool_jobIdle); /* not queued yet */

    epicsJobQueue(job[0]);
    testOk1(epicsJobUnqueue(job[0])==0);
    testOk1(epicsJobUnqueue(job[0])==S_pool_jobIdle);

    epicsThreadSleep(0.01);
    epicsJobQueue(job[0]);
    testOk1(epicsJobUnqueue(job[0])==0);
    testOk1(epicsJobUnqueue(job[0])==S_pool_jobIdle);

    epicsThreadPoolControl(pool, epicsThreadPoolQueueRun, 1);

    epicsJobQueue(job[1]); /* actually let it run this time */

    epicsEventMustWait(cancel[0]);
    testOk1(epicsJobUnqueue(job[0])==S_pool_jobIdle);
    epicsEventSignal(cancel[1]);

    epicsThreadPoolDestroy(pool);
    epicsEventDestroy(cancel[0]);
    epicsEventDestroy(cancel[1]);

    testOk1(shouldneverrun==0);
    testOk1(numtoolate==1);
}

/** Array generation ask that runs as a separate thread.  When the P_RunStop parameter is set to 1
  * it periodically generates a burst of arrays. */
void testArrayRingBuffer::arrayGenTask(void)
{
    double loopDelay;
    int runStop; 
    int i, j;
    int burstLength;
    double burstDelay;
    int maxArrayLength;
    int arrayLength;
    
    lock();
    /* Loop forever */ 
    getIntegerParam(P_MaxArrayLength, &maxArrayLength);   
    while (1) {
        getDoubleParam(P_LoopDelay, &loopDelay);
        getDoubleParam(P_BurstDelay, &burstDelay);
        getIntegerParam(P_RunStop, &runStop);
        // Release the lock while we wait for a command to start or wait for updateTime
        unlock();
        if (runStop) epicsEventWaitWithTimeout(eventId_, loopDelay);
        else         (void)epicsEventWait(eventId_);
        // Take the lock again
        lock(); 
        /* runStop could have changed while we were waiting */
        getIntegerParam(P_RunStop, &runStop);
        if (!runStop) continue;
        getIntegerParam(P_ArrayLength, &arrayLength);
        if (arrayLength > maxArrayLength) {
            arrayLength = maxArrayLength;
            setIntegerParam(P_ArrayLength, arrayLength);
        }
        getIntegerParam(P_BurstLength, &burstLength);
        for (i=0; i<burstLength; i++) {
            for (j=0; j<arrayLength; j++) {
                pData_[j] = i;
            }
            setIntegerParam(P_ScalarData, i);
            callParamCallbacks();
            doCallbacksInt32Array(pData_, arrayLength, P_ArrayData, 0);
            if (burstDelay > 0.0) 
                epicsThreadSleep(burstDelay);
        }
    }
}

int drvACQ196_ARM_enable(ST_STD_device *pSTDdev)
{

	if(pSTDdev->StatusDev & TASK_ARM_ENABLED ) 
	{
		printf("\n>>> drvACQ196_ARM_enable : ADC already run \n");
		return WR_ERROR;
	}

	acq196_set_ABORT(pSTDdev);
	epicsThreadSleep(0.1);

	acq196_set_ARM(pSTDdev);
	
	pSTDdev->StatusDev &= ~TASK_SYSTEM_IDLE;
	pSTDdev->StatusDev |= TASK_ARM_ENABLED;

	return WR_OK;
}

static void readdjob(void *arg, epicsJobMode mode)
{
    readdPriv *priv=arg;
    testOk1(mode==epicsJobModeRun||mode==epicsJobModeCleanup);
    if(mode==epicsJobModeCleanup)
        return;
    testOk1(priv->inprogress==0);
    testDiag("count==%u", priv->count);

    if(priv->count--) {
        priv->inprogress=1;
        epicsJobQueue(priv->job);
        epicsThreadSleep(0.05);
        priv->inprogress=0;
    }else{
        epicsEventSignal(priv->done);
        epicsJobDestroy(priv->job);
    }
}

//==============================================================================
//
int main () {
   std::cout << "test abstract client user starting ("
             << ACAI_VERSION_STRING << ")\n\n";

   ACAI::Client::initialise ();

   ClientUser* user = new ClientUser ();

   TestClient* t1 = new TestClient ("T1");
   TestClient* t2 = new TestClient ("T2");
   TestClient* t3 = new TestClient ("T3");
   TestClient* t4 = new TestClient ("T4");

   user->registerClient (t1);
   user->registerClient (t2);
   user->registerClient (t3);
   std::cout << "\n";

   std::cout << "open registered clients\n";
   user->openRegisteredChannels();
   std::cout << "registered clients opened\n";

   std::cout << "open T4 client\n";
   t4->openChannel();
   std::cout << "T4 client opened\n";

   bool ok = user->waitAllRegisteredChannelsReady (2.0, 0.1);
   std::cout << "all channels open " << (ok ? "yes" : "no") <<  "\n";

   for (int t = 0; t < 500; t++) {
      epicsThreadSleep (0.02);   // 20mSec
      ACAI::Client::poll ();     // call back function invoked from here
   }

   std::cout << "close registered clients\n";
   user->closeRegisteredChannels();

   delete user;

   ACAI::Client::finalise();
   std::cout << "\ntest abstract client user complete\n";
   return 0;
}

static void devAdmin_BO_SYS_RUN(ST_execParam *pParam)
{
	ST_ADMIN *pAdminCfg = drvAdmin_get_AdminPtr();
	struct dbCommon *precord = pParam->precord;
	ST_STD_device *pSTDdev = (ST_STD_device*) ellFirst(pAdminCfg->pList_DeviceTask);

	/**********************************************
		user add here modified code. 
	***********************************************/
	if( (int)pParam->setValue == 1 ) /* command to run  */
	{
		if( admin_check_Run_condition()  == WR_ERROR) 
			return;

		/* direct call to sub-device.. for status notify immediately */
		while(pSTDdev) 
		{
			pSTDdev->StatusDev &= ~TASK_ARM_ENABLED;
			pSTDdev->StatusDev |= TASK_DAQ_STARTED;

			pSTDdev = (ST_STD_device*) ellNext(&pSTDdev->node);
		}

		epicsThreadSleep(0.3);
	}
	else 
	{
#if 0	/* do nothing... 'cause aleady done in sub devices */	
		if( admin_check_Stop_condition()  == WR_ERROR) 
			return;

		/* do something, if you need */
		while(pSTDdev) 
		{
			drvACQ196_RUN_stop(pSTDdev);

			pSTDdev = (ST_STD_device*) ellNext(&pSTDdev->node);
		}
#endif
	}
	notify_refresh_admin_status();
	epicsPrintf("%s: %s: %d \n", pAdminCfg->taskName, precord->name, (int)pParam->setValue);
}

int main(int argc,char *argv[])
{
	int i, j;
	short daemon;
	char stcmd[256];

	/* Need to catch hang up signal to make sure semaphores are
	   cleaned up properly */
	SetSigShutdownHandler();

	daemon = 0;
	if(argc>=2) {    
		snprintf( stcmd, 255, "%s", argv[1]);
		j = 0;
		for( i = 2; i < argc; i++) {
		  if( strcmp( argv[i], "-D") == 0) {
		    i++;
		    DEBUG = atoi( argv[i]);
		  } else if( strcmp( argv[i], "-d") == 0) {
		    daemon = 1;
		  }
		}
	}
	else
	{
		printf( "Syntax: hvcontrol <st.cmd> [-d] [-D <debuglevel>]\n");
		printf( "              -d = run in daemon mode\n");
		printf( "              <debuglevel> = 0 no messages\n");
		printf( "              <debuglevel> = 10 all available messages\n");
	}

	if (strlen(stcmd)>0)	iocsh(stcmd);

	if (daemon)	{
		for(;;)
			epicsThreadSleep(1.0);
	} else {
		iocsh(NULL);
	}
	Shutdown();
    epicsExit(0);
	return(0);
}

TEST_CASE test_msg_throttle()
{
    ThrottledMsgLogger throttle("Test", 2.0);
    // OK to print one message.
    throttle.LOG_MSG("Hello!\n");
    // Then, messages are discouraged...
    TEST(!throttle.isPermitted());
    throttle.LOG_MSG("Hello, too!\n");
    throttle.LOG_MSG("Hello, too!\n");
    throttle.LOG_MSG("Hello, too!\n");
    throttle.LOG_MSG("Hello, too!\n");
    throttle.LOG_MSG("Hello, too!\n");

    TEST_MSG(1, "waiting a little bit...");
    epicsThreadSleep(2.5);
    throttle.LOG_MSG("Hello again\n");

    TEST_OK;
}

/* Some sample loads on MVME167:
*
*	-> sp jbk_artificial_load, 100000000, 10000, 1
*		Load average: 69%
*	-> sp jbk_artificial_load, 100000000, 100000, 1
*		Load average: 95%
*	-> sp jbk_artificial_load, 100000000, 25000, 1
*		Load average: 88%
*/
int jbk_artificial_load(unsigned long iter,unsigned long often,unsigned long tick_delay)
{
    volatile unsigned long i;
    double   delay = (double)tick_delay * epicsThreadSleepQuantum();

    if(iter==0)
    {
        printf("Usage: jbk_artificial_load(num_iterations,iter_betwn_delays,tick_delay)\n");
        return 0;
    }

    for(i=0; i<iter; i++)
    {
        if(i%often==0)
            epicsThreadSleep(delay);
    }

    return 0;
}

bool PvaClientMultiMonitorDouble::poll()
{
    if(!isMonitorConnected){
         connect();
         epicsThreadSleep(.01);
    }
    bool result = false;
    shared_vector<epics::pvData::boolean> isConnected = pvaClientMultiChannel->getIsConnected();
    for(size_t i=0; i<nchannel; ++i)
    {
         if(isConnected[i]) {
              if(pvaClientMonitor[i]->poll()) {
                   doubleValue[i] = pvaClientMonitor[i]->getData()->getDouble();
                   pvaClientMonitor[i]->releaseEvent();
                   result = true;
              }
         }
    }
    return result;
}

static void interruptThread(drvPvt *pdrvPvt)
{
    while(1) {
        epicsEventMustWait(pdrvPvt->waitWork);
        while(1) {
            int addr;
            epicsUInt32 value;
            ELLLIST *pclientList;
            interruptNode *pnode;
            asynUInt32DigitalInterrupt *pinterrupt;

            if(pdrvPvt->interruptDelay <= .0001) break;
            for(addr=0; addr<NCHANNELS; addr++) {
                chanPvt *pchannel = &pdrvPvt->channel[addr];
                epicsMutexMustLock(pdrvPvt->lock);
                value = pchannel->value;
                if(value<0xf) {
                    value +=1;
                } else if(value&0x80000000) {
                    value = 0;
                } else {
                    value <<= 1;
                }
                pchannel->value = value;
                epicsMutexUnlock(pdrvPvt->lock);
            }
            pasynManager->interruptStart(
                pdrvPvt->asynUInt32DigitalPvt,&pclientList);
            pnode = (interruptNode *)ellFirst(pclientList);
            while (pnode) {
                pinterrupt = pnode->drvPvt;
                addr = pinterrupt->addr;
                pinterrupt->callback(pinterrupt->userPvt, pinterrupt->pasynUser,
                                     pdrvPvt->channel[addr].value);
                pnode = (interruptNode *)ellNext(&pnode->node);
            }
            pasynManager->interruptEnd(pdrvPvt->asynUInt32DigitalPvt);
            epicsThreadSleep(pdrvPvt->interruptDelay);
        }
    }
}

void mdsPlusShotID(int param)
{
    int status;
    char buf[40];
    DBADDR    *paddr;

    paddr = (DBADDR *)dbCalloc(1, sizeof(struct dbAddr));

    /* sleep */
    epicsThreadSleep(2.0);
    /*status = dbPutLink(shotIdOutLink, DBR_LONG, &shotId, 1);*/
    /* Put next pulse number */
    sprintf(buf, "icrf:pulseid.VAL");
    status = dbNameToAddr(buf, paddr);
    status = dbPutField(paddr, DBR_LONG, &shotId, 1);

    free(paddr);
    if(genSubDebug > 0)
         printf("genSub: mdsPlusShotID()  next shot number [%ld]. Status=%d\n",shotId,status);

}

epicsShareFunc void epicsShareAPI epicsThreadOnce(epicsThreadOnceId *id, void (*func)(void *), void *arg)
{
    static struct epicsThreadOSD threadOnceComplete;
    #define EPICS_THREAD_ONCE_DONE &threadOnceComplete
    int status;

    epicsThreadInit();
    status = mutexLock(&onceLock);
    if(status) {
        fprintf(stderr,"epicsThreadOnce: pthread_mutex_lock returned %s.\n",
            strerror(status));
        exit(-1);
    }

    if (*id != EPICS_THREAD_ONCE_DONE) {
        if (*id == EPICS_THREAD_ONCE_INIT) { /* first call */
            *id = epicsThreadGetIdSelf();    /* mark active */
            status = pthread_mutex_unlock(&onceLock);
            checkStatusQuit(status,"pthread_mutex_unlock", "epicsThreadOnce");
            func(arg);
            status = mutexLock(&onceLock);
            checkStatusQuit(status,"pthread_mutex_lock", "epicsThreadOnce");
            *id = EPICS_THREAD_ONCE_DONE;    /* mark done */
        } else if (*id == epicsThreadGetIdSelf()) {
            status = pthread_mutex_unlock(&onceLock);
            checkStatusQuit(status,"pthread_mutex_unlock", "epicsThreadOnce");
            cantProceed("Recursive epicsThreadOnce() initialization\n");
        } else
            while (*id != EPICS_THREAD_ONCE_DONE) {
                /* Another thread is in the above func(arg) call. */
                status = pthread_mutex_unlock(&onceLock);
                checkStatusQuit(status,"pthread_mutex_unlock", "epicsThreadOnce");
                epicsThreadSleep(epicsThreadSleepQuantum());
                status = mutexLock(&onceLock);
                checkStatusQuit(status,"pthread_mutex_lock", "epicsThreadOnce");
            }
    }
    status = pthread_mutex_unlock(&onceLock);
    checkStatusQuit(status,"pthread_mutex_unlock","epicsThreadOnce");
}

/*
 * This function spawns additional thread that emulate PPS input. Function is used for
 * testing of timestamping functionality... DO NOT USE IN PRODUCTION!!!!!
 *
 * Change by: tslejko
 * Reason: testing utilities
 */
void mrmEvgSoftTime(void* pvt) {
    evgMrm* evg = static_cast<evgMrm*>(pvt);

    if (!evg) {
        errlogPrintf("mrmEvgSoftTimestamp: Could not find EVG!\n");
    }

    while (1) {
        epicsUInt32 data = evg->sendTimestamp();
        if (!data){
            errlogPrintf("mrmEvgSoftTimestamp: Could not retrive timestamp...\n");
            epicsThreadSleep(1);
            continue;
        }

        //Send out event reset
        evg->getSoftEvt()->setEvtCode(MRF_EVENT_TS_COUNTER_RST);

        //Clock out data...
        for (int i = 0; i < 32; data <<= 1, i++) {
            if (data & 0x80000000)
                evg->getSoftEvt()->setEvtCode(MRF_EVENT_TS_SHIFT_1);
            else
                evg->getSoftEvt()->setEvtCode(MRF_EVENT_TS_SHIFT_0);
        }

        struct timespec sleep_until_t;

        clock_gettime(CLOCK_REALTIME,&sleep_until_t); //Get current time
        /* Sleep until next full second */
        sleep_until_t.tv_nsec=0;
        sleep_until_t.tv_sec++;

        clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&sleep_until_t,0);


//		sleep(1);
    }
}