

#include <stdio.h>
#include "stpm10.h"

// promenne

// vysledna data
#pragma udata resdat

stpmResultType stpmResult[STPM_NUMBER];

#pragma udata 

// kalibracni data
stpmCalibrationType stpmCal[STPM_NUMBER];

// prubezna data
typedef struct {
	unsigned long actEnergy;
	unsigned long appEnergy;
	unsigned long reaEnergy;
	unsigned long t1Energy;
	unsigned char status;
	unsigned char mode;
	unsigned int freq;

	int uRMS;
	int uMOM;
	int iRMS;
	int iMOM;
	
	unsigned long confLow;
	unsigned long confHigh;
} stpmRealData;

#pragma udata srdat

stpmRealData srd;

#pragma udata 


// prubezna data - ukladani stavu
typedef struct {
	unsigned int freq;
	unsigned int iRMS;
	unsigned int uRMS;
	signed int iMax;
	signed int uMax;
	unsigned char status;
	//unsigned char mode;

	stpmEnergyCounter actEnergy;
	stpmEnergyCounter appEnergy;
	stpmEnergyCounter reaEnergy;
	stpmEnergyCounter t1Energy;

	unsigned long actEnergyL;
	unsigned long appEnergyL;
	unsigned long reaEnergyL;
	unsigned long t1EnergyL;


	unsigned long deltaTime;	//cas mezi merenimi v us

	unsigned char initialized;	//v nule pred prvnim prenosem z STPM
} stpmIntermediateData;

typedef struct {
	stpmEnergyCounter actEnergy;
	stpmEnergyCounter appEnergy;
	stpmEnergyCounter reaEnergy;
	stpmEnergyCounter t1Energy;
} stpmLastIntermediateData;

#pragma udata itdat

stpmIntermediateData sid[STPM_NUMBER];

#pragma udata 

#pragma udata litdat

stpmLastIntermediateData slid[STPM_NUMBER];

#pragma udata 

// inicializace knihovny
void initializeRealData(unsigned char id) {
	sid[id].freq = 0;
	sid[id].iRMS = 0;
	sid[id].uRMS = 0;
	sid[id].status = 0;
//	sid[id].mode = 0;
	sid[id].deltaTime = 0;

	sid[id].actEnergy.dec = 0;
	sid[id].actEnergy.frac = 0;
	sid[id].appEnergy.dec = 0;
	sid[id].appEnergy.frac = 0;
	sid[id].reaEnergy.dec = 0;
	sid[id].reaEnergy.frac = 0;
	sid[id].t1Energy.dec = 0;
	sid[id].t1Energy.frac = 0;

	slid[id].actEnergy.dec = 0;
	slid[id].actEnergy.frac = 0;
	slid[id].appEnergy.dec = 0;
	slid[id].appEnergy.frac = 0;
	slid[id].reaEnergy.dec = 0;
	slid[id].reaEnergy.frac = 0;
	slid[id].t1Energy.dec = 0;
	slid[id].t1Energy.frac = 0;


	sid[id].initialized = 0;

	reinitializeMaxMeasurements(id);
}


void initSTPM() {
	unsigned char i;

	initSPI();
	for (i=0;i<STPM_NUMBER;i++) {
		initializeRealData(i);
	}
}

unsigned long get28bits(unsigned char *bp, unsigned char adr) {
	unsigned long pom;
	pom=*(bp+adr+3);
	pom&=0x0F;
	pom<<=8;
	pom|=*(bp+adr+2);
	pom<<=8;
	pom|=*(bp+adr+1);
	pom<<=8;
	pom|=*(bp+adr);

	return pom;
}

int BadParity (unsigned char *bp, unsigned char adr)
{ 
	unsigned char prty;        /* temporary register */
	prty  = *(bp+adr);                          /* take the 1st byte of data */ 
	prty ^= *(bp+1+adr);                        /*  XOR it with the 2nd byte*/     
	prty ^= *(bp+2+adr);                     /*and with the 3rd byte */ 
	prty ^= *(bp+3+adr);                      /*and with the 4th byte */ 
	prty ^= prty<<4;
	prty &= 0xF0;        /* combine and remove the lower nibble */ 
	/*returns 1, if bad parity */
	if (prty != 0xF0) {
#ifdef STPM_DEBUG
		printf(_PFB_"Paritni chyba na adrese %d\r\n",adr);
#endif
		return 1;
	} else {
		return 0;
	}              
} 

#define DAP 0
#define DRP 4
#define DSP 8
#define DFP 12
#define DEV 16
#define DMV 20
#define CFL 24
#define CFH 28

unsigned char validateSTPM(unsigned char * buf) {
	if (BadParity(buf,DAP)) return 1;
	if (BadParity(buf,DRP)) return 1;
	if (BadParity(buf,DSP)) return 1;
	if (BadParity(buf,DFP)) return 1;
	if (BadParity(buf,DEV)) return 1;
	if (BadParity(buf,DMV)) return 1;
	if (BadParity(buf,CFL)) return 1;
	if (BadParity(buf,CFH)) return 1;
	return 0;
}

void prepareDelta(signed long * delta) {
		if (*delta>0) {
			//narust energie
	
			//neni to preteceni nazpet?
			if (*delta>524288) {
				*delta = 524288*2 - *delta;
				*delta = -*delta;
			}
		} else {
			//pokles energie
	
			//neni to preteceni nahoru?
			if (*delta<-524288) {
				*delta = -*delta;
				*delta = 524288*2 - *delta;
			}
	
		}
}


void addDelta(stpmEnergyCounter * Energy, signed long delta) {
		prepareDelta(&delta);
		Energy->frac += delta;
		while (Energy->frac>1000000) {
			Energy->frac-=1000000;
			Energy->dec++;
		}
		if (Energy->frac<0) {
			Energy->frac+=1000000;
			Energy->dec--;
		}
}

void processSTPMResults(stpmRealData * srd, unsigned char id, unsigned long deltaUs) {
	signed long delta;

	//frekvence a napeti
	sid[id].freq = srd->freq;
	sid[id].iRMS = srd->iRMS;
	sid[id].uRMS = srd->uRMS;
	sid[id].status = srd->status;
//	sid[id].mode = srd->mode;

	//okamzite hodnoty do maxim
	if ((srd->iMOM)>sid[id].iMax) sid[id].iMax = (srd->iMOM);
	if (-(srd->iMOM)>sid[id].iMax) sid[id].iMax = -(srd->iMOM);

	if ((srd->uMOM)>sid[id].uMax) sid[id].uMax = (srd->uMOM);
	if (-(srd->uMOM)>sid[id].uMax) sid[id].uMax = -(srd->uMOM);

	//energie
	//zjisti smer pohybu energie
	if (sid[id].initialized) {
		delta = srd->actEnergy - sid[id].actEnergyL;			
		addDelta(&(sid[id].actEnergy),delta);

		delta = srd->appEnergy - sid[id].appEnergyL;			
		addDelta(&(sid[id].appEnergy),delta);

		delta = srd->reaEnergy - sid[id].reaEnergyL;			
		addDelta(&(sid[id].reaEnergy),delta);

		delta = srd->t1Energy - sid[id].t1EnergyL;			
		addDelta(&(sid[id].t1Energy),delta);

	} else sid[id].initialized = 1;

	sid[id].actEnergyL = srd->actEnergy;
	sid[id].appEnergyL = srd->appEnergy;
	sid[id].reaEnergyL = srd->reaEnergy;
	sid[id].t1EnergyL = srd->t1Energy;

	sid[id].deltaTime += deltaUs;
	
}

#ifdef _H_USER
#define _PFB_ (const rom far char *)
#else
#define _PFB_ (char *)
#endif

#ifdef DEBUG_STPM
void printSTPM(stpmRealData * srd) {
	printf(_PFB_"Act energy = %lu\r\n", srd->actEnergy);
	printf(_PFB_"App energy = %lu\r\n", srd->appEnergy);
	printf(_PFB_"Rea energy = %lu\r\n", srd->reaEnergy);
	printf(_PFB_"T1  energy = %lu\r\n", srd->t1Energy);

	printf(_PFB_"Freq = %u\r\n", srd->freq);

	printf(_PFB_"Status = %d\r\n", srd->status);
	printf(_PFB_"Mode = %d\r\n", srd->mode);

	printf(_PFB_"iRMS = %u\r\n", srd->iRMS);
	printf(_PFB_"iMOM = %d\r\n", srd->iMOM);
	printf(_PFB_"uRMS = %u\r\n", srd->uRMS);
	printf(_PFB_"uMOM = %d\r\n", srd->uMOM);

	printf(_PFB_"cLow = %lX\r\n", srd->confLow);
	printf(_PFB_"cHigh = %lX\r\n", srd->confHigh);
}
#endif

//funkce prevadi buffer RAW dat z STPM do struktury stpmRealData
void translateSTPM(unsigned char * buf, stpmRealData * srd) {
	unsigned long pom;
	pom = get28bits(buf, DAP);
	srd -> status = pom&0xFF;
	srd -> actEnergy = pom>>8;

	//prevod do signed

	pom = get28bits(buf, DRP);
	srd -> freq = (pom&0x3F) << 8;
	srd -> reaEnergy = pom>>8;

	pom = get28bits(buf, DSP);
	srd -> freq |= (pom&0xFF);
	srd -> appEnergy = pom>>8;

	pom = get28bits(buf, DFP);
	srd -> mode = (pom&0xFF);
	srd -> t1Energy = pom>>8;

	pom = get28bits(buf, DEV);
	srd -> iRMS = (pom&0xFFFF);
	srd -> uRMS = (pom>>16) & 0x7FF;

	pom = get28bits(buf, DMV);
	srd -> iMOM = (pom&0xFFFF);
	srd -> uMOM = (pom>>16) & 0x7FF;

	if (srd->uMOM & 0x0400) {
		srd->uMOM &= 0x03FF;
	} else {
		srd->uMOM = 0x0400 - srd->uMOM;
		srd->uMOM *= (-1);
	}

	if (srd->iMOM & 0x08000) {
		srd->iMOM &= 0x07FFF;
	} else {
		srd->iMOM = 0x08000 - srd->iMOM;
		srd->iMOM *= (-1);
	}


	pom = get28bits(buf, CFL);
	srd -> confLow = pom;

	pom = get28bits(buf, CFH);
	srd -> confHigh = pom;
}

//funkce zpracuje jedno mereni jednim obvodem
//provede cteni dat, prevod do struktury a prepocet do sid
void processOneSTPM(unsigned char id, unsigned long deltaUs) {
	int i;
	unsigned char buf[32];
	readSTPM(id, 32, buf);

	if (validateSTPM(buf)) {
#ifdef DEBUG_STPM
		printf(_PFB_"Chyba parity (obvod %d)\r\n",id);
#endif 
	} else {
#ifdef DEBUG_STPM
		printf(_PFB_"Data prijata OK  (obvod %d)\r\n",id);
#endif 
	
	
		//prevod prijatych hodnot
		translateSTPM(buf, &srd);
	
	
#ifdef DEBUG_STPM
		//tisk prijatych hodnot
		if (DEBUG_STPM_ID & (1<<id)) {
			printSTPM(&srd);
		}
#endif
	
		processSTPMResults(&srd, id, deltaUs);

	};
}

//funkce prepocita rozdil stpmEnergyCounteru na hodnotu umernou vykonu
float getPower(stpmEnergyCounter sid, stpmEnergyCounter slid, unsigned long deltaTime) {
	//prevest vse do floatu?
	signed long deltaDec = sid.dec - slid.dec;
	float pom = deltaDec;
	
	signed long deltaFrac = sid.frac - slid.frac;
	pom += deltaFrac / 1000000.0;

	pom *= 1000000000000.0;

	//pom*= (float)deltaTime / 10.0;

	pom/= (float)deltaTime;
	
	return pom;
}

float getEnergyWs(unsigned char id, stpmEnergyCounter sid, unsigned char typDat) {
	signed long deltaDec = sid.dec;
	float pom = deltaDec;
	
	signed long deltaFrac = sid.frac;
	pom += deltaFrac / 1000000.0;

	pom *= 10000.0  / 0.48902 * stpmCal[id].calI * stpmCal[id].calU;

	pom *= (float) typDat;

	
	return pom;
}


float getEnergyWh(unsigned char id, stpmEnergyCounter sid, unsigned char typDat) {
	float pom = getEnergyWs(id, sid, typDat);
	pom /= 3600.0;
	return pom;
}


//funkce prevadi data z pole sid[] do vysledkoveho pole
//prepocitava dle kal. konstant frekvenci, napeti, proudy
void computeSTPMResults(unsigned char id) {
	float precalc = stpmCal[id].calI * stpmCal[id].calU / 0.48902 * 0.01;

	//frekvence
	stpmResult[id].freq = (float)(sid[id].freq) * 0.004743659016021384992741812254877;

	//napeti, proudy
	stpmResult[id].uRMS = (float)(sid[id].uRMS) * 0.3204 * stpmCal[id].calU;
	stpmResult[id].iRMS = (float)(sid[id].iRMS) * 0.00385 * stpmCal[id].calI ;
	stpmResult[id].uMax = (float)(sid[id].uMax) * 0.3204 * stpmCal[id].calU * 2.0;
	stpmResult[id].iMax = (float)(sid[id].iMax) * 0.00385 * stpmCal[id].calI * 2.0;

	//energie
	stpmResult[id].actEnergy = sid[id].actEnergy;
	stpmResult[id].appEnergy = sid[id].appEnergy;
	stpmResult[id].reaEnergy = sid[id].reaEnergy;
	stpmResult[id].t1Energy = sid[id].t1Energy;

	//okamzite vykony
	stpmResult[id].actPwr = getPower(sid[id].actEnergy, slid[id].actEnergy, sid[id].deltaTime) * precalc;
	stpmResult[id].appPwr = getPower(sid[id].appEnergy, slid[id].appEnergy, sid[id].deltaTime) * precalc;
	stpmResult[id].reaPwr = getPower(sid[id].reaEnergy, slid[id].reaEnergy, sid[id].deltaTime) * precalc * 2; //konstanty zahrnuji K[aw] STPM10
	stpmResult[id].t1Pwr = getPower(sid[id].t1Energy, slid[id].t1Energy, sid[id].deltaTime) * precalc * 4;    //konstanty zahrnuji K[aw] STPM10

	slid[id].actEnergy = sid[id].actEnergy;
	slid[id].appEnergy = sid[id].appEnergy;
	slid[id].reaEnergy = sid[id].reaEnergy;
	slid[id].t1Energy = sid[id].t1Energy;

	sid[id].deltaTime = 0;


}

void reinitializeMaxMeasurements(unsigned char id) {
	if (id==REI_ALL) {
		for (id=0;id<STPM_NUMBER;id++) {
			sid[id].uMax = 0;
			sid[id].iMax = 0;
		}
	} else {
		sid[id].uMax = 0;
		sid[id].iMax = 0;
	}
}

// funkce vycte data z obvodu elektromeru
// nutno volat periodicky po max. 100 ms
void processSTPM(unsigned long deltaUs) {
	unsigned char i;
	for (i=0;i<STPM_NUMBER;i++) {
		processOneSTPM(i, deltaUs);
	}
}

void initDefaultCalibrations() {
	unsigned char i;
	for (i=0;i<STPM_NUMBER;i++) {
		stpmCal[i].calI = 1.0;
		stpmCal[i].calU = 1.0;
	}
}


/////////////////// POMOCNE FUNKCE PRO ZPRACOVANI DAT /////////
float getUCrestFactor(stpmResultType * stres) {
	return stres->uMax/stres->uRMS/1.4142;
}

float getICrestFactor(stpmResultType * stres) {
	return stres->iMax/stres->iRMS/1.4142;
}

float getPowerFactor(stpmResultType * stres) {
	float pf = stres->actPwr / stres->appPwr;
	if (pf<0.0) pf = -pf;
	if (pf>1.0) pf = 1.0;
	return pf;
}

