/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi Ltd
 *
 * awb.cpp - AWB control algorithm
 */

#include <assert.h>
#include <functional>

#include <libcamera/base/log.h>

#include "../lux_status.h"

#include "awb.h"

using namespace RPiController;
using namespace libcamera;

LOG_DEFINE_CATEGORY(RPiAwb)

#define NAME "rpi.awb"

static constexpr unsigned int AwbStatsSizeX = DEFAULT_AWB_REGIONS_X;
static constexpr unsigned int AwbStatsSizeY = DEFAULT_AWB_REGIONS_Y;

/*
 * todo - the locking in this algorithm needs some tidying up as has been done
 * elsewhere (ALSC and AGC).
 */

int AwbMode::read(const libcamera::YamlObject &params)
{
	auto value = params["lo"].get<double>();
	if (!value)
		return -EINVAL;
	ctLo = *value;

	value = params["hi"].get<double>();
	if (!value)
		return -EINVAL;
	ctHi = *value;

	return 0;
}

int AwbPrior::read(const libcamera::YamlObject &params)
{
	auto value = params["lux"].get<double>();
	if (!value)
		return -EINVAL;
	lux = *value;

	return prior.read(params["prior"]);
}

static int readCtCurve(Pwl &ctR, Pwl &ctB, const libcamera::YamlObject &params)
{
	if (params.size() % 3) {
		LOG(RPiAwb, Error) << "AwbConfig: incomplete CT curve entry";
		return -EINVAL;
	}

	if (params.size() < 6) {
		LOG(RPiAwb, Error) << "AwbConfig: insufficient points in CT curve";
		return -EINVAL;
	}

	const auto &list = params.asList();

	for (auto it = list.begin(); it != list.end(); it++) {
		auto value = it->get<double>();
		if (!value)
			return -EINVAL;
		double ct = *value;

		assert(it == list.begin() || ct != ctR.domain().end);

		value = (++it)->get<double>();
		if (!value)
			return -EINVAL;
		ctR.append(ct, *value);

		value = (++it)->get<double>();
		if (!value)
			return -EINVAL;
		ctB.append(ct, *value);
	}

	return 0;
}

int AwbConfig::read(const libcamera::YamlObject &params)
{
	int ret;

	bayes = params["bayes"].get<int>(1);
	framePeriod = params["frame_period"].get<uint16_t>(10);
	startupFrames = params["startup_frames"].get<uint16_t>(10);
	convergenceFrames = params["convergence_frames"].get<unsigned int>(3);
	speed = params["speed"].get<double>(0.05);

	if (params.contains("ct_curve")) {
		ret = readCtCurve(ctR, ctB, params["ct_curve"]);
		if (ret)
			return ret;
		/* We will want the inverse functions of these too. */
		ctRInverse = ctR.inverse();
		ctBInverse = ctB.inverse();
	}

	if (params.contains("priors")) {
		for (const auto &p : params["priors"].asList()) {
			AwbPrior prior;
			ret = prior.read(p);
			if (ret)
				return ret;
			if (!priors.empty() && prior.lux <= priors.back().lux) {
				LOG(RPiAwb, Error) << "AwbConfig: Prior must be ordered in increasing lux value";
				return -EINVAL;
			}
			priors.push_back(prior);
		}
		if (priors.empty()) {
			LOG(RPiAwb, Error) << "AwbConfig: no AWB priors configured";
			return ret;
		}
	}
	if (params.contains("modes")) {
		for (const auto &[key, value] : params["modes"].asDict()) {
			ret = modes[key].read(value);
			if (ret)
				return ret;
			if (defaultMode == nullptr)
				defaultMode = &modes[key];
		}
		if (defaultMode == nullptr) {
			LOG(RPiAwb, Error) << "AwbConfig: no AWB modes configured";
			return -EINVAL;
		}
	}

	minPixels = params["min_pixels"].get<double>(16.0);
	minG = params["min_G"].get<uint16_t>(32);
	minRegions = params["min_regions"].get<uint32_t>(10);
	deltaLimit = params["delta_limit"].get<double>(0.2);
	coarseStep = params["coarse_step"].get<double>(0.2);
	transversePos = params["transverse_pos"].get<double>(0.01);
	transverseNeg = params["transverse_neg"].get<double>(0.01);
	if (transversePos <= 0 || transverseNeg <= 0) {
		LOG(RPiAwb, Error) << "AwbConfig: transverse_pos/neg must be > 0";
		return -EINVAL;
	}

	sensitivityR = params["sensitivity_r"].get<double>(1.0);
	sensitivityB = params["sensitivity_b"].get<double>(1.0);

	if (bayes) {
		if (ctR.empty() || ctB.empty() || priors.empty() ||
		    defaultMode == nullptr) {
			LOG(RPiAwb, Warning)
				<< "Bayesian AWB mis-configured - switch to Grey method";
			bayes = false;
		}
	}
	fast = params[fast].get<int>(bayes); /* default to fast for Bayesian, otherwise slow */
	whitepointR = params["whitepoint_r"].get<double>(0.0);
	whitepointB = params["whitepoint_b"].get<double>(0.0);
	if (bayes == false)
		sensitivityR = sensitivityB = 1.0; /* nor do sensitivities make any sense */
	return 0;
}

Awb::Awb(Controller *controller)
	: AwbAlgorithm(controller)
{
	asyncAbort_ = asyncStart_ = asyncStarted_ = asyncFinished_ = false;
	mode_ = nullptr;
	manualR_ = manualB_ = 0.0;
	asyncThread_ = std::thread(std::bind(&Awb::asyncFunc, this));
}

Awb::~Awb()
{
	{
		std::lock_guard<std::mutex> lock(mutex_);
		asyncAbort_ = true;
	}
	asyncSignal_.notify_one();
	asyncThread_.join();
}

char const *Awb::name() const
{
	return NAME;
}

int Awb::read(const libcamera::YamlObject &params)
{
	return config_.read(params);
}

void Awb::initialise()
{
	frameCount_ = framePhase_ = 0;
	/*
	 * Put something sane into the status that we are filtering towards,
	 * just in case the first few frames don't have anything meaningful in
	 * them.
	 */
	if (!config_.ctR.empty() && !config_.ctB.empty()) {
		syncResults_.temperatureK = config_.ctR.domain().clip(4000);
		syncResults_.gainR = 1.0 / config_.ctR.eval(syncResults_.temperatureK);
		syncResults_.gainG = 1.0;
		syncResults_.gainB = 1.0 / config_.ctB.eval(syncResults_.temperatureK);
	} else {
		/* random values just to stop the world blowing up */
		syncResults_.temperatureK = 4500;
		syncResults_.gainR = syncResults_.gainG = syncResults_.gainB = 1.0;
	}
	prevSyncResults_ = syncResults_;
	asyncResults_ = syncResults_;
}

bool Awb::isPaused() const
{
	return false;
}

void Awb::pause()
{
	/* "Pause" by fixing everything to the most recent values. */
	manualR_ = syncResults_.gainR = prevSyncResults_.gainR;
	manualB_ = syncResults_.gainB = prevSyncResults_.gainB;
	syncResults_.gainG = prevSyncResults_.gainG;
	syncResults_.temperatureK = prevSyncResults_.temperatureK;
}

void Awb::resume()
{
	manualR_ = 0.0;
	manualB_ = 0.0;
}

unsigned int Awb::getConvergenceFrames() const
{
	/*
	 * If not in auto mode, there is no convergence
	 * to happen, so no need to drop any frames - return zero.
	 */
	if (!isAutoEnabled())
		return 0;
	else
		return config_.convergenceFrames;
}

void Awb::setMode(std::string const &modeName)
{
	modeName_ = modeName;
}

void Awb::setManualGains(double manualR, double manualB)
{
	/* If any of these are 0.0, we swich back to auto. */
	manualR_ = manualR;
	manualB_ = manualB;
	/*
	 * If not in auto mode, set these values into the syncResults which
	 * means that Prepare() will adopt them immediately.
	 */
	if (!isAutoEnabled()) {
		syncResults_.gainR = prevSyncResults_.gainR = manualR_;
		syncResults_.gainG = prevSyncResults_.gainG = 1.0;
		syncResults_.gainB = prevSyncResults_.gainB = manualB_;
		if (config_.bayes) {
			/* Also estimate the best corresponding colour temperature from the curves. */
			double ctR = config_.ctRInverse.eval(config_.ctRInverse.domain().clip(1 / manualR_));
			double ctB = config_.ctBInverse.eval(config_.ctBInverse.domain().clip(1 / manualB_));
			prevSyncResults_.temperatureK = (ctR + ctB) / 2;
			syncResults_.temperatureK = prevSyncResults_.temperatureK;
		}
	}
}

void Awb::switchMode([[maybe_unused]] CameraMode const &cameraMode,
		     Metadata *metadata)
{
	/* Let other algorithms know the current white balance values. */
	metadata->set("awb.status", prevSyncResults_);
}

bool Awb::isAutoEnabled() const
{
	return manualR_ == 0.0 || manualB_ == 0.0;
}

void Awb::fetchAsyncResults()
{
	LOG(RPiAwb, Debug) << "Fetch AWB results";
	asyncFinished_ = false;
	asyncStarted_ = false;
	/*
	 * It's possible manual gains could be set even while the async
	 * thread was running, so only copy the results if still in auto mode.
	 */
	if (isAutoEnabled())
		syncResults_ = asyncResults_;
}

void Awb::restartAsync(StatisticsPtr &stats, double lux)
{
	LOG(RPiAwb, Debug) << "Starting AWB calculation";
	/* this makes a new reference which belongs to the asynchronous thread */
	statistics_ = stats;
	/* store the mode as it could technically change */
	auto m = config_.modes.find(modeName_);
	mode_ = m != config_.modes.end()
			? &m->second
			: (mode_ == nullptr ? config_.defaultMode : mode_);
	lux_ = lux;
	framePhase_ = 0;
	asyncStarted_ = true;
	size_t len = modeName_.copy(asyncResults_.mode,
				    sizeof(asyncResults_.mode) - 1);
	asyncResults_.mode[len] = '\0';
	{
		std::lock_guard<std::mutex> lock(mutex_);
		asyncStart_ = true;
	}
	asyncSignal_.notify_one();
}

void Awb::prepare(Metadata *imageMetadata)
{
	if (frameCount_ < (int)config_.startupFrames)
		frameCount_++;
	double speed = frameCount_ < (int)config_.startupFrames
			       ? 1.0
			       : config_.speed;
	LOG(RPiAwb, Debug)
		<< "frame_count " << frameCount_ << " speed " << speed;
	{
		std::unique_lock<std::mutex> lock(mutex_);
		if (asyncStarted_ && asyncFinished_)
			fetchAsyncResults();
	}
	/* Finally apply IIR filter to results and put into metadata. */
	memcpy(prevSyncResults_.mode, syncResults_.mode,
	       sizeof(prevSyncResults_.mode));
	prevSyncResults_.temperatureK = speed * syncResults_.temperatureK +
					(1.0 - speed) * prevSyncResults_.temperatureK;
	prevSyncResults_.gainR = speed * syncResults_.gainR +
				 (1.0 - speed) * prevSyncResults_.gainR;
	prevSyncResults_.gainG = speed * syncResults_.gainG +
				 (1.0 - speed) * prevSyncResults_.gainG;
	prevSyncResults_.gainB = speed * syncResults_.gainB +
				 (1.0 - speed) * prevSyncResults_.gainB;
	imageMetadata->set("awb.status", prevSyncResults_);
	LOG(RPiAwb, Debug)
		<< "Using AWB gains r " << prevSyncResults_.gainR << " g "
		<< prevSyncResults_.gainG << " b "
		<< prevSyncResults_.gainB;
}

void Awb::process(StatisticsPtr &stats, Metadata *imageMetadata)
{
	/* Count frames since we last poked the async thread. */
	if (framePhase_ < (int)config_.framePeriod)
		framePhase_++;
	LOG(RPiAwb, Debug) << "frame_phase " << framePhase_;
	/* We do not restart the async thread if we're not in auto mode. */
	if (isAutoEnabled() &&
	    (framePhase_ >= (int)config_.framePeriod ||
	     frameCount_ < (int)config_.startupFrames)) {
		/* Update any settings and any image metadata that we need. */
		struct LuxStatus luxStatus = {};
		luxStatus.lux = 400; /* in case no metadata */
		if (imageMetadata->get("lux.status", luxStatus) != 0)
			LOG(RPiAwb, Debug) << "No lux metadata found";
		LOG(RPiAwb, Debug) << "Awb lux value is " << luxStatus.lux;

		if (asyncStarted_ == false)
			restartAsync(stats, luxStatus.lux);
	}
}

void Awb::asyncFunc()
{
	while (true) {
		{
			std::unique_lock<std::mutex> lock(mutex_);
			asyncSignal_.wait(lock, [&] {
				return asyncStart_ || asyncAbort_;
			});
			asyncStart_ = false;
			if (asyncAbort_)
				break;
		}
		doAwb();
		{
			std::lock_guard<std::mutex> lock(mutex_);
			asyncFinished_ = true;
		}
		syncSignal_.notify_one();
	}
}

static void generateStats(std::vector<Awb::RGB> &zones,
			  bcm2835_isp_stats_region *stats, double minPixels,
			  double minG)
{
	for (unsigned int i = 0; i < AwbStatsSizeX * AwbStatsSizeY; i++) {
		Awb::RGB zone;
		double counted = stats[i].counted;
		if (counted >= minPixels) {
			zone.G = stats[i].g_sum / counted;
			if (zone.G >= minG) {
				zone.R = stats[i].r_sum / counted;
				zone.B = stats[i].b_sum / counted;
				zones.push_back(zone);
			}
		}
	}
}

void Awb::prepareStats()
{
	zones_.clear();
	/*
	 * LSC has already been applied to the stats in this pipeline, so stop
	 * any LSC compensation.  We also ignore config_.fast in this version.
	 */
	generateStats(zones_, statistics_->awb_stats, config_.minPixels,
		      config_.minG);
	/*
	 * we're done with these; we may as well relinquish our hold on the
	 * pointer.
	 */
	statistics_.reset();
	/*
	 * apply sensitivities, so values appear to come from our "canonical"
	 * sensor.
	 */
	for (auto &zone : zones_) {
		zone.R *= config_.sensitivityR;
		zone.B *= config_.sensitivityB;
	}
}

double Awb::computeDelta2Sum(double gainR, double gainB)
{
	/*
	 * Compute the sum of the squared colour error (non-greyness) as it
	 * appears in the log likelihood equation.
	 */
	double delta2Sum = 0;
	for (auto &z : zones_) {
		double deltaR = gainR * z.R - 1 - config_.whitepointR;
		double deltaB = gainB * z.B - 1 - config_.whitepointB;
		double delta2 = deltaR * deltaR + deltaB * deltaB;
		/* LOG(RPiAwb, Debug) << "deltaR " << deltaR << " deltaB " << deltaB << " delta2 " << delta2; */
		delta2 = std::min(delta2, config_.deltaLimit);
		delta2Sum += delta2;
	}
	return delta2Sum;
}

Pwl Awb::interpolatePrior()
{
	/*
	 * Interpolate the prior log likelihood function for our current lux
	 * value.
	 */
	if (lux_ <= config_.priors.front().lux)
		return config_.priors.front().prior;
	else if (lux_ >= config_.priors.back().lux)
		return config_.priors.back().prior;
	else {
		int idx = 0;
		/* find which two we lie between */
		while (config_.priors[idx + 1].lux < lux_)
			idx++;
		double lux0 = config_.priors[idx].lux,
		       lux1 = config_.priors[idx + 1].lux;
		return Pwl::combine(config_.priors[idx].prior,
				    config_.priors[idx + 1].prior,
				    [&](double /*x*/, double y0, double y1) {
					    return y0 + (y1 - y0) *
							(lux_ - lux0) / (lux1 - lux0);
				    });
	}
}

static double interpolateQuadatric(Pwl::Point const &a, Pwl::Point const &b,
				   Pwl::Point const &c)
{
	/*
	 * Given 3 points on a curve, find the extremum of the function in that
	 * interval by fitting a quadratic.
	 */
	const double eps = 1e-3;
	Pwl::Point ca = c - a, ba = b - a;
	double denominator = 2 * (ba.y * ca.x - ca.y * ba.x);
	if (abs(denominator) > eps) {
		double numerator = ba.y * ca.x * ca.x - ca.y * ba.x * ba.x;
		double result = numerator / denominator + a.x;
		return std::max(a.x, std::min(c.x, result));
	}
	/* has degenerated to straight line segment */
	return a.y < c.y - eps ? a.x : (c.y < a.y - eps ? c.x : b.x);
}

double Awb::coarseSearch(Pwl const &prior)
{
	points_.clear(); /* assume doesn't deallocate memory */
	size_t bestPoint = 0;
	double t = mode_->ctLo;
	int spanR = 0, spanB = 0;
	/* Step down the CT curve evaluating log likelihood. */
	while (true) {
		double r = config_.ctR.eval(t, &spanR);
		double b = config_.ctB.eval(t, &spanB);
		double gainR = 1 / r, gainB = 1 / b;
		double delta2Sum = computeDelta2Sum(gainR, gainB);
		double priorLogLikelihood = prior.eval(prior.domain().clip(t));
		double finalLogLikelihood = delta2Sum - priorLogLikelihood;
		LOG(RPiAwb, Debug)
			<< "t: " << t << " gain R " << gainR << " gain B "
			<< gainB << " delta2_sum " << delta2Sum
			<< " prior " << priorLogLikelihood << " final "
			<< finalLogLikelihood;
		points_.push_back(Pwl::Point(t, finalLogLikelihood));
		if (points_.back().y < points_[bestPoint].y)
			bestPoint = points_.size() - 1;
		if (t == mode_->ctHi)
			break;
		/* for even steps along the r/b curve scale them by the current t */
		t = std::min(t + t / 10 * config_.coarseStep, mode_->ctHi);
	}
	t = points_[bestPoint].x;
	LOG(RPiAwb, Debug) << "Coarse search found CT " << t;
	/*
	 * We have the best point of the search, but refine it with a quadratic
	 * interpolation around its neighbours.
	 */
	if (points_.size() > 2) {
		unsigned long bp = std::min(bestPoint, points_.size() - 2);
		bestPoint = std::max(1UL, bp);
		t = interpolateQuadatric(points_[bestPoint - 1],
					 points_[bestPoint],
					 points_[bestPoint + 1]);
		LOG(RPiAwb, Debug)
			<< "After quadratic refinement, coarse search has CT "
			<< t;
	}
	return t;
}

void Awb::fineSearch(double &t, double &r, double &b, Pwl const &prior)
{
	int spanR = -1, spanB = -1;
	config_.ctR.eval(t, &spanR);
	config_.ctB.eval(t, &spanB);
	double step = t / 10 * config_.coarseStep * 0.1;
	int nsteps = 5;
	double rDiff = config_.ctR.eval(t + nsteps * step, &spanR) -
		       config_.ctR.eval(t - nsteps * step, &spanR);
	double bDiff = config_.ctB.eval(t + nsteps * step, &spanB) -
		       config_.ctB.eval(t - nsteps * step, &spanB);
	Pwl::Point transverse(bDiff, -rDiff);
	if (transverse.len2() < 1e-6)
		return;
	/*
	 * unit vector orthogonal to the b vs. r function (pointing outwards
	 * with r and b increasing)
	 */
	transverse = transverse / transverse.len();
	double bestLogLikelihood = 0, bestT = 0, bestR = 0, bestB = 0;
	double transverseRange = config_.transverseNeg + config_.transversePos;
	const int maxNumDeltas = 12;
	/* a transverse step approximately every 0.01 r/b units */
	int numDeltas = floor(transverseRange * 100 + 0.5) + 1;
	numDeltas = numDeltas < 3 ? 3 : (numDeltas > maxNumDeltas ? maxNumDeltas : numDeltas);
	/*
	 * Step down CT curve. March a bit further if the transverse range is
	 * large.
	 */
	nsteps += numDeltas;
	for (int i = -nsteps; i <= nsteps; i++) {
		double tTest = t + i * step;
		double priorLogLikelihood =
			prior.eval(prior.domain().clip(tTest));
		double rCurve = config_.ctR.eval(tTest, &spanR);
		double bCurve = config_.ctB.eval(tTest, &spanB);
		/* x will be distance off the curve, y the log likelihood there */
		Pwl::Point points[maxNumDeltas];
		int bestPoint = 0;
		/* Take some measurements transversely *off* the CT curve. */
		for (int j = 0; j < numDeltas; j++) {
			points[j].x = -config_.transverseNeg +
				      (transverseRange * j) / (numDeltas - 1);
			Pwl::Point rbTest = Pwl::Point(rCurve, bCurve) +
					    transverse * points[j].x;
			double rTest = rbTest.x, bTest = rbTest.y;
			double gainR = 1 / rTest, gainB = 1 / bTest;
			double delta2Sum = computeDelta2Sum(gainR, gainB);
			points[j].y = delta2Sum - priorLogLikelihood;
			LOG(RPiAwb, Debug)
				<< "At t " << tTest << " r " << rTest << " b "
				<< bTest << ": " << points[j].y;
			if (points[j].y < points[bestPoint].y)
				bestPoint = j;
		}
		/*
		 * We have NUM_DELTAS points transversely across the CT curve,
		 * now let's do a quadratic interpolation for the best result.
		 */
		bestPoint = std::max(1, std::min(bestPoint, numDeltas - 2));
		Pwl::Point rbTest = Pwl::Point(rCurve, bCurve) +
					transverse * interpolateQuadatric(points[bestPoint - 1],
									points[bestPoint],
									points[bestPoint + 1]);
		double rTest = rbTest.x, bTest = rbTest.y;
		double gainR = 1 / rTest, gainB = 1 / bTest;
		double delta2Sum = computeDelta2Sum(gainR, gainB);
		double finalLogLikelihood = delta2Sum - priorLogLikelihood;
		LOG(RPiAwb, Debug)
			<< "Finally "
			<< tTest << " r " << rTest << " b " << bTest << ": "
			<< finalLogLikelihood
			<< (finalLogLikelihood < bestLogLikelihood ? " BEST" : "");
		if (bestT == 0 || finalLogLikelihood < bestLogLikelihood)
			bestLogLikelihood = finalLogLikelihood,
			bestT = tTest, bestR = rTest, bestB = bTest;
	}
	t = bestT, r = bestR, b = bestB;
	LOG(RPiAwb, Debug)
		<< "Fine search found t " << t << " r " << r << " b " << b;
}

void Awb::awbBayes()
{
	/*
	 * May as well divide out G to save computeDelta2Sum from doing it over
	 * and over.
	 */
	for (auto &z : zones_)
		z.R = z.R / (z.G + 1), z.B = z.B / (z.G + 1);
	/*
	 * Get the current prior, and scale according to how many zones are
	 * valid... not entirely sure about this.
	 */
	Pwl prior = interpolatePrior();
	prior *= zones_.size() / (double)(AwbStatsSizeX * AwbStatsSizeY);
	prior.map([](double x, double y) {
		LOG(RPiAwb, Debug) << "(" << x << "," << y << ")";
	});
	double t = coarseSearch(prior);
	double r = config_.ctR.eval(t);
	double b = config_.ctB.eval(t);
	LOG(RPiAwb, Debug)
		<< "After coarse search: r " << r << " b " << b << " (gains r "
		<< 1 / r << " b " << 1 / b << ")";
	/*
	 * Not entirely sure how to handle the fine search yet. Mostly the
	 * estimated CT is already good enough, but the fine search allows us to
	 * wander transverely off the CT curve. Under some illuminants, where
	 * there may be more or less green light, this may prove beneficial,
	 * though I probably need more real datasets before deciding exactly how
	 * this should be controlled and tuned.
	 */
	fineSearch(t, r, b, prior);
	LOG(RPiAwb, Debug)
		<< "After fine search: r " << r << " b " << b << " (gains r "
		<< 1 / r << " b " << 1 / b << ")";
	/*
	 * Write results out for the main thread to pick up. Remember to adjust
	 * the gains from the ones that the "canonical sensor" would require to
	 * the ones needed by *this* sensor.
	 */
	asyncResults_.temperatureK = t;
	asyncResults_.gainR = 1.0 / r * config_.sensitivityR;
	asyncResults_.gainG = 1.0;
	asyncResults_.gainB = 1.0 / b * config_.sensitivityB;
}

void Awb::awbGrey()
{
	LOG(RPiAwb, Debug) << "Grey world AWB";
	/*
	 * Make a separate list of the derivatives for each of red and blue, so
	 * that we can sort them to exclude the extreme gains.  We could
	 * consider some variations, such as normalising all the zones first, or
	 * doing an L2 average etc.
	 */
	std::vector<RGB> &derivsR(zones_);
	std::vector<RGB> derivsB(derivsR);
	std::sort(derivsR.begin(), derivsR.end(),
		  [](RGB const &a, RGB const &b) {
			  return a.G * b.R < b.G * a.R;
		  });
	std::sort(derivsB.begin(), derivsB.end(),
		  [](RGB const &a, RGB const &b) {
			  return a.G * b.B < b.G * a.B;
		  });
	/* Average the middle half of the values. */
	int discard = derivsR.size() / 4;
	RGB sumR(0, 0, 0), sumB(0, 0, 0);
	for (auto ri = derivsR.begin() + discard,
		  bi = derivsB.begin() + discard;
	     ri != derivsR.end() - discard; ri++, bi++)
		sumR += *ri, sumB += *bi;
	double gainR = sumR.G / (sumR.R + 1),
	       gainB = sumB.G / (sumB.B + 1);
	asyncResults_.temperatureK = 4500; /* don't know what it is */
	asyncResults_.gainR = gainR;
	asyncResults_.gainG = 1.0;
	asyncResults_.gainB = gainB;
}

void Awb::doAwb()
{
	prepareStats();
	LOG(RPiAwb, Debug) << "Valid zones: " << zones_.size();
	if (zones_.size() > config_.minRegions) {
		if (config_.bayes)
			awbBayes();
		else
			awbGrey();
		LOG(RPiAwb, Debug)
			<< "CT found is "
			<< asyncResults_.temperatureK
			<< " with gains r " << asyncResults_.gainR
			<< " and b " << asyncResults_.gainB;
	}
}

/* Register algorithm with the system. */
static Algorithm *create(Controller *controller)
{
	return (Algorithm *)new Awb(controller);
}
static RegisterAlgorithm reg(NAME, &create);

<a id='n558' href='#n558'>558</a>
<a id='n559' href='#n559'>559</a>
<a id='n560' href='#n560'>560</a>
<a id='n561' href='#n561'>561</a>
<a id='n562' href='#n562'>562</a>
<a id='n563' href='#n563'>563</a>
<a id='n564' href='#n564'>564</a>
<a id='n565' href='#n565'>565</a>
<a id='n566' href='#n566'>566</a>
<a id='n567' href='#n567'>567</a>
<a id='n568' href='#n568'>568</a>
<a id='n569' href='#n569'>569</a>
<a id='n570' href='#n570'>570</a>
<a id='n571' href='#n571'>571</a>
<a id='n572' href='#n572'>572</a>
<a id='n573' href='#n573'>573</a>
<a id='n574' href='#n574'>574</a>
<a id='n575' href='#n575'>575</a>
<a id='n576' href='#n576'>576</a>
<a id='n577' href='#n577'>577</a>
<a id='n578' href='#n578'>578</a>
<a id='n579' href='#n579'>579</a>
<a id='n580' href='#n580'>580</a>
<a id='n581' href='#n581'>581</a>
<a id='n582' href='#n582'>582</a>
<a id='n583' href='#n583'>583</a>
<a id='n584' href='#n584'>584</a>
<a id='n585' href='#n585'>585</a>
<a id='n586' href='#n586'>586</a>
<a id='n587' href='#n587'>587</a>
<a id='n588' href='#n588'>588</a>
<a id='n589' href='#n589'>589</a>
<a id='n590' href='#n590'>590</a>
<a id='n591' href='#n591'>591</a>
<a id='n592' href='#n592'>592</a>
<a id='n593' href='#n593'>593</a>
<a id='n594' href='#n594'>594</a>
<a id='n595' href='#n595'>595</a>
<a id='n596' href='#n596'>596</a>
<a id='n597' href='#n597'>597</a>
<a id='n598' href='#n598'>598</a>
<a id='n599' href='#n599'>599</a>
<a id='n600' href='#n600'>600</a>
<a id='n601' href='#n601'>601</a>
<a id='n602' href='#n602'>602</a>
<a id='n603' href='#n603'>603</a>
<a id='n604' href='#n604'>604</a>
<a id='n605' href='#n605'>605</a>
<a id='n606' href='#n606'>606</a>
<a id='n607' href='#n607'>607</a>
<a id='n608' href='#n608'>608</a>
<a id='n609' href='#n609'>609</a>
<a id='n610' href='#n610'>610</a>
<a id='n611' href='#n611'>611</a>
<a id='n612' href='#n612'>612</a>
<a id='n613' href='#n613'>613</a>
<a id='n614' href='#n614'>614</a>
<a id='n615' href='#n615'>615</a>
<a id='n616' href='#n616'>616</a>
<a id='n617' href='#n617'>617</a>
<a id='n618' href='#n618'>618</a>
<a id='n619' href='#n619'>619</a>
<a id='n620' href='#n620'>620</a>
<a id='n621' href='#n621'>621</a>
<a id='n622' href='#n622'>622</a>
<a id='n623' href='#n623'>623</a>
<a id='n624' href='#n624'>624</a>
<a id='n625' href='#n625'>625</a>
<a id='n626' href='#n626'>626</a>
<a id='n627' href='#n627'>627</a>
<a id='n628' href='#n628'>628</a>
<a id='n629' href='#n629'>629</a>
<a id='n630' href='#n630'>630</a>
<a id='n631' href='#n631'>631</a>
<a id='n632' href='#n632'>632</a>
<a id='n633' href='#n633'>633</a>
<a id='n634' href='#n634'>634</a>
<a id='n635' href='#n635'>635</a>
<a id='n636' href='#n636'>636</a>
<a id='n637' href='#n637'>637</a>
<a id='n638' href='#n638'>638</a>
<a id='n639' href='#n639'>639</a>
<a id='n640' href='#n640'>640</a>
<a id='n641' href='#n641'>641</a>
<a id='n642' href='#n642'>642</a>
<a id='n643' href='#n643'>643</a>
<a id='n644' href='#n644'>644</a>
<a id='n645' href='#n645'>645</a>
<a id='n646' href='#n646'>646</a>
<a id='n647' href='#n647'>647</a>
<a id='n648' href='#n648'>648</a>
<a id='n649' href='#n649'>649</a>
<a id='n650' href='#n650'>650</a>
<a id='n651' href='#n651'>651</a>
<a id='n652' href='#n652'>652</a>
<a id='n653' href='#n653'>653</a>
<a id='n654' href='#n654'>654</a>
<a id='n655' href='#n655'>655</a>
<a id='n656' href='#n656'>656</a>
<a id='n657' href='#n657'>657</a>
<a id='n658' href='#n658'>658</a>
<a id='n659' href='#n659'>659</a>
<a id='n660' href='#n660'>660</a>
<a id='n661' href='#n661'>661</a>
<a id='n662' href='#n662'>662</a>
<a id='n663' href='#n663'>663</a>
<a id='n664' href='#n664'>664</a>
<a id='n665' href='#n665'>665</a>
<a id='n666' href='#n666'>666</a>
<a id='n667' href='#n667'>667</a>
<a id='n668' href='#n668'>668</a>
<a id='n669' href='#n669'>669</a>
<a id='n670' href='#n670'>670</a>
<a id='n671' href='#n671'>671</a>
<a id='n672' href='#n672'>672</a>
<a id='n673' href='#n673'>673</a>
<a id='n674' href='#n674'>674</a>
<a id='n675' href='#n675'>675</a>
<a id='n676' href='#n676'>676</a>
<a id='n677' href='#n677'>677</a>
<a id='n678' href='#n678'>678</a>
<a id='n679' href='#n679'>679</a>
<a id='n680' href='#n680'>680</a>
<a id='n681' href='#n681'>681</a>
<a id='n682' href='#n682'>682</a>
<a id='n683' href='#n683'>683</a>
<a id='n684' href='#n684'>684</a>
<a id='n685' href='#n685'>685</a>
<a id='n686' href='#n686'>686</a>
<a id='n687' href='#n687'>687</a>
<a id='n688' href='#n688'>688</a>
<a id='n689' href='#n689'>689</a>
<a id='n690' href='#n690'>690</a>
<a id='n691' href='#n691'>691</a>
<a id='n692' href='#n692'>692</a>
<a id='n693' href='#n693'>693</a>
<a id='n694' href='#n694'>694</a>
<a id='n695' href='#n695'>695</a>
<a id='n696' href='#n696'>696</a>
<a id='n697' href='#n697'>697</a>
<a id='n698' href='#n698'>698</a>
<a id='n699' href='#n699'>699</a>
<a id='n700' href='#n700'>700</a>
<a id='n701' href='#n701'>701</a>
<a id='n702' href='#n702'>702</a>
<a id='n703' href='#n703'>703</a>
<a id='n704' href='#n704'>704</a>
<a id='n705' href='#n705'>705</a>
<a id='n706' href='#n706'>706</a>
<a id='n707' href='#n707'>707</a>
<a id='n708' href='#n708'>708</a>
<a id='n709' href='#n709'>709</a>
<a id='n710' href='#n710'>710</a>
<a id='n711' href='#n711'>711</a>
<a id='n712' href='#n712'>712</a>
<a id='n713' href='#n713'>713</a>
<a id='n714' href='#n714'>714</a>
<a id='n715' href='#n715'>715</a>
<a id='n716' href='#n716'>716</a>
<a id='n717' href='#n717'>717</a>
<a id='n718' href='#n718'>718</a>
<a id='n719' href='#n719'>719</a>
<a id='n720' href='#n720'>720</a>
<a id='n721' href='#n721'>721</a>
<a id='n722' href='#n722'>722</a>
<a id='n723' href='#n723'>723</a>
<a id='n724' href='#n724'>724</a>
<a id='n725' href='#n725'>725</a>
<a id='n726' href='#n726'>726</a>
<a id='n727' href='#n727'>727</a>
<a id='n728' href='#n728'>728</a>
<a id='n729' href='#n729'>729</a>
<a id='n730' href='#n730'>730</a>
<a id='n731' href='#n731'>731</a>
<a id='n732' href='#n732'>732</a>
<a id='n733' href='#n733'>733</a>
<a id='n734' href='#n734'>734</a>
<a id='n735' href='#n735'>735</a>
<a id='n736' href='#n736'>736</a>
<a id='n737' href='#n737'>737</a>
<a id='n738' href='#n738'>738</a>
<a id='n739' href='#n739'>739</a>
<a id='n740' href='#n740'>740</a>
<a id='n741' href='#n741'>741</a>
<a id='n742' href='#n742'>742</a>
<a id='n743' href='#n743'>743</a>
<a id='n744' href='#n744'>744</a>
<a id='n745' href='#n745'>745</a>
<a id='n746' href='#n746'>746</a>
<a id='n747' href='#n747'>747</a>
<a id='n748' href='#n748'>748</a>
<a id='n749' href='#n749'>749</a>
<a id='n750' href='#n750'>750</a>
<a id='n751' href='#n751'>751</a>
<a id='n752' href='#n752'>752</a>
<a id='n753' href='#n753'>753</a>
<a id='n754' href='#n754'>754</a>
<a id='n755' href='#n755'>755</a>
<a id='n756' href='#n756'>756</a>
<a id='n757' href='#n757'>757</a>
<a id='n758' href='#n758'>758</a>
<a id='n759' href='#n759'>759</a>
<a id='n760' href='#n760'>760</a>
<a id='n761' href='#n761'>761</a>
<a id='n762' href='#n762'>762</a>
<a id='n763' href='#n763'>763</a>
<a id='n764' href='#n764'>764</a>
<a id='n765' href='#n765'>765</a>
<a id='n766' href='#n766'>766</a>
<a id='n767' href='#n767'>767</a>
</pre></td>
<td class='lines'><pre><code><span class="hl com">/* SPDX-License-Identifier: LGPL-2.1-or-later */</span>
<span class="hl com">/*</span>
<span class="hl com"> * Copyright (C) 2019, Google Inc.</span>
<span class="hl com"> *</span>
<span class="hl com"> * imgu.cpp - Intel IPU3 ImgU</span>
<span class="hl com"> */</span>

<span class="hl ppc">#include</span> <span class="hl pps">&quot;imgu.h&quot;</span><span class="hl ppc"></span>

<span class="hl ppc">#include &lt;algorithm&gt;</span>
<span class="hl ppc">#include &lt;cmath&gt;</span>
<span class="hl ppc">#include &lt;limits&gt;</span>

<span class="hl ppc">#include &lt;linux/media-bus-format.h&gt;</span>

<span class="hl ppc">#include &lt;libcamera/base/log.h&gt;</span>
<span class="hl ppc">#include &lt;libcamera/base/utils.h&gt;</span>

<span class="hl ppc">#include &lt;libcamera/formats.h&gt;</span>
<span class="hl ppc">#include &lt;libcamera/stream.h&gt;</span>

<span class="hl ppc">#include</span> <span class="hl pps">&quot;libcamera/internal/media_device.h&quot;</span><span class="hl ppc"></span>

<span class="hl kwa">namespace</span> libcamera <span class="hl opt">{</span>

<span class="hl kwd">LOG_DECLARE_CATEGORY</span><span class="hl opt">(</span>IPU3<span class="hl opt">)</span>

<span class="hl kwa">namespace</span> <span class="hl opt">{</span>

<span class="hl com">/*</span>
<span class="hl com"> * The procedure to calculate the ImgU pipe configuration has been ported</span>
<span class="hl com"> * from the pipe_config.py python script, available at:</span>
<span class="hl com"> * https://github.com/intel/intel-ipu3-pipecfg</span>
<span class="hl com"> * at revision: 243d13446e44 (&quot;Fix some bug for some resolutions&quot;)</span>
<span class="hl com"> */</span>

<span class="hl com">/* BSD scaling factors: min=1, max=2.5, step=1/32 */</span>
<span class="hl kwb">const</span> <span class="hl kwc">std</span><span class="hl opt">::</span>vector<span class="hl opt">&lt;</span><span class="hl kwb">float</span><span class="hl opt">&gt;</span> bdsScalingFactors <span class="hl opt">= {</span>
	<span class="hl num">1</span><span class="hl opt">,</span> <span class="hl num">1.03125</span><span class="hl opt">,</span> <span class="hl num">1.0625</span><span class="hl opt">,</span> <span class="hl num">1.09375</span><span class="hl opt">,</span> <span class="hl num">1.125</span><span class="hl opt">,</span> <span class="hl num">1.15625</span><span class="hl opt">,</span> <span class="hl num">1.1875</span><span class="hl opt">,</span> <span class="hl num">1.21875</span><span class="hl opt">,</span> <span class="hl num">1.25</span><span class="hl opt">,</span>
	<span class="hl num">1.28125</span><span class="hl opt">,</span> <span class="hl num">1.3125</span><span class="hl opt">,</span> <span class="hl num">1.34375</span><span class="hl opt">,</span> <span class="hl num">1.375</span><span class="hl opt">,</span> <span class="hl num">1.40625</span><span class="hl opt">,</span> <span class="hl num">1.4375</span><span class="hl opt">,</span> <span class="hl num">1.46875</span><span class="hl opt">,</span> <span class="hl num">1.5</span><span class="hl opt">,</span> <span class="hl num">1.53125</span><span class="hl opt">,</span>
	<span class="hl num">1.5625</span><span class="hl opt">,</span> <span class="hl num">1.59375</span><span class="hl opt">,</span> <span class="hl num">1.625</span><span class="hl opt">,</span> <span class="hl num">1.65625</span><span class="hl opt">,</span> <span class="hl num">1.6875</span><span class="hl opt">,</span> <span class="hl num">1.71875</span><span class="hl opt">,</span> <span class="hl num">1.75</span><span class="hl opt">,</span> <span class="hl num">1.78125</span><span class="hl opt">,</span> <span class="hl num">1.8125</span><span class="hl opt">,</span>
	<span class="hl num">1.84375</span><span class="hl opt">,</span> <span class="hl num">1.875</span><span class="hl opt">,</span> <span class="hl num">1.90625</span><span class="hl opt">,</span> <span class="hl num">1.9375</span><span class="hl opt">,</span> <span class="hl num">1.96875</span><span class="hl opt">,</span> <span class="hl num">2</span><span class="hl opt">,</span> <span class="hl num">2.03125</span><span class="hl opt">,</span> <span class="hl num">2.0625</span><span class="hl opt">,</span> <span class="hl num">2.09375</span><span class="hl opt">,</span>
	<span class="hl num">2.125</span><span class="hl opt">,</span> <span class="hl num">2.15625</span><span class="hl opt">,</span> <span class="hl num">2.1875</span><span class="hl opt">,</span> <span class="hl num">2.21875</span><span class="hl opt">,</span> <span class="hl num">2.25</span><span class="hl opt">,</span> <span class="hl num">2.28125</span><span class="hl opt">,</span> <span class="hl num">2.3125</span><span class="hl opt">,</span> <span class="hl num">2.34375</span><span class="hl opt">,</span> <span class="hl num">2.375</span><span class="hl opt">,</span>
	<span class="hl num">2.40625</span><span class="hl opt">,</span> <span class="hl num">2.4375</span><span class="hl opt">,</span> <span class="hl num">2.46875</span><span class="hl opt">,</span> <span class="hl num">2.5</span>
<span class="hl opt">};</span>

<span class="hl com">/* GDC scaling factors: min=1, max=16, step=1/4 */</span>
<span class="hl kwb">const</span> <span class="hl kwc">std</span><span class="hl opt">::</span>vector<span class="hl opt">&lt;</span><span class="hl kwb">float</span><span class="hl opt">&gt;</span> gdcScalingFactors <span class="hl opt">= {</span>
	<span class="hl num">1</span><span class="hl opt">,</span> <span class="hl num">1.25</span><span class="hl opt">,</span> <span class="hl num">1.5</span><span class="hl opt">,</span> <span class="hl num">1.75</span><span class="hl opt">,</span> <span class="hl num">2</span><span class="hl opt">,</span> <span class="hl num">2.25</span><span class="hl opt">,</span> <span class="hl num">2.5</span><span class="hl opt">,</span> <span class="hl num">2.75</span><span class="hl opt">,</span> <span class="hl num">3</span><span class="hl opt">,</span> <span class="hl num">3.25</span><span class="hl opt">,</span> <span class="hl num">3.5</span><span class="hl opt">,</span> <span class="hl num">3.75</span><span class="hl opt">,</span> <span class="hl num">4</span><span class="hl opt">,</span> <span class="hl num">4.25</span><span class="hl opt">,</span>
	<span class="hl num">4.5</span><span class="hl opt">,</span> <span class="hl num">4.75</span><span class="hl opt">,</span> <span class="hl num">5</span><span class="hl opt">,</span> <span class="hl num">5.25</span><span class="hl opt">,</span> <span class="hl num">5.5</span><span class="hl opt">,</span> <span class="hl num">5.75</span><span class="hl opt">,</span> <span class="hl num">6</span><span class="hl opt">,</span> <span class="hl num">6.25</span><span class="hl opt">,</span> <span class="hl num">6.5</span><span class="hl opt">,</span> <span class="hl num">6.75</span><span class="hl opt">,</span> <span class="hl num">7</span><span class="hl opt">,</span> <span class="hl num">7.25</span><span class="hl opt">,</span> <span class="hl num">7.5</span><span class="hl opt">,</span> <span class="hl num">7.75</span><span class="hl opt">,</span>
	<span class="hl num">8</span><span class="hl opt">,</span> <span class="hl num">8.25</span><span class="hl opt">,</span> <span class="hl num">8.5</span><span class="hl opt">,</span> <span class="hl num">8.75</span><span class="hl opt">,</span> <span class="hl num">9</span><span class="hl opt">,</span> <span class="hl num">9.25</span><span class="hl opt">,</span> <span class="hl num">9.5</span><span class="hl opt">,</span> <span class="hl num">9.75</span><span class="hl opt">,</span> <span class="hl num">10</span><span class="hl opt">,</span> <span class="hl num">10.25</span><span class="hl opt">,</span> <span class="hl num">10.5</span><span class="hl opt">,</span> <span class="hl num">10.75</span><span class="hl opt">,</span> <span class="hl num">11</span><span class="hl opt">,</span>
	<span class="hl num">11.25</span><span class="hl opt">,</span> <span class="hl num">11.5</span><span class="hl opt">,</span> <span class="hl num">11.75</span><span class="hl opt">,</span> <span class="hl num">12</span><span class="hl opt">,</span> <span class="hl num">12.25</span><span class="hl opt">,</span> <span class="hl num">12.5</span><span class="hl opt">,</span> <span class="hl num">12.75</span><span class="hl opt">,</span> <span class="hl num">13</span><span class="hl opt">,</span> <span class="hl num">13.25</span><span class="hl opt">,</span> <span class="hl num">13.5</span><span class="hl opt">,</span> <span class="hl num">13.75</span><span class="hl opt">,</span> <span class="hl num">14</span><span class="hl opt">,</span>
	<span class="hl num">14.25</span><span class="hl opt">,</span> <span class="hl num">14.5</span><span class="hl opt">,</span> <span class="hl num">14.75</span><span class="hl opt">,</span> <span class="hl num">15</span><span class="hl opt">,</span> <span class="hl num">15.25</span><span class="hl opt">,</span> <span class="hl num">15.5</span><span class="hl opt">,</span> <span class="hl num">15.75</span><span class="hl opt">,</span> <span class="hl num">16</span><span class="hl opt">,</span>
<span class="hl opt">};</span>

<span class="hl kwc">std</span><span class="hl opt">::</span>vector<span class="hl opt">&lt;</span><span class="hl kwc">ImgUDevice</span><span class="hl opt">::</span>PipeConfig<span class="hl opt">&gt;</span> pipeConfigs<span class="hl opt">;</span>

<span class="hl kwb">struct</span> FOV <span class="hl opt">{</span>
	<span class="hl kwb">float</span> w<span class="hl opt">;</span>
	<span class="hl kwb">float</span> h<span class="hl opt">;</span>

	<span class="hl kwb">bool</span> <span class="hl kwd">isLarger</span><span class="hl opt">(</span><span class="hl kwb">const</span> FOV <span class="hl opt">&amp;</span>other<span class="hl opt">)</span>
	<span class="hl opt">{</span>
		<span class="hl kwa">if</span> <span class="hl opt">(</span>w <span class="hl opt">&gt;</span> other<span class="hl opt">.</span>w<span class="hl opt">)</span>
			<span class="hl kwa">return true</span><span class="hl opt">;</span>
		<span class="hl kwa">if</span> <span class="hl opt">(</span>w <span class="hl opt">==</span> other<span class="hl opt">.</span>w <span class="hl opt">&amp;&amp;</span> h <span class="hl opt">&gt;</span> other<span class="hl opt">.</span>h<span class="hl opt">)</span>
			<span class="hl kwa">return true</span><span class="hl opt">;</span>
		<span class="hl kwa">return false</span><span class="hl opt">;</span>
	<span class="hl opt">}</span>
<span class="hl opt">};</span>

<span class="hl com">/* Approximate a scaling factor sf to the closest one available in a range. */</span>
<span class="hl kwb">float</span> <span class="hl kwd">findScaleFactor</span><span class="hl opt">(</span><span class="hl kwb">float</span> sf<span class="hl opt">,</span> <span class="hl kwb">const</span> <span class="hl kwc">std</span><span class="hl opt">::</span>vector<span class="hl opt">&lt;</span><span class="hl kwb">float</span><span class="hl opt">&gt; &amp;</span>range<span class="hl opt">,</span>
		      <span class="hl kwb">bool</span> roundDown <span class="hl opt">=</span> <span class="hl kwa">false</span><span class="hl opt">)</span>
<span class="hl opt">{</span>
	<span class="hl kwa">if</span> <span class="hl opt">(</span>sf <span class="hl opt">&lt;=</span> range<span class="hl opt">[</span><span class="hl num">0</span><span class="hl opt">])</span>
		<span class="hl kwa">return</span> range<span class="hl opt">[</span><span class="hl num">0</span><span class="hl opt">];</span>
	<span class="hl kwa">if</span> <span class="hl opt">(</span>sf <span class="hl opt">&gt;=</span> range<span class="hl opt">[</span>range<span class="hl opt">.</span><span class="hl kwd">size</span><span class="hl opt">() -</span> <span class="hl num">1</span><span class="hl opt">])</span>
		<span class="hl kwa">return</span> range<span class="hl opt">[</span>range<span class="hl opt">.</span><span class="hl kwd">size</span><span class="hl opt">() -</span> <span class="hl num">1</span><span class="hl opt">];