libcapsclient/libs/gempa/caps/mseed/steim2.ipp

/*****************************************************************************
 * steim2.cc
 *
 * Steim2 encoder
 *
 * (c) 2004 Andres Heinloo, GFZ Potsdam
 *
 * 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 2, or (at your option) any later
 * version. For more information, see http://www.gnu.org/
 *
 * ================
 * Change log
 * ===============
 *
 * 01.01.2013 Adapted code to CAPS client library requirements (gempa GmbH)
 *****************************************************************************/

#include "../log.h"

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <assert.h>

#include <netinet/in.h>


namespace Gempa {
namespace CAPS {


template<typename T> Steim2Encoder<T>::~Steim2Encoder() {
	if ( _format ) {
		delete _format;
	}
}

template<typename T> void Steim2Encoder<T>::updateSpw(int bp) {
	assert(_bp < 7);

	if ( _buf[_bp] < -536870912 ) {
		CAPS_WARNING("%s.%s.%s.%s: value %d is too large for Steim2 encoding",
		             _format->networkCode.c_str(), _format->stationCode.c_str(),
		             _format->locationCode.c_str(), _format->channelCode.c_str(),
		             _buf[_bp]);
		_buf[_bp] = -536870912;
		_spw = 1;
		return;
	}

	if ( _buf[_bp] > 536870911 ) {
		CAPS_WARNING("%s.%s.%s.%s: value %d is too large for Steim2 encoding",
		             _format->networkCode.c_str(), _format->stationCode.c_str(),
		             _format->locationCode.c_str(), _format->channelCode.c_str(),
		             _buf[_bp]);
		_buf[_bp] = 536870911;
		_spw = 1;
		return;
	}

	int spw1 = 7;
	if ( _buf[_bp] < -16384 || _buf[_bp] > 16383 ) spw1 = 1;
	else if ( _buf[_bp] < -512 || _buf[_bp] > 511 ) spw1 = 2;
	else if ( _buf[_bp] < -128 || _buf[_bp] > 127 ) spw1 = 3;
	else if ( _buf[_bp] < -32 || _buf[_bp] > 31 ) spw1 = 4;
	else if ( _buf[_bp] < -16 || _buf[_bp] > 15 ) spw1 = 5;
	else if ( _buf[_bp] < -8  || _buf[_bp] > 7 ) spw1 = 6;
	if ( spw1 < _spw ) _spw = spw1;
}

template<typename T> void Steim2Encoder<T>::store(int32_t value) {
	assert(_bp < 7);
	_buf[_bp] = value - _lastSample;
	_lastSample = value;
	updateSpw(_bp);
	++_bp;
}

template<typename T> void Steim2Encoder<T>::initPacket() {
	int i;
	int32_t begin_sample = _lastSample;

	for ( i = 1; i < _bp; ++i ) {
		begin_sample -= _buf[i];
	}

	reset();
	_currentPacket.data[0].sampleWord[0] = htonl(begin_sample);
	_frameCount = 0;
	_nibbleWord = 0;
	_fp = 2;
}

template<typename T> void Steim2Encoder<T>::finishPacket() {
	int i;
	int32_t endSample = _lastSample;

	for ( i = 0; i < _bp; ++i ) {
		endSample -= _buf[i];
	}

	_currentPacket.data[0].sampleWord[1] = htonl(endSample);
}

template<typename T> void Steim2Encoder<T>::updatePacket() {
	unsigned int nibble = 0;
	u_int32_t sampleWord = 0;

	assert(_bp < 8);

	int used = _bp;

	while ( used > _spw ) {
		--used;
		_spw = 7;

		for ( int i = 0; i < used; ++i ) {
			updateSpw(i);
		}
	}

	_spw = used;

	switch ( _spw ) {
		case 7:
			nibble = 3;
			sampleWord = (2U << 30) | ((_buf[0] & 0xf) << 24) |
				((_buf[1] & 0xf) << 20) | ((_buf[2] & 0xf) << 16) |
				((_buf[3] & 0xf) << 12) | ((_buf[4] & 0xf) << 8) |
				((_buf[5] & 0xf) << 4) | (_buf[6] & 0xf);
			break;
		case 6:
			nibble = 3;
			sampleWord = (1U << 30) | ((_buf[0] & 0x1f) << 25) |
				((_buf[1] & 0x1f) << 20) | ((_buf[2] & 0x1f) << 15) |
				((_buf[3] & 0x1f) << 10) | ((_buf[4] & 0x1f) << 5) |
				(_buf[5] & 0x1f);
			break;
		case 5:
			nibble = 3;
			sampleWord = ((_buf[0] & 0x3f) << 24) | ((_buf[1] & 0x3f) << 18) |
				((_buf[2] & 0x3f) << 12) | ((_buf[3] & 0x3f) << 6) |
				(_buf[4] & 0x3f);
			break;
		case 4:
			nibble = 1;
			sampleWord = ((_buf[0] & 0xff) << 24) | ((_buf[1] & 0xff) << 16) |
				((_buf[2] & 0xff) <<  8) | (_buf[3] & 0xff);
			break;
		case 3:
			nibble = 2;
			sampleWord = (3U << 30) | ((_buf[0] & 0x3ff) << 20) |
				((_buf[1] & 0x3ff) << 10) | (_buf[2] & 0x3ff);
			break;
		case 2:
			nibble = 2;
			sampleWord = (2U << 30) | ((_buf[0] & 0x7fff) << 15) |
				(_buf[1] & 0x7fff);
			break;
		case 1:
			nibble = 2;
			sampleWord = (1U << 30) | (_buf[0] & 0x3fffffff);
			break;
		default:
			assert(0);
			break;
	}

	_nibbleWord |= (nibble << (30 - ((_fp + 1) << 1)));

	_spw = 7;
	for ( int i = 0; i < _bp - used; ++i ) {
		_buf[i] = _buf[i + used];
		updateSpw(i);
	}

	_bp -= used;
	_sampleCount += used;

	_currentPacket.data[_frameCount].nibbleWord = htonl(_nibbleWord);
	_currentPacket.data[_frameCount].sampleWord[_fp] = htonl(sampleWord);

	if ( ++_fp < 15 ) {
		return;
	}

	_nibbleWord = 0;
	_fp = 0;
	++_frameCount;

	return;
}

template<typename T> void Steim2Encoder<T>::queuePacket(MSEEDEncoderPacket<Steim2Frame> &pckt) {
	_format->updateBuffer(pckt.record, _sampleCount, _frameCount + (_fp > 0));

	Packet *packet = new Packet(DataRecordPtr(pckt.record), _format->networkCode,
	                            _format->stationCode,
	                            _format->locationCode,
	                            _format->channelCode);
	_packetQueue.push_back(PacketPtr(packet));
	pckt.reset();
	reset();
}


template<typename T> void Steim2Encoder<T>::push(void *value) {
	int32_t sample_val = *static_cast<T*>(value);
	store(sample_val);
	_clk.tick();

	while ( _bp >= _spw ) {
		if ( !_currentPacket.valid() ) {
			_currentPacket = getPacket();
			initPacket();
		}

		updatePacket();
		if ( _frameCount == numberOfFrames(_currentPacket) ) {
			finishPacket();
			queuePacket(_currentPacket);
		}
	}
}

template<typename T> void Steim2Encoder<T>::flush() {
	while ( _bp ) {
		if ( !_currentPacket.valid() ) {
			_currentPacket = getPacket();
			initPacket();
		}

		updatePacket();
		if ( _frameCount == numberOfFrames(_currentPacket) ) {
			finishPacket();
			queuePacket(_currentPacket);
		}
	}

	if ( _currentPacket.valid() ) {
		finishPacket();
		queuePacket(_currentPacket);
	}
}


}
}