# Copyright (c) 2011 pabr@pabr.org
# http://www.pabr.org/copernitron/

dataset = "dd45eeehelm"
#max_samples = inf;
max_samples = 10000000;
latitude = 48
rpm_nominal = 45
#rpm_nominal = 33+1/3

# Device-specific constants.
global sample_rate_nominal = 176;
global max_bias_nominal = 0.1;
global gain_accel = 9.81/8750;  # g per unit
global gain_gyro = 180/pi/640;  # °/s per unit
global gain_mag = 19e-6 / 63;   # T per unit

# Parse compressed output from linmctool.
file = concat(dataset, ".dat.bz2");
f = popen(concat("bzcat ", file), "r");
[data,_] = fscanf(f, concat("%f %*d %*s %*s seq=%*d aX=%d aY=%d aZ=%*d",
                            " gX=%d gY=%d gZ=%d mX=%d mY=%d mZ=%*d"),
                  [8, max_samples]);
pclose(f);

# Beginning and and are used for calibration in some data files.
# Drop at least the last column (it is sometimes padded with zeroes).
discard = 5 * 60 * sample_rate_nominal;
data = data(:, discard:end-discard);

global cos_lat = latitude * pi/180;
global pll_freq = rpm_nominal / 60;

function print_magfield(axis, demod)
  printf("%s: Residual mag. north %.1f µT at %.0f ° CW from PLL origin.\n",
	 axis, 1e6*abs(demod), arg(demod)*180/pi);
endfunction

function print_tilt(axis, demod)
  printf("%s: Turntable tilt %.3f ° up at %.0f ° CW from PLL origin.\n",
	 axis, atan(abs(demod)/9.81)*180/pi, arg(demod)*180/pi);
endfunction

function [demod,twosigma] = earth_rotation(axis, gyrodata, gz, pll, phase)
  global cos_lat max_bias_nominal;
  printf("Using gyro %s.\n", axis);
  mean_gz = mean(gz);
  demod_gz = dot(pll, gz);
  printf("  Range: %.3f °/s  mean: %.3f °/s  stddev: %.3f °/s.\n",
	 max(gyrodata)-min(gyrodata), mean(gyrodata), sqrt(var(gyrodata)));
  printf("  Tilt: %.3f ± %.3f °.\n",
	 atan(mean(gyrodata)/mean_gz)*180/pi,
	 max_bias_nominal/abs(mean_gz)*180/pi);
  demod = dot(pll, gyrodata) / cos_lat * 86164.09 / 360 * phase;
  rpsd = abs(demod);
  # Estimate random error from noise.
  stddev = sqrt(var(gyrodata)/size(gyrodata,2));
  twosigma = 2 * stddev/cos_lat * 86164.09 / 360;
  # Estimate maximum systematic error from tilt.
  tan_tilt_max = (max_bias_nominal+abs(mean(gyrodata))) / abs(mean_gz);
  tilt_error = abs(demod_gz) * tan_tilt_max;
  twosigma += tilt_error / cos_lat * 86164.09 / 360;
  printf("  Earth rotation rate: %.3f rotations per sidereal day\n", rpsd);
  printf("                      ±%.3f with 95%% confidence.\n", twosigma);
  azimuth = arg(demod) * 180/pi;
  printf("  True north: %.1f ° clockwise from PLL origin.\n", azimuth);
endfunction

function [dX, errX, dY, errY] = process_samples(dat)
  global gain_accel gain_gyro gain_mag pll_freq;
  count = size(dat, 2)
  duration = dat(1,end) - dat(1,1)
  sample_rate = (count-1) / duration;
  printf("Sample rate: %.3f Hz (ignoring packet loss).\n", sample_rate);

  pll_periods = 2;
  pll_size = floor(pll_periods*sample_rate/pll_freq);

  skip = pll_size;
  ts = dat(1,skip:end);
  aX = dat(2,skip:end) * gain_accel;
  aY = dat(3,skip:end) * gain_accel;
  gX = dat(4,skip:end) * gain_gyro;
  gY = dat(5,skip:end) * gain_gyro;
  gZ = dat(6,skip:end) * gain_gyro;
  mX = dat(7,skip:end) * gain_mag;
  mY = dat(8,skip:end) * gain_mag;
  pll_reference = - (dat(7,:)+j*dat(8,:)) * gain_mag;  # residual mag. north
  #pll_reference = (dat(2,:)+j*dat(3,:)) * gain_accel;   # upward tilt

  # Build pseudo pll.
  pll_coeffs = exp(2j*pi * pll_freq * [0:pll_size-1]/sample_rate);
  pll_filtered = filter(pll_coeffs, [1], pll_reference);
  pll_filtered = pll_filtered(skip:end);
  pll = pll_filtered ./ abs(pll_filtered);
  pll = pll - mean(pll);
  pll = pll * 2 / sum(abs(pll).^2);

  print_magfield("mX", dot(pll,-mX));
  print_magfield("mY", dot(pll,-mY)*-j);
  print_tilt("aX", dot(pll,aX));
  print_tilt("aY", dot(pll,aY)*-j);

  mean_gZ = mean(gZ);
  demod_gZ = dot(pll, gZ);
  printf("Rate fluctuation: %.0f ppm.\n", 1e6*abs(demod_gZ/mean_gZ));

  [dX, errX] = earth_rotation("gX", gX, gZ, pll, 1);
  [dY, errY] = earth_rotation("gY", gY, gZ, pll, -j);
endfunction

[dX,  errX,  dY,  errY ] = process_samples(data);
