/***************************************************************************** 
 * steim1.cc
 *
 * Steim1 encoder
 *
 * (c) 2000 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 <netinet/in.h>

#include <iostream>
#include <iomanip>
#include <algorithm>

namespace Gempa {
namespace CAPS {

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

template<typename T>
void Steim1Encoder<T>::updateSpw(int bp) {
	int spw1 = 4;

	assert(bp < 4);
	if ( _buf[bp] < -32768 || _buf[bp] > 32767 ) spw1 = 1;
	else if ( _buf[bp] < -128 || _buf[bp] > 127 ) spw1 = 2;
	if ( spw1 < _spw ) _spw = spw1;
}

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

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

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

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

template<typename T>
void Steim1Encoder<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 Steim1Encoder<T>::updatePacket() {
	unsigned int nibble = 0;
	u_int32_t sampleWord = 0;

	assert(_bp < 5);

	int used = _bp;

	while ( used > _spw ) {
		--used;
		_spw = 4;
		for ( int i = 0; i < used; ++i ) {
			updateSpw(i);
		}
	}

	while ( used < _spw ) {
		_spw >>= 1;
	}

	used = _spw;

	switch ( _spw ) {
		case 4:
			nibble = 1;
			sampleWord = ((_buf[0] & 0xff) << 24) | ((_buf[1] & 0xff) << 16) |
			              ((_buf[2] & 0xff) <<  8) | (_buf[3] & 0xff);
			break;
		case 2:
			nibble = 2;
			sampleWord = ((_buf[0] & 0xffff) << 16) | (_buf[1] & 0xffff);
			break;
		case 1:
			nibble = 3;
			sampleWord = _buf[0];
			break;
		default:
			assert(0);
	}

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

	_spw = 4;
	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 Steim1Encoder<T>::queuePacket(MSEEDEncoderPacket<Steim1Frame> &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 Steim1Encoder<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 Steim1Encoder<T>::flush() {
	while ( _bp ) {
		if ( !_currentPacket.valid() ) {
			_currentPacket = getPacket();
			initPacket();
		}

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

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


}
}