history_graph.cpp 5.89 KB
#include "blc_program.h"
#include "graph.h"
#include <pthread.h>
#include <sys/time.h>
#include <unistd.h>

using namespace std;

typedef struct history:blc_array{
	blc_channel *input;

	void init_timer();
	int  wait_next_k();
	template <typename D_t> void start_refresh();

	int iteration;
	int length, sampling_period;
	size_t element_size;
	unsigned long initial_us;
}type_history;

type_history history;

pthread_mutex_t mutex=PTHREAD_MUTEX_INITIALIZER;
int k;

void type_history::init_timer(){
	struct timeval initial_timer;

	gettimeofday(&initial_timer, NULL);
	length=dims[dims_nb-1].length;
	initial_us = initial_timer.tv_sec*1000000+initial_timer.tv_usec;
	k=length-1;
}

/**This is in order to take into account the absolute time. If an iteration is longer than it should, the next one will be shorter.*/
int type_history::wait_next_k(){
	div_t samples_div;
	struct timeval timer;

	gettimeofday(&timer, NULL);

	//sample_div.quot is the theoritical iteration.
	samples_div=div(timer.tv_sec*1000000+timer.tv_usec-initial_us, sampling_period);

	//We try to stay in the middle of the iteration
	if ((int)(iteration-samples_div.quot) >= 0)  usleep((iteration+1-samples_div.quot)*sampling_period+sampling_period/2-samples_div.rem);
	iteration++;
	return div(samples_div.quot, length).rem;
}

template <typename D_t> void history::start_refresh(){
	int i, j, next_k;

	while(1){
		if (input->sem_new_data) sem_wait(input->sem_new_data);
		next_k=wait_next_k();
		pthread_mutex_lock(&mutex);
		if(next_k==k+1){ //normal case
			FOR(i, input->total_length) ((D_t*)data)[i*length+next_k]=((D_t*)input->data)[i];
		}
		else if ((next_k-k)>1){
			color_eprintf(BLC_BRIGHT_YELLOW, "Missing '%d' samples\n", next_k-k);
			FOR(i, input->total_length) for(j=k; j!=next_k+1; j++) static_cast<D_t*>(data)[i*length+j]=static_cast<D_t*>(input->data)[i];
		}
		else if (next_k==k){
			color_eprintf(BLC_BRIGHT_YELLOW, "Twice the same sample iteration %d\n", iteration);
		}
		else FOR(i, input->total_length){ //It accross the end of the buffer
			for(j=k; j!=length; j++) static_cast<D_t*>(data)[i*length+j]=static_cast<D_t*>(input->data)[i];
			for(j=0; j!=next_k+1; j++) static_cast<D_t*>(data)[i*length+j]=static_cast<D_t*>(input->data)[i];
		}
		if (input->sem_ack_data) sem_post(input->sem_ack_data);

		k=next_k;
		pthread_mutex_unlock(&mutex);
	}
}

static void *refresh_history_cb(void *history_pt){
	type_history *history=(type_history*)history_pt;

	history->init_timer();

	switch (history->type) {
    case 'UIN8': case 'INT8':
		history->start_refresh<char>();
		break;
    case 'UI16': case 'IN16':
        history->start_refresh<int16_t>();
        break;
    case 'UI32': case 'IN32':
        history->start_refresh<int32_t>();
        break;
	case 'FL32':
		history->start_refresh<float>();
		break;
    default: EXIT_ON_ERROR("The type is not managed");
        break;
	}
	return NULL;
}

void create_history_graph(deque <blc_channel> inputs, char const *title, int history_length, int refresh_period, int sampling_period, float min, float max, char const *verbatim){
	
    blc_channel *input=nullptr;
    char const *gnuplot_format=NULL;
    char command[LINE_MAX];

	char code;
	int i, offset, columns_nb=0;
	FILE *pipef;
	pthread_t thread;
    
    if (inputs.size()>1) EXIT_ON_ERROR("Only one channel is managed for history plots");
    else input=&inputs.back();
    
	switch (input->dims_nb){
	case 0:
		columns_nb=1;
		break;
	case 1:
		columns_nb=input->dims[0].length;
		break;
	default:
		EXIT_ON_ARRAY_ERROR(input, "Too many dims");
		break;
	}

	SYSTEM_ERROR_CHECK(pipef=popen("gnuplot", "w"), NULL, "Calling gnuplot");

	switch (input->type){
	case 'UIN8':
		gnuplot_format="%uchar";
		history.element_size=sizeof(uchar);
		break;
	case 'INT8':
		gnuplot_format="%char";
		history.element_size=sizeof(char);
		break;
    case 'UI16':
        gnuplot_format="%uint16";
        history.element_size=sizeof(uint16_t);
        break;
    case 'IN16':
        gnuplot_format="%int16";
        history.element_size=sizeof(int16_t);
        break;
    case 'UI32':
        gnuplot_format="%uint32";
        history.element_size=sizeof(uint32_t);
        break;
    case 'IN32':
        gnuplot_format="%uint32";
        history.element_size=sizeof(int32_t);
        break;
	case 'FL32':
		gnuplot_format="%float";
		history.element_size=sizeof(float);
		break;
	default: EXIT_ON_ARRAY_ERROR(input, "The type is not managed");
	}

	init_term(pipef, title, verbatim);
	fprintf(pipef, "set xlabel 'time(s)'\n");


	if (min!=max) fprintf(pipef, "set yrange [%f:%f]\n", min, max);

	offset=snprintf(command, LINE_MAX, "plot  '-'  binary format='%s' record=%d using ($0*%f):1  title '%d' with %s",   gnuplot_format, history_length, sampling_period/1000000.f, 0, with_option);

	for(i=1; i!=columns_nb; i++){
		if (i<10) code=48+i; //01234567890
		else if (i<36) code=97+i-10; //abc...xyz
		else if (i<62) code=65+i-36; //ABC...XYZ
		else if (i==62) code='[';
		else if (i==63) code=']';
		else EXIT_ON_ERROR("Too many columns. You have '%d' max is 64", columns_nb);
        offset+=snprintf(command+offset, LINE_MAX-offset, ", '-'  binary format='%s' record=%d  u ($0*%f):1 t '%c' w %s", gnuplot_format, history_length, sampling_period/1000000.f, code, with_option);
	}

	history.init(input->type, input->format, 2, columns_nb, history_length);
	history.sampling_period=sampling_period;
	history.input=input;

	pthread_create(&thread, NULL, refresh_history_cb, &history);

	BLC_COMMAND_LOOP(refresh_period){
		fprintf(pipef, "\n%s\n", command);
		FOR(i, columns_nb){
			pthread_mutex_lock(&mutex);
			SYSTEM_ERROR_CHECK(fwrite(&history.chars[(i*history_length+k+1)*history.element_size], history.element_size, history_length-k-1, pipef), -1, NULL);
			SYSTEM_ERROR_CHECK(fwrite(&history.chars[(i*history_length)*history.element_size], history.element_size, k+1, pipef), -1, NULL);
			pthread_mutex_unlock(&mutex);
		}
		SYSTEM_ERROR_CHECK(fflush(pipef), -1, NULL);
	}
	SYSTEM_ERROR_CHECK(fclose(pipef), -1, NULL);
}