/*******************************************************************************
 *	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(){
	period = 0;
	bunchMask = 0;
	mean1Mask = 0;
	mean2Mask = 0;
	loHarmonic = 0;
	loPhase = 0;
	useLoFref = 0;
	acquireData = 0;
	gateWidth = 0;
	gatePhase = 0;
	blrWidth = 0;
	blrPhase = 0;
}

BString CycleParamState::getString(){
	BString	str;
	
	str = str + "period=" + period + ",";
	str = str + "bunchMask=" + BString::convertHex(bunchMask) + ",";
	str = str + "mean1Mask=" + BString::convertHex(mean1Mask) + ",";
	str = str + "mean2Mask=" + BString::convertHex(mean2Mask) + ",";
	str = str + "loHarmonic=" + loHarmonic + ",";
	str = str + "loPhase=" + BString::convert(loPhase) + ",";
	str = str + "useLoFref=" + useLoFref + ",";
	str = str + "acquireData=" + acquireData + ",";
	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] == "period")		period = s[1].retInt();
		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] == "loHarmonic")	loHarmonic = s[1].retInt();
		else if(s[0] == "loPhase")	loPhase = s[1].retDouble();
		else if(s[0] == "useLoFref")	useLoFref = s[1].retInt();
		else if(s[0] == "acquireData")	acquireData = s[1].retInt();
		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();
	}
	
	return err;
}


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

CycleParamEdit::CycleParamEdit(const CycleParam& param){
	cycleType = param.cycleType;
	info = param.info;
	channel = param.channel;
	pllCycleStartFrequency = param.pllCycleStartFrequency;
	pllInitialFrequency = param.pllInitialFrequency;
	pllInitialFrequencyDelay = param.pllInitialFrequencyDelay;
	pllFrefGain = param.pllFrefGain;
	pllGain = param.pllGain;
	pllDdsMinimum = param.pllDdsMinimum;
	pllDdsMaximum = param.pllDdsMaximum;
	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(int num, Tms::TmsState state, CycleParamState stateParam, int lo1Harmonic, double lo1Phase, int lo2Harmonic, double lo2Phase){
	BError		err;
	int		loHarmonic;
	int		numSamples = 512;
	int		s;
	int		b;
	double		e;
	double		w;
	double		ms;
	double		me;
	TmsPhase	pt;
	int		numBunches = 0;

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

	if(!stateParam.useLoFref){
		// If configured to use the PLL's address counters MSB as FREF, set the appropriate state table bits
		// In this case the appropriate LO1/LO2 phase table signal will not be used.
		if(state.pllReference1 == 0)
			state.pllLO1FromAddress = 1;
		if(state.pllReference2 == 0)
			state.pllLO2FromAddress = 1;
	}

	if(state.pllFeedbackSelect)
		loHarmonic = lo2Harmonic;
	else
		loHarmonic = 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, lo1Harmonic, -lo1Phase + 1.0 / (4 * lo1Harmonic), s);
		if(e >= 0.5)
			pt.lo1 = 0;
		else
			pt.lo1 = 1;

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

		if(stateParam.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;
	Tms::TmsState	state;
	UInt32		loHarmonic;
	double		loPhase;
	UInt32		loNextHarmonic;
	double		loNextPhase;
	int		lo = 0;
	int		num = 0;
	int		errorState = 15;
	int		stopState = 15;
	CycleParamState	defState;

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

	// Set up start state
#ifndef __Lynx__
	state.value = 0x0;
#else
	state.value() = 0x0;
#endif
	state.calStart = 1;
	state.calStop = errorState;
	state.injection = 3;
	state.hchange = errorState;
	state.cycleStop = stopState;
	state.delay = num;
	state.aquireData = 1;
	state.pllReference1 = 0;
	state.pllReference2 = 1;
	state.pllFeedbackSelect = lo;

	loHarmonic = 1;
	loPhase = 0.0;
	loNextHarmonic = cycleStates[1].loHarmonic;
	loNextPhase = cycleStates[1].loPhase;
	generateState(num, state, defState, loHarmonic, loPhase, loNextHarmonic, loNextPhase);
	num++;

	// Calibration state	
	state.calStart = errorState;
	state.calStop = 2;
	state.injection = errorState;
	state.hchange = errorState;
	state.cycleStop = stopState;
	state.delay = num;
	state.aquireData = 1;
	state.pllReference1 = 0;
	state.pllReference2 = 1;
	state.pllFeedbackSelect = 1;

	loHarmonic = 1;
	loPhase = 0.0;
	loNextHarmonic = cycleStates[0].loHarmonic;
	loNextPhase = cycleStates[0].loPhase;
	generateState(num, state, cycleStates[0], loHarmonic, loPhase, loNextHarmonic, loNextPhase);
	num++;

	// Await injection state
	state.calStart = errorState;
	state.calStop = errorState;
	state.injection = 3;
	state.hchange = errorState;
	state.cycleStop = stopState;
	state.delay = num;
	state.aquireData = 1;
	state.pllReference1 = 0;
	state.pllReference2 = 1;
	state.pllFeedbackSelect = 0;

	loHarmonic = 1;
	loPhase = 0.0;
	loNextHarmonic = cycleStates[1].loHarmonic;
	loNextPhase = cycleStates[1].loPhase;
	generateState(num, state, defState, loHarmonic, loPhase, loNextHarmonic, loNextPhase);
	num++;

	lo = 1;
	for(i = 1; i < cycleStates.number(); i++){
		// Set up state
#ifndef __Lynx__
		state.value = 0x0;
#else
		state.value() = 0x0;
#endif
		state.calStart = errorState;
		state.calStop = errorState;
		state.injection = errorState;
		state.cycleStop = stopState;
		state.delay = num;
		state.aquireData = 1;
		if(lo){
			state.pllReference1 = 0;
			state.pllReference2 = 1;
		}
		else {
			state.pllReference1 = 1;
			state.pllReference2 = 0;
		}
		state.pllFeedbackSelect = lo;
		
		loHarmonic = cycleStates[i].loHarmonic;
		loPhase = cycleStates[i].loPhase;
		if(i + 1 >= cycleStates.number()){
			loNextHarmonic = 1;
			loNextPhase = 0.0;
			state.injection = errorState;
			state.hchange = errorState;
		}
		else {
			loNextHarmonic = cycleStates[i + 1].loHarmonic;
			loNextPhase = cycleStates[i + 1].loPhase;
			state.injection = num + 1;
			state.hchange = num + 1;
		}
		
		if(lo){
			generateState(num, state, cycleStates[i], loNextHarmonic, loNextPhase, loHarmonic, loPhase);
			lo = 0;
		}
		else {
			generateState(num, state, cycleStates[i], loHarmonic, loPhase, loNextHarmonic, loNextPhase);
			lo = 1;
		}
		num++;
	}

	// 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 < settings.size(); i++){
		st.setString(settings[i]);
		cycleStates.append(st);
	}
	
	return err;
}

void CycleParamEdit::getDefaultState(CycleParamState& state){
	state.bunchMask = 0x00;
	state.mean1Mask = 0x00;
	state.mean2Mask = 0x00;
	state.loHarmonic = 1;
	state.loPhase = 0.0;
	state.useLoFref = 0;
	state.acquireData = 1;
	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<Int32>& 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("info: %s\n", info.retStr()); 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;
	
	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;
	UInt32		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:");
	info = eList.findValue("info:");
	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();

	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.resize(40);
	for(n = 0; n < 40; n++){
		frefPhaseDelay[n] = eList.findValue(BString("frefPhaseDelay") + n + ":").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:").retInt();
		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
