summaryrefslogtreecommitdiff
path: root/src/ipa/vimc/vimc.cpp
blob: ef257762a1d4006c1b1a3fa3d78a87cc43d9326d (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/* SPDX-License-Identifier: LGPL-2.1-or-later */
/*
 * Copyright (C) 2019, Google Inc.
 *
 * ipa_vimc.cpp - Vimc Image Processing Algorithm module
 */

#include <libcamera/ipa/ipa_vimc.h>

#include <fcntl.h>
#include <string.h>
#include <sys/stat.h>
#include <unistd.h>

#include <iostream>

#include <libcamera/ipa/ipa_interface.h>
#include <libcamera/ipa/ipa_module_info.h>

#include <libipa/ipa_interface_wrapper.h>

#include "libcamera/internal/file.h"
#include "libcamera/internal/log.h"

namespace libcamera {

LOG_DEFINE_CATEGORY(IPAVimc)

class IPAVimc : public IPAInterface
{
public:
	IPAVimc();
	~IPAVimc();

	int init(const IPASettings &settings) override;

	int start() override;
	void stop() override;

	void configure([[maybe_unused]] const CameraSensorInfo &sensorInfo,
		       [[maybe_unused]] const std::map<unsigned int, IPAStream> &streamConfig,
		       [[maybe_unused]] const std::map<unsigned int, const ControlInfoMap &> &entityControls,
		       [[maybe_unused]] const IPAOperationData &ipaConfig,
		       [[maybe_unused]] IPAOperationData *result) override {}
	void mapBuffers([[maybe_unused]] const std::vector<IPABuffer> &buffers) override {}
	void unmapBuffers([[maybe_unused]] const std::vector<unsigned int> &ids) override {}
	void processEvent([[maybe_unused]] const IPAOperationData &event) override {}

private:
	void initTrace();
	void trace(enum IPAOperationCode operation);

	int fd_;
};

IPAVimc::IPAVimc()
	: fd_(-1)
{
	initTrace();
}

IPAVimc::~IPAVimc()
{
	if (fd_)
		::close(fd_);
}

int IPAVimc::init(const IPASettings &settings)
{
	trace(IPAOperationInit);

	LOG(IPAVimc, Debug)
		<< "initializing vimc IPA with configuration file "
		<< settings.configurationFile;

	File conf(settings.configurationFile);
	if (!conf.open(File::ReadOnly)) {
		LOG(IPAVimc, Error) << "Failed to open configuration file";
		return -EINVAL;
	}

	return 0;
}

int IPAVimc::start()
{
	trace(IPAOperationStart);

	LOG(IPAVimc, Debug) << "start vimc IPA!";

	return 0;
}

void IPAVimc::stop()
{
	trace(IPAOperationStop);

	LOG(IPAVimc, Debug) << "stop vimc IPA!";
}

void IPAVimc::initTrace()
{
	struct stat fifoStat;
	int ret = stat(VIMC_IPA_FIFO_PATH, &fifoStat);
	if (ret)
		return;

	ret = ::open(VIMC_IPA_FIFO_PATH, O_WRONLY);
	if (ret < 0) {
		ret = errno;
		LOG(IPAVimc, Error) << "Failed to open vimc IPA test FIFO: "
				    << strerror(ret);
		return;
	}

	fd_ = ret;
}

void IPAVimc::trace(enum IPAOperationCode operation)
{
	if (fd_ < 0)
		return;

	int ret = ::write(fd_, &operation, sizeof(operation));
	if (ret < 0) {
		ret = errno;
		LOG(IPAVimc, Error) << "Failed to write to vimc IPA test FIFO: "
				    << strerror(ret);
	}
}

/*
 * External IPA module interface
 */

extern "C" {
const struct IPAModuleInfo ipaModuleInfo = {
	IPA_MODULE_API_VERSION,
	0,
	"PipelineHandlerVimc",
	"vimc",
};

struct ipa_context *ipaCreate()
{
	return new IPAInterfaceWrapper(std::make_unique<IPAVimc>());
}
}

} /* namespace libcamera */
n540'>540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907
/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi Ltd
 *
 * agc.cpp - AGC/AEC control algorithm
 */

#include <map>
#include <tuple>

#include <linux/bcm2835-isp.h>

#include <libcamera/base/log.h>

#include "../awb_status.h"
#include "../device_status.h"
#include "../histogram.h"
#include "../lux_status.h"
#include "../metadata.h"

#include "agc.h"

using namespace RPiController;
using namespace libcamera;
using libcamera::utils::Duration;
using namespace std::literals::chrono_literals;

LOG_DEFINE_CATEGORY(RPiAgc)

#define NAME "rpi.agc"

static constexpr unsigned int PipelineBits = 13; /* seems to be a 13-bit pipeline */

int AgcMeteringMode::read(const libcamera::YamlObject &params)
{
	const YamlObject &yamlWeights = params["weights"];
	if (yamlWeights.size() != AgcStatsSize) {
		LOG(RPiAgc, Error) << "AgcMeteringMode: Incorrect number of weights";
		return -EINVAL;
	}

	unsigned int num = 0;
	for (const auto &p : yamlWeights.asList()) {
		auto value = p.get<double>();
		if (!value)
			return -EINVAL;
		weights[num++] = *value;
	}

	return 0;
}

static std::tuple<int, std::string>
readMeteringModes(std::map<std::string, AgcMeteringMode> &metering_modes,
		  const libcamera::YamlObject &params)
{
	std::string first;
	int ret;

	for (const auto &[key, value] : params.asDict()) {
		AgcMeteringMode meteringMode;
		ret = meteringMode.read(value);
		if (ret)
			return { ret, {} };

		metering_modes[key] = std::move(meteringMode);
		if (first.empty())
			first = key;
	}

	return { 0, first };
}

static int readList(std::vector<double> &list,
		    const libcamera::YamlObject &params)
{
	for (const auto &p : params.asList()) {
		auto value = p.get<double>();
		if (!value)
			return -EINVAL;
		list.push_back(*value);
	}

	return list.size();
}

static int readList(std::vector<Duration> &list,
		    const libcamera::YamlObject &params)
{
	for (const auto &p : params.asList()) {
		auto value = p.get<double>();
		if (!value)
			return -EINVAL;
		list.push_back(*value * 1us);
	}

	return list.size();
}

int AgcExposureMode::read(const libcamera::YamlObject &params)
{
	int numShutters = readList(shutter, params["shutter"]);
	int numAgs = readList(gain, params["gain"]);

	if (numShutters < 2 || numAgs < 2) {
		LOG(RPiAgc, Error)
			<< "AgcExposureMode: must have at least two entries in exposure profile";
		return -EINVAL;
	}

	if (numShutters != numAgs) {
		LOG(RPiAgc, Error)
			<< "AgcExposureMode: expect same number of exposure and gain entries in exposure profile";
		return -EINVAL;
	}

	return 0;
}

static std::tuple<int, std::string>
readExposureModes(std::map<std::string, AgcExposureMode> &exposureModes,
		  const libcamera::YamlObject &params)
{
	std::string first;
	int ret;

	for (const auto &[key, value] : params.asDict()) {
		AgcExposureMode exposureMode;
		ret = exposureMode.read(value);
		if (ret)
			return { ret, {} };

		exposureModes[key] = std::move(exposureMode);
		if (first.empty())
			first = key;
	}

	return { 0, first };
}

int AgcConstraint::read(const libcamera::YamlObject &params)
{
	std::string boundString = params["bound"].get<std::string>("");
	transform(boundString.begin(), boundString.end(),
		  boundString.begin(), ::toupper);
	if (boundString != "UPPER" && boundString != "LOWER") {
		LOG(RPiAgc, Error) << "AGC constraint type should be UPPER or LOWER";
		return -EINVAL;
	}
	bound = boundString == "UPPER" ? Bound::UPPER : Bound::LOWER;

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

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

	return yTarget.read(params["y_target"]);
}

static std::tuple<int, AgcConstraintMode>
readConstraintMode(const libcamera::YamlObject &params)
{
	AgcConstraintMode mode;
	int ret;

	for (const auto &p : params.asList()) {
		AgcConstraint constraint;
		ret = constraint.read(p);
		if (ret)
			return { ret, {} };

		mode.push_back(std::move(constraint));
	}

	return { 0, mode };
}

static std::tuple<int, std::string>
readConstraintModes(std::map<std::string, AgcConstraintMode> &constraintModes,
		    const libcamera::YamlObject &params)
{
	std::string first;
	int ret;

	for (const auto &[key, value] : params.asDict()) {
		std::tie(ret, constraintModes[key]) = readConstraintMode(value);
		if (ret)
			return { ret, {} };

		if (first.empty())
			first = key;
	}

	return { 0, first };
}

int AgcConfig::read(const libcamera::YamlObject &params)
{
	LOG(RPiAgc, Debug) << "AgcConfig";
	int ret;

	std::tie(ret, defaultMeteringMode) =
		readMeteringModes(meteringModes, params["metering_modes"]);
	if (ret)
		return ret;
	std::tie(ret, defaultExposureMode) =
		readExposureModes(exposureModes, params["exposure_modes"]);
	if (ret)
		return ret;
	std::tie(ret, defaultConstraintMode) =
		readConstraintModes(constraintModes, params["constraint_modes"]);
	if (ret)
		return ret;

	ret = yTarget.read(params["y_target"]);
	if (ret)
		return ret;

	speed = params["speed"].get<double>(0.2);
	startupFrames = params["startup_frames"].get<uint16_t>(10);
	convergenceFrames = params["convergence_frames"].get<unsigned int>(6);
	fastReduceThreshold = params["fast_reduce_threshold"].get<double>(0.4);
	baseEv = params["base_ev"].get<double>(1.0);

	/* Start with quite a low value as ramping up is easier than ramping down. */
	defaultExposureTime = params["default_exposure_time"].get<double>(1000) * 1us;
	defaultAnalogueGain = params["default_analogue_gain"].get<double>(1.0);

	return 0;
}

Agc::ExposureValues::ExposureValues()
	: shutter(0s), analogueGain(0),
	  totalExposure(0s), totalExposureNoDG(0s)
{
}

Agc::Agc(Controller *controller)
	: AgcAlgorithm(controller), meteringMode_(nullptr),
	  exposureMode_(nullptr), constraintMode_(nullptr),
	  frameCount_(0), lockCount_(0),
	  lastTargetExposure_(0s), lastSensitivity_(0.0),
	  ev_(1.0), flickerPeriod_(0s),
	  maxShutter_(0s), fixedShutter_(0s), fixedAnalogueGain_(0.0)
{
	memset(&awb_, 0, sizeof(awb_));
	/*
	 * Setting status_.totalExposureValue_ to zero initially tells us
	 * it's not been calculated yet (i.e. Process hasn't yet run).
	 */
	memset(&status_, 0, sizeof(status_));
	status_.ev = ev_;
}

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

int Agc::read(const libcamera::YamlObject &params)
{
	LOG(RPiAgc, Debug) << "Agc";

	int ret = config_.read(params);
	if (ret)
		return ret;

	/*
	 * Set the config's defaults (which are the first ones it read) as our
	 * current modes, until someone changes them.  (they're all known to
	 * exist at this point)
	 */
	meteringModeName_ = config_.defaultMeteringMode;
	meteringMode_ = &config_.meteringModes[meteringModeName_];
	exposureModeName_ = config_.defaultExposureMode;
	exposureMode_ = &config_.exposureModes[exposureModeName_];
	constraintModeName_ = config_.defaultConstraintMode;
	constraintMode_ = &config_.constraintModes[constraintModeName_];
	/* Set up the "last shutter/gain" values, in case AGC starts "disabled". */
	status_.shutterTime = config_.defaultExposureTime;
	status_.analogueGain = config_.defaultAnalogueGain;
	return 0;
}

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

void Agc::pause()
{
	fixedShutter_ = status_.shutterTime;
	fixedAnalogueGain_ = status_.analogueGain;
}

void Agc::resume()
{
	fixedShutter_ = 0s;
	fixedAnalogueGain_ = 0;
}

unsigned int Agc::getConvergenceFrames() const
{
	/*
	 * If shutter and gain have been explicitly set, there is no
	 * convergence to happen, so no need to drop any frames - return zero.
	 */
	if (fixedShutter_ && fixedAnalogueGain_)
		return 0;
	else
		return config_.convergenceFrames;
}

void Agc::setEv(double ev)
{
	ev_ = ev;
}

void Agc::setFlickerPeriod(Duration flickerPeriod)
{
	flickerPeriod_ = flickerPeriod;
}

void Agc::setMaxShutter(Duration maxShutter)
{
	maxShutter_ = maxShutter;
}

void Agc::setFixedShutter(Duration fixedShutter)
{
	fixedShutter_ = fixedShutter;
	/* Set this in case someone calls Pause() straight after. */
	status_.shutterTime = clipShutter(fixedShutter_);
}

void Agc::setFixedAnalogueGain(double fixedAnalogueGain)
{
	fixedAnalogueGain_ = fixedAnalogueGain;
	/* Set this in case someone calls Pause() straight after. */
	status_.analogueGain = fixedAnalogueGain;
}

void Agc::setMeteringMode(std::string const &meteringModeName)
{
	meteringModeName_ = meteringModeName;
}

void Agc::setExposureMode(std::string const &exposureModeName)
{
	exposureModeName_ = exposureModeName;
}

void Agc::setConstraintMode(std::string const &constraintModeName)
{
	constraintModeName_ = constraintModeName;
}

void Agc::switchMode(CameraMode const &cameraMode,
		     Metadata *metadata)
{
	/* AGC expects the mode sensitivity always to be non-zero. */
	ASSERT(cameraMode.sensitivity);

	housekeepConfig();

	Duration fixedShutter = clipShutter(fixedShutter_);
	if (fixedShutter && fixedAnalogueGain_) {
		/* We're going to reset the algorithm here with these fixed values. */

		fetchAwbStatus(metadata);
		double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 });
		ASSERT(minColourGain != 0.0);

		/* This is the equivalent of computeTargetExposure and applyDigitalGain. */
		target_.totalExposureNoDG = fixedShutter_ * fixedAnalogueGain_;
		target_.totalExposure = target_.totalExposureNoDG / minColourGain;

		/* Equivalent of filterExposure. This resets any "history". */
		filtered_ = target_;

		/* Equivalent of divideUpExposure. */
		filtered_.shutter = fixedShutter;
		filtered_.analogueGain = fixedAnalogueGain_;
	} else if (status_.totalExposureValue) {
		/*
		 * On a mode switch, various things could happen:
		 * - the exposure profile might change
		 * - a fixed exposure or gain might be set
		 * - the new mode's sensitivity might be different
		 * We cope with the last of these by scaling the target values. After
		 * that we just need to re-divide the exposure/gain according to the
		 * current exposure profile, which takes care of everything else.
		 */

		double ratio = lastSensitivity_ / cameraMode.sensitivity;
		target_.totalExposureNoDG *= ratio;
		target_.totalExposure *= ratio;
		filtered_.totalExposureNoDG *= ratio;
		filtered_.totalExposure *= ratio;

		divideUpExposure();
	} else {
		/*
		 * We come through here on startup, when at least one of the shutter
		 * or gain has not been fixed. We must still write those values out so
		 * that they will be applied immediately. We supply some arbitrary defaults
		 * for any that weren't set.
		 */

		/* Equivalent of divideUpExposure. */
		filtered_.shutter = fixedShutter ? fixedShutter : config_.defaultExposureTime;
		filtered_.analogueGain = fixedAnalogueGain_ ? fixedAnalogueGain_ : config_.defaultAnalogueGain;
	}

	writeAndFinish(metadata, false);

	/* We must remember the sensitivity of this mode for the next SwitchMode. */
	lastSensitivity_ = cameraMode.sensitivity;
}

void Agc::prepare(Metadata *imageMetadata)
{
	status_.digitalGain = 1.0;
	fetchAwbStatus(imageMetadata); /* always fetch it so that Process knows it's been done */

	if (status_.totalExposureValue) {
		/* Process has run, so we have meaningful values. */
		DeviceStatus deviceStatus;
		if (imageMetadata->get("device.status", deviceStatus) == 0) {
			Duration actualExposure = deviceStatus.shutterSpeed *
						  deviceStatus.analogueGain;
			if (actualExposure) {
				status_.digitalGain = status_.totalExposureValue / actualExposure;
				LOG(RPiAgc, Debug) << "Want total exposure " << status_.totalExposureValue;
				/*
				 * Never ask for a gain < 1.0, and also impose
				 * some upper limit. Make it customisable?
				 */
				status_.digitalGain = std::max(1.0, std::min(status_.digitalGain, 4.0));
				LOG(RPiAgc, Debug) << "Actual exposure " << actualExposure;
				LOG(RPiAgc, Debug) << "Use digitalGain " << status_.digitalGain;
				LOG(RPiAgc, Debug) << "Effective exposure "
						   << actualExposure * status_.digitalGain;
				/* Decide whether AEC/AGC has converged. */
				updateLockStatus(deviceStatus);
			}
		} else
			LOG(RPiAgc, Warning) << name() << ": no device metadata";
		imageMetadata->set("agc.status", status_);
	}
}

void Agc::process(StatisticsPtr &stats, Metadata *imageMetadata)
{
	frameCount_++;
	/*
	 * First a little bit of housekeeping, fetching up-to-date settings and
	 * configuration, that kind of thing.
	 */
	housekeepConfig();
	/* Get the current exposure values for the frame that's just arrived. */
	fetchCurrentExposure(imageMetadata);
	/* Compute the total gain we require relative to the current exposure. */
	double gain, targetY;
	computeGain(stats.get(), imageMetadata, gain, targetY);
	/* Now compute the target (final) exposure which we think we want. */
	computeTargetExposure(gain);
	/*
	 * Some of the exposure has to be applied as digital gain, so work out
	 * what that is. This function also tells us whether it's decided to
	 * "desaturate" the image more quickly.
	 */
	bool desaturate = applyDigitalGain(gain, targetY);
	/* The results have to be filtered so as not to change too rapidly. */
	filterExposure(desaturate);
	/*
	 * The last thing is to divide up the exposure value into a shutter time
	 * and analogue gain, according to the current exposure mode.
	 */
	divideUpExposure();
	/* Finally advertise what we've done. */
	writeAndFinish(imageMetadata, desaturate);
}

void Agc::updateLockStatus(DeviceStatus const &deviceStatus)
{
	const double errorFactor = 0.10; /* make these customisable? */
	const int maxLockCount = 5;
	/* Reset "lock count" when we exceed this multiple of errorFactor */
	const double resetMargin = 1.5;

	/* Add 200us to the exposure time error to allow for line quantisation. */
	Duration exposureError = lastDeviceStatus_.shutterSpeed * errorFactor + 200us;
	double gainError = lastDeviceStatus_.analogueGain * errorFactor;
	Duration targetError = lastTargetExposure_ * errorFactor;

	/*
	 * Note that we don't know the exposure/gain limits of the sensor, so
	 * the values we keep requesting may be unachievable. For this reason
	 * we only insist that we're close to values in the past few frames.
	 */
	if (deviceStatus.shutterSpeed > lastDeviceStatus_.shutterSpeed - exposureError &&
	    deviceStatus.shutterSpeed < lastDeviceStatus_.shutterSpeed + exposureError &&
	    deviceStatus.analogueGain > lastDeviceStatus_.analogueGain - gainError &&
	    deviceStatus.analogueGain < lastDeviceStatus_.analogueGain + gainError &&
	    status_.targetExposureValue > lastTargetExposure_ - targetError &&
	    status_.targetExposureValue < lastTargetExposure_ + targetError)
		lockCount_ = std::min(lockCount_ + 1, maxLockCount);
	else if (deviceStatus.shutterSpeed < lastDeviceStatus_.shutterSpeed - resetMargin * exposureError ||
		 deviceStatus.shutterSpeed > lastDeviceStatus_.shutterSpeed + resetMargin * exposureError ||
		 deviceStatus.analogueGain < lastDeviceStatus_.analogueGain - resetMargin * gainError ||
		 deviceStatus.analogueGain > lastDeviceStatus_.analogueGain + resetMargin * gainError ||
		 status_.targetExposureValue < lastTargetExposure_ - resetMargin * targetError ||
		 status_.targetExposureValue > lastTargetExposure_ + resetMargin * targetError)
		lockCount_ = 0;

	lastDeviceStatus_ = deviceStatus;
	lastTargetExposure_ = status_.targetExposureValue;

	LOG(RPiAgc, Debug) << "Lock count updated to " << lockCount_;
	status_.locked = lockCount_ == maxLockCount;
}

static void copyString(std::string const &s, char *d, size_t size)
{
	size_t length = s.copy(d, size - 1);
	d[length] = '\0';
}

void Agc::housekeepConfig()
{
	/* First fetch all the up-to-date settings, so no one else has to do it. */
	status_.ev = ev_;
	status_.fixedShutter = clipShutter(fixedShutter_);
	status_.fixedAnalogueGain = fixedAnalogueGain_;
	status_.flickerPeriod = flickerPeriod_;
	LOG(RPiAgc, Debug) << "ev " << status_.ev << " fixedShutter "
			   << status_.fixedShutter << " fixedAnalogueGain "
			   << status_.fixedAnalogueGain;
	/*
	 * Make sure the "mode" pointers point to the up-to-date things, if
	 * they've changed.
	 */
	if (strcmp(meteringModeName_.c_str(), status_.meteringMode)) {
		auto it = config_.meteringModes.find(meteringModeName_);
		if (it == config_.meteringModes.end())
			LOG(RPiAgc, Fatal) << "No metering mode " << meteringModeName_;
		meteringMode_ = &it->second;
		copyString(meteringModeName_, status_.meteringMode,
			   sizeof(status_.meteringMode));
	}
	if (strcmp(exposureModeName_.c_str(), status_.exposureMode)) {
		auto it = config_.exposureModes.find(exposureModeName_);
		if (it == config_.exposureModes.end())
			LOG(RPiAgc, Fatal) << "No exposure profile " << exposureModeName_;
		exposureMode_ = &it->second;
		copyString(exposureModeName_, status_.exposureMode,
			   sizeof(status_.exposureMode));
	}
	if (strcmp(constraintModeName_.c_str(), status_.constraintMode)) {
		auto it =
			config_.constraintModes.find(constraintModeName_);
		if (it == config_.constraintModes.end())
			LOG(RPiAgc, Fatal) << "No constraint list " << constraintModeName_;
		constraintMode_ = &it->second;
		copyString(constraintModeName_, status_.constraintMode,
			   sizeof(status_.constraintMode));
	}
	LOG(RPiAgc, Debug) << "exposureMode "
			   << exposureModeName_ << " constraintMode "
			   << constraintModeName_ << " meteringMode "
			   << meteringModeName_;
}

void Agc::fetchCurrentExposure(Metadata *imageMetadata)
{
	std::unique_lock<Metadata> lock(*imageMetadata);
	DeviceStatus *deviceStatus =
		imageMetadata->getLocked<DeviceStatus>("device.status");
	if (!deviceStatus)
		LOG(RPiAgc, Fatal) << "No device metadata";
	current_.shutter = deviceStatus->shutterSpeed;
	current_.analogueGain = deviceStatus->analogueGain;
	AgcStatus *agcStatus =
		imageMetadata->getLocked<AgcStatus>("agc.status");
	current_.totalExposure = agcStatus ? agcStatus->totalExposureValue : 0s;
	current_.totalExposureNoDG = current_.shutter * current_.analogueGain;
}

void Agc::fetchAwbStatus(Metadata *imageMetadata)
{
	awb_.gainR = 1.0; /* in case not found in metadata */
	awb_.gainG = 1.0;
	awb_.gainB = 1.0;
	if (imageMetadata->get("awb.status", awb_) != 0)
		LOG(RPiAgc, Debug) << "No AWB status found";
}

static double computeInitialY(bcm2835_isp_stats *stats, AwbStatus const &awb,
			      double weights[], double gain)
{
	bcm2835_isp_stats_region *regions = stats->agc_stats;
	/*
	 * Note how the calculation below means that equal weights give you
	 * "average" metering (i.e. all pixels equally important).
	 */
	double rSum = 0, gSum = 0, bSum = 0, pixelSum = 0;
	for (unsigned int i = 0; i < AgcStatsSize; i++) {
		double counted = regions[i].counted;
		double rAcc = std::min(regions[i].r_sum * gain, ((1 << PipelineBits) - 1) * counted);
		double gAcc = std::min(regions[i].g_sum * gain, ((1 << PipelineBits) - 1) * counted);
		double bAcc = std::min(regions[i].b_sum * gain, ((1 << PipelineBits) - 1) * counted);
		rSum += rAcc * weights[i];
		gSum += gAcc * weights[i];
		bSum += bAcc * weights[i];
		pixelSum += counted * weights[i];
	}
	if (pixelSum == 0.0) {
		LOG(RPiAgc, Warning) << "computeInitialY: pixelSum is zero";
		return 0;
	}
	double ySum = rSum * awb.gainR * .299 +
		      gSum * awb.gainG * .587 +
		      bSum * awb.gainB * .114;
	return ySum / pixelSum / (1 << PipelineBits);
}

/*
 * We handle extra gain through EV by adjusting our Y targets. However, you
 * simply can't monitor histograms once they get very close to (or beyond!)
 * saturation, so we clamp the Y targets to this value. It does mean that EV
 * increases don't necessarily do quite what you might expect in certain
 * (contrived) cases.
 */

static constexpr double EvGainYTargetLimit = 0.9;

static double constraintComputeGain(AgcConstraint &c, Histogram &h, double lux,
				    double evGain, double &targetY)
{
	targetY = c.yTarget.eval(c.yTarget.domain().clip(lux));
	targetY = std::min(EvGainYTargetLimit, targetY * evGain);
	double iqm = h.interQuantileMean(c.qLo, c.qHi);
	return (targetY * NUM_HISTOGRAM_BINS) / iqm;
}

void Agc::computeGain(bcm2835_isp_stats *statistics, Metadata *imageMetadata,
		      double &gain, double &targetY)
{
	struct LuxStatus lux = {};
	lux.lux = 400; /* default lux level to 400 in case no metadata found */
	if (imageMetadata->get("lux.status", lux) != 0)
		LOG(RPiAgc, Warning) << "No lux level found";
	Histogram h(statistics->hist[0].g_hist, NUM_HISTOGRAM_BINS);
	double evGain = status_.ev * config_.baseEv;
	/*
	 * The initial gain and target_Y come from some of the regions. After
	 * that we consider the histogram constraints.
	 */
	targetY = config_.yTarget.eval(config_.yTarget.domain().clip(lux.lux));
	targetY = std::min(EvGainYTargetLimit, targetY * evGain);

	/*
	 * Do this calculation a few times as brightness increase can be
	 * non-linear when there are saturated regions.
	 */
	gain = 1.0;
	for (int i = 0; i < 8; i++) {
		double initialY = computeInitialY(statistics, awb_, meteringMode_->weights, gain);
		double extraGain = std::min(10.0, targetY / (initialY + .001));
		gain *= extraGain;
		LOG(RPiAgc, Debug) << "Initial Y " << initialY << " target " << targetY
				   << " gives gain " << gain;
		if (extraGain < 1.01) /* close enough */
			break;
	}

	for (auto &c : *constraintMode_) {
		double newTargetY;
		double newGain = constraintComputeGain(c, h, lux.lux, evGain, newTargetY);
		LOG(RPiAgc, Debug) << "Constraint has target_Y "
				   << newTargetY << " giving gain " << newGain;
		if (c.bound == AgcConstraint::Bound::LOWER && newGain > gain) {
			LOG(RPiAgc, Debug) << "Lower bound constraint adopted";
			gain = newGain;
			targetY = newTargetY;
		} else if (c.bound == AgcConstraint::Bound::UPPER && newGain < gain) {
			LOG(RPiAgc, Debug) << "Upper bound constraint adopted";
			gain = newGain;
			targetY = newTargetY;
		}
	}
	LOG(RPiAgc, Debug) << "Final gain " << gain << " (target_Y " << targetY << " ev "
			   << status_.ev << " base_ev " << config_.baseEv
			   << ")";
}

void Agc::computeTargetExposure(double gain)
{
	if (status_.fixedShutter && status_.fixedAnalogueGain) {
		/*
		 * When ag and shutter are both fixed, we need to drive the
		 * total exposure so that we end up with a digital gain of at least
		 * 1/minColourGain. Otherwise we'd desaturate channels causing
		 * white to go cyan or magenta.
		 */
		double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 });
		ASSERT(minColourGain != 0.0);
		target_.totalExposure =
			status_.fixedShutter * status_.fixedAnalogueGain / minColourGain;
	} else {
		/*
		 * The statistics reflect the image without digital gain, so the final
		 * total exposure we're aiming for is:
		 */
		target_.totalExposure = current_.totalExposureNoDG * gain;
		/* The final target exposure is also limited to what the exposure mode allows. */
		Duration maxShutter = status_.fixedShutter
					      ? status_.fixedShutter
					      : exposureMode_->shutter.back();
		maxShutter = clipShutter(maxShutter);
		Duration maxTotalExposure =
			maxShutter *
			(status_.fixedAnalogueGain != 0.0
				 ? status_.fixedAnalogueGain
				 : exposureMode_->gain.back());
		target_.totalExposure = std::min(target_.totalExposure, maxTotalExposure);
	}
	LOG(RPiAgc, Debug) << "Target totalExposure " << target_.totalExposure;
}

bool Agc::applyDigitalGain(double gain, double targetY)
{
	double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 });
	ASSERT(minColourGain != 0.0);
	double dg = 1.0 / minColourGain;
	/*
	 * I think this pipeline subtracts black level and rescales before we
	 * get the stats, so no need to worry about it.
	 */
	LOG(RPiAgc, Debug) << "after AWB, target dg " << dg << " gain " << gain
			   << " target_Y " << targetY;
	/*
	 * Finally, if we're trying to reduce exposure but the target_Y is
	 * "close" to 1.0, then the gain computed for that constraint will be
	 * only slightly less than one, because the measured Y can never be
	 * larger than 1.0. When this happens, demand a large digital gain so
	 * that the exposure can be reduced, de-saturating the image much more
	 * quickly (and we then approach the correct value more quickly from
	 * below).
	 */
	bool desaturate = targetY > config_.fastReduceThreshold &&
			  gain < sqrt(targetY);
	if (desaturate)
		dg /= config_.fastReduceThreshold;
	LOG(RPiAgc, Debug) << "Digital gain " << dg << " desaturate? " << desaturate;
	target_.totalExposureNoDG = target_.totalExposure / dg;
	LOG(RPiAgc, Debug) << "Target totalExposureNoDG " << target_.totalExposureNoDG;
	return desaturate;
}

void Agc::filterExposure(bool desaturate)
{
	double speed = config_.speed;
	/*
	 * AGC adapts instantly if both shutter and gain are directly specified
	 * or we're in the startup phase.
	 */
	if ((status_.fixedShutter && status_.fixedAnalogueGain) ||
	    frameCount_ <= config_.startupFrames)
		speed = 1.0;
	if (!filtered_.totalExposure) {