/**
 * @file
 * Analogy for Linux, device, subdevice, etc. related features
 *
 * @note Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
 * @note Copyright (C) 2014 Jorge A. Ramirez-Ortiz <jro@xenomai.org>
 *
 * This library 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 of the License, or (at your option) any later version.
 *
 * This library 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 library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <math.h>
#include <rtdm/analogy.h>
#include <stdio.h>
#include <errno.h>
#include "iniparser/iniparser.h"
#include "boilerplate/list.h"
#include "calibration.h"

#define CHK(func, ...)								\
do {										\
	int rc = func(__VA_ARGS__);						\
	if (rc < 0) 								\
		return -1;							\
} while (0)

#define ARRAY_LEN(a)  (sizeof(a) / sizeof((a)[0]))

static lsampl_t data32_get(void *src)
{
	return (lsampl_t) * ((lsampl_t *) (src));
}

static lsampl_t data16_get(void *src)
{
	return (lsampl_t) * ((sampl_t *) (src));
}

static lsampl_t data8_get(void *src)
{
	return (lsampl_t) * ((unsigned char *)(src));
}

static void data32_set(void *dst, lsampl_t val)
{
	*((lsampl_t *) (dst)) = val;
}

static void data16_set(void *dst, lsampl_t val)
{
	*((sampl_t *) (dst)) = (sampl_t) (0xffff & val);
}

static void data8_set(void *dst, lsampl_t val)
{
	*((unsigned char *)(dst)) = (unsigned char)(0xff & val);
}

static inline int read_dbl(double *d, struct _dictionary_ *f,const char *subd,
			   int subd_idx, char *type, int type_idx)
{
	char *str;
	int ret;

	/* if only contains doubles as coefficients */
	if (strncmp(type, COEFF_STR, strlen(COEFF_STR) != 0))
		return -ENOENT;

	ret = asprintf(&str, COEFF_FMT, subd, subd_idx, type, type_idx);
	if (ret < 0)
		return ret;

	*d = iniparser_getdouble(f, str, -255.0);
	if (*d == -255.0)
		ret = -ENOENT;
	free(str);

	return ret;
}

static inline int read_int(int *val, struct _dictionary_ *f, const char *subd,
			   int subd_idx, char *type)
{
	char *str;
	int ret;

	ret = (subd_idx >= 0) ?
	      asprintf(&str, ELEMENT_FIELD_FMT, subd, subd_idx, type):
	      asprintf(&str, ELEMENT_FMT, subd, type);
	if (ret < 0)
		return ret;

	*val = iniparser_getint(f, str, 0xFFFF);
	if (*val == 0xFFFF)
		ret = -ENOENT;
	free(str);

	return ret;
}

static inline int read_str(char **val, struct _dictionary_ *f, const char *subd,
			   const char *type)
{
	char *str;
	int ret;

	ret = asprintf(&str, ELEMENT_FMT, subd, type);
	if (ret < 0)
		return ret;

	*val = (char *) iniparser_getstring(f, str, NULL);
	if (*val == NULL)
		ret = -ENOENT;
	free(str);

	return ret;
}

static inline void write_calibration(FILE *file, char *fmt, ...)
{
	va_list ap;

	if (!file)
		return;

	va_start(ap, fmt);
	vfprintf(file, fmt, ap);
	fflush(file);
	va_end(ap);
}

void
write_calibration_file(FILE *dst, struct listobj *l,
		       struct a4l_calibration_subdev *subd, a4l_desc_t *desc)
{
	struct subdevice_calibration_node *e, *t;
	int i, j = 0;

	if (list_empty(l))
		return;

	/* TODO: modify the meaning of board/driver in the proc */
	if (desc) {
		write_calibration(dst, "[%s] \n",PLATFORM_STR);
		write_calibration(dst, DRIVER_STR" = %s;\n", desc->board_name);
		write_calibration(dst, BOARD_STR" = %s;\n", desc->driver_name);
	}

	write_calibration(dst, "\n[%s] \n", subd->name);
	write_calibration(dst, INDEX_STR" = %d;\n", subd->idx);
	list_for_each_entry_safe(e, t, l, node) {
		j++;
	}
	write_calibration(dst, ELEMENTS_STR" = %d;\n", j);

	j = 0;
	list_for_each_entry_safe(e, t, l, node) {
		write_calibration(dst, "[%s_%d] \n", subd->name, j);
		write_calibration(dst, CHANNEL_STR" = %d;\n", e->channel);
		write_calibration(dst, RANGE_STR" = %d;\n", e->range);
		write_calibration(dst, EXPANSION_STR" = %g;\n",
				  e->polynomial->expansion_origin);
		write_calibration(dst, NBCOEFF_STR"= %d;\n",
				  e->polynomial->nb_coefficients);

		for (i = 0; i < e->polynomial->nb_coefficients; i++)
			write_calibration(dst, COEFF_STR"_%d = %g;\n",
					  i,
					  e->polynomial->coefficients[i]);
		j++;
	}

	return;
}

/*!
 * @ingroup analogy_lib_level2
 * @defgroup analogy_lib_calibration Software calibration API
 * @{
 */

/**
 * @brief Read the analogy generated calibration file
 *
 * @param[in] name Name of the calibration file
 * @param[out] data Pointer to the calibration file contents
 *
 */

int a4l_read_calibration_file(char *name, struct a4l_calibration_data *data)
{
	const char *subdevice[2] = { AI_SUBD_STR, AO_SUBD_STR };
	int i, j, k, index = -1, nb_elements = -1;
	struct a4l_calibration_subdev_data *p = NULL;
	struct _dictionary_ *d;
	struct stat st;

	if (access(name, R_OK))
		return -1;

	if (stat(name, &st) || !st.st_size)
		return -1;

	d = iniparser_load(name);
	if (d == NULL)
		return -1;

	CHK(read_str, &data->driver_name, d, PLATFORM_STR, DRIVER_STR);
	CHK(read_str, &data->board_name, d, PLATFORM_STR, BOARD_STR);

	for (k = 0; k < ARRAY_LEN(subdevice); k++) {
		read_int(&nb_elements, d, subdevice[k], -1, ELEMENTS_STR);
		if (nb_elements < 0 ) {
			/* AO is optional */
			if (!strncmp(subdevice[k], AO_SUBD_STR, sizeof(AO_SUBD_STR)))
			     break;
			return -1;
		}

		CHK(read_int, &index, d, subdevice[k], -1, INDEX_STR);

		if (strncmp(subdevice[k], AI_SUBD_STR,
			    strlen(AI_SUBD_STR)) == 0) {
			data->ai = malloc(nb_elements *
					  sizeof(struct a4l_calibration_subdev_data));
			data->nb_ai = nb_elements;
			p  = data->ai;
		}

		if (strncmp(subdevice[k], AO_SUBD_STR,
			    strlen(AO_SUBD_STR)) == 0) {
			data->ao = malloc(nb_elements *
					  sizeof(struct a4l_calibration_subdev_data));
			data->nb_ao = nb_elements;
			p = data->ao;
		}

		for (i = 0; i < nb_elements; i++) {
			CHK(read_int, &p->expansion, d, subdevice[k], i,
				 EXPANSION_STR);
			CHK(read_int, &p->nb_coeff, d, subdevice[k], i,
				 NBCOEFF_STR);
			CHK(read_int, &p->channel, d, subdevice[k], i,
				 CHANNEL_STR);
			CHK(read_int, &p->range, d, subdevice[k], i,
				 RANGE_STR);

			p->coeff = malloc(p->nb_coeff * sizeof(double));

			for (j = 0; j < p->nb_coeff; j++) {
				CHK(read_dbl,&p->coeff[j], d, subdevice[k], i,
					 COEFF_STR, j);
			}

			p->index = index;
			p++;
		}
	}


	return 0;
}

/**
 * @brief Get the polynomial that will be use for the software calibration
 *
 * @param[out] converter Polynomial to be used on the software calibration
 * @param[in] subd Subdevice index
 * @param[in] chan Channel
 * @param[in] range Range
 * @param[in] data Calibration data read from the calibration file
 *
 * @return -1 on error
 *
 */

int a4l_get_softcal_converter(struct a4l_polynomial *converter,
			      int subd, int chan, int range,
			      struct a4l_calibration_data *data )
{
	int i;

	for (i = 0; i < data->nb_ai; i++) {
		if (data->ai[i].index != subd)
			break;
		if ((data->ai[i].channel == chan || data->ai[i].channel == -1)
		    &&
		    (data->ai[i].range == range || data->ai[i].range == -1)) {
			converter->expansion = data->ai[i].expansion;
			converter->nb_coeff = data->ai[i].nb_coeff;
			converter->coeff = data->ai[i].coeff;
			converter->order = data->ai[i].nb_coeff - 1;
			return 0;
		}
	}

	for (i = 0; i < data->nb_ao; i++) {
		if (data->ao[i].index != subd)
			break;
		if ((data->ao[i].channel == chan || data->ao[i].channel == -1)
		    &&
		    (data->ao[i].range == range || data->ao[i].range == -1)) {
			converter->expansion = data->ao[i].expansion;
			converter->nb_coeff = data->ao[i].nb_coeff;
			converter->coeff = data->ao[i].coeff;
			converter->order = data->ao[i].nb_coeff - 1;
			return 0;
		}
	}

	return -1;
}

/**
 * @brief Convert raw data (from the driver) to calibrated double units
 * @param[in] chan Channel descriptor
 * @param[out] dst Ouput buffer
 * @param[in] src Input buffer
 * @param[in] cnt Count of conversion to perform
 * @param[in] converter Conversion polynomial
 *
 *
 * @return the count of conversion performed, otherwise a negative
 * error code:
 *
 * - -EINVAL is returned if some argument is missing or wrong;
 *    chan, rng and the pointers should be checked; check also the
 *    kernel log ("dmesg"); WARNING: a4l_fill_desc() should be called
 *    before using a4l_rawtodcal()
 *
 *
 *
 */

int a4l_rawtodcal(a4l_chinfo_t *chan, double *dst, void *src,
		  int cnt, struct a4l_polynomial *converter)
{
	int i = 0, j = 0, k = 0;
	double term = 1.0;
	lsampl_t tmp;
	int size;

	/* Temporary data accessor */
	lsampl_t(*datax_get) (void *);

	/* Basic checking */
	if (chan == NULL)
		return -EINVAL;

	/* Find out the size in memory */
	size = a4l_sizeof_chan(chan);

	/* Get the suitable accessor */
	switch (a4l_sizeof_chan(chan)) {
	case 4:
		datax_get = data32_get;
		break;
	case 2:
		datax_get = data16_get;
		break;
	case 1:
		datax_get = data8_get;
		break;
	default:
		return -EINVAL;
	};

	while (j < cnt) {
		/* Properly retrieve the data */
		tmp = datax_get(src + i);

		/* Perform the conversion */
		dst[j] = 0.0;
		term = 1.0;
		for (k = 0; k < converter->nb_coeff; k++) {
			dst[j] += converter->coeff[k] * term;
			term *= tmp - converter->expansion;
		}

		/* Update the counters */
		i += size;
		j++;
	}

	return j;
}

/**
 * @brief Convert double values to raw calibrated data using polynomials
 *
 * @param[in] chan Channel descriptor
 * @param[out] dst Ouput buffer
 * @param[in] src Input buffer
 * @param[in] cnt Count of conversion to perform
 * @param[in] converter Conversion polynomial
 *
 * @return the count of conversion performed, otherwise a negative
 * error code:
 *
 * - -EINVAL is returned if some argument is missing or wrong;
 *    chan, rng and the pointers should be checked; check also the
 *    kernel log ("dmesg"); WARNING: a4l_fill_desc() should be called
 *    before using a4l_dcaltoraw()
 *
 */

int a4l_dcaltoraw( a4l_chinfo_t * chan, void *dst, double *src, int cnt,
		   struct a4l_polynomial *converter)
{
	int size, i = 0, j = 0, k = 0;
	double value, term;

	/* Temporary data accessor */
	void (*datax_set) (void *, lsampl_t);

	/* Basic checking */
	if (chan == NULL)
		return -EINVAL;

	/* Find out the size in memory */
	size = a4l_sizeof_chan(chan);

	/* Select the suitable accessor */
	switch (size) {
	case 4:
		datax_set = data32_set;
		break;
	case 2:
		datax_set = data16_set;
		break;
	case 1:
		datax_set = data8_set;
		break;
	default:
		return -EINVAL;
	};

	while (j < cnt) {

		/* Performs the conversion */
		value = 0.0;
		term = 1.0;
		for (k = 0; k < converter->nb_coeff; k++) {
			value += converter->coeff[k] * term;
			term *= src[j] - converter->expansion;
		}
		value = nearbyint(value);

		datax_set(dst + i, (lsampl_t) value);

		/* Updates the counters */
		i += size;
		j++;
	}

	return j;
}

/** @} Calibration API */