<commit_before>#define _POSIX_C_SOURCE 1
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <utility>
#include <typeinfo>
#include "timer.h"
#include "trie.h"
#include "signatures.h"
/** Starting with demo-only includes. **/
#include "../mp_dpi_api.h"
#include <pcap.h>
#include <netinet/in.h>
#include <net/ethernet.h>
#include <stdint.h>
#include <ff/mapping_utils.hpp>
#include <ff/utils.hpp>
#include <ff/ubuffer.hpp>

// #define USE_PF_RING_CLUSTER  1

#define ALARM_SLEEP  1

#include <time.h>
#include <signal.h>
#include <pfring.h>

using namespace antivirus;
#define CAPACITY_CHUNK 1000
#define SCANNER_POOL_SIZE 4096
#define CLOCK_FREQ 2000000000L

#define AVAILABLE_CORES 16
static u_int16_t mapping[AVAILABLE_CORES]=
{0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};

static struct timeval startTime;
unsigned long long numPkts = 0, numBytes = 0;
pfring *ring;
ff::SWSR_Ptr_Buffer *pkt_buff;
int verbose = 0;

static ff::uSWSR_Ptr_Buffer* scanner_pool;

dpi_mp_packet_reading_result_t reading_cb(void* user_data){
	static unsigned long last_ts=getticks();
	static u_int32_t last_sec=0;
#ifdef USE_PF_RING_CLUSTER
	pfring_pkt_buff *pkt_handle = NULL;
#else
	void *pkt_handle;
#endif
	struct pfring_pkthdr hdr;
	int rc;

	if(numPkts == 0) gettimeofday(&startTime, NULL);

	if(pkt_buff->empty()) {
		printf("Not enough buffer available: please start over\n");
		exit(-1);
	}

	pkt_buff->pop(&pkt_handle);

	if(verbose) printf("pkt_buff->pop()\n");

	while(1) {
#ifdef USE_PF_RING_CLUSTER
		rc = pfring_recv_pkt_buff(ring, pkt_handle, &hdr, 1);
#else
		rc = pfring_recv(ring, (u_char**)&pkt_handle, 1500, &hdr, 1);
#endif

		if(rc < 0) {
			printf("pfring_recv_pkt_buff() returned %d\n", rc);
			exit(-1);
		} else if(rc == 0) {
			continue;
		} else {
			dpi_mp_packet_reading_result_t r;
			const unsigned char *p = (const unsigned char*)pkt_handle;

			r.pkt = &p[14];

			if(getticks()-last_ts>CLOCK_FREQ){
				last_ts = getticks();
				++last_sec;
			}
			r.current_time=last_sec;
			r.length=hdr.caplen-14;
			r.user_pointer=pkt_handle;

			numPkts++, numBytes += hdr.len;
			return r;
		}
	} /* while */
}

void processing_cb(mp_dpi_processing_result_t* processing_result, void* user_data){
	pkt_buff->push((void*)processing_result->user_pointer);
	if(verbose) printf("pkt_buff->push()\n");
}

static void match_found(string::size_type position,
		trie::value_type const &match){
	using namespace std;
	cout << "Matched '" << match.second << "' at " << position << endl;
}

void body_cb(dpi_http_message_informations_t* http_informations,
		const u_char* app_data, u_int32_t data_length,
		dpi_pkt_infos_t* pkt,
		void** flow_specific_user_data, void* user_data,
		u_int8_t last){
	if(*flow_specific_user_data==NULL){
		if(scanner_pool->mc_pop(flow_specific_user_data)==false){
			*flow_specific_user_data=
					new byte_scanner(*((trie*) user_data), match_found);
		}
	}
	byte_scanner* scanner=(byte_scanner*) (*flow_specific_user_data);
	for(u_int32_t i=0; i<data_length; i++){
		scanner->match(app_data[i]);
	}
}

void flow_cleaner(void* flow_specific_user_data){
	if(scanner_pool->mp_push(flow_specific_user_data)==false){
		delete (byte_scanner*) flow_specific_user_data;
	}
}

/*
 * The time difference in millisecond
 */
double delta_time (struct timeval * now,
		struct timeval * before) {
	time_t delta_seconds;
	time_t delta_microseconds;

	/*
	 * compute delta in second, 1/10's and 1/1000's second units
	 */
	delta_seconds      = now -> tv_sec  - before -> tv_sec;
	delta_microseconds = now -> tv_usec - before -> tv_usec;

	if(delta_microseconds < 0) {
		/* manually carry a one from the seconds field */
		delta_microseconds += 1000000;  /* 1e6 */
		-- delta_seconds;
	}
	return((double)(delta_seconds * 1000) + (double)delta_microseconds/1000);
}

void print_stats() {
	pfring_stat pfringStat;
	struct timeval endTime;
	double deltaMillisec;
	static u_int64_t lastPkts = 0;
	u_int64_t diff;
	static struct timeval lastTime;

	if(startTime.tv_sec == 0) return;

	gettimeofday(&endTime, NULL);
	deltaMillisec = delta_time(&endTime, &startTime);

	if(pfring_stats(ring, &pfringStat) >= 0)
		fprintf(stderr, "=========================\n"
				"Absolute Stats: [%u pkts rcvd][%u pkts dropped]\n"
				"Total Pkts=%u/Dropped=%.1f %%\n",
				(unsigned int)pfringStat.recv, (unsigned int)pfringStat.drop,
				(unsigned int)(pfringStat.recv-pfringStat.drop),
				pfringStat.recv == 0 ? 0 : (double)(pfringStat.drop*100)/(double)pfringStat.recv);
	fprintf(stderr, "%llu pkts [%.1f pkt/sec] - %llu bytes [%.2f Mbit/sec]\n",
			numPkts, (double)(numPkts*1000)/deltaMillisec,
			numBytes, (double)8*numBytes/(double)(deltaMillisec));

	if(lastTime.tv_sec > 0) {
		deltaMillisec = delta_time(&endTime, &lastTime);
		diff = pfringStat.recv-lastPkts;
		fprintf(stderr, "=========================\n"
				"Actual Stats: %llu pkts [%.1f ms][%.1f pkt/sec]\n",
				(long long unsigned int)diff, deltaMillisec,
				((double)diff/(double)(deltaMillisec/1000)));
	}

	lastPkts = pfringStat.recv;

	lastTime.tv_sec = endTime.tv_sec, lastTime.tv_usec = endTime.tv_usec;

	fprintf(stderr, "=========================\n");
}

/* ******************************** */

void sigproc(int sig) {
	static int called = 0;

	if(called) return; else called = 1;

	print_stats();
	pfring_close(ring);
	exit(0);
}

void my_sigalarm(int sig) {
	print_stats();
	printf("\n");
	alarm(ALARM_SLEEP);
	signal(SIGALRM, my_sigalarm);
}

int main(int argc, char **argv){
	using namespace std;
	ff_mapThreadToCpu(mapping[0], -20);

	try {
		if (argc<4){
			cerr << "Usage: " << argv[0] <<
					" virus-signatures-file device "
					"par_degree\n";
			exit(EXIT_FAILURE);
		}

		string::size_type trie_depth=DEFAULT_TRIE_MAXIMUM_DEPTH;

		char const *virus_signatures_file_name=argv[1];
		char const *devide=argv[2];
		u_int16_t num_workers=atoi(argv[3]);

		ifstream signatures;
		signatures.open(virus_signatures_file_name);
		if(!signatures){
			cerr << argv[0] << ": failed to open '" <<
					virus_signatures_file_name << "'\n";
			exit(EXIT_FAILURE);
		}

		signature_reader reader(signatures);

		cout << "reading '" << virus_signatures_file_name << "'... ";

		timer read_signatures_timer;
		read_signatures_timer.start();
		trie t(trie_depth);
		while (reader.next()) {
			t.insert(make_pair(reader.current_pattern(),
					reader.current_name()));
		}
		read_signatures_timer.stop();
		cout << setiosflags(ios_base::fixed) << setprecision(3) <<
				read_signatures_timer.real_time() << " seconds.\n";
		cout << "preparing '" << virus_signatures_file_name << "'... ";
		timer prepare_signatures_timer;
		prepare_signatures_timer.start();
		t.prepare();
		prepare_signatures_timer.stop();
		cout << setiosflags(ios_base::fixed)
		    		 << setprecision(3)
		    		 << prepare_signatures_timer.real_time() << " seconds.\n";

		cout << "# of allocated trie nodes: " << t.node_count() << endl;

		timer full_timer;
		full_timer.start();
		/******************************************************/
		/*             Start scanning the files.              */
		/******************************************************/

		dpi_mp_parallelism_details_t details;
		bzero(&details, sizeof(dpi_mp_parallelism_details_t));
		details.available_processors=AVAILABLE_CORES;
		details.mapping=mapping;

		details.single_farm_num_workers=num_workers;
		dpi_mp_library_state_t* state=mp_dpi_init_stateful(
				32767, 32767, 1000000, 1000000, details);

		int num_pkt_buffers = 32768;

		int snaplen = 1500, flags = PF_RING_PROMISC;

		ring = pfring_open(device, snaplen, flags);

		if(ring==NULL){
			fprintf(stderr, "Couldn't open device %s\n", argv[1]);
			return (2);
		}

		scanner_pool=new ff::uSWSR_Ptr_Buffer(SCANNER_POOL_SIZE);
		scanner_pool->init();
		for(uint i=0; i<SCANNER_POOL_SIZE; i++){
			scanner_pool->push(new byte_scanner(t, match_found));
		}
		mp_dpi_set_read_and_process_callbacks(
				state, &reading_cb, &processing_cb, (void*) &x);
		mp_dpi_set_flow_cleaner_callback(state, &flow_cleaner);
		dpi_http_callbacks_t callback={0, 0, 0, 0, 0, &body_cb};
		mp_dpi_http_activate_callbacks(state, &callback, (void*)(&t));

		pkt_buff = new ff::SWSR_Ptr_Buffer(num_pkt_buffers);
		if(pkt_buff == NULL) {
			printf("SWSR_Ptr_Buffer() failed\n");
			return(-1);
		}
		pkt_buff->init();

		for(int i=0; i<num_pkt_buffers; i++) {
#ifdef USE_PF_RING_CLUSTER
			pfring_pkt_buff *pkt_handle;

			if((pkt_handle = pfring_alloc_pkt_buff(ring)) == NULL) {
				printf("Error allocating pkt buff\n");
				return(-1);
			} else
				pkt_buff->push(pkt_handle);
#else
			pkt_buff->push(malloc(1500));
			if(verbose) printf("pkt_buff->push() empty buffer\n");
#endif
		}

		signal(SIGINT, sigproc);
		signal(SIGALRM, my_sigalarm);
		alarm(ALARM_SLEEP);

		pfring_enable_ring(ring);

		timer scan_timer;
		scan_timer.start();
		mp_dpi_run(state);

		mp_dpi_wait_end(state);
		mp_dpi_print_stats(state);
		scan_timer.stop();

		byte_scanner* bs;
		while(!scanner_pool->empty()){
			scanner_pool->pop((void**) &bs);
			delete bs;
		}
		/* And close the session */
		mp_dpi_terminate(state);
		delete scanner_pool;

		full_timer.stop();

		cout << "Completion time: "
				<<	setiosflags(ios_base::fixed) << setprecision(3)
				<< full_timer.real_time() << " seconds.\n";

		u_int32_t i;
		exit(EXIT_SUCCESS);
	}catch(exception const &ex){
		cout.flush();
		cerr << typeid(ex).name() << " exception: " << ex.what() << "\n";
		throw;
	}
}
<commit_msg>Added PF_RING demo<commit_after>/*
 * http_pm_mc_pfring.cc
 *
 * This demo application analyzes the HTTP traffic (reordering the TCP packets to have
 * a well-formed stream), and searches for specific patterns (contained into a file
 * specified by a command line parameter) inside the HTTP body.
 * The traffic is captured by using PF_RING (low latency network capture mechanism).
 *
 * Over high bandwidth networks, getting the timestamp for each received packet
 * could have a very negative impact over the bandwidth of the application. In order
 * to have low latency timestamping, in this example we perform it by reading the
 * CPU clock. To correctly perform this kind of timestamping the macro
 * CLOCK_FREQ must be set with the correct CPU frequency (in Hz) and the
 * 'cpu-freq' tool should be used to set the CPU frequency manager to 'performance'.
 *
 * For low bandwidth networks this is not really needed and time(NULL) or gettimeofday
 * can be used for timestamping.
 *
 * Created on: 29/08/2013
 *
 * =========================================================================
 *  Copyright (C) 2012-2013, Daniele De Sensi (d.desensi.software@gmail.com)
 *
 *  This file is part of Peafowl.
 *
 *  Peafowl is free software: you can redistribute it and/or
 *  modify it under the terms of the Lesser GNU General Public
 *  License as published by the Free Software Foundation, either
 *  version 3 of the License, or (at your option) any later version.

 *  Peafowl is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  Lesser GNU General Public License for more details.
 *
 *  You should have received a copy of the Lesser GNU General Public
 *  License along with Peafowl.
 *  If not, see <http://www.gnu.org/licenses/>.
 *
 * =========================================================================
 */

#define _POSIX_C_SOURCE 1
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <utility>
#include <typeinfo>
#include "pattern_matching_lib/timer.h"
#include "pattern_matching_lib/trie.h"
#include "pattern_matching_lib/signatures.h"
/** Starting with demo-only includes. **/
#include <mc_api.h>
#include <pcap.h>
#include <netinet/in.h>
#include <net/ethernet.h>
#include <stdint.h>
#include <ff/mapping_utils.hpp>
#include <ff/utils.hpp>
#include <ff/ubuffer.hpp>

// #define USE_PF_RING_CLUSTER  1

#define ALARM_SLEEP 10 //Interval (seconds) between two successive stats print

#include <time.h>
#include <signal.h>
extern "C" {
#include <pfring.h>
}

using namespace antivirus;
#define CAPACITY_CHUNK 1000
#define SCANNER_POOL_SIZE 8192  //4096
#define CLOCK_FREQ 2000000000L
#define TEST_DURATION_SECS 120

#define SNAPLEN 2000

#define AVAILABLE_CORES 16
static u_int16_t mapping[AVAILABLE_CORES]=
  {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};

static struct timeval startTime;
u_int64_t numPkts = 0, numBytes = 0;
pfring *ring;
ff::SWSR_Ptr_Buffer *pkt_buff;
int verbose = 0;
unsigned long num_pkt_buffers = 128000;
//unsigned long num_pkt_buffers = 32000;

static ff::uSWSR_Ptr_Buffer* scanner_pool;

mc_dpi_packet_reading_result_t reading_cb(void* user_data){
	static unsigned long last_ts=getticks();
	static u_int32_t last_sec=0;
#ifdef USE_PF_RING_CLUSTER
	pfring_pkt_buff *pkt_handle = NULL;
#else
	void *pkt_handle;
#endif
	struct pfring_pkthdr hdr;
	int rc;

	if(numPkts == 0) gettimeofday(&startTime, NULL);

	while(pkt_buff->empty()){;}
	/**
	if(pkt_buff->empty()) {
		printf("Not enough buffer available: please start over\n");
		exit(-1);
	}
	**/

	pkt_buff->pop(&pkt_handle);

	if(verbose) printf("pkt_buff->pop()\n");

	while(1) {
#ifdef USE_PF_RING_CLUSTER
		rc = pfring_recv_pkt_buff(ring, pkt_handle, &hdr, 1);
#else
		rc = pfring_recv(ring, (u_char**)&pkt_handle, SNAPLEN, &hdr, 1);
#endif

		if(rc < 0) {
			printf("pfring_recv_pkt_buff() returned %d\n", rc);
			exit(-1);
		} else if(rc == 0) {
			continue;
		} else {
			mc_dpi_packet_reading_result_t r;
			const unsigned char *p = (const unsigned char*)pkt_handle;

			r.pkt = &p[14];

			if(getticks()-last_ts>CLOCK_FREQ){
				last_ts = getticks();
				++last_sec;
			}
			r.current_time=last_sec;
			r.length=hdr.caplen-14;
			r.user_pointer=pkt_handle;

			numPkts++, numBytes += r.length; // hdr.len;
			return r;
		}
	} /* while */
}

void processing_cb(mc_dpi_processing_result_t* processing_result, void* user_data){
	pkt_buff->push((void*)processing_result->user_pointer);
	if(verbose) printf("pkt_buff->push()\n");
}

static void match_found(string::size_type position,
		trie::value_type const &match){
	using namespace std;
	cout << "Matched '" << match.second << "' at " << position << endl;
}

void body_cb(dpi_http_message_informations_t* http_informations,
		const u_char* app_data, u_int32_t data_length,
		dpi_pkt_infos_t* pkt,
		void** flow_specific_user_data, void* user_data,
		u_int8_t last){

	if(*flow_specific_user_data==NULL){

		if(scanner_pool->mc_pop(flow_specific_user_data)==false){
			*flow_specific_user_data=
					new byte_scanner(*((trie*) user_data), match_found);
		}
	}
	byte_scanner* scanner=(byte_scanner*) (*flow_specific_user_data);
	for(u_int32_t i=0; i<data_length; i++){
		scanner->match(app_data[i]);
	}
}

void flow_cleaner(void* flow_specific_user_data){
	if(scanner_pool->mp_push(flow_specific_user_data)==false){
		delete (byte_scanner*) flow_specific_user_data;
	}
}

/*
 * The time difference in millisecond
 */
double delta_time (struct timeval * now,
		struct timeval * before) {
	time_t delta_seconds;
	time_t delta_microseconds;

	/*
	 * compute delta in second, 1/10's and 1/1000's second units
	 */
	delta_seconds      = now -> tv_sec  - before -> tv_sec;
	delta_microseconds = now -> tv_usec - before -> tv_usec;

	if(delta_microseconds < 0) {
		/* manually carry a one from the seconds field */
		delta_microseconds += 1000000;  /* 1e6 */
		-- delta_seconds;
	}
	return((double)(delta_seconds * 1000) + (double)delta_microseconds/1000);
}

void print_stats() {
	pfring_stat pfringStat;
	struct timeval endTime;
	double deltaMillisecInterval, deltaMillisecSinceStart;
	static u_int64_t lastPkts = 0;
	static u_int64_t lastBytes = 0;
	u_int64_t diffPkts, diffBytes;
	static struct timeval lastTime;

	if(startTime.tv_sec == 0) return;

	gettimeofday(&endTime, NULL);

	if(pfring_stats(ring, &pfringStat) >= 0){
		fprintf(stderr,
				"Absolute Stats: [%lu pkts rcvd][%lu pkts dropped]\n"
				"Total Pkts=%lu/Dropped=%.1f %%\n",
				(unsigned long)pfringStat.recv, (unsigned long)pfringStat.drop,
				(unsigned long)(pfringStat.recv),
			pfringStat.recv == 0 ? 0 : (double)(pfringStat.drop*100)/(double)(pfringStat.recv+pfringStat.drop));
	}

	if(lastTime.tv_sec > 0) {
	        deltaMillisecSinceStart = delta_time(&endTime, &startTime);
		deltaMillisecInterval = delta_time(&endTime, &lastTime);
		diffPkts = numPkts-lastPkts;
		diffBytes = numBytes-lastBytes;

                fprintf(stderr, "==================Average=====================\n"
			"[%.1f pkt/sec][%.2f Gbit/sec]\n",
			((double)(numPkts*1000)/(double)(deltaMillisecSinceStart)),
			(double)8*numBytes/(double)(deltaMillisecSinceStart*1000000));

		fprintf(stderr, "==================Current=====================\n"
		                "[%.1f pkt/sec][%.2f Gbit/sec]\n",
         			((double)(diffPkts*1000)/(double)(deltaMillisecInterval)),
                         	(double)8*diffBytes/(double)(deltaMillisecInterval*1000000));
	}

	lastPkts = numPkts;
	lastBytes = numBytes;

	lastTime.tv_sec = endTime.tv_sec, lastTime.tv_usec = endTime.tv_usec;

	fprintf(stderr, "==============================================\n");
	if(deltaMillisecSinceStart/1000 > TEST_DURATION_SECS){
	  exit(-1);
	}
}

/* ******************************** */

void sigproc(int sig) {
	static int called = 0;

	if(called) return; else called = 1;

	print_stats();
	pfring_close(ring);
	exit(0);
}

void my_sigalarm(int sig) {
	print_stats();
	printf("\n");
	alarm(ALARM_SLEEP);
	signal(SIGALRM, my_sigalarm);
}

int main(int argc, char **argv){
	using namespace std;
	ff_mapThreadToCpu(mapping[0], -20);

	try {
		if (argc<4){
			cerr << "Usage: " << argv[0] <<
					" virus-signatures-file device "
					"par_degree\n";
			exit(EXIT_FAILURE);
		}

		string::size_type trie_depth=DEFAULT_TRIE_MAXIMUM_DEPTH;

		char const *virus_signatures_file_name=argv[1];
		char const *device=argv[2];
		u_int16_t num_workers=atoi(argv[3]);

		ifstream signatures;
		signatures.open(virus_signatures_file_name);
		if(!signatures){
			cerr << argv[0] << ": failed to open '" <<
					virus_signatures_file_name << "'\n";
			exit(EXIT_FAILURE);
		}

		signature_reader reader(signatures);

		cout << "reading '" << virus_signatures_file_name << "'... ";

		timer read_signatures_timer;
		read_signatures_timer.start();
		trie t(trie_depth);
		while (reader.next()) {
			t.insert(make_pair(reader.current_pattern(),
					reader.current_name()));
		}
		read_signatures_timer.stop();
		cout << setiosflags(ios_base::fixed) << setprecision(3) <<
				read_signatures_timer.real_time() << " seconds.\n";
		cout << "preparing '" << virus_signatures_file_name << "'... ";
		timer prepare_signatures_timer;
		prepare_signatures_timer.start();
		t.prepare();
		prepare_signatures_timer.stop();
		cout << setiosflags(ios_base::fixed)
		    		 << setprecision(3)
		    		 << prepare_signatures_timer.real_time() << " seconds.\n";

		cout << "# of allocated trie nodes: " << t.node_count() << endl;

		timer full_timer;
		full_timer.start();
		/******************************************************/
		/*             Start scanning the files.              */
		/******************************************************/

		mc_dpi_parallelism_details_t details;
		bzero(&details, sizeof(mc_dpi_parallelism_details_t));
		details.available_processors=AVAILABLE_CORES;
		details.mapping=mapping;

		details.single_farm_num_workers=num_workers;
		mc_dpi_library_state_t* state=mc_dpi_init_stateful(
				32767, 32767, 1000000, 1000000, details);

		int snaplen = SNAPLEN;
		int flags = PF_RING_PROMISC;

		ring = pfring_open(device, snaplen, flags);

		if(ring==NULL){
			fprintf(stderr, "Couldn't open device %s\n", argv[1]);
			return (2);
		}

		pfring_set_direction(ring, rx_only_direction);
		scanner_pool=new ff::uSWSR_Ptr_Buffer(SCANNER_POOL_SIZE, true);
		scanner_pool->init();
		for(uint i=0; i<SCANNER_POOL_SIZE; i++){
			scanner_pool->push(new byte_scanner(t, match_found));
		}
		//		mc_dpi_tcp_reordering_disable(state);
		mc_dpi_set_read_and_process_callbacks(
				state, &reading_cb, &processing_cb, (void*) NULL);
		mc_dpi_set_flow_cleaner_callback(state, &flow_cleaner);
		dpi_http_callbacks_t callback={0, 0, 0, 0, 0, &body_cb};
				assert(mc_dpi_http_activate_callbacks(state, &callback, (void*)(&t))==DPI_STATE_UPDATE_SUCCESS);

		pkt_buff = new ff::SWSR_Ptr_Buffer(num_pkt_buffers);
		if(pkt_buff == NULL) {
			printf("SWSR_Ptr_Buffer() failed\n");
			return(-1);
		}
		pkt_buff->init();

		for(int i=0; i<num_pkt_buffers; i++) {
#ifdef USE_PF_RING_CLUSTER
			pfring_pkt_buff *pkt_handle;

			if((pkt_handle = pfring_alloc_pkt_buff(ring)) == NULL) {
				printf("Error allocating pkt buff\n");
				return(-1);
			} else
				pkt_buff->push(pkt_handle);
#else
			pkt_buff->push(malloc(SNAPLEN));
			if(verbose) printf("pkt_buff->push() empty buffer\n");
#endif
		}

		signal(SIGINT, sigproc);
		signal(SIGALRM, my_sigalarm);
		alarm(ALARM_SLEEP);

		pfring_enable_ring(ring);

		timer scan_timer;
		scan_timer.start();
		mc_dpi_run(state);

		mc_dpi_wait_end(state);
		mc_dpi_print_stats(state);
		scan_timer.stop();

		byte_scanner* bs;
		while(!scanner_pool->empty()){
			scanner_pool->pop((void**) &bs);
			delete bs;
		}
		/* And close the session */
		mc_dpi_terminate(state);
		delete scanner_pool;

		full_timer.stop();

		cout << "Completion time: "
				<<	setiosflags(ios_base::fixed) << setprecision(3)
				<< full_timer.real_time() << " seconds.\n";

		u_int32_t i;
		exit(EXIT_SUCCESS);
	}catch(exception const &ex){
		cout.flush();
		cerr << typeid(ex).name() << " exception: " << ex.what() << "\n";
		throw;
	}
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 The University of Texas at Austin and the
 * Institute of Human Machine Cognition. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 2.1 of
 * the License, or (at your option) any later version. See
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#include <controlit/robot_interface_library/RobotInterfaceSM.hpp>

#include <chrono>
#include <controlit/Command.hpp>
#include <controlit/RTControlModel.hpp>
#include <controlit/logging/RealTimeLogging.hpp>
#include <controlit/robot_interface_library/OdometryStateReceiverSM.hpp>
#include <controlit/addons/ros/ROSMsgToString.hpp>

namespace controlit {
namespace robot_interface_library {

// Uncomment one of the following lines to enable/disable detailed debug statements.
#define PRINT_INFO_STATEMENT(ss)
// #define PRINT_INFO_STATEMENT(ss) CONTROLIT_INFO << ss;

#define PRINT_INFO_STATEMENT_RT(ss)
// #define PRINT_INFO_STATEMENT_RT(ss) CONTROLIT_INFO_RT << ss;

#define PRINT_INFO_RT(ss)
// #define PRINT_INFO_RT(ss) CONTROLIT_INFO_RT << ss;

#define PRINT_INFO_STATEMENT_RT_ALWAYS(ss) CONTROLIT_INFO_RT << ss;
// #define PRINT_INFO_STATEMENT_RT_ALWAYS(ss) std::cout << ss << std::endl;

#define PRINT_RECEIVED_STATE 0
#define PRINT_COMMAND 0

// Parameters for shared memory subscribers
#define LISTEN_TO_ROS_TOPIC false
#define USE_POLLING false

RobotInterfaceSM::RobotInterfaceSM() :
    RobotInterface(), // Call super-class' constructor
    receivedRobotState(false),
    robotStateSubscriber(LISTEN_TO_ROS_TOPIC, USE_POLLING),
    rttRxSubscriber(LISTEN_TO_ROS_TOPIC, USE_POLLING),
    rcvdJointState(false),
    rcvdInitStateMsg(false),
    rcvdInitRTTMsg(false),
    rcvdFirstStateMsg(false),
    rcvdFirstRTTMsg(false)
{
}

RobotInterfaceSM::~RobotInterfaceSM()
{
}

bool RobotInterfaceSM::init(ros::NodeHandle & nh, RTControlModel * model)
{
    PRINT_INFO_STATEMENT("Method called!");

    //---------------------------------------------------------------------------------
    // Initialize the parent class. Abort if parent class fails to initialize.
    //---------------------------------------------------------------------------------

    if(!RobotInterface::init(nh, model))
    {
        CONTROLIT_ERROR_RT << "Super-class failed to initialize.";
        return false;
    }

    //---------------------------------------------------------------------------------
    // Initialize a torque command message that will be used to hold the torque
    // command data.
    //---------------------------------------------------------------------------------

    const std::vector<std::string> & names = model->get()->getRealJointNamesVector();

    PRINT_INFO_STATEMENT("Number of DoFs: " << names.size());

    torqueCmdMsg.layout.dim.resize(names.size());
    torqueCmdMsg.layout.dim[0].stride = names.size();
    torqueCmdMsg.layout.dim[0].size = names.size();
    torqueCmdMsg.layout.dim[1].stride = 1;
    torqueCmdMsg.layout.dim[1].size = 1;
    torqueCmdMsg.data.resize(names.size());

    //---------------------------------------------------------------------------------
    // Initialize the shared memory publshers and subscribers.
    // There are two publishers:
    //
    //   1. command publisher on topic "/cmd"
    //   2. seqno publisher on topic "/rtt_tx"
    //
    // There are two subscribers:
    //
    //   1. robot state subscriber on topic "/joint_states"
    //   2. seqno receiver on topic "/rtt_rx"
    //---------------------------------------------------------------------------------

    cmdPublisher.advertise("/cmd");
    rttTxPublisher.advertise("/rtt_tx");

    rttRxSubscriber.subscribe("/rtt_rx", boost::bind(&RobotInterfaceSM::rttCallback, this, _1));
    robotStateSubscriber.subscribe("/joint_states", boost::bind(&RobotInterfaceSM::jointStateCallback, this, _1));

    // rttRxSubscriber.waitForMessage(rttRxMsg);
    // robotStateSubscriber.waitForMessage(jointStateMsg);

    // int checkCount = 0;

    // while (!rttRxSubscriber.connected())
    // {
    //     // if (checkCount++ < 300)
    //     // {
    //     //     // ROS_WARN_THROTTLE(1.0, "Waiting for /rtt_rx shared memory topic.");
    //     //     if (checkCount % 10 == 0)
    //     //     {
    //     //     CONTROLIT_WARN << "Waiting for shared memory topic /rtt_rx to be connected....";
    //     //     }
    //     //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    //     // }
    //     // else
    //     // {
    //     //     CONTROLIT_ERROR << "Failed to subscribe to shared memory topic \"/rtt_rx\"!";
    //     //     return false;
    //     // }
    // }
    // CONTROLIT_INFO << "Subscription to /rtt_rx connected.";

    // checkCount = 0;
    // while (!robotStateSubscriber.connected())
    // {
    //     // if (checkCount++ < 300)
    //     // {
    //     //     // ROS_WARN_THROTTLE(1.0, "Waiting for /joint_states shared memory topic.");
    //     //     if (checkCount % 10 == 0)
    //     //     {
    //     //     CONTROLIT_WARN << "Waiting for shared memory topic /joint_states to be connected...";
    //     //     }
    //     //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    //     // }
    //     // else
    //     // {
    //     //     CONTROLIT_ERROR << "Failed to subscribe to shared memory topic \"/joint_states\"!";
    //     //     return false;
    //     // }
    // }
    // CONTROLIT_INFO << "Subscription to /joint_states connected.";

    //---------------------------------------------------------------------------------
    // Initialize odometry receiver.
    //---------------------------------------------------------------------------------

    CONTROLIT_INFO << "Creating and initializing the odometry state receiver...";
    odometryStateReceiver.reset(new OdometryStateReceiverSM());
    if (odometryStateReceiver->init(nh, model))
    {
        CONTROLIT_INFO << "Publishing dummy cmd and RTT TX messages...";
        cmdPublisher.publish(torqueCmdMsg);  // Send a dumy message to initialize shared memory connection
        rttTxPublisher.publish(currentRTTMsg);  // Send a dumy message to initialize shared memory connection
        return true;
    } else
        return false;

}

void RobotInterfaceSM::rttCallback(std_msgs::Int64 & msg)
{
    if (!rcvdInitRTTMsg)
    {
        CONTROLIT_INFO_RT << "Received initial RTT message!";
        rcvdInitRTTMsg = true;
    }
    else
    {
        if (!rcvdFirstRTTMsg)
        {
            CONTROLIT_INFO_RT << "Received first RTT message!";
            rcvdFirstRTTMsg = true;
        }
        rttRxMsgMutex.lock();
        rttRxMsg = msg;
        rttRxMsgMutex.unlock();
    }
}

void RobotInterfaceSM::jointStateCallback(sensor_msgs::JointState & msg)
{
    if (!rcvdInitStateMsg)
    {
        CONTROLIT_INFO_RT << "Received initial state message!";
        rcvdInitStateMsg = true;
    }
    else
    {
        if (!rcvdFirstStateMsg)
        {
            CONTROLIT_INFO_RT << "Received first state message!";
            rcvdFirstStateMsg = true;
        }

        jointStateMutex.lock();

        jointStateMsg = msg;
        rcvdJointState = true;

        jointStateMutex.unlock();
    }
}

bool RobotInterfaceSM::read(controlit::RobotState & latestRobotState, bool block)
{
    bool result = true;

    //---------------------------------------------------------------------------------
    // Reset the timestamp within robot state to remember when the state was obtained.
    //---------------------------------------------------------------------------------
    latestRobotState.resetTimestamp();

    //---------------------------------------------------------------------------------
    // Check whether the sequence number was reflected.  If it was, compute and publish
    // the round trip communication latency.
    //---------------------------------------------------------------------------------
    rttRxMsgMutex.lock();
    bool seqnoReflected = (rttRxMsg.data == seqno);
    rttRxMsgMutex.unlock();

    if(seqnoReflected)
    {
        double latency = rttTimer->getTime();
        publishCommLatency(latency);
    }

    //---------------------------------------------------------------------------------
    // Process the latest joint state information.
    //---------------------------------------------------------------------------------

    jointStateMutex.lock();

    if (rcvdJointState)
    {
        //---------------------------------------------------------------------------------
        // Sanity check to make sure the received joint state information is of
        // the correct length.
        //---------------------------------------------------------------------------------

        if(latestRobotState.getNumJoints() != jointStateMsg.name.size())
        {
            CONTROLIT_ERROR << "Received joint state message of incorrect size!\n"
                << "  - expected: " << latestRobotState.getNumJoints() << "\n"
                << "  - received: " << jointStateMsg.name.size();
            return false;
        }

        //---------------------------------------------------------------------------------
        // Intialize jointNameMap if it has not already been initalized.
        // This mapping stores each joint's index within the shared memory.
        //---------------------------------------------------------------------------------

        if (jointNameMap.size() == 0)
        {
            for (unsigned int ii = 0; ii < jointStateMsg.name.size(); ii++)
            {
                jointNameMap[jointStateMsg.name[ii]] = ii;
            }
        }

        //---------------------------------------------------------------------------------
        // Save the latest joint state information.
        //---------------------------------------------------------------------------------

        for(unsigned int ii = 0; ii < latestRobotState.getNumJoints(); ii++)
        {
            std::string name = latestRobotState.getJointNames()[ii];

            unsigned int shmIndx;
            try {
                shmIndx = jointNameMap.at(name);
            }
            catch(std::out_of_range exception)
            {
                CONTROLIT_ERROR_RT << "Unknown joint name: " << name;
                result = false;
            }

            if (result)
            {
                // Update the joint state
                latestRobotState.setJointPosition(ii, jointStateMsg.position[shmIndx]);
                latestRobotState.setJointVelocity(ii, jointStateMsg.velocity[shmIndx]);
                latestRobotState.setJointEffort(ii, jointStateMsg.effort[shmIndx]);

                // TEMPORARY!
                if (std::abs(jointStateMsg.position[shmIndx]) > 1e3)
                {
                    std::cerr << "RobotInterfaceSM: questionable position: shmIndx = " << shmIndx << ", pos = " << jointStateMsg.position[shmIndx];
                    assert(false);
                }
            }
        }

        //---------------------------------------------------------------------------------
        // Check to ensure all receive states are valid.
        // In this case, valid values are those less than 1000.
        // This is optional and can be commented out to increase execution speed.
        //---------------------------------------------------------------------------------

        {
            bool isValid = true;
            for(unsigned int ii = 0; ii < jointStateMsg.name.size() && isValid; ii++)
            {
                if (std::abs(jointStateMsg.position[ii] >= 1e3)) isValid = false;
                if (std::abs(jointStateMsg.velocity[ii] >= 1e3)) isValid = false;
                if (std::abs(jointStateMsg.effort[ii] >= 1e3)) isValid = false;
            }

            if (!isValid)
                CONTROLIT_ERROR_RT
                    << "Received questionable robot state:\n"
                    << controlit::addons::ros::jointStateMsgToString(jointStateMsg);
        }

        #if PRINT_RECEIVED_STATE
        CONTROLIT_INFO_RT
            << "Received state:\n"
            << controlit::addons::ros::jointStateMsgToString(jointStateMsg);;
        #endif

    } else
        result = false;

    jointStateMutex.unlock();

    //---------------------------------------------------------------------------------
    // Abort if we failed to receive joint state.
    //---------------------------------------------------------------------------------

    if (!result) return false;

    //---------------------------------------------------------------------------------
    // Get and save the latest odometry data.
    //---------------------------------------------------------------------------------
    if (!odometryStateReceiver->getOdometry(latestRobotState, block))
        return false;

    //---------------------------------------------------------------------------------
    // Call the the parent class' read method.  This causes the latestrobot state
    // to be published.
    //---------------------------------------------------------------------------------

    return controlit::RobotInterface::read(latestRobotState, block);
}

bool RobotInterfaceSM::write(const controlit::Command & command)
{
    //---------------------------------------------------------------------------------
    // Abort if jointNameMap is not initialized. It will be initialized the first
    // time a state message is received.
    //---------------------------------------------------------------------------------

    if (jointNameMap.size() == 0) return false;

    // The command may have fewer joints than the shared memory joint name map because
    // of unactuated joints.
    assert(torqueCmdMsg.data.size() >= command.getNumDOFs());

    // The command only applies to actuated joints
    const std::vector<std::string> & controlit_names = model->get()->getActuatedJointNamesVector();

    // Save the command into the message
    for(unsigned int ii = 0; ii < command.getNumDOFs(); ii++)
    {
        int shmIndx = jointNameMap[controlit_names[ii]]; //TODO: make sure the string is in the map!

        // CONTROLIT_INFO << "Saving command, shmIndx = " << shmIndx;
        // position_cmds[shmIndx] = command.getPositionCmd()[ii];
        // velocity_cmds[shmIndx] = command.getVelocityCmd()[ii];
        // torque_cmds[shmIndx] = command.getEffortCmd()[ii];
        torqueCmdMsg.data[shmIndx] = command.getEffortCmd()[ii];

        // #if PRINT_COMMAND
        // // ss << "  Joint: " << controlit_names[ii] << ", position_cmd: " << position_cmds[ii]
        // //    << ", velocity_cmd: " << velocity_cmds[ii] << ", torque_cmd: " << torque_cmds[ii] << std::endl;
        // #endif
    }

    // Print command (useful for debugging purposes)
    {
        #if PRINT_COMMAND
        std::stringstream ss;

        for(unsigned int ii = 0; ii < command.getNumDOFs(); ii++)
        {
            int shmIndx = jointNameMap[controlit_names[ii]];
            ss << "  Joint: " << controlit_names[ii] << ", effort_cmd: " << torqueCmdMsg.data[shmIndx] << std::endl;
        }

        std::cerr << "Writing command: " << std::endl << ss.str();
        #endif
    }

    cmdPublisher.publish(torqueCmdMsg);

    //---------------------------------------------------------------------------------
    // If necessary, send the next RTT sequence number.
    //---------------------------------------------------------------------------------

    if(sendSeqno)
    {
        sendSeqno = false;
        currentRTTMsg.data = ++seqno;
        rttTimer->start();
        rttTxPublisher.publish(currentRTTMsg);
    }

    //---------------------------------------------------------------------------------
    // Call the the parent class' write method.  This causes the command to be
    // published.
    //---------------------------------------------------------------------------------

    return controlit::RobotInterface::write(command);
}

} // namespace robot_interface_library
} // namespace controlit
<commit_msg>Abort controller if bad joint state is received.<commit_after>/*
 * Copyright (C) 2015 The University of Texas at Austin and the
 * Institute of Human Machine Cognition. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 2.1 of
 * the License, or (at your option) any later version. See
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#include <controlit/robot_interface_library/RobotInterfaceSM.hpp>

#include <chrono>
#include <controlit/Command.hpp>
#include <controlit/RTControlModel.hpp>
#include <controlit/logging/RealTimeLogging.hpp>
#include <controlit/robot_interface_library/OdometryStateReceiverSM.hpp>
#include <controlit/addons/ros/ROSMsgToString.hpp>

namespace controlit {
namespace robot_interface_library {

// Uncomment one of the following lines to enable/disable detailed debug statements.
#define PRINT_INFO_STATEMENT(ss)
// #define PRINT_INFO_STATEMENT(ss) CONTROLIT_INFO << ss;

#define PRINT_INFO_STATEMENT_RT(ss)
// #define PRINT_INFO_STATEMENT_RT(ss) CONTROLIT_INFO_RT << ss;

#define PRINT_INFO_RT(ss)
// #define PRINT_INFO_RT(ss) CONTROLIT_INFO_RT << ss;

#define PRINT_INFO_STATEMENT_RT_ALWAYS(ss) CONTROLIT_INFO_RT << ss;
// #define PRINT_INFO_STATEMENT_RT_ALWAYS(ss) std::cout << ss << std::endl;

#define PRINT_RECEIVED_STATE 0
#define PRINT_COMMAND 0

// Parameters for shared memory subscribers
#define LISTEN_TO_ROS_TOPIC false
#define USE_POLLING false

RobotInterfaceSM::RobotInterfaceSM() :
    RobotInterface(), // Call super-class' constructor
    receivedRobotState(false),
    robotStateSubscriber(LISTEN_TO_ROS_TOPIC, USE_POLLING),
    rttRxSubscriber(LISTEN_TO_ROS_TOPIC, USE_POLLING),
    rcvdJointState(false),
    rcvdInitStateMsg(false),
    rcvdInitRTTMsg(false),
    rcvdFirstStateMsg(false),
    rcvdFirstRTTMsg(false)
{
}

RobotInterfaceSM::~RobotInterfaceSM()
{
}

bool RobotInterfaceSM::init(ros::NodeHandle & nh, RTControlModel * model)
{
    PRINT_INFO_STATEMENT("Method called!");

    //---------------------------------------------------------------------------------
    // Initialize the parent class. Abort if parent class fails to initialize.
    //---------------------------------------------------------------------------------

    if(!RobotInterface::init(nh, model))
    {
        CONTROLIT_ERROR_RT << "Super-class failed to initialize.";
        return false;
    }

    //---------------------------------------------------------------------------------
    // Initialize a torque command message that will be used to hold the torque
    // command data.
    //---------------------------------------------------------------------------------

    const std::vector<std::string> & names = model->get()->getRealJointNamesVector();

    PRINT_INFO_STATEMENT("Number of DoFs: " << names.size());

    torqueCmdMsg.layout.dim.resize(names.size());
    torqueCmdMsg.layout.dim[0].stride = names.size();
    torqueCmdMsg.layout.dim[0].size = names.size();
    torqueCmdMsg.layout.dim[1].stride = 1;
    torqueCmdMsg.layout.dim[1].size = 1;
    torqueCmdMsg.data.resize(names.size());

    //---------------------------------------------------------------------------------
    // Initialize the shared memory publshers and subscribers.
    // There are two publishers:
    //
    //   1. command publisher on topic "/cmd"
    //   2. seqno publisher on topic "/rtt_tx"
    //
    // There are two subscribers:
    //
    //   1. robot state subscriber on topic "/joint_states"
    //   2. seqno receiver on topic "/rtt_rx"
    //---------------------------------------------------------------------------------

    cmdPublisher.advertise("/cmd");
    rttTxPublisher.advertise("/rtt_tx");

    rttRxSubscriber.subscribe("/rtt_rx", boost::bind(&RobotInterfaceSM::rttCallback, this, _1));
    robotStateSubscriber.subscribe("/joint_states", boost::bind(&RobotInterfaceSM::jointStateCallback, this, _1));

    // rttRxSubscriber.waitForMessage(rttRxMsg);
    // robotStateSubscriber.waitForMessage(jointStateMsg);

    // int checkCount = 0;

    // while (!rttRxSubscriber.connected())
    // {
    //     // if (checkCount++ < 300)
    //     // {
    //     //     // ROS_WARN_THROTTLE(1.0, "Waiting for /rtt_rx shared memory topic.");
    //     //     if (checkCount % 10 == 0)
    //     //     {
    //     //     CONTROLIT_WARN << "Waiting for shared memory topic /rtt_rx to be connected....";
    //     //     }
    //     //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    //     // }
    //     // else
    //     // {
    //     //     CONTROLIT_ERROR << "Failed to subscribe to shared memory topic \"/rtt_rx\"!";
    //     //     return false;
    //     // }
    // }
    // CONTROLIT_INFO << "Subscription to /rtt_rx connected.";

    // checkCount = 0;
    // while (!robotStateSubscriber.connected())
    // {
    //     // if (checkCount++ < 300)
    //     // {
    //     //     // ROS_WARN_THROTTLE(1.0, "Waiting for /joint_states shared memory topic.");
    //     //     if (checkCount % 10 == 0)
    //     //     {
    //     //     CONTROLIT_WARN << "Waiting for shared memory topic /joint_states to be connected...";
    //     //     }
    //     //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    //     // }
    //     // else
    //     // {
    //     //     CONTROLIT_ERROR << "Failed to subscribe to shared memory topic \"/joint_states\"!";
    //     //     return false;
    //     // }
    // }
    // CONTROLIT_INFO << "Subscription to /joint_states connected.";

    //---------------------------------------------------------------------------------
    // Initialize odometry receiver.
    //---------------------------------------------------------------------------------

    CONTROLIT_INFO << "Creating and initializing the odometry state receiver...";
    odometryStateReceiver.reset(new OdometryStateReceiverSM());
    if (odometryStateReceiver->init(nh, model))
    {
        CONTROLIT_INFO << "Publishing dummy cmd and RTT TX messages...";
        cmdPublisher.publish(torqueCmdMsg);  // Send a dumy message to initialize shared memory connection
        rttTxPublisher.publish(currentRTTMsg);  // Send a dumy message to initialize shared memory connection
        return true;
    } else
        return false;

}

void RobotInterfaceSM::rttCallback(std_msgs::Int64 & msg)
{
    if (!rcvdInitRTTMsg)
    {
        CONTROLIT_INFO_RT << "Received initial RTT message!";
        rcvdInitRTTMsg = true;
    }
    else
    {
        if (!rcvdFirstRTTMsg)
        {
            CONTROLIT_INFO_RT << "Received first RTT message!";
            rcvdFirstRTTMsg = true;
        }
        rttRxMsgMutex.lock();
        rttRxMsg = msg;
        rttRxMsgMutex.unlock();
    }
}

void RobotInterfaceSM::jointStateCallback(sensor_msgs::JointState & msg)
{
    if (!rcvdInitStateMsg)
    {
        CONTROLIT_INFO_RT << "Received initial state message!";
        rcvdInitStateMsg = true;
    }
    else
    {
        if (!rcvdFirstStateMsg)
        {
            CONTROLIT_INFO_RT << "Received first state message!";
            rcvdFirstStateMsg = true;
        }

        jointStateMutex.lock();

        jointStateMsg = msg;
        rcvdJointState = true;

        jointStateMutex.unlock();
    }
}

bool RobotInterfaceSM::read(controlit::RobotState & latestRobotState, bool block)
{
    bool result = true;

    //---------------------------------------------------------------------------------
    // Reset the timestamp within robot state to remember when the state was obtained.
    //---------------------------------------------------------------------------------
    latestRobotState.resetTimestamp();

    //---------------------------------------------------------------------------------
    // Check whether the sequence number was reflected.  If it was, compute and publish
    // the round trip communication latency.
    //---------------------------------------------------------------------------------
    rttRxMsgMutex.lock();
    bool seqnoReflected = (rttRxMsg.data == seqno);
    rttRxMsgMutex.unlock();

    if(seqnoReflected)
    {
        double latency = rttTimer->getTime();
        publishCommLatency(latency);
    }

    //---------------------------------------------------------------------------------
    // Process the latest joint state information.
    //---------------------------------------------------------------------------------

    jointStateMutex.lock();

    if (rcvdJointState)
    {
        //---------------------------------------------------------------------------------
        // Sanity check to make sure the received joint state information is of
        // the correct length.
        //---------------------------------------------------------------------------------

        if(latestRobotState.getNumJoints() != jointStateMsg.name.size())
        {
            CONTROLIT_ERROR << "Received joint state message of incorrect size!\n"
                << "  - expected: " << latestRobotState.getNumJoints() << "\n"
                << "  - received: " << jointStateMsg.name.size();
            return false;
        }

        //---------------------------------------------------------------------------------
        // Intialize jointNameMap if it has not already been initalized.
        // This mapping stores each joint's index within the shared memory.
        //---------------------------------------------------------------------------------

        if (jointNameMap.size() == 0)
        {
            for (unsigned int ii = 0; ii < jointStateMsg.name.size(); ii++)
            {
                jointNameMap[jointStateMsg.name[ii]] = ii;
            }
        }

        //---------------------------------------------------------------------------------
        // Save the latest joint state information.
        //---------------------------------------------------------------------------------

        for(unsigned int ii = 0; ii < latestRobotState.getNumJoints(); ii++)
        {
            std::string name = latestRobotState.getJointNames()[ii];

            unsigned int shmIndx;
            try {
                shmIndx = jointNameMap.at(name);
            }
            catch(std::out_of_range exception)
            {
                CONTROLIT_ERROR_RT << "Unknown joint name: " << name;
                result = false;
            }

            if (result)
            {
                // Update the joint state
                latestRobotState.setJointPosition(ii, jointStateMsg.position[shmIndx]);
                latestRobotState.setJointVelocity(ii, jointStateMsg.velocity[shmIndx]);
                latestRobotState.setJointEffort(ii, jointStateMsg.effort[shmIndx]);

                // TEMPORARY!
                if (std::abs(jointStateMsg.position[shmIndx]) > 1e3)
                {
                    std::cerr << "RobotInterfaceSM: questionable position: shmIndx = " << shmIndx << ", pos = " << jointStateMsg.position[shmIndx];
                    assert(false);
                }
            }
        }

        //---------------------------------------------------------------------------------
        // Check to ensure all receive states are valid.
        // In this case, valid values are those less than 1000.
        // This is optional and can be commented out to increase execution speed.
        //---------------------------------------------------------------------------------

        {
            bool isValid = true;
            for(unsigned int ii = 0; ii < jointStateMsg.name.size() && isValid; ii++)
            {
                if (std::abs(jointStateMsg.position[ii] >= 1e3)) isValid = false;
                if (std::abs(jointStateMsg.velocity[ii] >= 1e3)) isValid = false;
                if (std::abs(jointStateMsg.effort[ii] >= 1e3)) isValid = false;
            }

            if (!isValid)
            {
                CONTROLIT_ERROR_RT
                    << "Received questionable robot state:\n"
                    << controlit::addons::ros::jointStateMsgToString(jointStateMsg);
                assert(false);
            }
        }

        #if PRINT_RECEIVED_STATE
        CONTROLIT_INFO_RT
            << "Received state:\n"
            << controlit::addons::ros::jointStateMsgToString(jointStateMsg);;
        #endif

    } else
        result = false;

    jointStateMutex.unlock();

    //---------------------------------------------------------------------------------
    // Abort if we failed to receive joint state.
    //---------------------------------------------------------------------------------

    if (!result) return false;

    //---------------------------------------------------------------------------------
    // Get and save the latest odometry data.
    //---------------------------------------------------------------------------------
    if (!odometryStateReceiver->getOdometry(latestRobotState, block))
        return false;

    //---------------------------------------------------------------------------------
    // Call the the parent class' read method.  This causes the latestrobot state
    // to be published.
    //---------------------------------------------------------------------------------

    return controlit::RobotInterface::read(latestRobotState, block);
}

bool RobotInterfaceSM::write(const controlit::Command & command)
{
    //---------------------------------------------------------------------------------
    // Abort if jointNameMap is not initialized. It will be initialized the first
    // time a state message is received.
    //---------------------------------------------------------------------------------

    if (jointNameMap.size() == 0) return false;

    // The command may have fewer joints than the shared memory joint name map because
    // of unactuated joints.
    assert(torqueCmdMsg.data.size() >= command.getNumDOFs());

    // The command only applies to actuated joints
    const std::vector<std::string> & controlit_names = model->get()->getActuatedJointNamesVector();

    // Save the command into the message
    for(unsigned int ii = 0; ii < command.getNumDOFs(); ii++)
    {
        int shmIndx = jointNameMap[controlit_names[ii]]; //TODO: make sure the string is in the map!

        // CONTROLIT_INFO << "Saving command, shmIndx = " << shmIndx;
        // position_cmds[shmIndx] = command.getPositionCmd()[ii];
        // velocity_cmds[shmIndx] = command.getVelocityCmd()[ii];
        // torque_cmds[shmIndx] = command.getEffortCmd()[ii];
        torqueCmdMsg.data[shmIndx] = command.getEffortCmd()[ii];

        // #if PRINT_COMMAND
        // // ss << "  Joint: " << controlit_names[ii] << ", position_cmd: " << position_cmds[ii]
        // //    << ", velocity_cmd: " << velocity_cmds[ii] << ", torque_cmd: " << torque_cmds[ii] << std::endl;
        // #endif
    }

    // Print command (useful for debugging purposes)
    {
        #if PRINT_COMMAND
        std::stringstream ss;

        for(unsigned int ii = 0; ii < command.getNumDOFs(); ii++)
        {
            int shmIndx = jointNameMap[controlit_names[ii]];
            ss << "  Joint: " << controlit_names[ii] << ", effort_cmd: " << torqueCmdMsg.data[shmIndx] << std::endl;
        }

        std::cerr << "Writing command: " << std::endl << ss.str();
        #endif
    }

    cmdPublisher.publish(torqueCmdMsg);

    //---------------------------------------------------------------------------------
    // If necessary, send the next RTT sequence number.
    //---------------------------------------------------------------------------------

    if(sendSeqno)
    {
        sendSeqno = false;
        currentRTTMsg.data = ++seqno;
        rttTimer->start();
        rttTxPublisher.publish(currentRTTMsg);
    }

    //---------------------------------------------------------------------------------
    // Call the the parent class' write method.  This causes the command to be
    // published.
    //---------------------------------------------------------------------------------

    return controlit::RobotInterface::write(command);
}

} // namespace robot_interface_library
} // namespace controlit
<|endoftext|>


<commit_before>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <atlbase.h>

#include "base/logging.h"
#include "chrome_frame/crash_reporting/vectored_handler-impl.h"
#include "chrome_frame/crash_reporting/veh_test.h"
#include "testing/gmock/include/gmock/gmock.h"
#include "testing/gtest/include/gtest/gtest.h"

using testing::_;

ACTION_P(StackTraceDump, s) {
  memcpy(arg2, s->stack_, s->count_ * sizeof(s->stack_[0]));
  return s->count_;
}
namespace {
class MockApi : public Win32VEHTraits, public ModuleOfInterest {
 public:
  MockApi() {
    Win32VEHTraits::InitializeIgnoredBlocks();
  }

  MOCK_METHOD1(WriteDump, void(const EXCEPTION_POINTERS*));
  MOCK_METHOD0(RtlpGetExceptionList, const EXCEPTION_REGISTRATION_RECORD*());
  MOCK_METHOD4(RtlCaptureStackBackTrace, WORD(DWORD FramesToSkip,
      DWORD FramesToCapture, void** BackTrace, DWORD* BackTraceHash));

  // Helpers
  void SetSEH(const SEHChain& sehchain) {
    EXPECT_CALL(*this, RtlpGetExceptionList())
        .Times(testing::AnyNumber())
        .WillRepeatedly(testing::Return(sehchain.chain_));
  }

  void SetStack(const StackHelper& s) {
    EXPECT_CALL(*this, RtlCaptureStackBackTrace(_, _, _, _))
        .Times(testing::AnyNumber())
        .WillRepeatedly(StackTraceDump(&s));
  }

  enum {max_back_trace = 15};
};
};  // namespace

typedef VectoredHandlerT<MockApi> VectoredHandlerMock;
TEST(ChromeFrame, ExceptionReport) {
  MockApi api;
  VectoredHandlerMock veh(&api);

  // Start address of our module.
  char* s = reinterpret_cast<char*>(0x30000000);
  char *e = s + 0x10000;
  api.SetModule(s, e);

  SEHChain have_seh(s - 0x1000, s + 0x1000, e + 0x7127, NULL);
  SEHChain no_seh(s - 0x1000, e + 0x7127, NULL);
  StackHelper on_stack(s - 0x11283, s - 0x278361, s + 0x9171, e + 1231, NULL);
  StackHelper not_on_stack(s - 0x11283, s - 0x278361, e + 1231, NULL);

  char* our_code = s + 0x1111;
  char* not_our_code = s - 0x5555;
  char* not_our_code2 = e + 0x5555;

  // Exception in our code, but we have SEH filter => no dump.
  api.SetSEH(have_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
            veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // RPC_E_DISCONNECTED (0x80010108) is "The object invoked has disconnected
  // from its clients", shall not be caught since it's a warning only.
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(RPC_E_DISCONNECTED, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, not in our code, we do not have SEH and we are not in stack.
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_INTEGER_DIVIDE_BY_ZERO, not_our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, not in our code, no SEH, but we are on the stack.
  api.SetSEH(no_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(1);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_INTEGER_DIVIDE_BY_ZERO,
                                 not_our_code2)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in our code, no SEH, not on stack (assume FPO screwed us)
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(1);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_PRIVILEGED_INSTRUCTION, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in IsBadStringPtrA, we are on the stack.
  api.SetSEH(no_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  char* ignore_address = reinterpret_cast<char*>(GetProcAddress(
      GetModuleHandleA("kernel32.dll"), "IsBadStringPtrA")) + 10;
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, ignore_address)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in IsBadStringPtrA, we are not on the stack.
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, ignore_address)));
  testing::Mock::VerifyAndClearExpectations(&api);
}

MATCHER_P(ExceptionCodeIs, code, "") {
  return (arg->ExceptionRecord->ExceptionCode == code);
}

void OverflowStack() {
  char tmp[1024 * 2048];
  ZeroMemory(tmp, sizeof(tmp));
}

DWORD WINAPI CrashingThread(PVOID tmp) {
  __try {
    OverflowStack();
  } __except(EXCEPTION_EXECUTE_HANDLER) {

  }

  // This will cause STATUS_ACCESS_VIOLATION
  __try {
    OverflowStack();
  } __except(EXCEPTION_EXECUTE_HANDLER) {

  }

  return 0;
}

static VectoredHandlerMock* g_mock_veh = NULL;
static LONG WINAPI VEH(EXCEPTION_POINTERS* exptrs) {
  return g_mock_veh->Handler(exptrs);
}

TEST(ChromeFrame, TrickyStackOverflow) {
  MockApi api;
  VectoredHandlerMock veh(&api);

  // Start address of our module.
  char* s = reinterpret_cast<char*>(0x30000000);
  char *e = s + 0x10000;
  api.SetModule(s, e);

  SEHChain no_seh(s - 0x1000, e + 0x7127, NULL);
  StackHelper on_stack(s - 0x11283, s - 0x278361, s + 0x9171, e + 1231, NULL);
  api.SetSEH(no_seh);
  api.SetStack(on_stack);

  EXPECT_CALL(api, WriteDump(ExceptionCodeIs(STATUS_STACK_OVERFLOW))).Times(1);

  g_mock_veh = &veh;
  void* id = ::AddVectoredExceptionHandler(FALSE, VEH);

  DWORD tid;
  HANDLE h = ::CreateThread(0, 0, CrashingThread, 0, 0, &tid);
  ::WaitForSingleObject(h, INFINITE);
  ::CloseHandle(h);

  EXPECT_EQ(2, veh.get_exceptions_seen());
  ::RemoveVectoredExceptionHandler(id);
  g_mock_veh = NULL;
}
<commit_msg>Committing for stoyan since we need this now...<commit_after>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <atlbase.h>

#include "base/logging.h"
#include "chrome_frame/crash_reporting/vectored_handler-impl.h"
#include "chrome_frame/crash_reporting/veh_test.h"
#include "testing/gmock/include/gmock/gmock.h"
#include "testing/gtest/include/gtest/gtest.h"

using testing::_;

ACTION_P(StackTraceDump, s) {
  memcpy(arg2, s->stack_, s->count_ * sizeof(s->stack_[0]));
  return s->count_;
}
namespace {
class MockApi : public Win32VEHTraits, public ModuleOfInterest {
 public:
  MockApi() {
    Win32VEHTraits::InitializeIgnoredBlocks();
  }

  MOCK_METHOD1(WriteDump, void(const EXCEPTION_POINTERS*));
  MOCK_METHOD0(RtlpGetExceptionList, const EXCEPTION_REGISTRATION_RECORD*());
  MOCK_METHOD4(RtlCaptureStackBackTrace, WORD(DWORD FramesToSkip,
      DWORD FramesToCapture, void** BackTrace, DWORD* BackTraceHash));

  // Helpers
  void SetSEH(const SEHChain& sehchain) {
    EXPECT_CALL(*this, RtlpGetExceptionList())
        .Times(testing::AnyNumber())
        .WillRepeatedly(testing::Return(sehchain.chain_));
  }

  void SetStack(const StackHelper& s) {
    EXPECT_CALL(*this, RtlCaptureStackBackTrace(_, _, _, _))
        .Times(testing::AnyNumber())
        .WillRepeatedly(StackTraceDump(&s));
  }

  enum {max_back_trace = 15};
};
};  // namespace

typedef VectoredHandlerT<MockApi> VectoredHandlerMock;
TEST(ChromeFrame, ExceptionReport) {
  MockApi api;
  VectoredHandlerMock veh(&api);

  // Start address of our module.
  char* s = reinterpret_cast<char*>(0x30000000);
  char *e = s + 0x10000;
  api.SetModule(s, e);

  SEHChain have_seh(s - 0x1000, s + 0x1000, e + 0x7127, NULL);
  SEHChain no_seh(s - 0x1000, e + 0x7127, NULL);
  StackHelper on_stack(s - 0x11283, s - 0x278361, s + 0x9171, e + 1231, NULL);
  StackHelper not_on_stack(s - 0x11283, s - 0x278361, e + 1231, NULL);

  char* our_code = s + 0x1111;
  char* not_our_code = s - 0x5555;
  char* not_our_code2 = e + 0x5555;

  // Exception in our code, but we have SEH filter => no dump.
  api.SetSEH(have_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
            veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // RPC_E_DISCONNECTED (0x80010108) is "The object invoked has disconnected
  // from its clients", shall not be caught since it's a warning only.
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(RPC_E_DISCONNECTED, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, not in our code, we do not have SEH and we are not in stack.
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_INTEGER_DIVIDE_BY_ZERO, not_our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, not in our code, no SEH, but we are on the stack.
  api.SetSEH(no_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(1);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_INTEGER_DIVIDE_BY_ZERO,
                                 not_our_code2)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in our code, no SEH, not on stack (assume FPO screwed us)
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(1);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_PRIVILEGED_INSTRUCTION, our_code)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in IsBadStringPtrA, we are on the stack.
  api.SetSEH(no_seh);
  api.SetStack(on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  char* ignore_address = reinterpret_cast<char*>(GetProcAddress(
      GetModuleHandleA("kernel32.dll"), "IsBadStringPtrA")) + 10;
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, ignore_address)));
  testing::Mock::VerifyAndClearExpectations(&api);

  // Exception, in IsBadStringPtrA, we are not on the stack.
  api.SetSEH(no_seh);
  api.SetStack(not_on_stack);
  EXPECT_CALL(api, WriteDump(_)).Times(0);
  EXPECT_EQ(ExceptionContinueSearch,
      veh.Handler(&ExceptionInfo(STATUS_ACCESS_VIOLATION, ignore_address)));
  testing::Mock::VerifyAndClearExpectations(&api);
}

MATCHER_P(ExceptionCodeIs, code, "") {
  return (arg->ExceptionRecord->ExceptionCode == code);
}

DECLSPEC_NOINLINE static void OverflowStack() {
  char tmp[1024 * 2048] = {0};
  GetSystemInfo(reinterpret_cast<SYSTEM_INFO*>(&tmp));
}

DWORD WINAPI CrashingThread(PVOID tmp) {
  __try {
    OverflowStack();
  } __except(EXCEPTION_EXECUTE_HANDLER) {

  }

  // This will cause STATUS_ACCESS_VIOLATION
  __try {
    OverflowStack();
  } __except(EXCEPTION_EXECUTE_HANDLER) {

  }

  return 0;
}

static VectoredHandlerMock* g_mock_veh = NULL;
static LONG WINAPI VEH(EXCEPTION_POINTERS* exptrs) {
  return g_mock_veh->Handler(exptrs);
}

TEST(ChromeFrame, TrickyStackOverflow) {
  MockApi api;
  VectoredHandlerMock veh(&api);

  // Start address of our module.
  char* s = reinterpret_cast<char*>(0x30000000);
  char *e = s + 0x10000;
  api.SetModule(s, e);

  SEHChain no_seh(s - 0x1000, e + 0x7127, NULL);
  StackHelper on_stack(s - 0x11283, s - 0x278361, s + 0x9171, e + 1231, NULL);
  api.SetSEH(no_seh);
  api.SetStack(on_stack);

  EXPECT_CALL(api, WriteDump(ExceptionCodeIs(STATUS_STACK_OVERFLOW))).Times(1);

  g_mock_veh = &veh;
  void* id = ::AddVectoredExceptionHandler(FALSE, VEH);

  DWORD tid;
  HANDLE h = ::CreateThread(0, 0, CrashingThread, 0, 0, &tid);
  ::WaitForSingleObject(h, INFINITE);
  ::CloseHandle(h);

  EXPECT_EQ(2, veh.get_exceptions_seen());
  ::RemoveVectoredExceptionHandler(id);
  g_mock_veh = NULL;
}
<|endoftext|>


<commit_before>/*
    SWARM

    Copyright (C) 2012-2021 Torbjorn Rognes and Frederic Mahe

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

    Contact: Torbjorn Rognes <torognes@ifi.uio.no>,
    Department of Informatics, University of Oslo,
    PO Box 1080 Blindern, NO-0316 Oslo, Norway
*/

#include "swarm.h"

struct bucket
{
  uint64_t hash;
  unsigned int seqno_first;
  unsigned int seqno_last;
  uint64_t mass;
  unsigned int size;
  unsigned int singletons;
};

auto derep_compare(const void * a, const void * b) -> int;

auto derep_compare(const void * a, const void * b) -> int
{
  const auto * x = static_cast<const struct bucket *>(a);
  const auto * y = static_cast<const struct bucket *>(b);
  int status {0};

  /* highest abundance first, otherwise keep order */

  if (x->mass < y->mass) {
    status = +1;
  }
  else if (x->mass > y->mass) {
    status = -1;
  }
  else {
    if (x->seqno_first < y->seqno_first) {
      status = -1;
    }
    else if (x->seqno_first > y->seqno_first) {
      status = +1;
    }
    else {
      status = 0;
    }
  }

  return status;
}

auto compute_hashtable_size(const uint64_t sequence_count) -> uint64_t {
  /* adjust size of hash table for 2/3 fill rate */
  constexpr unsigned int hashfillpct {70};
  constexpr unsigned int one_hundred_pct {100};
  uint64_t hashtablesize {1};
  // db seqs > 70% hash table size (avoid division to keep working with ints)
  while (one_hundred_pct * sequence_count > hashfillpct * hashtablesize) {
    hashtablesize <<= 1;
  }
  return hashtablesize;
}

auto write_stats_file(const uint64_t swarmcount,
                      struct Parameters const & p,
                      struct bucket * hashtable) -> void {
  progress_init("Writing stats:    ", swarmcount);
  for(auto i = 0ULL; i < swarmcount; i++)
    {
      struct bucket * sp = hashtable + i;
      fprintf(statsfile, "%u\t%" PRIu64 "\t", sp->size, sp->mass);
      fprint_id_noabundance(statsfile, sp->seqno_first, p.opt_usearch_abundance);
      fprintf(statsfile, "\t%" PRIu64 "\t%u\t%u\t%u\n",
              db_getabundance(sp->seqno_first),
              sp->singletons, 0U, 0U);
      progress_update(i);
    }
  progress_done();
}

auto write_structure_file(const uint64_t swarmcount,
                          struct Parameters const & p,
                          struct bucket * hashtable,
                          unsigned int * nextseqtab) -> void {
  progress_init("Writing structure:", swarmcount);

  for(uint64_t i = 0; i < swarmcount; i++)
    {
      struct bucket * sp = hashtable + i;
      uint64_t seed = sp->seqno_first;
      unsigned int a = nextseqtab[seed];
      while (a != 0U)
        {
          fprint_id_noabundance(internal_structure_file, seed, p.opt_usearch_abundance);
          fprintf(internal_structure_file, "\t");
          fprint_id_noabundance(internal_structure_file, a, p.opt_usearch_abundance);
          fprintf(internal_structure_file, "\t%d\t%" PRIu64 "\t%d\n", 0, i+1, 0);
          a = nextseqtab[a];
        }
      progress_update(i);
    }
  progress_done();
}

auto write_swarms_uclust_format(const uint64_t swarmcount,
                                struct Parameters const & p,
                                struct bucket * hashtable,
                                unsigned int * nextseqtab) -> void {
  progress_init("Writing UCLUST:   ", swarmcount);

  for(auto swarmid = 0U; swarmid < swarmcount; swarmid++)
    {
      struct bucket * bp = hashtable + swarmid;

      unsigned int seed = bp->seqno_first;

      fprintf(uclustfile, "C\t%u\t%u\t*\t*\t*\t*\t*\t",
              swarmid,
              bp->size);
      fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      fprintf(uclustfile, "\t*\n");

      fprintf(uclustfile, "S\t%u\t%u\t*\t*\t*\t*\t*\t",
              swarmid,
              db_getsequencelen(seed));
      fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      fprintf(uclustfile, "\t*\n");

      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fprintf(uclustfile,
                  "H\t%u\t%u\t%.1f\t+\t0\t0\t%s\t",
                  swarmid,
                  db_getsequencelen(a),
                  100.0,
                  "=");
          fprint_id(uclustfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          fprintf(uclustfile, "\t");
          fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
          fprintf(uclustfile, "\n");
          a = nextseqtab[a];
        }

      progress_update(swarmid+1);
    }
  progress_done();
}

auto write_representative_sequences(const uint64_t swarmcount,
                                    struct Parameters const & p,
                                    struct bucket * hashtable) -> void {
  progress_init("Writing seeds:    ", swarmcount);
  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fprintf(fp_seeds, ">");
      fprint_id_with_new_abundance(fp_seeds, seed, hashtable[i].mass, p.opt_usearch_abundance);
      fprintf(fp_seeds, "\n");
      db_fprintseq(fp_seeds, seed, 0);
      progress_update(i+1);
    }
  progress_done();
}

auto write_swarms_mothur_format(const uint64_t swarmcount,
                                struct Parameters const & p,
                                struct bucket * hashtable,
                                unsigned int * nextseqtab) -> void {
  progress_init("Writing swarms:   ", swarmcount);
  fprintf(outfile, "swarm_%" PRId64 "\t%" PRIu64, p.opt_differences, swarmcount);

  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fputc('\t', outfile);
      fprint_id(outfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fputc(',', outfile);
          fprint_id(outfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          a = nextseqtab[a];
        }

      progress_update(i+1);
    }
    fputc('\n', outfile);

  progress_done();
}

auto write_swarms_default_format(const uint64_t swarmcount,
                                 struct Parameters const & p,
                                 struct bucket * hashtable,
                                 unsigned int * nextseqtab) -> void {
  progress_init("Writing swarms:   ", swarmcount);
  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fprint_id(outfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fputc(sepchar, outfile);
          fprint_id(outfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          a = nextseqtab[a];
        }
      fputc('\n', outfile);
      progress_update(i+1);
    }

  progress_done();
}

void dereplicate(struct Parameters const & p)
{
  const uint64_t dbsequencecount = db_getsequencecount();
  const uint64_t hashtablesize {compute_hashtable_size(dbsequencecount)};
  const uint64_t derep_hash_mask = hashtablesize - 1;

  auto * hashtable =
    static_cast<struct bucket *>(xmalloc(sizeof(bucket) * hashtablesize));

  memset(hashtable, 0, sizeof(bucket) * hashtablesize);

  uint64_t swarmcount = 0;
  uint64_t maxmass = 0;
  unsigned int maxsize = 0;

  /* alloc and init table of links to other sequences in cluster */
  auto * nextseqtab = static_cast<unsigned int *>
    (xmalloc(sizeof(unsigned int) * dbsequencecount));
  memset(nextseqtab, 0, sizeof(unsigned int) * dbsequencecount);

  progress_init("Dereplicating:    ", dbsequencecount);

  for(auto i = 0U; i < dbsequencecount; i++)
    {
      unsigned int seqlen = db_getsequencelen(i);
      char * seq = db_getsequence(i);

      /*
        Find free bucket or bucket for identical sequence.
        Make sure sequences are exactly identical
        in case of any hash collision.
        With 64-bit hashes, there is about 50% chance of a
        collision when the number of sequences is about 5e9.
      */

      uint64_t hash = zobrist_hash(reinterpret_cast<unsigned char *>(seq),
                                   seqlen);

      uint64_t j = hash & derep_hash_mask;
      struct bucket * bp = hashtable + j;

      while (((bp->mass) != 0U) &&
             ((bp->hash != hash) ||
              (seqlen != db_getsequencelen(bp->seqno_first)) ||
              (memcmp(seq,
                      db_getsequence(bp->seqno_first),
                      nt_bytelength(seqlen)) != 0)))
        {
          bp++;
          j++;
          if (bp >= hashtable + hashtablesize)
            {
              bp = hashtable;
              j = 0;
            }
        }

      uint64_t ab = db_getabundance(i);

      if ((bp->mass) != 0U)
        {
          /* at least one identical sequence already */
          nextseqtab[bp->seqno_last] = i;
        }
      else
        {
          /* no identical sequences yet, start a new cluster */
          swarmcount++;
          bp->hash = hash;
          bp->seqno_first = i;
          bp->size = 0;
          bp->singletons = 0;
        }

      bp->size++;
      bp->seqno_last = i;
      bp->mass += ab;

      if (ab == 1) {
        bp->singletons++;
      }

      if (bp->mass > maxmass) {
        maxmass = bp->mass;
      }

      if (bp->size > maxsize) {
        maxsize = bp->size;
      }

      progress_update(i);
    }
  progress_done();

  progress_init("Sorting:          ", 1);
  qsort(hashtable, hashtablesize, sizeof(bucket), derep_compare);
  progress_done();

  /* dump swarms */
  if (p.opt_mothur) {
    write_swarms_mothur_format(swarmcount, p, hashtable, nextseqtab);
  }
  else {
    write_swarms_default_format(swarmcount, p, hashtable, nextseqtab);
  }

  /* dump seeds in fasta format with sum of abundances */
  if (not p.opt_seeds.empty()) {
    write_representative_sequences(swarmcount, p, hashtable);
  }

  /* output swarm in uclust format */
  if (uclustfile != nullptr) {
    write_swarms_uclust_format(swarmcount, p, hashtable, nextseqtab);
  }

  /* output internal structure to file */
  if (not p.opt_internal_structure.empty()) {
    write_structure_file(swarmcount, p, hashtable, nextseqtab);
  }

  /* output statistics to file */
  if (statsfile != nullptr) {
    write_stats_file(swarmcount, p, hashtable);
  }

  fprintf(logfile, "\n");
  fprintf(logfile, "Number of swarms:  %" PRIu64 "\n", swarmcount);
  fprintf(logfile, "Largest swarm:     %u\n", maxsize);
  fprintf(logfile, "Heaviest swarm:    %" PRIu64 "\n", maxmass);

  xfree(nextseqtab);
  xfree(hashtable);
}
<commit_msg>remove unnecessary function declaration<commit_after>/*
    SWARM

    Copyright (C) 2012-2021 Torbjorn Rognes and Frederic Mahe

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

    Contact: Torbjorn Rognes <torognes@ifi.uio.no>,
    Department of Informatics, University of Oslo,
    PO Box 1080 Blindern, NO-0316 Oslo, Norway
*/

#include "swarm.h"

struct bucket
{
  uint64_t hash;
  unsigned int seqno_first;
  unsigned int seqno_last;
  uint64_t mass;
  unsigned int size;
  unsigned int singletons;
};

auto derep_compare(const void * a, const void * b) -> int
{
  const auto * x = static_cast<const struct bucket *>(a);
  const auto * y = static_cast<const struct bucket *>(b);
  int status {0};

  /* highest abundance first, otherwise keep order */

  if (x->mass < y->mass) {
    status = +1;
  }
  else if (x->mass > y->mass) {
    status = -1;
  }
  else {
    if (x->seqno_first < y->seqno_first) {
      status = -1;
    }
    else if (x->seqno_first > y->seqno_first) {
      status = +1;
    }
    else {
      status = 0;
    }
  }

  return status;
}

auto compute_hashtable_size(const uint64_t sequence_count) -> uint64_t {
  /* adjust size of hash table for 2/3 fill rate */
  constexpr unsigned int hashfillpct {70};
  constexpr unsigned int one_hundred_pct {100};
  uint64_t hashtablesize {1};
  // db seqs > 70% hash table size (avoid division to keep working with ints)
  while (one_hundred_pct * sequence_count > hashfillpct * hashtablesize) {
    hashtablesize <<= 1;
  }
  return hashtablesize;
}

auto write_stats_file(const uint64_t swarmcount,
                      struct Parameters const & p,
                      struct bucket * hashtable) -> void {
  progress_init("Writing stats:    ", swarmcount);
  for(auto i = 0ULL; i < swarmcount; i++)
    {
      struct bucket * sp = hashtable + i;
      fprintf(statsfile, "%u\t%" PRIu64 "\t", sp->size, sp->mass);
      fprint_id_noabundance(statsfile, sp->seqno_first, p.opt_usearch_abundance);
      fprintf(statsfile, "\t%" PRIu64 "\t%u\t%u\t%u\n",
              db_getabundance(sp->seqno_first),
              sp->singletons, 0U, 0U);
      progress_update(i);
    }
  progress_done();
}

auto write_structure_file(const uint64_t swarmcount,
                          struct Parameters const & p,
                          struct bucket * hashtable,
                          unsigned int * nextseqtab) -> void {
  progress_init("Writing structure:", swarmcount);

  for(uint64_t i = 0; i < swarmcount; i++)
    {
      struct bucket * sp = hashtable + i;
      uint64_t seed = sp->seqno_first;
      unsigned int a = nextseqtab[seed];
      while (a != 0U)
        {
          fprint_id_noabundance(internal_structure_file, seed, p.opt_usearch_abundance);
          fprintf(internal_structure_file, "\t");
          fprint_id_noabundance(internal_structure_file, a, p.opt_usearch_abundance);
          fprintf(internal_structure_file, "\t%d\t%" PRIu64 "\t%d\n", 0, i+1, 0);
          a = nextseqtab[a];
        }
      progress_update(i);
    }
  progress_done();
}

auto write_swarms_uclust_format(const uint64_t swarmcount,
                                struct Parameters const & p,
                                struct bucket * hashtable,
                                unsigned int * nextseqtab) -> void {
  progress_init("Writing UCLUST:   ", swarmcount);

  for(auto swarmid = 0U; swarmid < swarmcount; swarmid++)
    {
      struct bucket * bp = hashtable + swarmid;

      unsigned int seed = bp->seqno_first;

      fprintf(uclustfile, "C\t%u\t%u\t*\t*\t*\t*\t*\t",
              swarmid,
              bp->size);
      fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      fprintf(uclustfile, "\t*\n");

      fprintf(uclustfile, "S\t%u\t%u\t*\t*\t*\t*\t*\t",
              swarmid,
              db_getsequencelen(seed));
      fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      fprintf(uclustfile, "\t*\n");

      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fprintf(uclustfile,
                  "H\t%u\t%u\t%.1f\t+\t0\t0\t%s\t",
                  swarmid,
                  db_getsequencelen(a),
                  100.0,
                  "=");
          fprint_id(uclustfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          fprintf(uclustfile, "\t");
          fprint_id(uclustfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
          fprintf(uclustfile, "\n");
          a = nextseqtab[a];
        }

      progress_update(swarmid+1);
    }
  progress_done();
}

auto write_representative_sequences(const uint64_t swarmcount,
                                    struct Parameters const & p,
                                    struct bucket * hashtable) -> void {
  progress_init("Writing seeds:    ", swarmcount);
  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fprintf(fp_seeds, ">");
      fprint_id_with_new_abundance(fp_seeds, seed, hashtable[i].mass, p.opt_usearch_abundance);
      fprintf(fp_seeds, "\n");
      db_fprintseq(fp_seeds, seed, 0);
      progress_update(i+1);
    }
  progress_done();
}

auto write_swarms_mothur_format(const uint64_t swarmcount,
                                struct Parameters const & p,
                                struct bucket * hashtable,
                                unsigned int * nextseqtab) -> void {
  progress_init("Writing swarms:   ", swarmcount);
  fprintf(outfile, "swarm_%" PRId64 "\t%" PRIu64, p.opt_differences, swarmcount);

  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fputc('\t', outfile);
      fprint_id(outfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fputc(',', outfile);
          fprint_id(outfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          a = nextseqtab[a];
        }

      progress_update(i+1);
    }
    fputc('\n', outfile);

  progress_done();
}

auto write_swarms_default_format(const uint64_t swarmcount,
                                 struct Parameters const & p,
                                 struct bucket * hashtable,
                                 unsigned int * nextseqtab) -> void {
  progress_init("Writing swarms:   ", swarmcount);
  for(auto i = 0U; i < swarmcount; i++)
    {
      unsigned int seed = hashtable[i].seqno_first;
      fprint_id(outfile, seed, p.opt_usearch_abundance, p.opt_append_abundance);
      unsigned int a = nextseqtab[seed];

      while (a != 0U)
        {
          fputc(sepchar, outfile);
          fprint_id(outfile, a, p.opt_usearch_abundance, p.opt_append_abundance);
          a = nextseqtab[a];
        }
      fputc('\n', outfile);
      progress_update(i+1);
    }

  progress_done();
}

void dereplicate(struct Parameters const & p)
{
  const uint64_t dbsequencecount = db_getsequencecount();
  const uint64_t hashtablesize {compute_hashtable_size(dbsequencecount)};
  const uint64_t derep_hash_mask = hashtablesize - 1;

  auto * hashtable =
    static_cast<struct bucket *>(xmalloc(sizeof(bucket) * hashtablesize));

  memset(hashtable, 0, sizeof(bucket) * hashtablesize);

  uint64_t swarmcount = 0;
  uint64_t maxmass = 0;
  unsigned int maxsize = 0;

  /* alloc and init table of links to other sequences in cluster */
  auto * nextseqtab = static_cast<unsigned int *>
    (xmalloc(sizeof(unsigned int) * dbsequencecount));
  memset(nextseqtab, 0, sizeof(unsigned int) * dbsequencecount);

  progress_init("Dereplicating:    ", dbsequencecount);

  for(auto i = 0U; i < dbsequencecount; i++)
    {
      unsigned int seqlen = db_getsequencelen(i);
      char * seq = db_getsequence(i);

      /*
        Find free bucket or bucket for identical sequence.
        Make sure sequences are exactly identical
        in case of any hash collision.
        With 64-bit hashes, there is about 50% chance of a
        collision when the number of sequences is about 5e9.
      */

      uint64_t hash = zobrist_hash(reinterpret_cast<unsigned char *>(seq),
                                   seqlen);

      uint64_t j = hash & derep_hash_mask;
      struct bucket * bp = hashtable + j;

      while (((bp->mass) != 0U) &&
             ((bp->hash != hash) ||
              (seqlen != db_getsequencelen(bp->seqno_first)) ||
              (memcmp(seq,
                      db_getsequence(bp->seqno_first),
                      nt_bytelength(seqlen)) != 0)))
        {
          bp++;
          j++;
          if (bp >= hashtable + hashtablesize)
            {
              bp = hashtable;
              j = 0;
            }
        }

      uint64_t ab = db_getabundance(i);

      if ((bp->mass) != 0U)
        {
          /* at least one identical sequence already */
          nextseqtab[bp->seqno_last] = i;
        }
      else
        {
          /* no identical sequences yet, start a new cluster */
          swarmcount++;
          bp->hash = hash;
          bp->seqno_first = i;
          bp->size = 0;
          bp->singletons = 0;
        }

      bp->size++;
      bp->seqno_last = i;
      bp->mass += ab;

      if (ab == 1) {
        bp->singletons++;
      }

      if (bp->mass > maxmass) {
        maxmass = bp->mass;
      }

      if (bp->size > maxsize) {
        maxsize = bp->size;
      }

      progress_update(i);
    }
  progress_done();

  progress_init("Sorting:          ", 1);
  qsort(hashtable, hashtablesize, sizeof(bucket), derep_compare);
  progress_done();

  /* dump swarms */
  if (p.opt_mothur) {
    write_swarms_mothur_format(swarmcount, p, hashtable, nextseqtab);
  }
  else {
    write_swarms_default_format(swarmcount, p, hashtable, nextseqtab);
  }

  /* dump seeds in fasta format with sum of abundances */
  if (not p.opt_seeds.empty()) {
    write_representative_sequences(swarmcount, p, hashtable);
  }

  /* output swarm in uclust format */
  if (uclustfile != nullptr) {
    write_swarms_uclust_format(swarmcount, p, hashtable, nextseqtab);
  }

  /* output internal structure to file */
  if (not p.opt_internal_structure.empty()) {
    write_structure_file(swarmcount, p, hashtable, nextseqtab);
  }

  /* output statistics to file */
  if (statsfile != nullptr) {
    write_stats_file(swarmcount, p, hashtable);
  }

  fprintf(logfile, "\n");
  fprintf(logfile, "Number of swarms:  %" PRIu64 "\n", swarmcount);
  fprintf(logfile, "Largest swarm:     %u\n", maxsize);
  fprintf(logfile, "Heaviest swarm:    %" PRIu64 "\n", maxmass);

  xfree(nextseqtab);
  xfree(hashtable);
}
<|endoftext|>


<commit_before>#include "kernel.h"
#include "ilwisdata.h"
#include "range.h"
#include "itemrange.h"
#include "colorrange.h"
#include "numericrange.h"
#include "continuouscolorLookup.h"

using namespace Ilwis;

ContinuousColorLookup::ContinuousColorLookup(const QString &definition)
{
    QStringList parts = definition.split(";");
    for( QString group : parts){
        QStringList groupdef = group.split("|");
        if ( groupdef.size() != 3) {
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        QStringList limits = groupdef[0].split(":");
        if ( limits.size() != 2){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        bool ok1, ok2;
        NumericRange numrange(limits[0].toDouble(&ok1), limits[1].toDouble(&ok2));
        if ( !(ok1 && ok2)){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        QColor color1 = string2color(groupdef[1]);
        if ( !(color1.isValid())){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }

        QColor color2 = string2color(groupdef[2]);
        if ( !(color2.isValid())){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        ContinuousColorRange colorrange(color1, color2);
        addGroup(numrange,colorrange);

    }

}

QColor ContinuousColorLookup::value2color(double value, const NumericRange& actualRange, const NumericRange& stretchRange) const
{
    if ( stretchRange.isValid())  {
        if ( _linear){
            if ( value < stretchRange.center()){
                double stretchFraction = (value - stretchRange.min())/ stretchRange.distance();
                value = actualRange.min() + stretchFraction * actualRange.distance();
            }else{
                if ( value >= stretchRange.center()){
                    double stretchFraction = (stretchRange.max() - value)/ stretchRange.distance();
                    value = actualRange.max() - stretchFraction * actualRange.distance();
                }
            }
        }
    }
    value = (value - actualRange.min()) / actualRange.distance(); // scale it between 0..1

    for(int i = 0; i < _groups.size(); ++i){
        if ( value <= _groups[i].max()){
            double delta = _groups[i].distance();
            double position = 0;
            if ( _step == 0){
                position = (value - _groups[i].min())/ delta;
            }else
                position = ((quint32)(value - _groups[i].min())/ _step)/( (quint32)(delta / _step));

            return ContinuousColorRange::valueAt(position,&_colorranges[i]);
        }
    }
    return QColor();
}

void ContinuousColorLookup::addGroup(const NumericRange &range, const ContinuousColorRange &colorrange)
{
    if ( !(range.min() >= 0 && range.max() <= 1.0)){
        ERROR2(ERR_INVALID_INIT_FOR_2, TR("Numerical range"), "Representation");
        return;
    }
    if ( _colorranges.size() > 0){
        if ( range.min() < _groups.back().min()){
            kernel()->issues()->log(TR("Numerical ranges for representation must be added in order"));
            return ;
        }
    }
    _colorranges.push_back(colorrange);
    _groups.push_back(range);
}
<commit_msg>undefines get transparent as color<commit_after>#include "kernel.h"
#include "ilwisdata.h"
#include "range.h"
#include "itemrange.h"
#include "colorrange.h"
#include "numericrange.h"
#include "continuouscolorLookup.h"

using namespace Ilwis;

ContinuousColorLookup::ContinuousColorLookup(const QString &definition)
{
    QStringList parts = definition.split(";");
    for( QString group : parts){
        QStringList groupdef = group.split("|");
        if ( groupdef.size() != 3) {
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        QStringList limits = groupdef[0].split(":");
        if ( limits.size() != 2){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        bool ok1, ok2;
        NumericRange numrange(limits[0].toDouble(&ok1), limits[1].toDouble(&ok2));
        if ( !(ok1 && ok2)){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        QColor color1 = string2color(groupdef[1]);
        if ( !(color1.isValid())){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }

        QColor color2 = string2color(groupdef[2]);
        if ( !(color2.isValid())){
            ERROR2(ERR_ILLEGAL_VALUE_2,TR("Representation definition"), definition);
            return;
        }
        ContinuousColorRange colorrange(color1, color2);
        addGroup(numrange,colorrange);

    }

}

QColor ContinuousColorLookup::value2color(double value, const NumericRange& actualRange, const NumericRange& stretchRange) const
{
    if ( value == rUNDEF)
        return QColor("transparent");

    if ( stretchRange.isValid())  {
        if ( _linear){
            if ( value < stretchRange.center()){
                double stretchFraction = (value - stretchRange.min())/ stretchRange.distance();
                value = actualRange.min() + stretchFraction * actualRange.distance();
            }else{
                if ( value >= stretchRange.center()){
                    double stretchFraction = (stretchRange.max() - value)/ stretchRange.distance();
                    value = actualRange.max() - stretchFraction * actualRange.distance();
                }
            }
        }
    }
    value = (value - actualRange.min()) / actualRange.distance(); // scale it between 0..1
    for(int i = 0; i < _groups.size(); ++i){
        if ( value <= _groups[i].max()){
            double delta = _groups[i].distance();
            double position = 0;
            if ( _step == 0){
                position = (value - _groups[i].min())/ delta;
            }else
                position = ((quint32)(value - _groups[i].min())/ _step)/( (quint32)(delta / _step));

            return ContinuousColorRange::valueAt(position,&_colorranges[i]);
        }
    }
    return QColor();
}

void ContinuousColorLookup::addGroup(const NumericRange &range, const ContinuousColorRange &colorrange)
{
    if ( !(range.min() >= 0 && range.max() <= 1.0)){
        ERROR2(ERR_INVALID_INIT_FOR_2, TR("Numerical range"), "Representation");
        return;
    }
    if ( _colorranges.size() > 0){
        if ( range.min() < _groups.back().min()){
            kernel()->issues()->log(TR("Numerical ranges for representation must be added in order"));
            return ;
        }
    }
    _colorranges.push_back(colorrange);
    _groups.push_back(range);
}
<|endoftext|>