// Average Current Monitor (ACM) waveform controls
// Includes trigger states Run, Stop, 1-Shot, Force
// And a set of the last N waveforms.

// Inputs:
// A: Normalized waveform, 32,768 point in the range -1 to 1, 0 to 1, or -180 to 180 degrees
// B: Scale factor
// C: Trigger state (0 = stop, 1 = 1-shot, 2 = force, 3 = run
// D: Delay in seconds, for run mode

#include <iostream>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <aSubRecord.h>
#include <recGbl.h>
#include <alarm.h>
#include <dbAccess.h>
#include <epicsGuard.h>
#include <epicsMutex.h>
typedef epicsGuard<epicsMutex> oscopeGuard;

#include <registryFunction.h>
#include <epicsExport.h>
#include <array>
#include <algorithm>
#include <iterator>
#define BOOST_CB_ENABLE_DEBUG 1
#include <boost/circular_buffer.hpp>
//#include <boost/assert.hpp>
//#include <assert.h>

// Pseudocode
// target processing time <1ms
// process on FLNK macro in waveform, or state input
// Read requested state input (Force, single, run, stop)
//
// State Force
//    copy x to valx
//    set state to Stop
//
// State Single (difference from Force is when waveform is copied, but same action)
//    wait for the next waveform update
//    copy x to valx
//    set state to Stop
//
// State Run
//    if incoming timestamp is more than readInterval since last update
//     copy x to valx
//     add readInterval to time of last update
//
// State Stop

using namespace std;

enum runState {stop=0, run=1, single=2, force=3 };
static const size_t acmWfSize = 32768;
typedef std::array<double, acmWfSize> acmWf;

static const size_t acmWfBufSize = 100;
typedef boost::circular_buffer<acmWf> acmWfBuf;

class acmWfGroup
{
public:
	boost::circular_buffer<epicsTimeStamp> groupTimes;
	acmWfBuf iWfBuf;
	acmWfBuf qWfBuf;
	acmWfBuf currentWfBuf;
	acmWfBuf phaseWfBuf;
	acmWfBuf integratedWfBuf;

	acmWfGroup():
		groupTimes(acmWfBufSize),
		iWfBuf(acmWfBufSize),
		qWfBuf(acmWfBufSize),
		currentWfBuf(acmWfBufSize),
		phaseWfBuf(acmWfBufSize),
		integratedWfBuf(acmWfBufSize)
		{
	};
};

struct acmOscope
{

	/* Inputs */
	double *iWfIn,          // ADC units I waveform
	*qWfIn,          // ADC units Q wafeform
	*currentWfIn,    // ADC Units current (Amps) waveform
	*phaseWfIn,      // phase waveform in degrees
	*integratedWfIn, // ADC units integrated current
	*aslo,           // from ACM:HTR:9xx:x:ProbeBeam.ASLO
	*aoff,           // from ACM:HTR:9xx:x:ProbeBeam.AOFF
	*readInterval;   // seconds to wait between reading waveforms in Run mode
	runState *pRunState;    // User input, also changed by this function after single or force
	unsigned long *wfHistOffset, wfOldOffset,// x Waveforms are stored in a circular buffer. Set this to see nth previous wf
	*iWfSize,          // Number of points in I waveform
	*qWfSize,          // Number of points in Q wafeform
	*currentWfSize,    // Number of points in current (Amps) waveform
	*phaseWfSize,      // Number of points in phase waveform
	*integratedWfSize; // Number of points in integrated current

	// Outputs
	double  **iWfOut,     // ADC units I waveform
	**qWfOut,             // ADC units Q wafeform
	**currentWfOut,       // ADC Units current (Amps) waveform
	**phaseWfOut,         // phase waveform in degrees
	**integratedWfOut,    // ADC unites integrated current
	*iSclWfOut,           // I waveform, Amps
	*qSclWfOut,           // Q wafeform,Amps
	*currentSclWfOut,     // Current (Amps) waveform
	*integratedSclWfOut,  // Integrated current
	*iScl,                // I waveform aslo
	*qScl,                // Q wafeform aslo
	*currentScl,          // Current (Amps) aslo
	*integratedScl,       // Integrated current aslo
	*nextReadCountdown;

	acmWfGroup historyBufs;
	unsigned char *trstnToStop;
	bool bGetNextWfFlag; // Set in the "single" state
	epicsTimeStamp nextWfTime;
    mutable epicsMutex oscopeLock;
};

int wfInAndOut(aSubRecord *prec);
int wfOut(aSubRecord *prec);
void copyAndScale(const double *wfIn, double *wfOutScaled, const double aslo, const double aoff, const size_t wfSize);

int stateForce(aSubRecord *prec)
{
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	short debug = (prec->tpro > 1) ? 1 : 0;
	if(debug) printf("--> %s\n", __PRETTY_FUNCTION__ );
	int retVal = -1;

	if(*(thisOscope->trstnToStop) == true)
	{
		if(debug) printf("Waiting for transition to stop, exiting %s\n", __PRETTY_FUNCTION__);
		return -1;
	}

	if( wfInAndOut(prec) ==0)
	{
		*(thisOscope->trstnToStop) = true;
		retVal = 0;
	}
	if(debug) printf("<-- %s returning %d\n", __PRETTY_FUNCTION__, retVal );
	return retVal;
};

// Return 0 to process outputs, -1 otherwise
int stateSingle(aSubRecord *prec)
{
	short debug = (prec->tpro > 1) ? 1 : 0;
	if(debug) printf("--> %s\n", __PRETTY_FUNCTION__ );
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	int retVal = -1;

	if(*(thisOscope->trstnToStop) == true)
	{
		if(debug) printf("Waiting for transition to stop, exiting %s\n", __PRETTY_FUNCTION__);
	}
	else
	{
		// The first processing in this state is when we enter the state.
		// Wait until the subsequent processing triggered by new waveforms before capturing them.
		if(thisOscope->bGetNextWfFlag)
		{
			thisOscope->bGetNextWfFlag = false;
			*(thisOscope->trstnToStop) = true;
			retVal = wfInAndOut(prec);
		}
		else
		{
			thisOscope->bGetNextWfFlag = true;
			dbGetTimeStamp(&prec->inpe, &thisOscope->nextWfTime);
		}
	}
	if(debug) printf("<-- %s returning %d\n", __PRETTY_FUNCTION__, retVal );
	return retVal;
};

int stateRun(aSubRecord *prec)
{
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	short debug = (prec->tpro > 1) ? 1 : 0;
	if(debug) printf("--> %s\n", __PRETTY_FUNCTION__ );
	epicsTimeStamp timea;
	double tdiffNext;
	int retVal = -1;

	dbGetTimeStamp(&prec->inpe, &timea);
	tdiffNext =epicsTimeDiffInSeconds(&thisOscope->nextWfTime,&timea);

	// If the input timestamps are consistent, and it's past time to get new waveforms
	if( tdiffNext < 0.0 && wfInAndOut(prec) == 0)
	{
		wfInAndOut(prec);
		epicsTimeAddSeconds(&thisOscope->nextWfTime, *thisOscope->readInterval);
		// If adding the wait time to the timer results in a time before this set
		// of waveforms, use latest timestamp as a starting point and add timer.
		// Handles the first waveform, and cases where waveforms arrive slower than
		// the timeout. You don't want the timeout getting farther and farther into
		// the past.
		tdiffNext = epicsTimeDiffInSeconds(&thisOscope->nextWfTime,&timea);
		if(debug) printf("Next waveform in %f seconds.\n", tdiffNext);
		if(tdiffNext < 0)
		{
			if(debug) printf("Next waveform is in the past, so setting timeout to now.\n");
			thisOscope->nextWfTime = timea;
		}
		retVal = 0;
	}
	else
	{
		if(debug) printf("Next waveform in %f seconds.\n", tdiffNext);
	}
	if(debug) printf("<-- %s returning %d\n", __PRETTY_FUNCTION__, retVal );
	return retVal;
};

void copyInWf(const double *wfIn, acmWfBuf &circWfBuf)
{
	acmWf newWf;
	size_t i;

	if( wfIn == NULL )
	{
		return;
	}
	for (i = 0 ; i< acmWfSize; i++)
	{
		newWf[i] =  wfIn[i];
	}

	circWfBuf.push_front(newWf);
}

// return -1 and skip copy if waveform timestamps are inconsistent.
int wfInAndOut(aSubRecord *prec)
{
	short debug = (prec->tpro > 1) ? 1 : 0;
	if(debug) printf("--> %s\n", __PRETTY_FUNCTION__ );
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	epicsTimeStamp timei, timea;
	double tdiffIn;
	int retVal = -1;

	dbGetTimeStamp(&prec->inpa, &timei);
	dbGetTimeStamp(&prec->inpe, &timea);
	tdiffIn =epicsTimeDiffInSeconds(&timei,&timea);

	// If the input timestamps are consistent
	if( tdiffIn == 0.0 )
	{
		// Keep sets of waveforms coherent, don't start processing more until the last set is finished.
		oscopeGuard oG(thisOscope->oscopeLock);
		// Save incoming waveforms
//		cout << "Currently saved waveform count: " << thisOscope->historyBufs.groupTimes.size() << " of " << thisOscope->historyBufs.groupTimes.capacity() << endl;
		thisOscope->historyBufs.groupTimes.push_front(timei);
		// Make raw and scaled copies, store them
		copyInWf(thisOscope->iWfIn, thisOscope->historyBufs.iWfBuf);
		copyInWf(thisOscope->qWfIn, thisOscope->historyBufs.qWfBuf);
		copyInWf(thisOscope->currentWfIn, thisOscope->historyBufs.currentWfBuf);
		copyInWf(thisOscope->phaseWfIn, thisOscope->historyBufs.phaseWfBuf);
		copyInWf(thisOscope->integratedWfIn, thisOscope->historyBufs.integratedWfBuf);

	}

	// Let the mutex go out of scope before calling this, but same "if" as above
	if( tdiffIn == 0.0 )
	{
		retVal = wfOut(prec);
	}

	if(debug) printf("<-- %s returning %d\n", __PRETTY_FUNCTION__, retVal );
	return retVal;
}

void copyAndScale(const double *wfIn, double *wfOutScaled, const double aslo, const double aoff, const size_t wfSize)
{
	size_t i;

	if(wfIn == NULL) return;
	if( wfOutScaled == NULL) return;
	if (wfSize !=acmWfSize) return;

	for (i = 0 ; i < acmWfSize; i++)
	{
		wfOutScaled[i] = aslo * wfIn[i] + aoff;

	}
}

int wfOut(aSubRecord *prec)
{
	short debug = (prec->tpro > 1) ? 1 : 0;
	if(debug) printf("--> %s\n", __PRETTY_FUNCTION__ );
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	size_t bufferIndex = *thisOscope->wfHistOffset;
	int retVal = -1;

	// Keep sets of waveforms coherent, don't start processing more until the last set is finished.
	oscopeGuard oG(thisOscope->oscopeLock);

	if(thisOscope->historyBufs.iWfBuf.size() >= bufferIndex)
	{
		*thisOscope->iWfOut = thisOscope->historyBufs.iWfBuf[bufferIndex].data();
		copyAndScale(*thisOscope->iWfOut, thisOscope->iSclWfOut, *thisOscope->iScl, *thisOscope->aoff, acmWfSize);
	}

	if(thisOscope->historyBufs.qWfBuf.size() >=bufferIndex)
	{
		*thisOscope->qWfOut = thisOscope->historyBufs.qWfBuf[bufferIndex].data();
		copyAndScale(*thisOscope->qWfOut, thisOscope->qSclWfOut, *thisOscope->qScl, *thisOscope->aoff, acmWfSize);
	}

	if(thisOscope->historyBufs.currentWfBuf.size() >=bufferIndex)
	{
		*thisOscope->currentWfOut = thisOscope->historyBufs.currentWfBuf[bufferIndex].data();
		copyAndScale(*thisOscope->currentWfOut, thisOscope->currentSclWfOut, *thisOscope->currentScl, *thisOscope->aoff, acmWfSize);
	}

	if(thisOscope->historyBufs.phaseWfBuf.size() >=bufferIndex)
	{
		*thisOscope->phaseWfOut = thisOscope->historyBufs.phaseWfBuf[bufferIndex].data();
		// Phase is phase, no scaled version
	}

	if(thisOscope->historyBufs.integratedWfBuf.size() >=bufferIndex)
	{
		*thisOscope->integratedWfOut = thisOscope->historyBufs.integratedWfBuf[bufferIndex].data();
		copyAndScale(*thisOscope->integratedWfOut, thisOscope->integratedSclWfOut, *thisOscope->integratedScl, *thisOscope->aoff, acmWfSize);
	}

	if(thisOscope->historyBufs.groupTimes.size() >=bufferIndex)
	{
		prec->time.nsec = thisOscope->historyBufs.groupTimes[bufferIndex].nsec;
		prec->time.secPastEpoch = thisOscope->historyBufs.groupTimes[bufferIndex].secPastEpoch;
	}
	retVal = 0;
	return retVal;
}


static long acmInitOscope(aSubRecord *prec)
{
	printf("--> %s %s\n", __PRETTY_FUNCTION__, prec->name );
	acmOscope *newOscope = new acmOscope();
	prec->dpvt = (void *)newOscope;
	//short debug = (prec->tpro > 1) ? 1 : 0;
	/* Inputs */
	newOscope->iWfIn          = (double *)prec->a;   // ADC units I waveform
	newOscope->qWfIn          = (double *)prec->b;   // ADC units Q wafeform
	newOscope->currentWfIn    = (double *)prec->c;   // ADC Units current (Amps) waveform
	newOscope->phaseWfIn      = (double *)prec->d;   // phase waveform in degrees
	newOscope->integratedWfIn = (double *)prec->e;   // ADC units integrated current waveform
	newOscope->aslo           = (double *)prec->f;   // from ACM:HTR:9xx:x:ProbeBeam.ASLO
	newOscope->aoff           = (double *)prec->g;   // from ACM:HTR:9xx:x:ProbeBeam.AOFF
	newOscope->readInterval   = (double *)prec->h;   // seconds to wait between reading waveforms in Run mode
	newOscope->pRunState       = (runState *)prec->i; // User input; also changed by this function after single or force
	newOscope->wfHistOffset   = (unsigned long *)prec->j;     // x Waveforms are stored in a circular buffer. Set this to see nth previous wf
	*newOscope->wfHistOffset = 0;

	//	Scale factors
	newOscope->iScl               = (double *) prec->k;
	newOscope->qScl               = (double *) prec->l;
//	newOscope->phaseScl           = (double *) prec->m;
	newOscope->currentScl         = (double *) prec->n;
	newOscope->integratedScl      = (double *) prec->o;

	newOscope->iWfSize          = (unsigned long *)&prec->nea;   // ADC units I waveform
	newOscope->qWfSize          = (unsigned long *)&prec->neb;   // ADC units Q wafeform
	newOscope->currentWfSize    = (unsigned long *)&prec->nec;   // ADC Units current (Amps) waveform
	newOscope->phaseWfSize      = (unsigned long *)&prec->ned;   // phase waveform in degrees
	newOscope->integratedWfSize = (unsigned long *)&prec->nee;   // ADC units integrated current waveform

	// Outputs
	newOscope->iWfOut             = (double **) &prec->vala;
	newOscope->qWfOut             = (double **) &prec->valb;
	newOscope->currentWfOut       = (double **) &prec->valc;
	newOscope->phaseWfOut         = (double **) &prec->vald;
	newOscope->integratedWfOut    = (double **) &prec->vale;
	newOscope->iSclWfOut          = (double *) prec->valf;
	newOscope->qSclWfOut          = (double *) prec->valg;
	newOscope->currentSclWfOut    = (double *) prec->valh;
	newOscope->integratedSclWfOut = (double *) prec->vali;
	newOscope->nextReadCountdown  = (double *) prec->valj;
	newOscope->trstnToStop        = (unsigned char *) prec->valk;
	newOscope->nextWfTime = {0, 0};
	newOscope->wfOldOffset = *(newOscope->wfHistOffset) = 0;
	printf("<-- %s\n", __PRETTY_FUNCTION__ );
	return 0;
};

static long acmProcOscope(aSubRecord *prec)
{
	acmOscope *thisOscope = (acmOscope *)prec->dpvt;
	short debug = (prec->tpro > 1) ? 1 : 0;
	if (debug) printf("--> %s\n", __PRETTY_FUNCTION__ );

	epicsEnum16  sevrInt;
	int retVal=-1;

	// Check integrated current data is valid. Expected error states are shared by all waveforms
	dbGetSevr(&prec->inpe, &sevrInt);
	if ( sevrInt == INVALID_ALARM ) {
		if ( debug ) {
			printf("acmProcOscope %s: Waveform %i data invalid\n", prec->name, sevrInt);
		}
		// Do not process outputs
		return -1;
	}

	switch( (int) *(thisOscope->pRunState) )
	{
	case stop:
		if (*(thisOscope->wfHistOffset) == thisOscope->wfOldOffset )
		{
			*(thisOscope->trstnToStop) = false;
		}
		else
		{
			thisOscope->wfOldOffset = *(thisOscope->wfHistOffset);
			retVal = wfOut(prec);
		}
		break;
	case run:
		retVal = stateRun(prec);
		break;
	case single:
		retVal = stateSingle(prec);
		break;
	case force:
		retVal = stateForce(prec);
		break;
	default:
		break;
	}

	if (debug) printf("<-- %s\n", __PRETTY_FUNCTION__ );

	return retVal;
}

extern "C" {
epicsRegisterFunction(acmInitOscope);
epicsRegisterFunction(acmProcOscope);
}
