path: root/micropeak/MicroData.java

/*
 * Copyright © 2012 Keith Packard <keithp@keithp.com>
 *
 * 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; version 2 of the License.
 *
 * 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, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
 */

package org.altusmetrum.micropeak;

import java.lang.*;
import java.io.*;
import java.util.*;
import org.altusmetrum.AltosLib.*;

abstract class MicroIterator implements Iterator<Double> {
	int		i;
	MicroData	data;

	public boolean hasNext() {
		return i < data.pressures.length;
	}

	public MicroIterator (MicroData data) {
		this.data = data;
		i = 0;
	}

	public void remove() {
	}
}

class MicroHeightIterator extends MicroIterator {
	public Double next() {
		return data.height(i++);
	}

	public MicroHeightIterator(MicroData data) {
		super(data);
	}
}

class MicroHeightIterable implements Iterable<Double> {
	MicroData	data;

	public Iterator<Double> iterator() {
		return new MicroHeightIterator(data);
	}

	public MicroHeightIterable(MicroData data) {
		this.data = data;
	}
}

class MicroSpeedIterator extends MicroIterator {
	public Double next() {
		return data.speed(i++);
	}
	public MicroSpeedIterator(MicroData data) {
		super(data);
	}
}

class MicroSpeedIterable implements Iterable<Double> {
	MicroData	data;

	public Iterator<Double> iterator() {
		return new MicroSpeedIterator(data);
	}

	public MicroSpeedIterable(MicroData data) {
		this.data = data;
	}
}

class MicroAccelIterator extends MicroIterator {
	public Double next() {
		return data.acceleration(i++);
	}
	public MicroAccelIterator(MicroData data) {
		super(data);
	}
}

class MicroAccelIterable implements Iterable<Double> {
	MicroData	data;

	public Iterator<Double> iterator() {
		return new MicroAccelIterator(data);
	}

	public MicroAccelIterable(MicroData data) {
		this.data = data;
	}
}

public class MicroData {
	public int		ground_pressure;
	public int		min_pressure;
	public int[]		pressures;
	private double		time_step;
	private double		ground_altitude;
	private ArrayList<Integer>	bytes;
	String			name;
	

	class FileEndedException extends Exception {
	}

	class NonHexcharException extends Exception {
	}

	class InvalidCrcException extends Exception {
	}

	private int getc(InputStream f) throws IOException, FileEndedException {
		int	c = f.read();

		if (c == -1)
			throw new FileEndedException();
		bytes.add(c);
		return c;
	}

	private int get_nonwhite(InputStream f) throws IOException, FileEndedException {
		int	c;

		for (;;) {
			c = getc(f);
			if (!Character.isWhitespace(c))
				return c;
		}
	}

	private int get_hexc(InputStream f) throws IOException, FileEndedException, NonHexcharException {
		int	c = get_nonwhite(f);

		if ('0' <= c && c <= '9')
			return c - '0';
		if ('a' <= c && c <= 'f')
			return c - 'a' + 10;
		if ('A' <= c && c <= 'F')
			return c - 'A' + 10;
		throw new NonHexcharException();
	}

	private static final int POLY = 0x8408;

	private int log_crc(int crc, int b) {
		int	i;

		for (i = 0; i < 8; i++) {
			if (((crc & 0x0001) ^ (b & 0x0001)) != 0)
				crc = (crc >> 1) ^ POLY;
			else
				crc = crc >> 1;
			b >>= 1;
		}
		return crc & 0xffff;
	}

	int	file_crc;

	private int get_hex(InputStream f) throws IOException, FileEndedException, NonHexcharException {
		int	a = get_hexc(f);
		int	b = get_hexc(f);

		int h = (a << 4) + b;

		file_crc = log_crc(file_crc, h);
		return h;
	}

	private boolean find_header(InputStream f) throws IOException {
		try {
			for (;;) {
				if (get_nonwhite(f) == 'M' && get_nonwhite(f) == 'P')
					return true;
			}
		} catch (FileEndedException fe) {
			return false;
		}
	} 

	private int get_32(InputStream f)  throws IOException, FileEndedException, NonHexcharException {
		int	v = 0;
		for (int i = 0; i < 4; i++) {
			v += get_hex(f) << (i * 8);
		}
		return v;
	}

	private int get_16(InputStream f) throws IOException, FileEndedException, NonHexcharException {
		int	v = 0;
		for (int i = 0; i < 2; i++) {
			v += get_hex(f) << (i * 8);
		}
		return v;
	}

	private int swap16(int i) {
		return ((i << 8) & 0xff00) | ((i >> 8) & 0xff);
	}

	public boolean	crc_valid;

	int mix_in (int high, int low) {
		return  high - (high & 0xffff) + low;
	}

	boolean closer (int target, int a, int b) {
		return Math.abs (target - a) < Math.abs(target - b);
	}

	public double altitude(int i) {
		return AltosConvert.pressure_to_altitude(pressures[i]);
	}

	public Iterable<Double> heights() {
		return new MicroHeightIterable(this);
	}

	public Iterable<Double> speeds() {
		return new MicroSpeedIterable(this);
	}

	public Iterable<Double> accels() {
		return new MicroAccelIterable(this);
	}

	int fact(int n) {
		if (n == 0)
			return 1;
		return n * fact(n-1);
	}

	int choose(int n, int k) {
		return fact(n) / (fact(k) * fact(n-k));
	}


	public double avg_altitude(int center, int dist) {
		int	start = center - dist;
		int	stop = center + dist;

		if (start < 0)
			start = 0;
		if (stop >= pressures.length)
			stop = pressures.length - 1;

		double	sum = 0;
		double	div = 0;

		int	n = dist * 2;

		for (int i = start; i <= stop; i++) {
			int	k = i - (center - dist);
			int	c = choose (n, k);

			sum += c * pressures[i];
			div += c;
		}

		double pres = sum / div;

		double alt = AltosConvert.pressure_to_altitude(pres);
		return alt;
	}

	public double height(int i) {
		return altitude(i) - ground_altitude;
	}

	static final int speed_avg = 3;
	static final int accel_avg = 5;

	private double avg_speed(int center, int dist) {
		if (center == 0)
			return 0;

		double ai = avg_altitude(center, dist);
		double aj = avg_altitude(center - 1, dist);
		double s = (ai - aj) / time_step;

		return s;
	}

	public double speed(int i) {
		return avg_speed(i, speed_avg);
	}

	public double acceleration(int i) {
		if (i == 0)
			return 0;
		return (avg_speed(i, accel_avg) - avg_speed(i-1, accel_avg)) / time_step;
	}

	public double time(int i) {
		return i * time_step;
	}

	public void save (OutputStream f) throws IOException {
		for (int c : bytes)
			f.write(c);
		f.write('\n');
	}

	public void set_name(String name) {
		this.name = name;
	}

	public MicroData (InputStream f, String name) throws IOException, InterruptedException {
		this.name = name;
		bytes = new ArrayList<Integer>();
		if (!find_header(f))
			throw new IOException("No MicroPeak data header found");
		try {
			file_crc = 0xffff;
			ground_pressure = get_32(f);
			min_pressure = get_32(f);
			int nsamples = get_16(f);
			pressures = new int[nsamples + 1];

			ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
			int cur = ground_pressure;
			pressures[0] = cur;
			for (int i = 0; i < nsamples; i++) {
				int	k = get_16(f);
				int	same = mix_in(cur, k);
				int	up = mix_in(cur + 0x10000, k);
				int	down = mix_in(cur - 0x10000, k);

				if (closer (cur, same, up)) {
					if (closer (cur, same, down))
						cur = same;
					else
						cur = down;
				} else {
					if (closer (cur, up, down))
						cur = up;
					else
						cur = down;
				}
				
				pressures[i+1] = cur;
			}

			int current_crc = swap16(~file_crc & 0xffff);
			int crc = get_16(f);

			crc_valid = crc == current_crc;

			time_step = 0.192;
		} catch (FileEndedException fe) {
			throw new IOException("File Ended Unexpectedly");
		} catch (NonHexcharException ne) {
			throw new IOException("Non hexadecimal character found");
		}
	}

	public MicroData() {
		ground_pressure = 101000;
		min_pressure = 101000;
		pressures = new int[1];
		pressures[0] = 101000;
	}
	
}