/*
Copyright 2019 - Stettbacher Signal Processing AG

Author: Patrick Roth

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/

#include <string.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <errno.h>
#include <tiffio.h>
#include <stdlib.h>

#include "helpers.h"



/**
 * Save image as JPEG format
 *
 * @param filename filename as string
 * @param format save image with this format
 * @param color_pipe piped out image data
 * @param compress 0 if no compression is used
 * @return 0 on success, -1 on error
 */
static int save_jpeg(char *filename, enum frame_format_t format, struct color_pipe_t *__restrict__ color_pipe, int compress_img) {
	printf("%s: saving JPEG image failed --> not implemented yet!!\n", __func__);
	return -1;
}


/**
 * Save image with TIFF format
 *
 * @param filename filename as string
 * @param format save image with this format
 * @param color_pipe piped out image data
 * @param compress 0 if no compression is used
 * @return 0 on success, -1 on error
 */
static int save_tiff(char *filename, enum frame_format_t format, struct color_pipe_t *__restrict__ color_pipe, int compress_img) {
	
	TIFF *tif;
	int samples_per_pixel;
	int i, u, height, width;
	uint8_t *in_img8 = color_pipe->img_out;
	uint16_t *in_img16 = color_pipe->img_out;
	uint16_t *linebuf16;
	int bitpersample;
	
	
	if(color_pipe->is_color) {
		samples_per_pixel = 3;
	}
	else {
		samples_per_pixel = 1;
	}
	height = color_pipe->height;
	width = color_pipe->width;
	
	if(color_pipe->bit_channel == 8) {
		bitpersample = 8;
	}
	else if(color_pipe->bit_channel == 12) {
		bitpersample = 16;
	}
	else {
		printf("%s: %d bits per channel is not supported yet\n", __func__, color_pipe->bit_channel);
		return -1;
	}
	
	tif = TIFFOpen(filename,"w");
	TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, width);
	TIFFSetField(tif, TIFFTAG_IMAGELENGTH, height);
	TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, samples_per_pixel);
	TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, bitpersample);
	TIFFSetField(tif, TIFFTAG_ORIENTATION, ORIENTATION_TOPLEFT);
	TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP, width*samples_per_pixel);
	TIFFSetField(tif, TIFFTAG_XRESOLUTION, (float)width);
	TIFFSetField(tif, TIFFTAG_YRESOLUTION, (float)height);
	TIFFSetField(tif, TIFFTAG_RESOLUTIONUNIT, RESUNIT_NONE);
	TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, 0);
	TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);
	if(compress_img) {
		TIFFSetField(tif, TIFFTAG_COMPRESSION, COMPRESSION_LZW);
	}
	else {
		TIFFSetField(tif, TIFFTAG_COMPRESSION, COMPRESSION_NONE);
	}
	TIFFSetField(tif, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB);
	
	if(color_pipe->is_color) {
		TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_RGB);
	}
	else {
		TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
	}
	
	if(bitpersample == 8) {
		// 8 bit per channel RGB image
		for(i = 0; i < height; i++) {
			TIFFWriteScanline(tif, (void*)&(in_img8[i*width*samples_per_pixel]), i, 0);
		}
	}
	else if(bitpersample == 16) {
		// 12 bit per channel RGB image (upscaling from 12 to 16 bit needed)
		linebuf16 = malloc(width*samples_per_pixel*2);
		if(linebuf16 == NULL) {
			printf("%s: allocating memory failed: %s\n", __func__, strerror(errno));
			goto _end_savetiff;
		}
		
		for(i = 0; i < height; i++) {
			// upscale 12 to 16 bit
			for(u = 0; u < width*samples_per_pixel; u++) {
				linebuf16[u] = in_img16[i*width*samples_per_pixel+u] << 4;
			}
			TIFFWriteScanline(tif, (void*)linebuf16, i, 0);
		}
		free(linebuf16);
	}
	
_end_savetiff:
	TIFFClose(tif);
	return 0;
}


/**
 * Save image frame with given file format.
 * 
 * @param filename filename as string
 * @param format save image with this format
 * @param color_pipe piped out image data
 * @param compress 0 if no compression is used
 * @return 0 on success, -1 on error
 */
int save_imgframe(char *filename, enum frame_format_t format, struct color_pipe_t *color_pipe, int compress) {
	
	char filename_ext[256];
	
	switch(format) {
		case IMGFRAME_FORMAT_TIF:
			sprintf(filename_ext, "%s.tif", filename);
			save_tiff(filename_ext, format, color_pipe, compress);
			break;
		
		case IMGFRAME_FORMAT_JPEG:
			sprintf(filename_ext, "%s.jpeg", filename);
			save_jpeg(filename_ext, format, color_pipe, compress);
			break;
			
		default:
			printf("%s: saving image with format %d not implemeted yet\n", __func__, format);
			return -1;
	}
	return 0;
}


/**
 * Tokenise (split) string
 * 
 * This function uses strtok_r and therefore it's thread safe.
 *
 * @param s input string
 * @param argv On return: array of string pointers containing the substrings
 * @param argv_len string array length
 * @param delim string delimiter used to tokenize input string ('\0' terminated)
 * @return number of substrings splitted or -1 on error
 */

int tokenise(char *s, char **argv, int argv_len, char *delim) {

    char *str;
    int argc=0, i;
	char *save_ptr;

	for(i = 0; i < argv_len; i++) {
		argv[i] = NULL;
	}
	
	str = strtok_r(s, delim, &save_ptr);
	
	while((str != NULL) && (argc < argv_len)) {
		argv[argc++] = str;
		str = strtok_r(NULL,delim,&save_ptr);
	}
	
	if(argc > argv_len) {
		return -1;
	}
	return argc;
}



/**
 * generic function to start thread
 * This function is used to start a new thread. Normally all threads are scheduled with real time policy. If the
 * priority is defined as @ref PRIO_TIME_SLICED a normal scheduling policy is used (no real time).
 *
 * @param thr On return: The ID of the new thread.
 * @param func The start function.
 * @param prio The thread priority.
 * @param param The thread parameters.
 * @return 0 on success otherwis -1.
 */
int generic_start_thread(pthread_t *thr, void*(*func)(void*), int prio, void *param) {
	int r;
	pthread_attr_t attr;
	sigset_t bmask, oldmask;
	struct sched_param	sp;
	
	r = pthread_attr_init(&attr);	// create attribute object
	if(r) {
		printf("%s: pthread_attr_init failed: %s\n", __func__, strerror(r));
		return -1;
	}
		
	if(prio == PRIO_TIME_SLICED) {
		r = pthread_attr_setschedpolicy(&attr, SCHED_OTHER);
		sp.sched_priority = 0;
	} 
	else {
		r = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);		// set realtime scheduling policy
		sp.sched_priority = sched_get_priority_max(SCHED_FIFO) - prio;
	}
	if(r) {
		printf("%s: pthread_attr_setschedpolicy failed: %s\n", __func__, strerror(r));
		goto _generic_start_thread_abort;
	}
	r = pthread_attr_setschedparam(&attr, &sp);
	if(r) {
		printf("%s: pthread_attr_setschedparam failed: %s\n", __func__, strerror(r));
		goto _generic_start_thread_abort;
	}
	r = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
	if(r) {
		printf("%s: pthread_attr_setinheritsched failed: %s\n", __func__, strerror(r));
		goto _generic_start_thread_abort;
	}
	
	//temporarely block some signals and inherit the signal mask to the new thread
	if(sigemptyset(&bmask)) {
		printf("%s: sigemptyset failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGTERM)) {
		printf("%s: sigaddset (SIGTERM) failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGINT)) {
		printf("%s: sigaddset (SIGINT) failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGALRM)) {
		printf("%s: sigaddset (SIGALRM) \n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGCHLD)) {
		printf("%s: sigaddset (SIGCHLD) failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGUSR1)) {
		printf("%s: sigaddset (SIGUSR1) failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigaddset(&bmask, SIGUSR2)) {
		printf("%s: sigaddset (SIGUSR2) failed\n", __func__);
		goto _generic_start_thread_abort;
	}
	if(sigprocmask(SIG_BLOCK, &bmask, &oldmask)) {
		printf("%s: sigprocmask failed: %s\n", __func__, strerror(errno));
		goto _generic_start_thread_abort;
	}
	
	//create thread
	r = pthread_create(thr, &attr, func, param);
	if(r) {
		printf("%s: pthread_create failed: %s\n", __func__, strerror(r));
		goto _generic_start_thread_abort;
	}
	
	//restore old signalmask
	if(sigprocmask (SIG_SETMASK, &oldmask, NULL)) {
		printf("%s: sigprocmask failed: %s\n", __func__, strerror(errno));
		goto _generic_start_thread_abort;
	}

	pthread_attr_destroy(&attr);	//release attribute object
	return 0;
	
_generic_start_thread_abort:
	pthread_attr_destroy(&attr);	// release attribute object
	return -1;
}


/**
 * Execute a shell command.
 * This function is thread safe.
 *
 * @param cmd  shell command to be executed
 * @param retstr On return: The return string from shell command execution. If NULL is defined nothing is returned.
 * @param len maximum length of return string
 * @return -1 if an error is detected
 */
int exec_shell_command(char *cmd, char *retstr, int len) {
    FILE *substream;
    char buf[512];
	int ret, len_tmp;
	
	if(retstr == NULL) {
		ret = system(cmd);
	}
	else {
		substream = popen(cmd, "r");	// start subprocess with streampipe connection
		if (substream == NULL) {
			printf("%s: popen failed: %s\n", __func__, strerror(errno));
			return -1;
		}
		
		if(setvbuf(substream, NULL, _IOLBF, 0)) {	// set line buffering for the stream
			printf("%s: setvbuf failed: %s\n", __func__, strerror(errno));
			return -1;
		}
		
		if (len > 0) {
			retstr[0] = '\0';
		}
		while(fgets(buf, sizeof(buf), substream) != NULL) {
			len_tmp = strlen(buf);
			if(len > 0) {
				strncpy(retstr, buf, len);
				retstr[len-1] = '\0';
				retstr += len_tmp;
				len -= len_tmp;
			}
		}
		ret = pclose(substream);
	}
	return ret;
}


/**
 * Set the given process name to realtime scheduling with the definded priority.
 * 0 means highest priority.
 * 
 * NOTE
 * Time critical scheduling policy SCHED_RR (real-time round-robin) is used!
 * 
 * @param p_name process name (name given command 'ps')
 * @param prio priority
 * @return 0 on success, -1 on error
 */
int setRealtimeScheduling(char *p_name, int prio) {
	
	char shell_cmd[256];
	int pid;
	char str[256];
	struct sched_param sp;
	
	// check string length of given process name
	if(strlen(p_name) > (sizeof(shell_cmd)-30)) {
		printf("%s: process name is too long\n", __func__);
		return -1;
	}
	
	sprintf(shell_cmd, "pidof %s", p_name);
	if(exec_shell_command(shell_cmd, str, sizeof(str))) {
		printf("%s: Setting realtime scheduling for '%s' failed: '%s'\n", __func__, p_name, str);
		return -1;
	}
	
	pid = atoi(str);
	sp.sched_priority = sched_get_priority_max(SCHED_RR) - prio;
	if(sched_setscheduler(pid, SCHED_RR, &sp)) {
		printf("%s: sched_setscheduler failed: %s\n", __func__, strerror(errno));
		return -1;
	}
	return 0;
}