/*******************************************************************************
 *	TmsCycleParam.h	TMS Client library functions
 *			T.Barnaby,	BEAM Ltd,	2007-07-19
 *	updated by	D.Korchagin,	CERN AB-BI-SW,	2007-08-31
 *******************************************************************************
 */

#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <TmsCycleParam.h>
#include <BFile.h>
#include <BEntry.h>

#ifndef __Lynx__
#else
#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#endif

namespace Tms {

CycleParamState::CycleParamState(){
	clear();
}

void CycleParamState::clear(){
	num = 0;
	period = 0;
#ifndef __Lynx__
	state.value = 0x0;
#else
	state.value() = 0x0;
#endif
	bunchMask = 0;
	mean1Mask = 0;
	mean2Mask = 0;
	lo1Harmonic = 0;
	lo1Phase = 0;
	lo2Harmonic = 0;
	lo2Phase = 0;
	gateWidth = 0;
	gatePhase = 0;
	blrWidth = 0;
	blrPhase = 0;
}

void CycleParamState::setNext(int nextNum, BUInt32 nextPeriod, bool f1RefSigma, bool f1LoMsb, bool f2RefSigma, bool f2LoMsb, bool pllF2, bool acquire){
	num = nextNum;
	period = nextPeriod;

#ifndef __Lynx__
	state.value = (num << 28) | 0x0fffff00;
#else
	state.value() = (num << 28) | 0x0fffff00;
#endif
	state.acquireData = acquire;
	state.pllReference1 = f1RefSigma;
	state.pllLO1FromAddress = f1LoMsb;
	state.pllReference2 = f2RefSigma;
	state.pllLO2FromAddress = f2LoMsb;
	state.pllFeedbackSelect = pllF2;
}

BString CycleParamState::getString(){
	BString	str;
	
	str = str + "num=" + num + ",";
	str = str + "period=" + period + ",";
#ifndef __Lynx__
	str = str + "state=" + BString::convertHex(state.value) + ",";
#else
	str = str + "state=" + BString::convertHex(state.value()) + ",";
#endif
	str = str + "bunchMask=" + BString::convertHex(bunchMask) + ",";
	str = str + "mean1Mask=" + BString::convertHex(mean1Mask) + ",";
	str = str + "mean2Mask=" + BString::convertHex(mean2Mask) + ",";
	str = str + "lo1Harmonic=" + lo1Harmonic + ",";
	str = str + "lo1Phase=" + BString::convert(lo1Phase) + ",";
	str = str + "lo2Harmonic=" + lo2Harmonic + ",";
	str = str + "lo2Phase=" + BString::convert(lo2Phase) + ",";
	str = str + "gateWidth=" + BString::convert(gateWidth) + ",";
	str = str + "gatePhase=" + BString::convert(gatePhase) + ",";
	str = str + "blrWidth=" + BString::convert(blrWidth) + ",";
	str = str + "blrPhase=" + BString::convert(blrPhase);
	
	return str;
}

BError CycleParamState::setString(BString str){
	BError		err;
	BList<BString>	sl;
	BList<BString>	s;
	BIter		i;
	
	sl = str.getTokenList(",");
	for(sl.start(i); !sl.isEnd(i); sl.next(i)){
		s = sl[i].getTokenList("=");
		
		if(s[0] == "num")		num = s[1].retInt();
		else if(s[0] == "period")	period = s[1].retInt();
#ifndef __Lynx__
		else if(s[0] == "state")	state.value = s[1].retInt();
#else
		else if(s[0] == "state")	state.value() = s[1].retInt();
#endif
		else if(s[0] == "bunchMask")	bunchMask = s[1].retInt();
		else if(s[0] == "mean1Mask")	mean1Mask = s[1].retInt();
		else if(s[0] == "mean2Mask")	mean2Mask = s[1].retInt();
		else if(s[0] == "lo1Harmonic")	lo1Harmonic = s[1].retInt();
		else if(s[0] == "lo1Phase")	lo1Phase = s[1].retDouble();
		else if(s[0] == "lo2Harmonic")	lo2Harmonic = s[1].retInt();
		else if(s[0] == "lo2Phase")	lo2Phase = s[1].retDouble();
		else if(s[0] == "gateWidth")	gateWidth = s[1].retDouble();
		else if(s[0] == "gatePhase")	gatePhase = s[1].retDouble();
		else if(s[0] == "blrWidth")	blrWidth = s[1].retDouble();
		else if(s[0] == "blrPhase")	blrPhase = s[1].retDouble();
		else if(s[0] == "loHarmonic")	lo1Harmonic = s[1].retInt();
		else if(s[0] == "loPhase")	lo1Phase = s[1].retDouble();
	}
	
	return err;
}


CycleParamEdit::CycleParamEdit(){
	ring = 0;
	channel = 0;
	pllCycleStartFrequency = 0;
	pllInitialFrequency = 0;
	pllInitialFrequencyDelay = 0;
	pllFrefGain = 0;
	pllGain = 0;
	pllDdsMinimum = 0;
	pllDdsMaximum = 0;
	stateDelay = 0;
}

void CycleParamEdit::clear(){
	cycleType = "";
	name = "";
	info = "";
	ring = 0;
	channel = 0;
	pllCycleStartFrequency = 0;
	pllInitialFrequency = 0;
	pllInitialFrequencyDelay = 0;
	pllFrefGain = 0;
	pllGain = 0;
	pllDdsMinimum = 0;
	pllDdsMaximum = 0;
	stateDelay = 0;
	frefPhaseDelay.clear();
	stateTable.clear();
	settings.clear();
}

CycleParamEdit::CycleParamEdit(const CycleParam& param){
	cycleType = param.cycleType;
	name = param.name;
	info = param.info;
	ring = param.ring;
	channel = param.channel;
	pllCycleStartFrequency = param.pllCycleStartFrequency;
	pllInitialFrequency = param.pllInitialFrequency;
	pllInitialFrequencyDelay = param.pllInitialFrequencyDelay;
	pllFrefGain = param.pllFrefGain;
	pllGain = param.pllGain;
	pllDdsMinimum = param.pllDdsMinimum;
	pllDdsMaximum = param.pllDdsMaximum;
	stateDelay = param.stateDelay;
	frefPhaseDelay = param.frefPhaseDelay;
	stateTable = param.stateTable;
	settings = param.settings;
}

double CycleParamEdit::value(int numSamples, int harmonic, double phase, int sample){
	double		d;
	double		p;
	double		e;

	d = double(numSamples) / harmonic;
	p = double(numSamples) * phase;
	e = fmod(sample - p, d) / d;
	if(e < 0)
		e = 1 + e;

	return e;
}

int CycleParamEdit::bunch(int numSamples, int harmonic, double phase, int sample){
	double		d;
	double		p;

	d = double(numSamples) / harmonic;
	p = double(numSamples) * phase;
	
	if(p > sample)
		return int((numSamples + sample - p) / d) % harmonic;
	else
		return int((sample - p) / d) % harmonic;
}

/// This function will generate the phase tables for a given state.
/// It is passed the parameters for the LO1 reference and the LO2 reference.
/// If lo?Harmonic is 1, then FREF is generated.
BError CycleParamEdit::generateState(CycleParamState stateParam){
	BError		err;
	int		loHarmonic;
	int		numSamples = 512;
	int		s;
	int		b;
	double		e;
	double		w;
	double		ms;
	double		me;
	TmsPhase	pt;
	int		numBunches = 0;
	int		num = stateParam.num;
	TmsState	state = stateParam.state; 

//	printf("CycleParamEdit::generateState: %d: LO: %d %d,%f %d,%f\n", num, state.pllFeedbackSelect, lo1Harmonic, lo1Phase, lo2Harmonic, lo2Phase);
	stateTable[num].phaseTable.resize(numSamples);

	if(state.pllFeedbackSelect){
		if(state.pllLO2FromAddress)
			loHarmonic = 1;
		else
			loHarmonic = stateParam.lo2Harmonic;
	}
	else {
		if(state.pllLO1FromAddress)
			loHarmonic = 1;
		else
			loHarmonic = stateParam.lo1Harmonic;
	}
		
	// Calculate the number of particle bunches
	for(s = 0; s < loHarmonic; s++){
		if(stateParam.bunchMask & (1 << s)){
			numBunches++;
		}
	}

	// Generate the phase table entries
	for(s = 0; s < numSamples; s++){
#ifndef __Lynx__
		pt.value = 0;
#else
		pt.value() = 0;
#endif

		// Generate the LO1 reference
		e = value(numSamples, stateParam.lo1Harmonic, -stateParam.lo1Phase + 1.0 / (4 * stateParam.lo1Harmonic), s);
		if(e >= 0.5)
			pt.lo1 = 0;
		else
			pt.lo1 = 1;

		// Generate the LO2 reference
		e = value(numSamples, stateParam.lo2Harmonic, -stateParam.lo2Phase + 1.0 / (4 * stateParam.lo2Harmonic), s);
		if(e >= 0.5)
			pt.lo2 = 0;
		else
			pt.lo2 = 1;

		if(state.acquireData){
			// Generate the GATE
			e = value(numSamples, loHarmonic, stateParam.gatePhase/loHarmonic, s);
			b = bunch(numSamples, loHarmonic, stateParam.gatePhase/loHarmonic, s);
			w = stateParam.gateWidth;
#ifdef ZAP
if(num == 3)
printf("%d: %d %f %f\n", s, b, e, w);
#endif
			if(stateParam.bunchMask & (1 << b)){
				if(e < w)
					pt.gate = 1;
			}

			// Generate the mean filter strobes
			ms = w + 4.0 * loHarmonic / 512;
			me = ms + 4.0 * loHarmonic / 512;
			if(stateParam.mean1Mask & (1 << b)){
				if((e >= ms) && (e < me))
					pt.meanFilter1 = 1;
			}
			if(stateParam.mean2Mask & (1 << b)){
				if((e >= ms) && (e < me))
					pt.meanFilter2 = 1;
			}

			// Generate the BLR
#ifndef ZAP
			e = value(numSamples, loHarmonic, stateParam.blrPhase/loHarmonic, s);
			w = stateParam.blrWidth;

			if(e < w)
				pt.blr = 1;
#else
			if(s < 64)
				pt.blr = 1;
			if(s > 512 - 64)
				pt.blr = 1;
#endif
		}
		
#ifndef __Lynx__
		stateTable[num].phaseTable[s] = pt.value;
#else
		stateTable[num].phaseTable[s] = pt.value();
#endif
	}

	stateTable[num].period = stateParam.period;
#ifndef __Lynx__
	stateTable[num].state = state.value;
#else
	stateTable[num].state = state.value();
#endif
	stateTable[num].harmonic = loHarmonic;
	stateTable[num].numBunches = numBunches;
	stateTable[num].bunchMask = stateParam.bunchMask;
	
	return err;
}

BError CycleParamEdit::setStates(BList<CycleParamState> cycleStates){
	BError		err;
	unsigned int	i;

	if((cycleStates.number() < 2) || (cycleStates.number() > 16)){
		return err.set(ErrorParam, "Needs at least two and less than 17 cycle states to be set up");
	}
	
	stateTable.clear();
	stateTable.resize(cycleStates.number());

	for(i = 0; i < cycleStates.number(); i++){
		generateState(cycleStates[i]);
	}

	// Save settings for further edits
	settings.resize(cycleStates.number());
	for(i = 0; i < cycleStates.number(); i++){
		settings[i] = cycleStates[i].getString();
	}
	
	return err;
}

BError CycleParamEdit::getStates(BList<CycleParamState>& cycleStates){
	BError		err;
	CycleParamState	st;
	unsigned int	i;
	
	cycleStates.clear();

	for(i = 0; i < stateTable.size(); i++){
		getDefaultState(st);
		
		// Get information from settings
		if(settings.size() > i)
			st.setString(settings[i]);
		
		// Override real state
		st.num = i;
#ifndef __Lynx__
		st.state.value = stateTable[i].state;
#else
		st.state.value() = stateTable[i].state;
#endif
		
		cycleStates.append(st);
	}
	return err;
}

void CycleParamEdit::getDefaultState(CycleParamState& state){
	state.num = 0;
	state.period = 0;
#ifndef __Lynx__
	state.state.value = 0xffffff00;
#else
	state.state.value() = 0xffffff00;
#endif
	state.bunchMask = 0x00;
	state.mean1Mask = 0x00;
	state.mean2Mask = 0x00;
	state.lo1Harmonic = 1;
	state.lo1Phase = 0.0;
	state.lo2Harmonic = 1;
	state.lo2Phase = 0.0;
	state.gateWidth = 0.8;
	state.gatePhase = 0.0;
	state.blrWidth = 0.1;
	state.blrPhase = 0.85;
}

/// Calculates the base pickup phase values for the PS ring
void CycleParamEdit::getdefaultPickupPositions(BArray<BInt32>& pos){
	unsigned int	p = 43;		// The first pickup in the PS ring after injection
	unsigned int	n;
	
	pos.clear();
	pos.resize(tmsNumPickups);

	for(n = 0; n < tmsNumPickups; n++){
		pos[n] = int(round(((p - 43) * 512.0) / 100));

		if((p % 10) == 0){
			p += 3;
		}
		else if((p % 10) == 3){
			p += 2;
		}
		else if((p % 10) == 5){
			p += 2;
		}
		else if((p % 10) == 7){
			p += 3;
		}
	}
}

BString CycleParamEdit::getString(){
	unsigned int	n;
	unsigned int	p;
	BString		s;
	BString		st;
	BString		str;

	s.printf("cycleType: %s\n", cycleType.retStr()); str += s;
	s.printf("name: %s\n", name.retStr()); str += s;
	s.printf("info: %s\n", info.retStr()); str += s;
	s.printf("ring: %u\n", ring); str += s;
	s.printf("channel: %u\n", channel); str += s;
	s.printf("pllCycleStartFrequency: %u\n", pllCycleStartFrequency); str += s;
	s.printf("pllInitialFrequency: %u\n", pllInitialFrequency); str += s;
	s.printf("pllInitialFrequencyDelay: %u\n", pllInitialFrequencyDelay); str += s;
	s.printf("pllFrefGain: %u\n", pllFrefGain); str += s;
	s.printf("pllGain: %u\n", pllGain); str += s;
	s.printf("pllDdsMinimum: %u\n", pllDdsMinimum); str += s;
	s.printf("pllDdsMaximum: %u\n", pllDdsMaximum); str += s;
	s.printf("stateDelay: %u\n", stateDelay); str += s;
	
	for(n = 0; n < settings.size(); n++){
		s.printf("settings%d: %s\n", n, settings[n].retStr());
		str += s;
	}

	for(n = 0; n < frefPhaseDelay.size(); n++){
		s.printf("frefPhaseDelay%d: %u\n", n, frefPhaseDelay[n]);
		str += s;
	}

	for(n = 0; n < stateTable.size(); n++){
		s.printf("stateTable%d.period: %d\n", n, stateTable[n].period);
		str += s;
		s.printf("stateTable%d.state: 0x%x\n", n, stateTable[n].state);
		str += s;
		s.printf("stateTable%d.harmonic: %u\n", n, stateTable[n].harmonic);
		str += s;
		s.printf("stateTable%d.numBunches: %u\n", n, stateTable[n].numBunches);
		str += s;
		s.printf("stateTable%d.bunchMask: 0x%x\n", n, stateTable[n].bunchMask);
		str += s;

		st = "";
		for(p = 0; p < stateTable[n].phaseTable.size(); p++){
			if(st.len())
				st = st + ",";
			s.printf("0x%x", stateTable[n].phaseTable[p]);
			st = st + s;
		}
		s.printf("stateTable%d.phaseTable: %s\n", n, st.retStr());
		str += s;
	}

	return str;
}

BError CycleParamEdit::setString(BString string){
	BError		err;
	BList<BString>	lines;
	BEntryList	eList;
	BIter		i;
	int		n;
	BUInt32		p;
	BString		str;
	BList<BString>	strList;

	lines = string.getTokenList("\n");
	
	for(lines.start(i); !lines.isEnd(i); lines.next(i)){
		eList.append(BEntry(lines[i]));
	}
	
	cycleType = eList.findValue("cycleType:");
	name = eList.findValue("name:");
	info = eList.findValue("info:");
	ring = eList.findValue("ring:").retInt();
	channel = eList.findValue("channel:").retInt();
	pllCycleStartFrequency = eList.findValue("pllCycleStartFrequency:").retInt();
	pllInitialFrequency = eList.findValue("pllInitialFrequency:").retInt();
	pllInitialFrequencyDelay = eList.findValue("pllInitialFrequencyDelay:").retInt();
	pllFrefGain = eList.findValue("pllFrefGain:").retInt();
	pllGain = eList.findValue("pllGain:").retInt();
	pllDdsMinimum = eList.findValue("pllDdsMinimum:").retInt();
	pllDdsMaximum = eList.findValue("pllDdsMaximum:").retInt();
	stateDelay = eList.findValue("stateDelay:").retInt();

	settings.resize(0);
	for(n = 0; n < 16; n++){
		str = eList.findValue(BString("settings") + n + ":");
		if(str == "")
			break;
		settings.resize(n + 1);
		settings[n] = str;
	}

	frefPhaseDelay.clear();
	for(n = 0; n < 256; n++){
		str = eList.findValue(BString("frefPhaseDelay") + n + ":");
		if(str == "")
			break;
		frefPhaseDelay.resize(n + 1);
		frefPhaseDelay[n] = str.retInt();
	}

	stateTable.resize(0);
	for(n = 0; n < 14; n++){
		if(!eList.find(BString("stateTable") + n + ".state:"))
			break;

		stateTable.resize(n + 1);
		stateTable[n].period =  eList.findValue(BString("stateTable") + n + ".period:").retInt();
		stateTable[n].state =  eList.findValue(BString("stateTable") + n + ".state:").retUInt();
		stateTable[n].harmonic =  eList.findValue(BString("stateTable") + n + ".harmonic:").retInt();
		stateTable[n].numBunches =  eList.findValue(BString("stateTable") + n + ".numBunches:").retInt();
		stateTable[n].bunchMask =  eList.findValue(BString("stateTable") + n + ".bunchMask:").retInt();

		str = eList.findValue(BString("stateTable") + n + ".phaseTable:");
		strList = str.getTokenList(",");
		stateTable[n].phaseTable.resize(tmsPhaseTableSize);
		for(p = 0, strList.start(i); (p < tmsPhaseTableSize) && (p < strList.number()); p++, strList.next(i)){
			stateTable[n].phaseTable[p] = strList[i].retInt();
		}
	}

	return err;
}

BError CycleParamEdit::readFromFile(BString fileName){
	BError		err;
	BFile		file;
	BString		s;
	BString		str;
	
	if(err = file.open(fileName, "r")){
		err.set(err.getErrorNo(), BString("Unable to open file: ") + fileName);
		return err;
	}

	while(file.readString(s) > 0){
		str += s;
	}
	
	return setString(str);
}

BError CycleParamEdit::writeToFile(BString fileName){
	BError		err;
	BFile		file;

	if(err = file.open(fileName, "w")){
		err.set(err.getErrorNo(), BString("Unable to open file: ") + fileName);
		return err;
	}
	
	if(file.writeString(getString()) < 0)
		err.set(-errno, strerror(errno));;

	file.close();
	
	return err;
}

}

#if TESTBUILD
using namespace Tms;

int main(){
	CycleParamState		st;
	CycleParamEdit		cp;
	CycleParamEdit		cpnew;
	BList<CycleParamState>	cpl;
	unsigned int		i;
	int			s;
	TmsPhase		pt;
	BIter			ii;
	BString			str;
	
	cp.getDefaultState(st);

	st.loHarmonic = 8;
	st.loPhase = 0.0;
	st.bunchMask = 0x3C;
	st.mean1Mask = 0x3C;
	cpl.append(st);

	st.loHarmonic = 8;
	st.loPhase = 0.0;
	st.bunchMask = 0x3C;
	st.mean1Mask = 0x3C;
	cpl.append(st);
	
	st.loHarmonic = 16;
	st.loPhase = 0.0;
	st.bunchMask = 0x3C;
	st.mean1Mask = 0x3C;
	cpl.append(st);
	
	printf("String: %s\n", st.getString().retStr());
	CycleParamState		st1;
	st1.setString(st.getString());
	printf("String: %s\n", st1.getString().retStr());
	
	cp.setStates(cpl);

	printf("\nDisplay Settings\n");
	cp.getStates(cpl);
	for(cpl.start(ii); !cpl.isEnd(ii); cpl.next(ii)){
		printf("Settings: %s\n", cpl[ii].getString().retStr());
	}

	printf("Test getString/setString functions\n");
	str = cp.getString();
	cpnew.setString(str);
	str = cpnew.getString();
	
	printf("%s\n", str.retStr());
	
	printf("Write to file\n");

	
	for(i = 0; i < cp.stateTable.size(); i++){
		BFile		f(BString("phaseTable") + i + ".txt", "w");
		
		printf("State: %d: %x\n", i, cp.stateTable[i].state);
		
		for(s = 0; s < 512; s++){
#ifndef __Lynx__
			pt.value = cp.stateTable[i].phaseTable[s];
#else
			pt.value() = cp.stateTable[i].phaseTable[s];
#endif
			f.printf("%d ", pt.lo1);
			f.printf("%d ", pt.lo2);
			f.printf("%d ", pt.gate);
			f.printf("%d ", pt.blr);
			f.printf("%d ", pt.meanFilter1);
			f.printf("%d\n", pt.meanFilter2);
		}
	}

	
	return 0;
}
#endif