// TODO: Currently this can only advance forwards (never backwards) -- as it's sorted, we could do a binary search for the nearest time
void InterpolatorSeek(interpolator_t *interpolator, double t)
{
interpolator->clipped = false;
interpolator->valid = true;
// Skip segment if needed
while (interpolator->seg != NULL && t > interpolator->seg->endTime)
{
interpolator->seg = interpolator->seg->segmentNext;
interpolator->timeIndex = -1;
if (interpolator->seg != NULL)
{
interpolator->scale = interpolator->seg->scaling;
}
}
if (interpolator->seg != NULL)
{
// Skip time indices if needed
while (interpolator->timeIndex + 1 < interpolator->seg->timestampCount && t >= interpolator->seg->timestamps[interpolator->timeIndex + 1].timestamp)
{
interpolator->timeIndex++;
}
// Check we're between two time indices
if (interpolator->seg->numSamples > 0)
{
int i1, i2;
double t1, t2;
if (interpolator->timeIndex >= 0 && interpolator->timeIndex < interpolator->seg->timestampCount)
{
i1 = interpolator->seg->timestamps[interpolator->timeIndex].sample;
t1 = interpolator->seg->timestamps[interpolator->timeIndex].timestamp;
}
else
{
i1 = 0;
t1 = interpolator->seg->startTime;
}
if (interpolator->timeIndex + 1 < interpolator->seg->timestampCount)
{
i2 = interpolator->seg->timestamps[interpolator->timeIndex + 1].sample;
t2 = interpolator->seg->timestamps[interpolator->timeIndex + 1].timestamp;
}
else
{
i2 = interpolator->seg->numSamples - 1;
t2 = interpolator->seg->endTime;
}
// Interpolate the timestamps here (don't think cubic is valid as the source points should be equidistant for that to be valid, so use linear)
double timeProp = (t2 - t1) != 0.0 ? (t - t1) / (t2 - t1) : 0.0; // Linear interpolate in time (see note above)
double index = (i2 - i1) * timeProp + i1; // Fractional index at that position
interpolator->sampleIndex = (int)index; // Actual index (v1)
interpolator->prop = index - (int)index; // Offset towards next value (v2)
//printf(">>> %f => %d . %f\n", index, interpolator->sampleIndex, interpolator->prop);
// Have we got enough for (-1, 0, 1, 2)?
int idx[4];
idx[1] = interpolator->sampleIndex; // v1 (@0)
if (interpolator->sampleIndex >= 1)
{
idx[0] = interpolator->sampleIndex - 1; // v0 (@-1)
}
else
{
idx[0] = idx[1];
}
if (interpolator->sampleIndex + 1 < interpolator->seg->numSamples)
{
idx[2] = interpolator->sampleIndex + 1; // v2 (@1)
}
else
{
idx[2] = idx[1];
}
if (interpolator->sampleIndex + 2 < interpolator->seg->numSamples)
{
idx[3] = interpolator->sampleIndex + 2; // v3 (@2)
}
else
{
idx[3] = idx[2];
}
// For each index (-1, 0, 1, 2), cache the underlying values (for the interpolator to work over)
int z;
for (z = 0; z < 4; z++)
{
char clipped = OmDataGetValues(interpolator->data, interpolator->seg, idx[z], interpolator->values[z]);
if (z == 1 || z == 2) { interpolator->clipped |= clipped; }
}
return;
}
}
// Invalid
interpolator->valid = false;
}
// (Internal) Find stationary points (using either a player or direct data)
static omcalibrate_stationary_points_t *OmCalibrateFindStationaryPoints(omcalibrate_config_t *config, omdata_t *data, om_convert_player_t *player)
{
double sampleRate, startTime;
double firstSampleTime = 0;
int lastWindow = -1;
int numSamples;
int samplesInPreviousSegments = 0;
omdata_segment_t *dataSegment = NULL;
if (player != NULL)
{
sampleRate = player->sampleRate;
startTime = player->arrangement->startTime;
numSamples = player->numSamples;
}
else if (data != NULL)
{
omdata_stream_t *stream = &data->stream['a'];
if (!stream->inUse)
{
fprintf(stderr, "ERROR: Calibration failed as accelerometer stream not found.\n");
return NULL;
}
dataSegment = stream->segmentFirst;
sampleRate = dataSegment->sampleRate;
startTime = dataSegment->startTime;
// Determine total number of samples
numSamples = 0;
omdata_segment_t *seg;
for (seg = dataSegment; seg != NULL; seg = seg->segmentNext)
{
numSamples += seg->numSamples;
}
}
else
{
return NULL;
}
omcalibrate_stationary_points_t *stationaryPoints = (omcalibrate_stationary_points_t *)malloc(sizeof(omcalibrate_stationary_points_t));
memset(stationaryPoints, 0, sizeof(omcalibrate_stationary_points_t));
// For 'ignore repeated' option
double initialMean[OMCALIBRATE_AXES] = { 0 };
double initialTemp = 0.0;
int consecutive = 0;
// Trackers
#ifdef ALT_SD
// See Knuth TAOCP vol 2, 3rd edition, page 232
double oldM[OMCALIBRATE_AXES] = { 0 };
double newM[OMCALIBRATE_AXES] = { 0 };
double oldS[OMCALIBRATE_AXES] = { 0 };
double newS[OMCALIBRATE_AXES] = { 0 };
int n[OMCALIBRATE_AXES] = { 0 };
#else
double axisSum[OMCALIBRATE_AXES] = { 0 };
double axisSumSquared[OMCALIBRATE_AXES] = { 0 };
#endif
double tempSum = 0;
int sampleCount = 0;
// Time interpolation for direct data
int lastSectorIndex = -1;
int nextTimestampSample = -1, lastTimestampSample = -1;
double nextTimestampValue = 0, lastTimestampValue = 0;
int sample;
for (sample = 0; sample < numSamples; sample++)
{
int c;
double values[OMCALIBRATE_AXES];
double temp;
double currentTime;
bool windowFilled = false;
if (player != NULL)
{
OmConvertPlayerSeek(player, sample);
if (!player->valid) { sampleCount = 0; continue; }
// Convert to units
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
values[c] = player->scale[c] * player->values[c];
}
temp = player->temp;
currentTime = ((double)sample / sampleRate) + startTime;
// Have we filled a window?
int numStationary = (int)(config->stationaryTime * sampleRate + 0.5);
if (sampleCount >= numStationary) { windowFilled = true; }
}
else if (data != NULL)
{
int sampleWithinSegment = sample - samplesInPreviousSegments;
// Check we have data
if (dataSegment == NULL)
{
fprintf(stderr, "WARNING: Less data than was expected.\n");
break;
}
int sectorWithinSegment = sampleWithinSegment / dataSegment->samplesPerSector;
if (sectorWithinSegment >= dataSegment->sectorCount)
{
fprintf(stderr, "WARNING: Invalid sector within segment.\n");
break;
}
// Advance to next segment?
if (sampleWithinSegment >= dataSegment->numSamples)
{
dataSegment = dataSegment->segmentNext;
samplesInPreviousSegments = sample;
sampleWithinSegment = 0;
// Update rates/times
if (dataSegment != NULL)
{
sampleRate = dataSegment->sampleRate;
startTime = dataSegment->startTime;
}
// Reset accumulator on segment change (break in sample stream)
sampleCount = 0;
firstSampleTime = 0; // Trigger time reset
lastSectorIndex = -1; // Force no time interpolation
nextTimestampSample = -1; // Force restart of time interpolation
lastWindow = -1;
continue;
}
// Sector index
int sectorIndex = dataSegment->sectorIndex[sectorWithinSegment];
#if 1
if (sectorIndex != lastSectorIndex)
{
lastSectorIndex = sectorIndex;
// Get current sector time
int sampleIndexOffset = 0;
double newTimestampValue = OmDataTimestampForSector(data, sectorIndex, &sampleIndexOffset);
int newTimestampSample = sample + sampleIndexOffset;
// Replace 'next' timestamp if different
if (newTimestampSample != nextTimestampSample)
{
if (nextTimestampSample < 0)
{
nextTimestampSample = newTimestampSample;
nextTimestampValue = newTimestampValue;
}
lastTimestampSample = nextTimestampSample;
lastTimestampValue = nextTimestampValue;
nextTimestampSample = newTimestampSample;
nextTimestampValue = newTimestampValue;
}
}
// If we only have one timestamp to estimate from
int elapsedSamples = sample - lastTimestampSample + 1;
if (nextTimestampSample <= lastTimestampSample)
{
currentTime = lastTimestampValue + (elapsedSamples / sampleRate);
}
else
{
currentTime = lastTimestampValue + (elapsedSamples * (nextTimestampValue - lastTimestampValue) / (nextTimestampSample - lastTimestampSample));
}
#else
// HACK: This is a bogus time as the sample rate is not fixed
currentTime = ((double)sampleWithinSegment / sampleRate) + startTime;
#endif
#if 0
// Debug out of time delta
static double lastTime = -1;
if (lastTime < 0) { lastTime = currentTime; }
double deltaTime = currentTime - lastTime;
printf("%f\n", deltaTime);
lastTime = currentTime;
#endif
// Get samples
int16_t intvalues[OMCALIBRATE_AXES];
OmDataGetValues(data, dataSegment, sampleWithinSegment, intvalues);
// Get temperature
temp = 0;
if (dataSegment->offset == 30)
{
const unsigned char *p = (const unsigned char *)data->buffer + (OMDATA_SECTOR_SIZE * sectorIndex);
int16_t inttemp = p[20] | ((int16_t)p[21] << 8); // @20 WORD Temperature
// Convert
temp = ((int)inttemp * 150 - 20500) / 1000.0;
//temp = (double)inttemp * 75 / 256.0 - 50;
}
// Scale values
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
values[c] = intvalues[c] * dataSegment->scaling;
}
// Check whether a window is filled
if (firstSampleTime <= 0) { firstSampleTime = currentTime; lastWindow = -1; }
int currentWindow = (int)((currentTime - firstSampleTime) / config->stationaryTime);
if (lastWindow < 0) { lastWindow = currentWindow; }
if (currentWindow != lastWindow)
{
windowFilled = true;
lastWindow = currentWindow;
}
}
else
{
break;
}
// Filled window
if (windowFilled)
{
bool stationary = true;
if (sampleCount <= 0) { sampleCount = 1; stationary = false; }
// Check whether stationary
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
//double count = n[c];
//double mean = (n[c] > 0) ? newM[c] : 0.0;
double variance = (n[c] > 1) ? newS[c] / (n[c] - 1) : 0.0;
double standardDeviation = sqrt(variance);
#else
double mean = axisSum[c] / sampleCount;
double squareOfMean = mean * mean;
double averageOfSquares = (axisSumSquared[c] / sampleCount);
double standardDeviation = sqrt(averageOfSquares - squareOfMean);
//double standardDeviation = sqrt(sampleCount * axisSumSquared[c] - (axisSum[c] * axisSum[c])) / sampleCount;
#endif
if (standardDeviation > config->stationaryMaxDeviation)
{
stationary = false;
}
}
// Calculate mean values
double time = currentTime - ((((double)sampleCount / 2)) / sampleRate);
double mean[OMCALIBRATE_AXES];
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
mean[c] = (n[c] > 0) ? newM[c] : 0.0;
#else
mean[c] = axisSum[c] / sampleCount;
#endif
}
// Check if this is a repeat
if (stationary && config->stationaryRepeated)
{
if (consecutive == 0)
{
// Update 'initial' values
for (c = 0; c < OMCALIBRATE_AXES; c++) { initialMean[c] = mean[c]; }
initialTemp = temp;
consecutive = 1;
}
else
{
// Compare 'initial' values
double diff = 0;
for (c = 0; c < OMCALIBRATE_AXES; c++) { diff += (initialMean[c] - mean[c]) * (initialMean[c] - mean[c]); }
diff = sqrt(diff);
if (diff < config->stationaryRepeatedAccel && fabs(initialTemp - temp) <= config->stationaryRepeatedTemp)
{
consecutive++;
stationary = false;
}
else
{
consecutive = 0;
}
}
}
else
{
consecutive = 0;
}
#if 0
// Debug out of time delta
static int windowNumber = 0;
static double lastTime = -1;
if (lastTime < 0) { lastTime = currentTime; }
double deltaTime = currentTime - lastTime;
if (stationary)
printf("WINDOW: %d, from=%.3f, to=%.3f, period=%.3f\n", windowNumber++, lastTime, currentTime, deltaTime);
lastTime = currentTime;
windowNumber++;
#endif
// Create new point
if (stationary)
{
omcalibrate_point_t point;
point.time = time;
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
point.mean[c] = mean[c];
}
point.actualTemperature = tempSum / sampleCount;
//point.temperature = 0;
// Check whether we have to grow the buffer
if (stationaryPoints->numValues >= stationaryPoints->capacity)
{
stationaryPoints->capacity = (15 * stationaryPoints->capacity / 10) + 1;
stationaryPoints->values = (omcalibrate_point_t *)realloc(stationaryPoints->values, stationaryPoints->capacity * sizeof(omcalibrate_point_t));
}
stationaryPoints->values[stationaryPoints->numValues] = point;
stationaryPoints->numValues++;
}
// Trigger reset of accumulators
sampleCount = 0;
}
// If the accumulators need to be reset...
if (sampleCount == 0)
{
if (firstSampleTime <= 0) { firstSampleTime = currentTime; }
// Clear accumulators
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
oldM[c] = newM[c] = oldS[c] = newS[c] = 0;
n[c] = 0;
#else
axisSum[c] = 0;
axisSumSquared[c] = 0;
#endif
}
tempSum = 0;
}
sampleCount++;
// Accumulate
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
double x = values[c];
#ifdef ALT_SD
n[c]++;
if (n[c] == 1)
{
oldM[c] = newM[c] = x;
oldS[c] = 0.0;
}
else
{
newM[c] = oldM[c] + (x - oldM[c]) / n[c];
newS[c] = oldS[c] + (x - oldM[c]) * (x - newM[c]);
oldM[c] = newM[c];
oldS[c] = newS[c];
}
#else
axisSum[c] += x;
axisSumSquared[c] += x * x;
#endif
}
tempSum += temp;
}
return stationaryPoints;
}
