/*
 * ProfilerPV.cpp
 *
 *  Created on: Sep 27, 2015
 *      Author: Hovanes Egiyan
 */

#include "ProfilerPVs.hh"
#include "bpScalers.h"


using namespace std;

unsigned 	ProfilerPVs::ppMaxConnectChecks		= 	100; 	// Max tries for assign/connect checks
unsigned 	ProfilerPVs::ppMaxReadbackChecks	=  1000; 	// Max tries for readback during readback checks
unsigned	ProfilerPVs::ppSleepTime			=	500;	// Sleep time during readback checks
double		ProfilerPVs::ppReadbackTolerance	=	0.1; 	// Tolerance for readback during readback checks

const unsigned ProfilerPVs::ppNFIFO 			=  NFIFO;	// FIFO depth

typedef long* arrayPtrType;


template short				ProfilerPVs::getPV<short>( const std::string label,  enum compType syncType, short fakePar );
template unsigned			ProfilerPVs::getPV<unsigned>( const std::string label,  enum compType syncType, unsigned fakePar );
template double				ProfilerPVs::getPV<double>( const std::string label,  enum compType syncType, double fakePar );

template short 				ProfilerPVs::setValue<short>( std::string pvKey, const short value, enum compType syncType ) ;
template unsigned			ProfilerPVs::setValue<unsigned>( std::string pvKey, const unsigned value, enum compType syncType ) ;
template double				ProfilerPVs::setValue<double>( std::string pvKey, const double value, enum compType syncType ) ;

template bool 				ProfilerPVs::gotReadback<short>( const short setValue, const std::string readChannel, double tolerance) ;
template bool 				ProfilerPVs::gotReadback<unsigned>( const unsigned setValue, const std::string readChannel, double tolerance) ;
template bool 				ProfilerPVs::gotReadback<double>( const double setValue, const std::string readChannel, double tolerance) ;




// Add a PV with label pvKey and var type pvType to the bookkeeping maps.
void ProfilerPVs::addPV( const std::string pvType, const std::string pvKey, const std::string pvName ) {
	MtxLock objLock( mcMutex );
	cout << "Adding PV called " << pvName << " of type " << pvType << " with key " << pvKey << endl;
  	// Check if a PV with a label (key) have already be used.
	// If it has, then throw an exception. Otherwise, continue executing the function.
	if( ppIndex.count(pvKey) > 0 || ppName.count(pvKey) > 0 || ppType.count(pvKey) > 0 ) {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::addPV : PV with label " << pvKey << " has already been defined" ;
		throw std::runtime_error( errStream.str());
	}
	// Assign all the maps except the ppIndex map which is defined at the
	// connection time.
	if( ppCurrentTypeIndex.count( pvType) > 0 ) {
		ppTypeIndex[pvKey] = ppCurrentTypeIndex[pvType];
		ppName[pvKey] = pvName;
		ppType[pvKey] = pvType;
		ppCurrentIndex++;
		ppCurrentTypeIndex[pvType] += 1;
	} else {
		// throw an exception if pvType has not been defined a a key on the pvType map.
		stringstream errStream;
		errStream << "ProfilerPVs::addPV : unknown PV type " << pvType  ;
		throw std::runtime_error( errStream.str() );
	}
	return;
}

// Assign all PVs in this object using state sequence ID given by ssID.
// Also set monitor for all those PVs.
void ProfilerPVs::assignPVs() {
	MtxLock objLock( mcMutex );
	// Get number if elements in the PV arrays. All arrays
	// are expected to have the smae length.
	unsigned arrayLength = ppArrays.getNElm();
	// Get the offset of the first array, which is expected to be the
	// first array defined as a PV for C++ interface. The other arrays
	// are expected to follow the short array in the list of the PVs,
	// in other words not other PV should have been defined inbetween the
	// arrays for C++ interface.
	unsigned arrayOffset = ppArrays.getOffset();
	cout << "Length of Arrays is " << arrayLength << " , offset is " << arrayOffset << endl;
	// Define the offsets in the pvIndex array for our short, unsigned and double arrays.
	// It consists of an overall offset plus offsets by the number of elements in each
	// arrays of given type.
	map<std::string,unsigned> offsetMap;
	offsetMap["short"] 		= arrayOffset + 0*arrayLength;
	offsetMap["unsigned"] 	= arrayOffset + 1*arrayLength;
	offsetMap["double"]		= arrayOffset + 2*arrayLength;
	offsetMap["wave"]		= arrayOffset + 3*arrayLength;
	EV_ID lastEventID = 10;
	// Loop through all defined PVs and assign them, and then set monitor
	for( std::map<std::string,unsigned>::iterator it = ppTypeIndex.begin(); it != ppTypeIndex.end(); it++ ) {
		std::string pvLabel = it->first;			// Get the key
		// Only connect PVs that have not been connected, do not reassign PVs
		if (ppIndex.count(pvLabel) == 0) {
			// This is the index for pvLabel variable that can be used to
			// find the value in the PV array of the given type .
			unsigned indexForType = ppTypeIndex[pvLabel];
			// ppIndex for each PV that gets send to seq_ functions gets assigned the index value
			// for the  within the array of its type plus an offset from zero for the array of that type.
			ppIndex[pvLabel] = offsetMap[ppType[pvLabel]] + indexForType;
			const std::string pvName = ppName[pvLabel]; // Get the PV name that needs to be assigned
			cout << "Assigning pvIndex " << ppIndex[pvLabel] << " to " << pvName << endl;
			bool pvIsDisconnected = true; unsigned trialCount = 0;
			// Loop until the PV is connected ot the number of trials is exhausted.
			while ( pvIsDisconnected && trialCount < 100 ) {
				seq_pvAssign(ppSSid, ppIndex[pvLabel], pvName.c_str());
				usleep(ppSleepTime);
				short pvIsAssigned = seq_pvAssigned( ppSSid, ppIndex[pvLabel] );
				short pvIsConnected = seq_pvConnected( ppSSid, ppIndex[pvLabel]) ;
				pvIsDisconnected = (!pvIsAssigned) || (!pvIsConnected);
//				cout << "Trial # " << trialCount << " , pvIsAssigned is " << pvIsAssigned << " , pvIsConnected is " << pvIsConnected <<
//						" , pvIsDisconnected is " << pvIsDisconnected << endl;
				trialCount++;
			}
//			cout << "Trial count number is " << trialCount << endl;
			// If there is no connection after 100 attempts throw an exception
			if( trialCount >= ppMaxConnectChecks ) {
				std::stringstream errStream;
				errStream << "ProfilerPVs::assignPVs : PV with label " << pvLabel << " could not connect to " <<  pvName;
				throw std::runtime_error( errStream.str() );
			}
			// We need to monitor all PVs to be able to access the values from the pointers
			// without doing pvGets.
			seq_pvMonitor(ppSSid, ppIndex[pvLabel]);
			usleep(50); // Sleeping after setting monitor seems to help, although may not be needed.
			ppEventID[pvLabel] = (lastEventID++);
			cout << "Assigning event flag " <<  ppEventID[pvLabel] << " label " << pvLabel << " with pvIndex " <<  ppIndex[pvLabel] << endl;
			seq_pvSync( ppSSid,  ppIndex[pvLabel], ppEventID[pvLabel] );
		}
	}
	return;
}


template <>
inline short* ProfilerPVs::getPtr4Type<short>(short fakePar) {
//	cout << "Short array is at " << ppArrays.getShortPtr() << endl;
	return ppArrays.getShortPtr();
}

template <>
inline unsigned* ProfilerPVs::getPtr4Type<unsigned>(unsigned fakePar) {
	return ppArrays.getUnsignedPtr();
}

template <>
inline double* ProfilerPVs::getPtr4Type<double>(double fakePar) {
	return ppArrays.getDoublePtr();
}

template <>
arrayPtrType* ProfilerPVs::getPtr4Type<arrayPtrType>( arrayPtrType fakePar ) {
	return reinterpret_cast<arrayPtrType*>(ppArrays.getWavePtr());
}

// Return the value of the PV with label label
template<typename T>
T ProfilerPVs::getPV( const std::string label, enum compType syncType, T fakePar ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count( label ) > 0 ) {
		T* arrayPtr = getPtr4Type<T>( fakePar );
		cout << "arrayPtr for " << label << " is " << hex << showbase << arrayPtr << dec << endl;
		cout << "Will return  value at index " << ppTypeIndex[label]  << endl;
		return arrayPtr[ ppTypeIndex[label] ];
	} else {
		// throw an exception
		std::stringstream errStream;
		errStream << "ProfilerPVs::getPV : PV with label " << label << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
}

// Specialization for getPV for waveforms/arrays
template <>
arrayPtrType ProfilerPVs::getPV<arrayPtrType>( const std::string label, enum compType syncType, arrayPtrType fakePar ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count( label ) > 0 ) {
		arrayPtrType arrayPtr = reinterpret_cast<arrayPtrType>( getPtr4Type( fakePar ) );
		return arrayPtr + ( ppNFIFO * ppTypeIndex[label] );
	} else {
		// throw an exception
		std::stringstream errStream;
		errStream << "ProfilerPVs::getPV<arrayPtrType> : PV with label " << label << " does not exists" ;
		throw std::runtime_error( errStream.str() );

	}
}

void ProfilerPVs::getPV( const std::string label, enum compType syncType, vector<double>& vec ) {
	if( ppIndex.count( label ) > 0 ) {
		arrayPtrType arrBegin = getPV<arrayPtrType>( label, syncType, arrayPtrType() );
//		arrayPtrType arrBegin = arrayPtr  + ( ppArrays.getNFIFO() * ppTypeIndex[label] );

//		MtxLock objLock( mcMutex );
//		cout << " Printout for " << label << endl;
//		for( unsigned i = 0; i < vec.size(); i++ ) {
//			cout << " i " << i << " value " << arrayPtr[i] << " , ";
//			vec[i] = arrayPtr[i];
//		}
//		cout << endl;

//		std::copy( arrayPtr, arrayPtr + vec.size(), vec.begin() );
		std::copy( arrBegin, arrBegin + vec.size(), vec.begin() );
		return;
	} else {
		// throw an exception
		std::stringstream errStream;
		errStream << "ProfilerPVs::getPV<double> : PV with label " << label << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
}

// Set the value of the EPICS PV of type T given by the label (key) pvKey to the value value.
template <typename T>
T ProfilerPVs::setValue( std::string pvKey, const T value, enum compType syncType ) {
//	cout << "setValue receive value of " << value << endl;
	if( ppIndex.count(pvKey) > 0 ) {
		T* arrayPtr = getPtr4Type( T() );
		{ MtxLock objLock( mcMutex ); arrayPtr[ ppTypeIndex[pvKey] ]  = value; }
		seq_pvPut( ppSSid, static_cast<VAR_ID>( ppIndex[pvKey] ), syncType );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::setValue : PV with label " << pvKey << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
	return value;
}

// Specialization for setValue for waveforms/arrays of long type
template<>
arrayPtrType ProfilerPVs::setValue<arrayPtrType>( std::string pvKey, const arrayPtrType valPtr,
		enum compType syncType ) {
//	cout << "setValue<arrayPtrType> receive value pointer of " << valPtr << endl;
	if ( ppIndex.count( pvKey ) > 0 ) {
		arrayPtrType arrayPtr = reinterpret_cast<arrayPtrType>( getPtr4Type( valPtr ) );
		arrayPtrType arrBegin = arrayPtr  + ( ppNFIFO * ppTypeIndex[pvKey] );
//		arrayPtrType arrEnd = arrBegin + ppArrays.getNFIFO();
//		cout << "Array for " << pvKey << " is at " << hex << showbase << arrayPtr <<
//				" , Begin is at " << arrBegin << " , End is at " << arrEnd << dec << endl;
//			std::copy( arrBegin, arrEnd, valPtr );
		{
			MtxLock objLock( mcMutex );
			memcpy( arrBegin, valPtr, ppNFIFO * sizeof( long ) );
		}
//			memcpy( reinterpret_cast<arrayPtrType>( arrayPtr ), valPtr, ppArrays.nFIFO );
//		cout << "Will write for pvIndex " << ppIndex[pvKey] << endl;
		seq_pvPut( ppSSid, static_cast<VAR_ID>( ppIndex[pvKey] ), syncType );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::setValue<arrayPtrType> : PV with label " << pvKey
				<< " does not exists";
		throw std::runtime_error( errStream.str() );
	}
	return valPtr;
}

bool ProfilerPVs::pvCheckEvtFlag( const std::string pvKey ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count(pvKey) > 0 ) {
		return ( seq_efTest( ppSSid, ppEventID[pvKey] ) == 1 ? true : false );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::pvCheckEvtFlag : PV with label " << pvKey << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
}

bool ProfilerPVs::pvCheckAndClearEvtFlag( const std::string pvKey ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count(pvKey) > 0 ) {
		return ( seq_efTestAndClear( ppSSid, ppEventID[pvKey] ) == 1 ? true : false );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::pvCheckAndClearEvtFlag : PV with label " << pvKey << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
}

void ProfilerPVs::pvClearEvtFlag( const std::string pvKey ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count(pvKey) > 0 ) {
		seq_efClear( ppSSid, ppEventID[pvKey] );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::pvClearEvtFlag : PV with label " << pvKey << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
	return;
}
void ProfilerPVs::pvSetEvtFlag( const std::string pvKey ) {
//	MtxLock objLock( mcMutex );
	if( ppIndex.count(pvKey) > 0 ) {
		seq_efSet( ppSSid, ppEventID[pvKey] );
	} else {
		// throw an exception
		stringstream errStream;
		errStream << "ProfilerPVs::pvSetEvtFlag : PV with label " << pvKey << " does not exists" ;
		throw std::runtime_error( errStream.str() );
	}
	return;
}

// Method that waits until the setpoint and
// readback are the same within tolerance. If at the end the
// matching readback is not achieved, returns false. Otherwise,
// returns true.
template <typename T>
bool ProfilerPVs::gotReadback( const T setValue, const std::string readChannel, double tolerance) {
	bool pvIsNotSet = true;
	unsigned trialNumber = 0;
	// Loop until the pv is connected of the number of trials is exhausted
	while( pvIsNotSet && (trialNumber < ppMaxReadbackChecks) ) {
		usleep(ppSleepTime);
		double valueDifference = getPV( readChannel, SYNC, T() ) - setValue;
		pvIsNotSet =  (fabs( valueDifference ) > tolerance) ? true : false;
		trialNumber++;
	}
	if( trialNumber >= ppMaxReadbackChecks ) return false;
	else return true;
}

// Specialization for arrays/waveforms <T>=<arrayPtrType>
template <>
bool ProfilerPVs::gotReadback<arrayPtrType>( const arrayPtrType setValue, const std::string readChannel, double tolerance ) {
	bool pvIsNotSet = true;
	unsigned trialNumber = 0;
	// Loop until the pv is connected of the number of trials is exhausted
	while( pvIsNotSet and (trialNumber < ppMaxReadbackChecks) ) {
		usleep(ppSleepTime);
		arrayPtrType arrayPtr = static_cast<arrayPtrType>( getPV<arrayPtrType>( readChannel, SYNC, 0 ) );
		double diff = 0;
		for( unsigned ix = 0; ix < ppNFIFO; ix++ ) {
			diff += fabs( arrayPtr[ix] - setValue[ix] );
		}
		double valueDifference = diff / ppNFIFO;
		pvIsNotSet =  (fabs( valueDifference ) > tolerance) ? true : false;
		trialNumber++;
	}
	if( trialNumber >= ppMaxReadbackChecks ) return false;
	else return true;
}


template<>
bool ProfilerPVs::inputsChanged<short>() {
//	MtxLock objLock( mcMutex );
	EV_ID pvFlag = ppArrays.getShortFlag();
	if( seq_efTestAndClear(ppSSid, pvFlag) > 0 ) return true;
	return false;
}
template<>
bool ProfilerPVs::inputsChanged<unsigned>() {
//	MtxLock objLock( mcMutex );
	EV_ID pvFlag = ppArrays.getUnsignedFlag();
	if( seq_efTestAndClear(ppSSid, pvFlag) > 0 ) return true;
	return false;
}
template<>
bool ProfilerPVs::inputsChanged<double>() {
//	MtxLock objLock( mcMutex );
	EV_ID pvFlag = ppArrays.getDoubleFlag();
	if( seq_efTestAndClear(ppSSid, pvFlag) > 0 ) return true;
	return false;
}
template <>
bool ProfilerPVs::inputsChanged<arrayPtrType>() {
//	MtxLock objLock( mcMutex );
	EV_ID pvFlag = ppArrays.getWaveFlag();
	if( seq_efTestAndClear(ppSSid, pvFlag) > 0 ) return true;
	return false;
}