/*
 * 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);
	}
}

/* -----------------------------------------------------------------------------
 * CCDC parameters configuration
 */

int omap3_isp_ccdc_set_black_level(struct omap3_isp_device *isp, unsigned int value)
{
	struct omap3isp_ccdc_update_config config;
	struct omap3isp_ccdc_bclamp bclamp;
	int ret;

	v4l2_subdev_open(isp->ccdc.entity);

	memset(&config, 0, sizeof config);
	config.update = OMAP3ISP_CCDC_BLCLAMP;
	config.flag = 0;
	config.bclamp = &bclamp;

	memset(&bclamp, 0, sizeof bclamp);
	bclamp.dcsubval = value;

	ret = ioctl(isp->ccdc.entity->fd, VIDIOC_OMAP3ISP_CCDC_CFG, &config);
	if (ret < 0) {
		printf("%s: %s (%d)\n", __func__, strerror(errno), errno);
		return -errno;
	}

	return ret;
}

/* -----------------------------------------------------------------------------
 * 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_set_contrast(struct omap3_isp_device *isp, unsigned int value)
{
	int contrast = value;
	int ret;

	ret = v4l2_subdev_set_control(isp->preview.entity, 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.entity->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
	if (ret < 0)
		return -errno;

	return ret;
}

int omap3_isp_preview_update_white_balance(struct omap3_isp_device *isp)
{
	struct omap3isp_prev_update_config config;
	int ret;

	memset(&config, 0, sizeof config);
	config.update = OMAP3ISP_PREV_WB;
	config.flag = OMAP3ISP_PREV_WB;
	config.wbal = &isp->preview.wbal;

	ret = ioctl(isp->preview.entity->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
	if (ret < 0) {
		printf("%s: %s (%d)\n", __func__, strerror(errno), errno);
		return -errno;
	}

	return ret;
}

int omap3_isp_preview_set_gain(struct omap3_isp_device *isp, float gain)
{
	isp->preview.wbal.dgain = (__u16)(gain * (1 << 8)) & 0x3ff;

	return omap3_isp_preview_update_white_balance(isp);
}

int omap3_isp_preview_set_white_balance(struct omap3_isp_device *isp, float gains[4])
{
	isp->preview.wbal.coef0 = (__u8)clamp(gains[0] * (1 << 5), 0.0, 255.0);
	isp->preview.wbal.coef1 = (__u8)clamp(gains[1] * (1 << 5), 0.0, 255.0);
	isp->preview.wbal.coef2 = (__u8)clamp(gains[2] * (1 << 5), 0.0, 255.0);
	isp->preview.wbal.coef3 = (__u8)clamp(gains[3] * (1 << 5), 0.0, 255.0);

	return omap3_isp_preview_update_white_balance(isp);
}

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_WB | OMAP3ISP_PREV_RGB2RGB
		      | OMAP3ISP_PREV_COLOR_CONV;
	config.flag = OMAP3ISP_PREV_WB | OMAP3ISP_PREV_RGB2RGB
		    | OMAP3ISP_PREV_COLOR_CONV;
	config.wbal = &isp->preview.wbal;
	config.rgb2rgb = &rgb2rgb;
	config.csc = &csc;

	isp->preview.wbal.dgain = 1 << 8;
	isp->preview.wbal.coef0 = 1 << 5;
	isp->preview.wbal.coef1 = 1 << 5;
	isp->preview.wbal.coef2 = 1 << 5;
	isp->preview.wbal.coef3 = 1 << 5;

	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.entity);

	ret = ioctl(isp->preview.entity->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
	if (ret < 0) {
		printf("%s: %s (%d)\n", __func__, strerror(errno), errno);
		return -errno;
	}

	return ret;
}

/* -----------------------------------------------------------------------------
 * Sensor parameters configuration
 */

#define V4L2_CID_GAIN_RED		(V4L2_CTRL_CLASS_CAMERA | 0x1001)
#define V4L2_CID_GAIN_GREEN1		(V4L2_CTRL_CLASS_CAMERA | 0x1002)
#define V4L2_CID_GAIN_GREEN2		(V4L2_CTRL_CLASS_CAMERA | 0x1003)
#define V4L2_CID_GAIN_BLUE		(V4L2_CTRL_CLASS_CAMERA | 0x1004)

#define SENSOR_GAIN_GLOBAL		(1 << 0)
#define SENSOR_GAIN_RED			(1 << 1)
#define SENSOR_GAIN_GREEN1		(1 << 2)
#define SENSOR_GAIN_GREEN2		(1 << 3)
#define SENSOR_GAIN_BLUE		(1 << 4)
#define SENSOR_GAIN_COLORS		(SENSOR_GAIN_RED | \
					 SENSOR_GAIN_GREEN1 | \
					 SENSOR_GAIN_GREEN2 | \
					 SENSOR_GAIN_BLUE)

int omap3_isp_sensor_init(struct omap3_isp_device *isp)
{
	struct v4l2_queryctrl query;
	unsigned int gains = 0;
	__u32 id;
	int ret;

	/* Retrieve the sensor default format. */
	ret = v4l2_subdev_get_format(isp->sensor.entity, &isp->sensor.format, 0,
				     V4L2_SUBDEV_FORMAT_TRY);
	if (ret < 0) {
		printf("error: unable to get sensor default format.\n");
		return ret;
	}

	for (id = 0; ; id = query.id) {
		memset(&query, 0, sizeof query);
		query.id = id | V4L2_CTRL_FLAG_NEXT_CTRL;
		ret = ioctl(isp->sensor.entity->fd, VIDIOC_QUERYCTRL, &query);
		if (ret < 0) {
			if (errno == EINVAL)
				break;

			printf("error: unable to get sensor default format.\n");
			return -errno;
		}

		switch (query.id) {
		case V4L2_CID_GAIN:
			gains |= SENSOR_GAIN_GLOBAL;
			break;
		case V4L2_CID_GAIN_RED:
			gains |= SENSOR_GAIN_RED;
			break;
		case V4L2_CID_GAIN_GREEN1:
			gains |= SENSOR_GAIN_GREEN1;
			break;
		case V4L2_CID_GAIN_GREEN2:
			gains |= SENSOR_GAIN_GREEN2;
			break;
		case V4L2_CID_GAIN_BLUE:
			gains |= SENSOR_GAIN_BLUE;
			break;
		}
	}

	if ((gains & SENSOR_GAIN_COLORS) == SENSOR_GAIN_COLORS)
		isp->sensor.has_color_gains = true;
	if (gains & SENSOR_GAIN_GLOBAL)
		isp->sensor.has_global_gain = true;

	return 0;
}

int omap3_isp_sensor_get_exposure(struct omap3_isp_device *isp,
				  unsigned int *exposure)
{
	struct v4l2_ext_control ctrls[1];
	int ret;

	ctrls[0].id = V4L2_CID_EXPOSURE;

	ret = v4l2_subdev_get_controls(isp->sensor.entity, ARRAY_SIZE(ctrls), ctrls);
	if (ret < 0)
		return ret;

	*exposure = ctrls[0].value;
	return 0;
}

int omap3_isp_sensor_set_exposure(struct omap3_isp_device *isp,
				  unsigned int exposure)
{
	struct v4l2_ext_control ctrls[1];

	ctrls[0].id = V4L2_CID_EXPOSURE;
	ctrls[0].value = exposure;

	return v4l2_subdev_set_controls(isp->sensor.entity, ARRAY_SIZE(ctrls), ctrls);
}

int omap3_isp_sensor_set_gain(struct omap3_isp_device *isp, unsigned int gain)
{
	struct v4l2_ext_control ctrls[1];

	if (!isp->sensor.has_global_gain)
		return omap3_isp_sensor_set_gains(isp, gain, gain, gain);

	ctrls[0].id = V4L2_CID_GAIN;
	ctrls[0].value = gain;

	return v4l2_subdev_set_controls(isp->sensor.entity, ARRAY_SIZE(ctrls), ctrls);
}

int omap3_isp_sensor_set_gains(struct omap3_isp_device *isp,
			       int red, int green, int blue)
{
	struct v4l2_ext_control ctrls[4];
	unsigned int i = 0;

	if (!isp->sensor.has_color_gains)
		return -EINVAL;

	if (red != OMAP3_ISP_SENSOR_GAIN_KEEP) {
		ctrls[i].id = V4L2_CID_GAIN_RED;
		ctrls[i++].value = red;
	}
	if (green != OMAP3_ISP_SENSOR_GAIN_KEEP) {
		ctrls[i].id = V4L2_CID_GAIN_GREEN1;
		ctrls[i++].value = green;
		ctrls[i].id = V4L2_CID_GAIN_GREEN2;
		ctrls[i++].value = green;
	}
	if (blue != OMAP3_ISP_SENSOR_GAIN_KEEP) {
		ctrls[i].id = V4L2_CID_GAIN_BLUE;
		ctrls[i++].value = blue;
	}

	return v4l2_subdev_set_controls(isp->sensor.entity, i, ctrls);
}