/*	--*- c++ -*--
 * Copyright (C) 2017 Enrico Scholz <enrico.scholz@sigma-chemnitz.de>
 *
 * 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; version 3 of the License.
 *
 * 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/>.
 */

#include "bayer2rgb.h"
#include "bayer2rgb-internal.h"
#include "convert-internal.hh"

#define _always_inline_	__attribute__((__always_inline__))

static inline _always_inline_ unsigned int avg1(unsigned int max, int sft,
						unsigned int a)
{
	unsigned int		res = a;

	if (sft) {
		res += (1u << (sft-1));
		res >>= sft;
	}

	if (sft && res > max)
		res = max;

	return res;
}

static inline _always_inline_ unsigned int avg2(
	bool do_round, unsigned int max, int sft,
	unsigned int a, unsigned int b)
{
	unsigned int		res;

	if (!do_round)
		res = (a + b) / 2;
	else
		res = (a + b + 1) / 2;

	if (sft) {
		res += (1u << (sft-1));
		res >>= sft;
	}

	if ((sft || do_round) && res > max)
		res = max;

	return res;
}

static inline _always_inline_ unsigned int avg4(
	bool do_round, unsigned int max, int sft,
	unsigned int a, unsigned int b,
	unsigned int c, unsigned int d)
{
	unsigned int		res;

	if (!do_round)
		res = (a + b + c + d) / 4;
	else
		res = (a + b + c + d + 2) / 4;

	if (sft) {
		res += (1u << (sft-1));
		res >>= sft;
	}

	if ((sft || do_round) && res > max)
		res = max;

	return res;
}

template <typename CONV>	struct conv_bg;
template <typename CONV>	struct conv_gr;
template <typename CONV>	struct conv_gb;
template <typename CONV>	struct conv_rg;

template <typename CONV>
struct conv_bg {
	typedef CONV			info_t;
	typedef struct conv_gb<CONV>	conv_right;
	typedef struct conv_gr<CONV>	conv_down;

	template <typename OPT>
	inline static void conv(typename info_t::pixout_t & __restrict__ out,
				typename info_t::pixin_t const * __restrict__ in,
				size_t stride,
				unsigned int max_rb, unsigned int sft_rb,
				unsigned int max_g, unsigned int sft_g,
				OPT const &opt) {
		CONV::fmtout_t::zero_alpha(out);

		// | r | g | r |
		// | g | B | g |
		// | r | g | r |
		out.r = avg4(opt.round_4,  max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-stride - 1]),
			     CONV::fmtin_t::as_uint(in[-stride + 1]),
			     CONV::fmtin_t::as_uint(in[+stride - 1]),
			     CONV::fmtin_t::as_uint(in[+stride + 1]));
		out.b = avg1(max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[0]));
		out.g = avg4(opt.round_4, max_g, sft_g,
			     CONV::fmtin_t::as_uint(in[-1]),
			     CONV::fmtin_t::as_uint(in[+1]),
			     CONV::fmtin_t::as_uint(in[-stride]),
			     CONV::fmtin_t::as_uint(in[+stride]));
	}
};

template <typename CONV>
struct conv_rg {
	typedef CONV			info_t;
	typedef struct conv_gr<CONV>	conv_right;
	typedef struct conv_gb<CONV>	conv_down;

	template <typename OPT>
	inline static void conv(typename info_t::pixout_t & __restrict__ out,
				typename info_t::pixin_t const * __restrict__ in,
				size_t stride,
				unsigned int max_rb, unsigned int sft_rb,
				unsigned int max_g, unsigned int sft_g,
				OPT const &opt) {
		CONV::fmtout_t::zero_alpha(out);

		// | b | g | b |
		// | g | R | g |
		// | b | g | b |
		out.r = avg1(max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[0]));
		out.b = avg4(opt.round_4, max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-stride - 1]),
			     CONV::fmtin_t::as_uint(in[-stride + 1]),
			     CONV::fmtin_t::as_uint(in[+stride - 1]),
			     CONV::fmtin_t::as_uint(in[+stride + 1]));
		out.g = avg4(opt.round_4, max_g, sft_g,
			     CONV::fmtin_t::as_uint(in[-stride]),
			     CONV::fmtin_t::as_uint(in[+stride]),
			     CONV::fmtin_t::as_uint(in[-1]),
			     CONV::fmtin_t::as_uint(in[+1]));
	}
};

template <typename CONV>
struct conv_gr {
	typedef CONV			info_t;
	typedef struct conv_rg<CONV>	conv_right;
	typedef struct conv_bg<CONV>	conv_down;

	template <typename OPT>
	inline static void conv(typename info_t::pixout_t & __restrict__ out,
				typename info_t::pixin_t const * __restrict__ in,
				size_t stride,
				unsigned int max_rb, unsigned int sft_rb,
				unsigned int max_g, unsigned int sft_g,
				OPT const &opt) {
		CONV::fmtout_t::zero_alpha(out);

		// | g | b | g |
		// | r | G | r |
		// | g | b | g |
		out.r = avg2(opt.round_2, max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-1]),
			     CONV::fmtin_t::as_uint(in[+1]));
		out.b = avg2(opt.round_2, max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-stride]),
			     CONV::fmtin_t::as_uint(in[+stride]));
		out.g = avg1(max_g, sft_g,
			     CONV::fmtin_t::as_uint(in[0]));
	}
};

template <typename CONV>
struct conv_gb {
	typedef CONV			info_t;
	typedef struct conv_bg<CONV>	conv_right;
	typedef struct conv_rg<CONV>	conv_down;

	template <typename OPT>
	inline static void conv(typename info_t::pixout_t & __restrict__ out,
				typename info_t::pixin_t const *__restrict__ in,
				size_t stride,
				unsigned int max_rb, unsigned int sft_rb,
				unsigned int max_g, unsigned int sft_g,
				OPT const &opt) {
		CONV::fmtout_t::zero_alpha(out);

		// | g | r | g |
		// | b | G | b |
		// | g | r | g |
		out.r = avg2(opt.round_2, max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-stride]),
			     CONV::fmtin_t::as_uint(in[+stride]));
		out.b = avg2(opt.round_2, max_rb, sft_rb,
			     CONV::fmtin_t::as_uint(in[-1]),
			     CONV::fmtin_t::as_uint(in[+1]));
		out.g = avg1(max_g, sft_g,
			     CONV::fmtin_t::as_uint(in[0]));
	}
};

#define assert_same_type(_a, _b) do {		\
		_a		*a;		\
		_b const	*b;		\
		if (0)				\
			*a = *b;		\
	} while (0)

template <typename CONV_FN, typename OPT>
static void convert_inner1(struct image_in const *input,
			   struct image_out const *output,
			   struct image_conversion_info *info,
			   OPT const &opt)
{
	typedef CONV_FN				_conv00;
	typedef typename _conv00::conv_right	_conv01;
	typedef typename _conv00::conv_down	_conv10;
	typedef typename _conv10::conv_right	_conv11;

	assert_same_type(_conv00, typename _conv01::conv_right);
	assert_same_type(_conv01, typename _conv11::conv_down);
	assert_same_type(_conv10, typename _conv11::conv_right);
	assert_same_type(_conv11, typename _conv01::conv_down);

	typedef typename CONV_FN::info_t	CI;
	typedef typename CI::pixout_t		pixout_t;
	typedef typename CI::pixin_t		pixin_t;

	static unsigned int const	border = 2;

	static_assert(CI::fmtin_t::bpp >= CI::fmtout_t::bpp_rb,
		      "output R/B width larger than input");
	static_assert(CI::fmtin_t::bpp >= CI::fmtout_t::bpp_g,
		      "output G width larger than input");

	static unsigned int const	max_rb = CI::fmtout_t::max_rb;
	static unsigned int const	max_g  = CI::fmtout_t::max_g;
	static unsigned int const	sft_rb = (CI::fmtin_t::bpp -
						  CI::fmtout_t::bpp_rb);
	static unsigned int const	sft_g  = (CI::fmtin_t::bpp -
						  CI::fmtout_t::bpp_g);

	pixout_t * __restrict__	out = static_cast<pixout_t *>(output->data);
	pixin_t const		*in;

	size_t			stride;
	size_t			out_stride;

	out_stride = output->info.stride / sizeof *out;
	stride	   = input->info.stride  / sizeof *in;

	/* move input to (1,1) pixel */
	in  = static_cast<pixin_t const *>(input->data);
	in += border * stride;
	in += border;

	/* Step 5: the real conversion */

	for (size_t y = border; y+border < input->info.h; y += 2) {
		pixout_t	*out_next = out + 2 * out_stride;

		/* skip 2 columns left and 2 columns right */
		for (size_t x = border; x+border < input->info.w; x += 2) {
			_conv00::conv(
				out[0], in,
				stride, max_rb, sft_rb, max_g, sft_g, opt);
			_conv01::conv(
				out[1], in + 1,
				stride, max_rb, sft_rb, max_g, sft_g, opt);
			_conv10::conv(
				out[out_stride], in + stride,
				stride, max_rb, sft_rb, max_g, sft_g, opt);
			_conv11::conv(
				out[out_stride + 1], in + stride + 1,
				stride, max_rb, sft_rb, max_g, sft_g, opt);

			out += 2;
			in  += 2;
		}

		out  = out_next;
		in  += stride + 2*border;
	}

	if (info)
		info->fn = __func__;
}

template <typename CI>
static void convert_inner(struct image_in const *input,
			  struct image_out const *output,
			  struct image_conversion_info *info)
{
	bool		_round_2 = output->quality & (1u << QUALITY_ROUND_2);
	bool		_round_4 = output->quality & (1u << QUALITY_ROUND_4);

	/* Step 4: apply optimizaton options */

	if (!_round_2 && !_round_4) {
		auto	opt = avg_fixedopt<false, false>();
		convert_inner1<CI>(input, output, info, opt);
	} else if (_round_2 && _round_4) {
		auto	opt = avg_fixedopt<true, true>();
		convert_inner1<CI>(input, output, info, opt);
	} else {
		auto	opt = avg_dynopt(_round_2, _round_4);
		convert_inner1<CI>(input, output, info, opt);
	}
}

template <typename INFMT, typename OUTFMT>
static bool convert_outer1(struct image_in const *input,
			   struct image_out const *output,
			   struct image_conversion_info *info)
{
	typedef struct conversion<INFMT, OUTFMT>	conv_t;

	/* Step 3: split by input color order */
	switch (input->type) {
	case image_in::BAYER_GBRG:
		convert_inner<conv_gb<conv_t>>(input, output, info);
		return true;

	case image_in::BAYER_RGGB:
		convert_inner<conv_rg<conv_t>>(input, output, info);
		return true;

	case image_in::BAYER_BGGR:
		convert_inner<conv_bg<conv_t>>(input, output, info);
		return true;

	case image_in::BAYER_GRBG:
		convert_inner<conv_gr<conv_t>>(input, output, info);
		return true;

	default:
		set_fallback_reason(info, "cc: unsupported input format");
		return false;
	}
}

template <typename OUTFMT>
static bool convert_outer0(struct image_in const *input,
			   struct image_out const *output,
			   struct image_conversion_info *info)
{
	/* Step 2: split by input bpp */
	switch (input->info.bpp) {
	case 8:
		return convert_outer1<uint8_t, OUTFMT>(input, output, info);

	case 10:
		if (input->info.endian == image_info::BAYER_E_LITTLE)
			return convert_outer1<struct bitfmt_10le, OUTFMT>(input, output, info);
		else
			return convert_outer1<struct bitfmt_10be, OUTFMT>(input, output, info);

	case 12:
		if (input->info.endian == image_info::BAYER_E_LITTLE)
			return convert_outer1<struct bitfmt_12le, OUTFMT>(input, output, info);
		else
			return convert_outer1<struct bitfmt_12be, OUTFMT>(input, output, info);

	case 16:
		if (input->info.endian == image_info::BAYER_E_LITTLE)
			return convert_outer1<struct bitfmt_16le, OUTFMT>(input, output, info);
		else
			return convert_outer1<struct bitfmt_16be, OUTFMT>(input, output, info);

	default:
		set_fallback_reason(info, "cc: unsupported input bpp");
		return false;
	}
}

void bayer2rgb_convert_cc(struct image_in const *input,
			  struct image_out const *output,
			  struct image_conversion_info *info)
{
	bool				handled = false;

	/* Step 1: split by output format */
	switch (output->type) {
	case image_out::RGB_FMT_RGBx:
		handled = convert_outer0<struct rgbx32_pixel>(input, output, info);
		break;
	case image_out::RGB_FMT_BGRx:
		handled = convert_outer0<struct bgrx32_pixel>(input, output, info);
		break;
	case image_out::RGB_FMT_xBGR:
		handled = convert_outer0<struct xbgr32_pixel>(input, output, info);
		break;
	case image_out::RGB_FMT_xRGB:
		handled = convert_outer0<struct xbgr32_pixel>(input, output, info);
		break;
	case image_out::RGB_FMT_RGB16:
		handled = convert_outer0<struct rgb16_pixel>(input, output, info);
		break;
	default:
		set_fallback_reason(info, "cc: unsupported output format");
		handled = false;
		break;
	}

	if (!handled)
		abort();
}