/*
 * ISDM.c
 */
//#include <stdlib.h>			// for abs() function
//#include <stdio.h>			// for printf() function
#include "app_EWO.h"
	
// === Moved from ISDM.h ===
typedef struct ISDM_conf {
    unsigned int	    E_Bgnd;				// below this we have nothing
    unsigned int	    E_Proc;				// above this is strong signal
    unsigned short		Min_Max_E_Step;		//
    unsigned char		Min_Max_Lines;
    unsigned short		NB_SNR;				// if peak is higher (above noise background) than this value it can be narrow band
    unsigned short		NB_Pw_SN;			// peak width calculated at (peak*NB_Pw_SN)/256
    unsigned char		NB_Pw_Lines;		// peak width at (peak - NB_Pw_SN) bigger than this it is not narrow
    unsigned short		NB_QF;  			// quality factor for narrow band signal
    unsigned short		Mid_SNR;			// if peak is higher (above noise background) than this value it can be mid band
    unsigned short		Mid_Pw_SN;			// peak width calculated at (peak*Mid_Pw_SN)/256
    unsigned char		Mid_Pw_Lines;		// peak width at (peak - Mid_Pw_SN) bigger than this it is not mid
    unsigned short		Mid_QF; 			// quality factor for mid band signal
    unsigned short		WB_SNR;				// if peak is higher (above noise background) than this value it can be wide band
    unsigned short		WB_Pw_SN;			// peak width calculated at (peak*WB_Pw_SN)/256
    unsigned char		WB_Pw_Lines;		// peak width at (peak - WB_Pw_SN) bigger than this it is not wide
    unsigned short		WB_QF;  			// quality factor for wide band signal
    unsigned short		Multi_SNR;			// if more than 2 peak is higher (above noise background) than this value it can be multi peak
    unsigned short		Multi_Pw_SN;		// peak width calculated at (peak*Multi_Pw_SN)/256
    unsigned char		Multi_Pw_Lines;		// peak width at (peak - Multi_Pw_SN) bigger than this it is not peak for multi
    unsigned short		Multi_QF; 			// quality factor for multi peak signal
    unsigned short		Init[40];			// [0] peak energy limit - only above this energy and heigth value we call a peak to peak
    							// [1] frame limit - above this number of frames we have long signal
    							// [2] peak high limit
    							// [3] below this energy delta quality is 0
    							// [4] below this energy delta quality is 1
    							// [5] below this energy delta quality is 2 above this 3
    							// [6] allowed position delta for long discrete
}__attribute__((__packed__)) ISDM_configstr;

//struct ISDM_event_str {
//	unsigned char Event;
//	int e_time;
//};

struct ISDM_peak_buffer{
	unsigned char pos;
	unsigned char wide;
	unsigned short h;
}__attribute__((__packed__));

#define SM_POINTS 3

//struct ISDM_event_str app13_EWO_ISDM(int Time, unsigned short* Spect, unsigned char Sensor);
unsigned char isdm_stat;			// 3,2,1: still counting, 0: calculation done
unsigned char isdm_avg_done;		// 0: noise background calculation is going on, not allowed to kill process
								// 1: background calculation done process kill is allowed
// === End of "Moved from ISDM.h" ===


//extern void read_eeprom_ref(unsigned short * ref, unsigned char s);	// read RefSpect values from eeprom for sensor 's'
//extern void read_eeprom_conf(ISDM_configstr * conf, unsigned char s);		// read Config values from eeprom for sensor 's'

ISDM_configstr Config;						// config values for actual sensor
//unsigned short G13us_ISDM_RefSpect[256];			// reference spectrum for actual sensor

//unsigned int G13ui_ISDM_noiseback_cnt[256];		// summ of last 16 spectra line
//unsigned short G13us_ISDM_noiseback[256];			// averaged noise background
unsigned char noiseback_ptr = 15;				// pointer for last
//unsigned short G13us_ISDM_in_spect[16][256];		// last 16 incoming spectra line

int energy_ns = 0;						// energy of noise bacground
unsigned char lastsensor = 0xFF;					// ID of the last sensor
unsigned char event_on = 0;						// 1: inside event, 0: no event
unsigned char event_fcnt = 0;					// counting frames during event
struct ISDM_peak_buffer peak_b[16][4];		// buffer for storage 4 largest peaks for every frame in in_spect buffer
unsigned char pb_cnt[16] = {0};					// buffer peak_cnt
unsigned char avg_lock = 0;						// when 16 noise background contains valid spectra
unsigned char isdm_stat = 0;						// 3,2,1: still counting, 0: calculation done
unsigned char isdm_avg_done = 0;					// 0: background calculation is going not allowed to kill process
										// 1: background calculation done process kill is allowed

/*
 *  Reset variables and read config and reference spectrum values for new sensor
 *  This is called when sensor ID is changed
 */
void reset_var(unsigned char Sensor){
	int i,k;

	//read_eeprom_ref(&G13us_ISDM_RefSpect[0], Sensor);		// read the RefSpect for Sensor from eeprom
	//read_eeprom_conf(&Config, Sensor);		// read the Config for Sensor from eeprom

	energy_ns = 0;
	noiseback_ptr = 15;
	avg_lock = 0;
	for (i=0;i<256;i++){
		G13us_ISDM_noiseback[i] = 0;
		G13ui_ISDM_noiseback_cnt[i] = 0;
		for (k=0;k<16;k++){
			G13us_ISDM_in_spect[k][i] = 0;
		}
	}
}

/*
 *  Insert new peak into the peak buffer
 *  Called when new peak is found by the peak detector
 */
void insert_peak(unsigned char peak_pos, unsigned char peak_wide, unsigned short peak_peak){
	int i;
	unsigned short comp;
	int pos;

	if (pb_cnt[noiseback_ptr]<4){		// there is empty place
		peak_b[noiseback_ptr][pb_cnt[noiseback_ptr]].pos = peak_pos;
		peak_b[noiseback_ptr][pb_cnt[noiseback_ptr]].wide = peak_wide;
		peak_b[noiseback_ptr][pb_cnt[noiseback_ptr]].h = peak_peak;
		pb_cnt[noiseback_ptr]++;
	} else {			// no empty place, check for smaller than actual
		comp = peak_b[noiseback_ptr][0].h;
		pos = 0;
		for (i=0;i<4;i++){		// looking for smaller
			if (peak_b[noiseback_ptr][i].h<comp){
				comp = peak_b[noiseback_ptr][i].h;
				pos = i;
			}
		}
		if (peak_b[noiseback_ptr][pos].h < peak_peak) {		// if actual is bigger than update
			peak_b[noiseback_ptr][pos].h = peak_peak;
			peak_b[noiseback_ptr][pos].pos = peak_pos;
			peak_b[noiseback_ptr][pos].wide = peak_wide;
		}
	}
}

/*
 *  Calculate an average background spectrum
 *  Called after new spectrum is passed to f_isdm function
 */
void average(int Time, unsigned short * Spect){
	int i;

	energy_ns = 0;
	noiseback_ptr = (noiseback_ptr + 1) % 16;

	for (i=0;i<256;i++){
		G13ui_ISDM_noiseback_cnt[i] -= G13us_ISDM_in_spect[noiseback_ptr][i];
		G13us_ISDM_in_spect[noiseback_ptr][i] = Spect[i];
		G13ui_ISDM_noiseback_cnt[i] += G13us_ISDM_in_spect[noiseback_ptr][i];
		G13us_ISDM_noiseback[i] = G13ui_ISDM_noiseback_cnt[i]>>4;				// calc. noisebackground line
		energy_ns += G13us_ISDM_noiseback[i];  					// calc. energy
	}
	for (i=0;i<4;i++){
		peak_b[noiseback_ptr][i].h = 0;
		peak_b[noiseback_ptr][i].pos = 0;
		peak_b[noiseback_ptr][i].wide = 0;
	}
	if (avg_lock<16) avg_lock++;
}

/*
 *  Smooth the incoming spectrum to remove small noise spikes
 */
void smooth_in(unsigned short * in, unsigned short * out){

	int k,i;
	int add;

	for (k=0;k<(SM_POINTS>>1);k++) out[k] = in[k];
	for(k=(SM_POINTS>>1);k<(256-(SM_POINTS>>1));k++){
		add = 0;
		for (i=-(SM_POINTS>>1);i<=((SM_POINTS>>1));i++){
			add += in[k+i];
		}
		out[k] = add / SM_POINTS;
	}
	for (k=(256-(SM_POINTS>>1));k<256;k++) out[k] = in[k];
}

/*
 *  Check the width of a peak against config values
 */
int calc_width(struct ISDM_peak_buffer * peak, unsigned short * spect, unsigned short hlimit, unsigned char wlimit){
	int i;
	int pos;
	int end;
	int cnt;
	int ret;
	int l;

	ret = -1;
	cnt = 0;
	pos = peak->pos - (wlimit>>1);
	end = pos + wlimit;
	if (pos<0) {
		pos = 0;
	}
	if (end>255){
		end = 256;
	}
	l = (spect[peak->pos]*hlimit)>>8;
//	printf("b:%d, e:%d, l:%d, SP:%d, pos:%d¥n", pos, end, l, spect[peak->pos], peak->pos);
	for (i=pos;i<end;i++){
		if ( spect[i] < l ){		// if heigth is below limit -> width is below limit
			ret = 1;
			break;
		} else				// width is greater or equal limit
			ret = -1;
	}
//	printf("ret:%d¥n", ret);
	return ret;
}

/*
 *  Check the peak is inside narrow peak definition
 */
int chk_narrow(struct ISDM_peak_buffer * pb, unsigned char pb_c, unsigned short * spect){
	int i,n;
	int cnt;

	cnt = 0;
	for (i=0;i<pb_c;i++){
		if (pb[i].h > Config.NB_SNR){
			n = calc_width(&pb[i], spect, Config.NB_Pw_SN, Config.NB_Pw_Lines);
			if (n>0) { // peak is inside width parameter
				cnt++;
			}
		}
	}
	if (cnt>0) return 1;

	return -1;
}

/*
 *  Check the peak is inside mid peak definition
 */
int chk_mid(struct ISDM_peak_buffer * pb, unsigned char pb_c, unsigned short * spect){
	int i,n;
	int cnt;

	cnt = 0;
	for (i=0;i<pb_c;i++){
		if (pb[i].h > Config.Mid_SNR){
			n = calc_width(&pb[i], spect, Config.Mid_Pw_SN, Config.Mid_Pw_Lines);
			if (n>0) { // peak is inside width parameter
				cnt++;
			}
		}
	}
	if (cnt>0) return 1;

	return -1;
}

/*
 *  Check the peak is inside wide peak definition
 */
int chk_wide(struct ISDM_peak_buffer * pb, unsigned char pb_c, unsigned short * spect){
	int i,n;
	int cnt;

	cnt = 0;
	for (i=0;i<pb_c;i++){
		if (pb[i].h > Config.WB_SNR){
			n = calc_width(&pb[i], spect, Config.WB_Pw_SN, Config.WB_Pw_Lines);
			if (n>0) { // peak is inside width parameter
				cnt++;
			}
		}
	}
	if (cnt>0) return 1;

	return -1;
}

/*
 *  Check peaks against multi peak definition
 */
int chk_multi(struct ISDM_peak_buffer * pb, unsigned char pb_c, unsigned short * spect){
	int i,n;
	int cnt;

	cnt = 0;
	for (i=0;i<pb_c;i++){
		if (pb[i].h > Config.Multi_SNR){
			n = calc_width(&pb[i], spect, Config.Multi_Pw_SN, Config.Multi_Pw_Lines);
			if (n>0) { // peak is inside width parameter
				cnt++;
			}
		}
	}
	if (cnt>1) return cnt;	// more than 1 peak is within parameters

	return -1;
}

/*
 *  Check previous peaks for long discrete signal
 */
int chk_discrete(int ev_fcnt, unsigned char pos){
	int i,k;
	int bptr;
	int f_cnt;
	unsigned char tmp;

	f_cnt = 0;
	bptr = noiseback_ptr;
	if (ev_fcnt>15) ev_fcnt = 15;
	for (i=0;i<ev_fcnt;i++){
		for (k=0;k<pb_cnt[bptr];k++){
//			if ( abs(peak_b[bptr][k].pos-pos )<Config.Init[6]) f_cnt++;
			if( peak_b[bptr][k].pos-pos >= 0 )	tmp = peak_b[bptr][k].pos-pos;
			else								tmp = peak_b[bptr][k].pos-pos * -1;
			if ( tmp < Config.Init[6]) f_cnt++;
		}
		bptr--;
		if (bptr<0) bptr=15;
	}

	if (f_cnt>=ev_fcnt) return 1;

	return -1;
}

/*
 *  Main function of isdm.
 */
struct ISDM_event_str app13_EWO_ISDM(int Time, unsigned short* Spect, unsigned char Sensor){
	struct ISDM_event_str ret;			// return variable
	int energy_c;					// energy of incoming spectrum
	int i,n;						// loop counters
	int e_delta, e_deltawide;		// energy difference between background and new spectrum
									// and the difference / 256

	unsigned short sm_Spect[256];	// smoothed spectrum

	int peak_energy;
	unsigned char peak_wide;
	unsigned int peak_peak;
	unsigned char peak_pos;
	int peak_cnt = 0;

	isdm_avg_done = 0;			// average calculation started
	isdm_stat = 3;

	ret.Event = 0;
	ret.e_time = Time;

	if (lastsensor != Sensor){			// if sensor ID changed reset all variables
										// and load the apropriate configuration from EEPROM
		reset_var(Sensor);
	}
	lastsensor = Sensor;				// store sensor ID
	smooth_in((unsigned short *)Spect, &sm_Spect[0]);	// smooth the incoming FFT

	average(Time,&sm_Spect[0]);			// calc. background spectra and energy

	isdm_avg_done = 1;			// average calculation done
	if (avg_lock == 16){				// noise background is valid
		// energy calc. on incoming FFT
		energy_c = 0;
		for (i=0;i<256;i++){
			energy_c += sm_Spect[i];
		}

		e_deltawide = 0;

		e_delta = energy_c - energy_ns;	// calc. the delta between noise backgrond energy and actual frame energy
//		printf("T:%d, ec:%d, ens:%d, d:%d¥n", Time, energy_c, energy_ns, e_delta);
		if (e_delta<0) e_delta = 0;

		if (e_delta > Config.E_Bgnd){	// if energy is higher than limit we look for peaks
			event_on = 1;
		} else {						// if below than previous signal ended, length count is set to 0
			event_on = 0;
			event_fcnt = 0;
		}
		if (event_on == 1){				// delta above limit look for peaks
			event_on = 1;

			e_deltawide = e_delta>>8;	// energy spread among the 256 lines
			peak_energy = 0;			// zero energy of the peak
			peak_wide = 0;				// zero the width of the peak
			peak_peak = 0;				// zero the heigth of the peak
			peak_cnt = 0;				// zero the number of peaks
			peak_pos = 0;				// zero the position of the peak
			for (i=0;i<256;i++){
				if (G13us_ISDM_RefSpect[i] != 0){	// where mask is 0 we dont look for peak
//					printf("pd:[%d]%d : ed:%d, nb:%d¥n", i, sm_Spect[i], e_deltawide, noiseback[i]);
					if ((G13us_ISDM_noiseback[i]+e_deltawide) < sm_Spect[i]){	// if line is above the background + delta/256
																	// start peak
						peak_energy += sm_Spect[i] - (G13us_ISDM_noiseback[i]+e_deltawide);
						peak_wide++;
						if (peak_peak < (sm_Spect[i]-G13us_ISDM_noiseback[i])){
							peak_pos = i;
							peak_peak = (sm_Spect[i]-G13us_ISDM_noiseback[i]);
						}
					} else {
						if (peak_energy != 0){	// peak end
							if ((peak_energy>Config.Init[0]) && ((peak_peak) > Config.Init[2])){	// peak energy and heigth is big enough
								insert_peak(peak_pos, peak_wide, peak_peak);
								if (peak_cnt<7)peak_cnt++;
							}
							peak_wide = 0;
							peak_energy = 0;
							peak_peak = 0;
						} else {		// no peak before
						}
					}
				}
			}

			// decide frame quality
			if (e_delta < Config.Init[3]){
				ret.Event = 0x00;				// Q = 0
			} else {
				if (e_delta < Config.Init[4]){
					ret.Event = 0x08;			// Q = 1
				} else {
					if (e_delta < Config.Init[5]){
						ret.Event = 0x10;		// Q = 2
					} else {
						ret.Event = 0x18;		// Q = 3
					}
				}
			}
			// decide frame type
//			printf("ISDM: pc:%d¥n", peak_cnt);
			switch (peak_cnt){
			case 0:		// no peak
				if (e_delta < Config.E_Proc){	// check delta is below reference
					// type 5 (weak signal)
					ret.Event |= 0xA0;
					ret.e_time = Time;
				} else {	// type 4 diffuse signal
					ret.Event |= 0x80;
					ret.e_time = Time;
				}
				break;
			case 1:		// 1 peak
				n = chk_narrow(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
				if (n<0) {		// wider than narrow
					n = chk_mid(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
					if (n<0){	// wider than mid
						n = chk_wide(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
						if (n<0){	// wider than wide
							if (e_delta < Config.E_Proc){
								// type 5 (weak signal)
								ret.Event |= 0xA0;
								ret.e_time = Time;
							} else {	// type 4 diffuse signal
								ret.Event |= 0x80;
								ret.e_time = Time;
							}
						} else {	// wide
							ret.Event |= 0x22;
							ret.e_time = Time;
						}
					} else {	// mid
						ret.Event |= 0x21;
						ret.e_time = Time;
					}
				} else {	// narrow
					ret.Event |= 0x20;
					ret.e_time = Time;
				}
				break;
			default:	// multiple peaks
				n = chk_multi(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
				if (n<0){		// not multi
					n = chk_narrow(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
					if (n<0){	// not narrow -> wider
						n = chk_mid(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
						if (n<0){		// not mid -> wider
							n = chk_wide(&peak_b[noiseback_ptr][0], pb_cnt[noiseback_ptr], &sm_Spect[0]);
							if (n<0){	// not wide -> wider
								if (e_delta < Config.E_Proc){
									// type 5 (weak signal)
									ret.Event |= 0xA0;
									ret.e_time = Time;
								} else {	// type 4 diffuse signal
									ret.Event |= 0x80;
									ret.e_time = Time;
								}
							} else {	// wide
								ret.Event |= 0x22;
								ret.e_time = Time;
							}
						} else {	// mid
							ret.Event |= 0x21;
							ret.e_time = Time;
						}
					} else {	// narrow
						ret.Event |= 0x20;
						ret.e_time = Time;
					}
				} else {	// multi
					ret.Event |= 0x40;
					ret.Event |= (peak_cnt&0x07);
					ret.e_time = Time;
				}
				break;
			}
//			printf("ev_fcnt %d¥n",event_fcnt);
			if (event_fcnt<255) event_fcnt++;
			if (event_fcnt > Config.Init[1]){
				if ((ret.Event&0xE0) == 0x20){		// when current find is short single check previous peaks for long discrete
					n = chk_discrete(event_fcnt, peak_b[noiseback_ptr][0].pos);
					if (n>0){		// all peak buffer has peak at similar position -> long discrete
						ret.Event &= ‾0xE0;
						ret.Event |= 0x60;
					}
				}
			}
		} else {	// delta is below E_Bgnd return no event
			ret.Event = 0;
			ret.e_time = Time;
		}
	} else {		// noise background is not valid yet return no event
		ret.Event = 0;
		ret.e_time = Time;
	}
	isdm_stat = 0;
	return ret;
}