//
// decode_f11.c - Polar F11 SonicLink decoder
//
// Copyright (c) 2009 Mike Mueller <mike@cybnet.ch>
//
//
//
// This piece of code allows you to decode the SonicLink noise of a 
// Polar F11 heartbeat monitor. There is currently no validation of
// the decoded data (CRC) so don't be too sure in what you get
//
// Thanks to Tomas Oliveira e Silva <tos@ua.pt> for the pretty code
// to decode a S410.
//
//
//
// Compile it with gcc:
// 	~# gcc -o decode_f11 decode_f11.c
//
// By default use /dev/dsp
// 	~# ./decode_f11
//
// Or use arecord
// 	~# arecord -f cd -c 1 | ./decode_f11 -
//
// Or a record
// 	~# arecord -f cd -c 1 > my_lovely_noise
// 	~# ./decode_f11 my_lovely_noise
//
//
//
// This program is free software; you can redistribute it and/or modify 
// it under the terms of the GNU General Public License as published by 
// the Free Software Foundation; either version 3 of the License, or (at 
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but 
// WITHOUT ANY WARRANTY; without even the implied warranty of 
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License along 
// with this program; if not, see <http://www.gnu.org/licenses/>



#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <linux/soundcard.h>



static char  *v_status   = "";
static double v_level    = 0.0;
static double v_progress = 0.0;
static char *data_file_name = NULL;



static void display_info(void) {
	fprintf(stderr,"%5.3f  %3.0f%%  %-24s\r",v_level,100.0 * v_progress,v_status);
}



static void update_status(char *s,int new_line) {
	v_status = s;
	display_info();
	if(new_line)
		fprintf(stderr,"\n");
}



static void update_level(double l) {
	static double i_level = 0.0;
	static int c = 0;

	if(l > i_level)
		i_level = l;

	if(++c >= 4410) {
		v_level = i_level;
		i_level = 0.0;
		c = 0;
		display_info();
	}
}



static void update_progress(double p) {
	v_progress = p;
	display_info();
}



static void fail(char *message) {
	fprintf(stderr,"\n%s\n",message);
	exit(1);
}



static FILE *open_dsp(void) {
	FILE *dsp_fp;
	int fd,val;

	// open dsp device
	dsp_fp = fopen("/dev/dsp","r");
	if(dsp_fp == NULL) {
		perror("fopen(\"/dev/dsp\",\"r\")");
		exit(1);
	}

	fd = fileno(dsp_fp);

	// set recording source (mic)
	val = SOUND_MASK_MIC;
	if(ioctl(fd,SOUND_MIXER_WRITE_RECSRC,&val)) {
		perror("ioctl(fd,SOUND_MIXER_WRITE_RECSRC,&val)");
		exit(1);
	}
	fprintf(stderr,"Recording source set to %04X\n",val);

	// set mic recording volume
	val = 0xFFFF;
	if(ioctl(fd,SOUND_MIXER_WRITE_MIC,&val)) {
		perror("ioctl(fd,SOUND_MIXER_WRITE_MIC,&val)");
		exit(1);
	}
	fprintf(stderr,"Mic volume set to %04X\n",val);

	// set master recording volume
	val = 0xFFFF;
	if(ioctl(fd,SOUND_MIXER_WRITE_VOLUME,&val)) {
		perror("ioctl(fd,SOUND_MIXER_WRITE_VOLUME,&val)");
		exit(1);
	}
	fprintf(stderr,"Master recording volume set to %04X\n",val);

	// set one channel (mono)
	val = 1;
	if(ioctl(fd,SNDCTL_DSP_CHANNELS,&val)) {
		perror("ioctl(fd,SNDCTL_DSP_CHANNELS,&val)");
		exit(1);
	}
	fprintf(stderr,"Number of channels: %d\n",val);

	// set format (16 bits, lsb first)
	val = AFMT_S16_LE;
	if(ioctl(fd,SNDCTL_DSP_SETFMT,&val)) {
		perror("ioctl(fd,SNDCTL_DSP_SETFMT,&val)");
		exit(1);
	}
	fprintf(stderr,"Number of bits: %d\n",val);

	// set sampling frequency (44100Hz)
	val = 44100;
	if(ioctl(fd,SNDCTL_DSP_SPEED,&val)) {
		perror("ioctl(fd,SNDCTL_DSP_SPEED,&val)");
		exit(1);
	}
	fprintf(stderr,"Sampling frequency: %d\n",val);

	return dsp_fp;
}



static int next_sample(void) {
	static FILE *fp = NULL;
	int c1,c2;

	if(fp == NULL) {
		if (data_file_name == NULL) {
			fprintf(stderr,"Getting data from /dev/dsp\n");
			fp = open_dsp();
		} else if(strcmp(data_file_name,"-") == 0) {
			fprintf(stderr,"Getting data from stdin\n");
			fp = stdin;
		} else {
			fprintf(stderr,"Getting data from %s\n",data_file_name);
			fp = fopen(data_file_name,"r");
			if(fp == NULL)
				fail("unable to open data file");
		}

		// skip the first 44 bytes (size of the header of a wav file)
		for(c1 = 0;c1 < 44;c1++)
			if(getc(fp) == EOF)
				fail("EOF reached in data file");
	}

	c1 = getc(fp);
	c2 = getc(fp);
	if(c1 == EOF || c2 == EOF)
		fail("EOF reached in data file");
	c1 += c2 << 8;
	if(c1 > 32767)
		c1 -= 65536;
	return c1;
}

// pass input signal through a (linear phase FIR) high-pass filter
// y = filter(remez(40,[0 2000 4000 22050]/22050,[0 0 1 1],[2 1]),1,x);
static double filter(double x) {
	static double h[41] = {
		 0.00379802504960,-0.01015567605831,-0.00799539667861,-0.00753846964193,
		-0.00613119820747,-0.00283788804837, 0.00237999784896, 0.00894445674876,
		 0.01562664364293, 0.02085836025169, 0.02288210087128, 0.02006200096880,
		 0.01129559749322,-0.00374264061795,-0.02440163523552,-0.04905850773028,
		-0.07526471722031,-0.10006201874319,-0.12045502068751,-0.13386410688608,
		 0.86146086608477,-0.13386410688608,-0.12045502068751,-0.10006201874319,
		-0.07526471722031,-0.04905850773028,-0.02440163523552,-0.00374264061795,
		 0.01129559749322, 0.02006200096880, 0.02288210087128, 0.02085836025169,
		 0.01562664364293, 0.00894445674876, 0.00237999784896,-0.00283788804837,
		-0.00613119820747,-0.00753846964193,-0.00799539667861,-0.01015567605831,
		 0.00379802504960
	};
	static double input_signal[64];
	static int pos = -1;
	double y;
	int i;

	if(pos == -1) {
		pos = 0;
		for(i = 0;i < 64;i++)
			input_signal[i] = 0.0;
	}

	input_signal[pos] = x;
	y = 0.0;

	for(i = 0;i <= 40;i++)
		y += h[i] * input_signal[(pos - i) & 63];

	pos = (pos + 1) & 63;
	return y;
}


// dilate the rectified signal
// y = delay(3,ordfilt2(abs(x),7,ones(1,7)));
static double dilate(double x) {
	static double input_signal[8];
	static int pos = -1;
	double y;
	int i;

	if(pos == -1) {
		pos = 0;
		for(i = 0;i < 8;i++)
			input_signal[i] = 0.0;
	}

	input_signal[pos] = (x >= 0.0) ? x : -x;
	y = input_signal[pos];

	for(i = 1;i <= 6;i++)
		if(input_signal[(pos - i) & 7] > y)
			y = input_signal[(pos - i) & 7];
	pos = (pos + 1) & 7;
	return y;
}


// apply a median filter to the signal
// y = delay(2,ordfilt2(x,3,ones(1,5)));
static double median(double x) {
	static double input_signal[8];
	static int pos = -1;
	double t,y[5];
	int i,j;

	if(pos == -1) {
		pos = 0;
		for(i = 0;i < 8;i++)
			input_signal[i] = 0.0;
	}

	input_signal[pos] = x;

	for(i = 0;i <= 4;i++) {
		// sort
		t = input_signal[(pos - i) & 7];

		for(j = 0;j < i;j++)
			if(t < y[j]) {
				y[i] = y[j];
				y[j] = t;
				t = y[i];
			}
		y[i] = t;
	}

	pos = (pos + 1) & 7;
	return y[2];
}


// decide if each signal sample corresponds to part of a one or not
//
// y = x;
// for k=2:length(x)
//   y(k) = max(x(k),0.9999*y(k-1));
// end
// amplitude = delay(20,ordfilt2(y,41,ones(1,41)));
// decisions = [ delay(20,x) >= level*amplitude  amplitude >= 15*delay(30,amplitude) ]);
static double make_decision(double x,int *decisions) {
	// decisions[0..4] = above/below threshold for five different threshold levels
	// decisions[5] = best above/below decision based on a majority rule
	// decisions[6] = abrupt change/no abrupt change in the amplitude (start flag)
	static double threshold_levels[5] = { 0.25,0.30,0.35,0.40,0.45 };
	static double input_signal[64],amplitude[64];
	static int pos = -1,count_down;
	double t;
	int i;

	if(pos == -1) {
		pos = 0;
		count_down = 64;
		for(i = 0;i < 64;i++) {
			input_signal[i] = 0.0;
			amplitude[i] = 0.0;
		}
	}

	// compute the amplitude
	input_signal[pos] = x;
	amplitude[pos] = 0.9999 * amplitude[(pos - 1) & 63];
	if(x > amplitude[pos])
		amplitude[pos] = x;

	// dilate the amplitude
	t = amplitude[pos];
	for(i = 1;i <= 40;i++)
		if(amplitude[(pos - i) & 63] > t)
			t = amplitude[(pos - i) & 63];

	// compare the (delayed) signal with the (dilated) amplitude scaled by a threshold level
	decisions[5] = 0;
	for(i = 0;i < 5;i++) {
		decisions[i] = (input_signal[(pos - 20) & 63] >= threshold_levels[i] * t) ? 1 : 0;
		decisions[5] += decisions[i];
	}

	// majority rule (median)
	decisions[5] = (decisions[5] >= 3) ? 1 : 0;
	decisions[6] = (amplitude[pos] >= 15.0 * amplitude[(pos - 30) & 63]) ? 1 : 0;

	if(count_down > 0) {
		--count_down;
		decisions[6] = 0;
	}

	i = pos;
	pos = (pos + 1) & 63;
	return amplitude[i];
}


// apply a median filter to the decisions
// d = delay(2,ordfilt2(d,3,ones(1,5)));
static void b_median(int *d) {
	static int inputs[8][8];
	static int pos = -1;
	int i,j;

	if(pos == -1) {
		pos = 0;
		for(i = 0;i < 8;i++)
			for(j = 0;j < 8;j++)
				inputs[i][j] = 0;
	}

	for(i = 0;i < 7;i++) {
		inputs[i][pos] = d[i];
		for(j = 1;j <= 4;j++)
			d[i] += inputs[i][(pos - j) & 7];

		// median (majority rule)
		d[i] = (d[i] >= 3) ? 1 : 0;
	}
	pos = (pos + 1) & 7;
}



static int byte_decode(int restart) {
	static int t,t_active,t_byte;
	static int byte,n_bytes,c[6];
	int i,j,k,d[7];
	double x;

	if(restart) {
		t = t_active = 0;
		update_progress(0.0);
		update_status("no signal",0);
	}

	// process next sample
	t++;
	x = (double)next_sample() / 32768.0;
	x = filter(x);
	x = dilate(x);
	x = median(x);
	x = make_decision(x,d);
	b_median(d);
	update_level(x);

	// activation test
	if(d[6] && t_active == 0) {
		t_active = t;
		t_byte = 0;
		n_bytes = 0;
		update_status("processing signal ...",0);
	}

	if(t_active == 0)
		return -1;

	// deactivation test after a long period of inactivity
	if(t > t_active + 10000) {
		update_status("byte start time out",1);
		return -2;
	}

	// new byte?
	if(t_byte == 0 && d[5]) {

		// new byte
		t_active = t_byte = t;
		n_bytes++;
		byte = 0;
		for(i = 0;i < 6;i++)
			c[i] = 0;
	}

	// processing a byte?
	if(t_byte > 0) {

		// bit number; the duration of a bit is very close to 2ms=88.2 samples
		j = (t - t_byte) / 88;
		if(j >= 10) {

			// end of the byte - first bit (lsb) must be a one
			if((byte & 1) == 0) {
				update_status("bad byte start",1);
				return -2;
			}

			// last bit (msb) must be a zero
			if(byte >= 512) {
				update_status("bad byte finish",1);
				return -2;
			}

			byte >>= 1;
			t_byte = 0;
			if(n_bytes > 5)
				return byte;

			// bad synchronization byte
			if(byte != 0xAA) {
				update_status("bad synchronization byte",1);
				return -2;
			}

			// discard synchronization byte
			return -1;
		}

		// sample number inside the bit
		k = t - t_byte - 88 * j;

		// the bit burst has a duration of close to 60 samples (here we use 64 but latter we use 60...)
		if(k < 64) {
			for(i = 0;i < 6;i++)
				c[i] += d[i];

		} else if(k == 64){
			k = 0;
			for(i = 0;i < 6;i++) {

				// threshold = 60/2
				if(c[i] >= 30)
					k += (i < 5) ? 1 : 2;
				c[i] = 0;
			}

			// majority rule
			if(k >= 4)
				byte += 1 << j;
		}
	}
	return -1;
}



static int extract_bytes(int *data) {
	int i,byte,restart=1,size=0;
	char b[4];

	for (;;) {
		byte = byte_decode(restart);
		if (byte >= 0) {
			data[size++] = byte;
			update_progress((double)size / 1029);
			if (size >= 1029) {
				break;
			}
		} else if (byte == -1) {
			restart=0;
		} else if (byte == -2) {
			restart=1;
		}
	}
}



static void usage(void) {
	fprintf(stderr,"usage: decode_f11\n");
	fprintf(stderr,"       decode_f11 -h\n");
	fprintf(stderr,"       decode_f11 -\n");
	fprintf(stderr,"       decode_f11 [input file]\n");
	exit(1);
}



int main(int argc,char **argv) {
	int i,data[8192];

	// check if we want the usage
	if(argc == 2 && argv[1][0] == '-' && argv[1][1] == 'h')
		usage();

	// check if we want to define an input file
	if (argc == 0) {

		// get a dsp
		data_file_name = NULL;
	} else {

		// input file defined
		data_file_name = argv[1];
	}

	// listen to the clock ;)
	extract_bytes(data);

	for (i=0;i<1030;i++) {
		printf("%d ",data[i]);
	}
	 
	return 0;
}