/***************************************************************************** 
 * 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 != NULL ) delete format;
}

template<typename T> void Steim1Encoder<T>::update_spw(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 - last_sample;
	last_sample = value;
	update_spw(bp);
	++bp;
}

template<typename T> void Steim1Encoder<T>::init_packet() {
	int i;
	int32_t begin_sample = last_sample;

	for(i = 1; i < bp; ++i) {
		begin_sample -= buf[i];
	}

	reset();
	current_packet.data[0].sample_word[0] = htonl(begin_sample);
	frame_count = 0;
	nibble_word = 0;
	fp = 2;
}

template<typename T> void Steim1Encoder<T>::finish_packet() {
	int i;
	int32_t end_sample = last_sample;

	for(i = 0; i < bp; ++i) {
		end_sample -= buf[i];
	}

	current_packet.data[0].sample_word[1] = htonl(end_sample);
}

template<typename T> void Steim1Encoder<T>::update_packet() {
	unsigned int nibble = 0;
	u_int32_t sample_word = 0;

	assert(bp < 5);

	int used = bp;

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

	while(used < spw) spw >>= 1;

	used = spw;

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

	nibble_word |= (nibble << (30 - ((fp + 1) << 1)));

	spw = 4;
	for(int i = 0; i < bp - used; ++i) {
		buf[i] = buf[i + used];
		update_spw(i);
	}

	bp -= used;
	_sampleCount += used;

	current_packet.data[frame_count].nibble_word = htonl(nibble_word);
	current_packet.data[frame_count].sample_word[fp] = htonl(sample_word);
	if(++fp < 15) return;

	nibble_word = 0;
	fp = 0;
	++frame_count;
	return;
}

template<typename T> void Steim1Encoder<T>::queue_packet(MSEEDEncoderPacket<Steim1Frame> &pckt) {
	format->updateBuffer(pckt.record, _sampleCount, frame_count + (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(!current_packet.valid()) {
			current_packet = get_packet();
			init_packet();
		}

		update_packet();
		if(frame_count == number_of_frames(current_packet)) {
			finish_packet();
			queue_packet(current_packet);
		}
	}
}

template<typename T> void Steim1Encoder<T>::flush() {
	while(bp) {
		if(!current_packet.valid()) {
			current_packet = get_packet();
			init_packet();
		}

		update_packet();
		if(frame_count == number_of_frames(current_packet)) {
			finish_packet();
			queue_packet(current_packet);
		}
	}

	if(current_packet.valid()) {
		finish_packet();
		queue_packet(current_packet);
	}
}

}
}