/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2013

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 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 General Public License for more details.

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

 */
/*
	controller_block_trace.c
	
	This module takes care of tracing block outputs.
 */

#include <stdlib.h>
#include <string.h>
#include <semaphore.h>
#include <pthread.h>

#include <controller/controller_block.h>
#include <controller/controller_time.h>
#include <controller/controller_mem.h>
#include <log/log.h>

static int nr_traces = 0;
static struct controller_trace **traces;
static int max_traces = 0;

static pthread_mutex_t trace_lock;
static sem_t trace_sync_sem;
static struct controller_trace *trace_add_ptr = NULL;
static struct controller_trace *trace_del_ptr = NULL;

void controller_block_trace_init(int max)
{
	traces = controller_mem_calloc(
	    CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
	    max, sizeof(struct controller_trace *));
	max_traces = max;

	pthread_mutex_init(&trace_lock, NULL);
	sem_init(&trace_sync_sem, 0, 0);
}

int controller_block_trace_add(char *blockname, char *outterm, 
    struct controller_trace *trace)
{
	void *ptr = NULL;
	enum controller_block_term_type type = 0;
	int i, j;
	int ret = 0;
	int nr_blocks = controller_block_nr();
	
	for (i = 0; i < nr_blocks; i++) {
		struct controller_block *block;
		
		block = controller_block_get(i);
	
		log_send(LOG_T_DEBUG, "block %s", block->name);
		if (strcmp(blockname, block->name))
			continue;
			
		for (j = 0; j < block->outputs; j++) {
			log_send(LOG_T_DEBUG, "block %s %s", block->name, block->output[j].name);
			if (!strcmp(outterm, block->output[j].name)){
				ptr = block->output[j].value.v;
				type = block->output[j].type;
			}
		}
	}
	if (!ptr) {
		log_send(LOG_T_ERROR, 
		    "Terminal '%s' of block '%s' not found, cannot add trace",
		    outterm, blockname);
		return -1;
	}

	trace->timestamp = controller_time_seconds;
	trace->rd_pos = 0;
	trace->wr_pos = 0;
	trace->type = type;
	trace->ptr = ptr;
	
	/* touch the entire buffer atleast once */
	for (i = 0; i < trace->len; i++) {
		trace->buffer[i].s32 = i;
	}

	pthread_mutex_lock(&trace_lock);

	if (nr_traces < max_traces) {
		trace_add_ptr = trace;

		log_send(LOG_T_DEBUG, "Addded %s.%s to trace list",
		    blockname, outterm);
	} else {
		ret = 1;
	}
	sem_wait(&trace_sync_sem);

	pthread_mutex_unlock(&trace_lock);

	return ret;
}

void controller_block_trace_del(struct controller_trace *trace)
{
	pthread_mutex_lock(&trace_lock);

	trace_del_ptr = trace;
	sem_wait(&trace_sync_sem);

	pthread_mutex_unlock(&trace_lock);

	return;
}

void controller_block_trace(void)
{
	int i;
	
	if (trace_add_ptr) {
		traces[nr_traces] = trace_add_ptr;
		nr_traces++;

		trace_add_ptr = NULL;
		sem_post(&trace_sync_sem);
	}
	if (trace_del_ptr) {
		for (i = 0; i < nr_traces; i++) {
			if (traces[i] == trace_del_ptr) {
				if (i + 1 < nr_traces) {
					memmove(&traces[i + 0],
					    &traces[i + 1],
					    sizeof(struct controller_trace *) *
					    (nr_traces - i - 1));
				}
				break;
			}
		}
		nr_traces--;
		trace_del_ptr = NULL;
		sem_post(&trace_sync_sem);
	}
	
	for (i = 0; i < nr_traces; i++) {
		size_t wr_pos = traces[i]->wr_pos;

		if (controller_time_nsec == 0)
		{
			traces[i]->timestamp_pos = wr_pos;
			traces[i]->timestamp = controller_time_seconds;
		} else if (traces[i]->timestamp_pos == wr_pos) {
			traces[i]->timestamp_pos = traces[i]->len;
		}
		switch (traces[i]->type) {
			case CONTROLLER_BLOCK_TERM_BOOL:
				traces[i]->buffer[wr_pos].b = 
				    *(bool*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_FLOAT:
				traces[i]->buffer[wr_pos].f = 
				    *(float*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_UINT32:
				traces[i]->buffer[wr_pos].u32 = 
				    *(uint32_t*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_UINT16:
				traces[i]->buffer[wr_pos].u16 = 
				    *(uint16_t*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_UINT8:
				traces[i]->buffer[wr_pos].u8 = 
				    *(uint8_t*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_SINT32:
				traces[i]->buffer[wr_pos].s32 = 
				    *(int32_t*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_SINT16:
				traces[i]->buffer[wr_pos].s16 = 
				    *(int16_t*)traces[i]->ptr;
				break;
			case CONTROLLER_BLOCK_TERM_SINT8:
				traces[i]->buffer[wr_pos].s8 = 
				    *(int8_t*)traces[i]->ptr;
				break;
			default:
				traces[i]->buffer[wr_pos].s32 = -1;
		}
		wr_pos++;
		if (wr_pos == traces[i]->len)
			wr_pos = 0;
		__sync_synchronize();
		traces[i]->wr_pos = wr_pos;
		
	}
}