/*
 * OMAP3 ISP library - OMAP3 ISP controls
 *
 * Copyright (C) 2010-2011 Ideas on board SPRL
 *
 * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
 *
 * 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.1 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 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <sys/ioctl.h>
#include <sys/time.h>
#include <linux/omap3isp.h>

#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "controls.h"
#include "omap3isp.h"
#include "omap3isp-priv.h"
#include "subdev.h"

/* -----------------------------------------------------------------------------
 * Matrix algebra helper functions
 */

struct matrix {
	float coeff[3][3];
};

static void matrix_zero(struct matrix *m)
{
	unsigned int i, j;

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j)
			m->coeff[i][j] = 0.0;
	}
}

static void matrix_invert(struct matrix *m)
{
	/* Invert the matrix using the transpose of the matrix of cofactors. The
	 * Gauss-Jordan elimination would be faster in the general case, but we
	 * know that the matrix is 3x3.
	 */
	const float eps = 1e-6;
	struct matrix out;
	unsigned int i, j;
	float det;

	out.coeff[0][0] = m->coeff[1][1] * m->coeff[2][2]
			- m->coeff[1][2] * m->coeff[2][1];
	out.coeff[0][1] = m->coeff[0][2] * m->coeff[2][1]
			- m->coeff[0][1] * m->coeff[2][2];
	out.coeff[0][2] = m->coeff[0][1] * m->coeff[1][2]
			- m->coeff[0][2] * m->coeff[1][1];
	out.coeff[1][0] = m->coeff[1][2] * m->coeff[2][0]
			- m->coeff[1][0] * m->coeff[2][2];
	out.coeff[1][1] = m->coeff[0][0] * m->coeff[2][2]
			- m->coeff[0][2] * m->coeff[2][0];
	out.coeff[1][2] = m->coeff[0][2] * m->coeff[1][0]
			- m->coeff[0][0] * m->coeff[1][2];
	out.coeff[2][0] = m->coeff[1][0] * m->coeff[2][1]
			- m->coeff[1][1] * m->coeff[2][0];
	out.coeff[2][1] = m->coeff[0][1] * m->coeff[2][0]
			- m->coeff[0][0] * m->coeff[2][1];
	out.coeff[2][2] = m->coeff[0][0] * m->coeff[1][1]
			- m->coeff[1][0] * m->coeff[0][1];

	det = m->coeff[0][0] * out.coeff[0][0] +
	      m->coeff[0][1] * out.coeff[1][0] +
	      m->coeff[0][2] * out.coeff[2][0];

	if (det < eps)
		return;

	det = 1/det;

	for (i = 0; i < 3; ++i)
		for (j = 0; j < 3; ++j)
			m->coeff[i][j] = out.coeff[i][j] * det;
}

static void matrix_multiply(struct matrix *a, const struct matrix *b)
{
	struct matrix out;

	/* Compute a * b and return the result in a. */
	out.coeff[0][0] = a->coeff[0][0] * b->coeff[0][0]
			+ a->coeff[0][1] * b->coeff[1][0]
			+ a->coeff[0][2] * b->coeff[2][0];
	out.coeff[0][1] = a->coeff[0][0] * b->coeff[0][1]
			+ a->coeff[0][1] * b->coeff[1][1]
			+ a->coeff[0][2] * b->coeff[2][1];
	out.coeff[0][2] = a->coeff[0][0] * b->coeff[0][2]
			+ a->coeff[0][1] * b->coeff[1][2]
			+ a->coeff[0][2] * b->coeff[2][2];
	out.coeff[1][0] = a->coeff[1][0] * b->coeff[0][0]
			+ a->coeff[1][1] * b->coeff[1][0]
			+ a->coeff[1][2] * b->coeff[2][0];
	out.coeff[1][1] = a->coeff[1][0] * b->coeff[0][1]
			+ a->coeff[1][1] * b->coeff[1][1]
			+ a->coeff[1][2] * b->coeff[2][1];
	out.coeff[1][2] = a->coeff[1][0] * b->coeff[0][2]
			+ a->coeff[1][1] * b->coeff[1][2]
			+ a->coeff[1][2] * b->coeff[2][2];
	out.coeff[2][0] = a->coeff[2][0] * b->coeff[0][0]
			+ a->coeff[2][1] * b->coeff[1][0]
			+ a->coeff[2][2] * b->coeff[2][0];
	out.coeff[2][1] = a->coeff[2][0] * b->coeff[0][1]
			+ a->coeff[2][1] * b->coeff[1][1]
			+ a->coeff[2][2] * b->coeff[2][1];
	out.coeff[2][2] = a->coeff[2][0] * b->coeff[0][2]
			+ a->coeff[2][1] * b->coeff[1][2]
			+ a->coeff[2][2] * b->coeff[2][2];

	*a = out;
}

static void matrix_float_to_s10q8(__u16 out[3][3], const struct matrix *in)
{
	unsigned int i, j;

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j)
			out[i][j] = (__u16)((__s16)(in->coeff[i][j] * 256) & 0x3ff);
	}
}

static void matrix_float_to_s12q8(__u16 out[3][3], const struct matrix *in)
{
	unsigned int i, j;

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j)
			out[i][j] = (__u16)((__s16)(in->coeff[i][j] * 256) & 0xfff);
	}
}

/* -----------------------------------------------------------------------------
 * Preview engine parameters configuration
 */

static const struct matrix omap3isp_preview_csc = {
	.coeff = {
		/* Default values. */
		{  0.2968750,  0.5937500,  0.1093750 },
		{ -0.1718750, -0.3281250,  0.5000000 },
		{  0.5000000, -0.3828125, -0.0781250 },
#if 0
		/* Default values for fluorescent light. */
		{  0.25781250,  0.50390625,  0.09765625 },
		{ -0.14843750, -0.29296875,  0.43750000 },
		{  0.43750000, -0.36718750, -0.07031250 },
#endif
	},
};

static const struct matrix omap3isp_preview_rgb2rgb = {
	.coeff = {
		/* Default values. */
		{ 1.0, 0.0, 0.0 },
		{ 0.0, 1.0, 0.0 },
		{ 0.0, 0.0, 1.0 },
#if 0
		/* Default values for fluorescent light. */
		{  1.88281250, -0.812500, -0.07031250 },
		{ -0.39453125,  1.671875, -0.27734375 },
		{ -0.12500000, -1.250000,  2.37500000 },
#endif
	},
};

int omap3_isp_preview_setup(struct omap3_isp_device *isp)
{
	struct omap3isp_prev_update_config config;
	struct omap3isp_prev_rgbtorgb rgb2rgb;
	struct omap3isp_prev_csc csc;
	int ret;

	memset(&config, 0, sizeof config);
	config.update = OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV;
	config.flag = OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV;
	config.rgb2rgb = &rgb2rgb;
	config.csc = &csc;

	memset(&rgb2rgb, 0, sizeof rgb2rgb);
	matrix_float_to_s12q8(rgb2rgb.matrix, &omap3isp_preview_rgb2rgb);

	memset(&csc, 0, sizeof csc);
	matrix_float_to_s10q8(csc.matrix, &omap3isp_preview_csc);

	v4l2_subdev_open(isp->preview);

	ret = ioctl(isp->preview->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
	if (ret < 0)
		return -errno;

	return ret;
}

int omap3_isp_preview_set_contrast(struct omap3_isp_device *isp, unsigned int value)
{
	int contrast = value;
	int ret;

	ret = v4l2_subdev_set_control(isp->preview, V4L2_CID_CONTRAST, &contrast);
	if (ret < 0)
		return -errno;

	return ret;
}
int omap3_isp_preview_set_saturation(struct omap3_isp_device *isp, float value)
{
	struct omap3isp_prev_update_config config;
	struct omap3isp_prev_rgbtorgb rgb2rgb;
	struct matrix saturation;
	struct matrix gain;
	int ret;

	matrix_zero(&gain);
	gain.coeff[0][0] = 1.0;
	gain.coeff[1][1] = value;
	gain.coeff[2][2] = value;

	saturation = omap3isp_preview_csc;
	matrix_invert(&saturation);
	matrix_multiply(&saturation, &gain);
	matrix_multiply(&saturation, &omap3isp_preview_csc);

	memset(&config, 0, sizeof config);
	config.update = OMAP3ISP_PREV_RGB2RGB;
	config.flag = OMAP3ISP_PREV_RGB2RGB;
	config.rgb2rgb = &rgb2rgb;

	memset(&rgb2rgb, 0, sizeof rgb2rgb);
	matrix_float_to_s12q8(rgb2rgb.matrix, &saturation);

	ret = ioctl(isp->preview->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
	if (ret < 0)
		return -errno;

	return ret;
}