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

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

template<typename T> void Steim2Encoder<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 Steim2Encoder<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 Steim2Encoder<T>::update_packet() {
	unsigned int nibble = 0;
	u_int32_t sample_word = 0;

	assert(bp < 8);

	int used = bp;

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

	spw = used;

	switch(spw) {
		case 7:
			nibble = 3;
			sample_word = (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;
			sample_word = (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;
			sample_word = ((buf[0] & 0x3f) << 24) | ((buf[1] & 0x3f) << 18) |
				((buf[2] & 0x3f) << 12) | ((buf[3] & 0x3f) << 6) |
				(buf[4] & 0x3f);
			break;
		case 4:
			nibble = 1;
			sample_word = ((buf[0] & 0xff) << 24) | ((buf[1] & 0xff) << 16) |
				((buf[2] & 0xff) <<  8) | (buf[3] & 0xff);
			break;
		case 3:
			nibble = 2;
			sample_word = (3U << 30) | ((buf[0] & 0x3ff) << 20) |
				((buf[1] & 0x3ff) << 10) | (buf[2] & 0x3ff);
			break;
		case 2:
			nibble = 2;
			sample_word = (2U << 30) | ((buf[0] & 0x7fff) << 15) |
				(buf[1] & 0x7fff);
			break;
		case 1:
			nibble = 2;
			sample_word = (1U << 30) | (buf[0] & 0x3fffffff);
			break;
		default:
			assert(0);
			break;
	}

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

	spw = 7;
	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 Steim2Encoder<T>::queue_packet(MSEEDEncoderPacket<Steim2Frame> &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 Steim2Encoder<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 Steim2Encoder<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);
	}
}


}
}