/* SPDX-License-Identifier: LGPL-2.1-or-later */
/*
 * Copyright (C) 2021, Red Hat
 *
 * af.cpp - IPU3 auto focus algorithm
 */

#include "af.h"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <fcntl.h>
#include <numeric>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>

#include <linux/videodev2.h>

#include <libcamera/base/log.h>

#include <libcamera/ipa/core_ipa_interface.h>

#include "libipa/histogram.h"

/**
 * \file af.h
 */

/*
 * Static variables from ChromiumOS Intel Camera HAL and ia_imaging library:
 * - https://chromium.googlesource.com/chromiumos/platform/arc-camera/+/master/hal/intel/psl/ipu3/statsConverter/ipu3-stats.h
 * - https://chromium.googlesource.com/chromiumos/platform/camera/+/refs/heads/main/hal/intel/ipu3/include/ia_imaging/af_public.h
 */

/** The minimum horizontal grid dimension. */
static constexpr uint8_t kAfMinGridWidth = 16;
/** The minimum vertical grid dimension. */
static constexpr uint8_t kAfMinGridHeight = 16;
/** The maximum horizontal grid dimension. */
static constexpr uint8_t kAfMaxGridWidth = 32;
/** The maximum vertical grid dimension. */
static constexpr uint8_t kAfMaxGridHeight = 24;
/** The minimum value of Log2 of the width of the grid cell. */
static constexpr uint16_t kAfMinGridBlockWidth = 4;
/** The minimum value of Log2 of the height of the grid cell. */
static constexpr uint16_t kAfMinGridBlockHeight = 3;
/** The maximum value of Log2 of the width of the grid cell. */
static constexpr uint16_t kAfMaxGridBlockWidth = 6;
/** The maximum value of Log2 of the height of the grid cell. */
static constexpr uint16_t kAfMaxGridBlockHeight = 6;
/** The number of blocks in vertical axis per slice. */
static constexpr uint16_t kAfDefaultHeightPerSlice = 2;

namespace libcamera {

using namespace std::literals::chrono_literals;

namespace ipa::ipu3::algorithms {

LOG_DEFINE_CATEGORY(IPU3Af)

/**
 * Maximum focus steps of the VCM control
 * \todo should be obtained from the VCM driver
 */
static constexpr uint32_t kMaxFocusSteps = 1023;

/* Minimum focus step for searching appropriate focus */
static constexpr uint32_t kCoarseSearchStep = 30;
static constexpr uint32_t kFineSearchStep = 1;

/* Max ratio of variance change, 0.0 < kMaxChange < 1.0 */
static constexpr double kMaxChange = 0.5;

/* The numbers of frame to be ignored, before performing focus scan. */
static constexpr uint32_t kIgnoreFrame = 10;

/* Fine scan range 0 < kFineRange < 1 */
static constexpr double kFineRange = 0.05;

/* Settings for IPU3 AF filter */
static struct ipu3_uapi_af_filter_config afFilterConfigDefault = {
	.y1_coeff_0 = { 0, 1, 3, 7 },
	.y1_coeff_1 = { 11, 13, 1, 2 },
	.y1_coeff_2 = { 8, 19, 34, 242 },
	.y1_sign_vec = 0x7fdffbfe,
	.y2_coeff_0 = { 0, 1, 6, 6 },
	.y2_coeff_1 = { 13, 25, 3, 0 },
	.y2_coeff_2 = { 25, 3, 177, 254 },
	.y2_sign_vec = 0x4e53ca72,
	.y_calc = { 8, 8, 8, 8 },
	.nf = { 0, 9, 0, 9, 0 },
};

/**
 * \class Af
 * \brief An auto-focus algorithm based on IPU3 statistics
 *
 * This algorithm is used to determine the position of the lens to make a
 * focused image. The IPU3 AF processing block computes the statistics that
 * are composed by two types of filtered value and stores in a AF buffer.
 * Typically, for a clear image, it has a relatively higher contrast than a
 * blurred one. Therefore, if an image with the highest contrast can be
 * found through the scan, the position of the len indicates to a clearest
 * image.
 */
Af::Af()
	: focus_(0), bestFocus_(0), currentVariance_(0.0), previousVariance_(0.0),
	  coarseCompleted_(false), fineCompleted_(false)
{
}

/**
 * \copydoc libcamera::ipa::Algorithm::prepare
 */
void Af::prepare(IPAContext &context, ipu3_uapi_params *params)
{
	const struct ipu3_uapi_grid_config &grid = context.configuration.af.afGrid;
	params->acc_param.af.grid_cfg = grid;
	params->acc_param.af.filter_config = afFilterConfigDefault;

	/* Enable AF processing block */
	params->use.acc_af = 1;
}

/**
 * \brief Configure the Af given a configInfo
 * \param[in] context The shared IPA context
 * \param[in] configInfo The IPA configuration data
 * \return 0 on success, a negative error code otherwise
 */
int Af::configure(IPAContext &context, const IPAConfigInfo &configInfo)
{
	struct ipu3_uapi_grid_config &grid = context.configuration.af.afGrid;
	grid.width = kAfMinGridWidth;
	grid.height = kAfMinGridHeight;
	grid.block_width_log2 = kAfMinGridBlockWidth;
	grid.block_height_log2 = kAfMinGridBlockHeight;

	/*
	 * \todo - while this clamping code is effectively a no-op, it satisfies
	 * the compiler that the constant definitions of the hardware limits
	 * are used, and paves the way to support dynamic grid sizing in the
	 * future. While the block_{width,height}_log2 remain assigned to the
	 * minimum, this code should be optimized out by the compiler.
	 */
	grid.width = std::clamp(grid.width, kAfMinGridWidth, kAfMaxGridWidth);
	grid.height = std::clamp(grid.height, kAfMinGridHeight, kAfMaxGridHeight);

	grid.block_width_log2 = std::clamp(grid.block_width_log2,
					   kAfMinGridBlockWidth,
					   kAfMaxGridBlockWidth);

	grid.block_height_log2 = std::clamp(grid.block_height_log2,
					    kAfMinGridBlockHeight,
					    kAfMaxGridBlockHeight);

	grid.height_per_slice = kAfDefaultHeightPerSlice;

	/* Position the AF grid in the center of the BDS output. */
	Rectangle bds(configInfo.bdsOutputSize);
	Size gridSize(grid.width << grid.block_width_log2,
		      grid.height << grid.block_height_log2);

	/*
	 * \todo - Support request metadata
	 * - Set the ROI based on any input controls in the request
	 * - Return the AF ROI as metadata in the Request
	 */
	Rectangle roi = gridSize.centeredTo(bds.center());
	Point start = roi.topLeft();

	/* x_start and y_start should be even */
	grid.x_start = utils::alignDown(start.x, 2);
	grid.y_start = utils::alignDown(start.y, 2);
	grid.y_start |= IPU3_UAPI_GRID_Y_START_EN;

	/* Initial max focus step */
	maxStep_ = kMaxFocusSteps;

	/* Initial frame ignore counter */
	afIgnoreFrameReset();

	/* Initial focus value */
	context.activeState.af.focus = 0;
	/* Maximum variance of the AF statistics */
	context.activeState.af.maxVariance = 0;
	/* The stable AF value flag. if it is true, the AF should be in a stable state. */
	context.activeState.af.stable = false;

	return 0;
}

/**
 * \brief AF coarse scan
 *
 * Find a near focused image using a coarse step. The step is determined by coarseSearchStep.
 *
 * \param[in] context The shared IPA context
 */
void Af::afCoarseScan(IPAContext &context)
{
	if (coarseCompleted_)
		return;

	if (afNeedIgnoreFrame())
		return;

	if (afScan(context, kCoarseSearchStep)) {
		coarseCompleted_ = true;
		context.activeState.af.maxVariance = 0;
		focus_ = context.activeState.af.focus -
			 (context.activeState.af.focus * kFineRange);
		context.activeState.af.focus = focus_;
		previousVariance_ = 0;
		maxStep_ = std::clamp(focus_ + static_cast<uint32_t>((focus_ * kFineRange)),
				      0U, kMaxFocusSteps);
	}
}

/**
 * \brief AF fine scan
 *
 * Find an optimum lens position with moving 1 step for each search.
 *
 * \param[in] context The shared IPA context
 */
void Af::afFineScan(IPAContext &context)
{
	if (!coarseCompleted_)
		return;

	if (afNeedIgnoreFrame())
		return;

	if (afScan(context, kFineSearchStep)) {
		context.activeState.af.stable = true;
		fineCompleted_ = true;
	}
}

/**
 * \brief AF reset
 *
 * Reset all the parameters to start over the AF process.
 *
 * \param[in] context The shared IPA context
 */
void Af::afReset(IPAContext &context)
{
	if (afNeedIgnoreFrame())
		return;

	context.activeState.af.maxVariance = 0;
	context.activeState.af.focus = 0;
	focus_ = 0;
	context.activeState.af.stable = false;
	ignoreCounter_ = kIgnoreFrame;
	previousVariance_ = 0.0;
	coarseCompleted_ = false;
	fineCompleted_ = false;
	maxStep_ = kMaxFocusSteps;
}

/**
 * \brief AF variance comparison.
 * \param[in] context The IPA context
 * \param min_step The VCM movement step.
 *
 * We always pick the largest variance to replace the previous one. The image
 * with a larger variance also indicates it is a clearer image than previous
 * one. If we find a negative derivative, we return immediately.
 *
 * \return True, if it finds a AF value.
 */
bool Af::afScan(IPAContext &context, int min_step)
{
	if (focus_ > maxStep_) {
		/* If reach the max step, move lens to the position. */
		context.activeState.af.focus = bestFocus_;
		return true;
	} else {
		/*
		 * Find the maximum of the variance by estimating its
		 * derivative. If the direction changes, it means we have
		 * passed a maximum one step before.
		 */
		if ((currentVariance_ - context.activeState.af.maxVariance) >=
		    -(context.activeState.af.maxVariance * 0.1)) {
			/*
			 * Positive and zero derivative:
			 * The variance is still increasing. The focus could be
			 * increased for the next comparison. Also, the max variance
			 * and previous focus value are updated.
			 */
			bestFocus_ = focus_;
			focus_ += min_step;
			context.activeState.af.focus = focus_;
			context.activeState.af.maxVariance = currentVariance_;
		} else {
			/*
			 * Negative derivative:
			 * The variance starts to decrease which means the maximum
			 * variance is found. Set focus step to previous good one
			 * then return immediately.
			 */
			context.activeState.af.focus = bestFocus_;
			return true;
		}
	}

	previousVariance_ = currentVariance_;
	LOG(IPU3Af, Debug) << " Previous step is "
			   << bestFocus_
			   << " Current step is "
			   << focus_;
	return false;
}

/**
 * \brief Determine the frame to be ignored.
 * \return Return True if the frame should be ignored, false otherwise
 */
bool Af::afNeedIgnoreFrame()
{
	if (ignoreCounter_ == 0)
		return false;
	else
		ignoreCounter_--;
	return true;
}

/**
 * \brief Reset frame ignore counter.
 */
void Af::afIgnoreFrameReset()
{
	ignoreCounter_ = kIgnoreFrame;
}

/**
 * \brief Estimate variance
 * \param y_item The AF filter data set from the IPU3 statistics buffer
 * \param len The quantity of table item entries which are valid to process
 * \param isY1 Selects between filter Y1 or Y2 to calculate the variance
 *
 * Calculate the mean of the data set provided by \a y_item, and then calculate
 * the variance of that data set from the mean.
 *
 * The operation can work on one of two sets of values contained within the
 * y_item data set supplied by the IPU3. The two data sets are the results of
 * both the Y1 and Y2 filters which are used to support coarse (Y1) and fine
 * (Y2) calculations of the contrast.
 *
 * \return The variance of the values in the data set \a y_item selected by \a isY1
 */
double Af::afEstimateVariance(Span<const y_table_item_t> y_items, bool isY1)
{
	uint32_t total = 0;
	double mean;
	double var_sum = 0;

	for (auto y : y_items)
		total += isY1 ? y.y1_avg : y.y2_avg;

	mean = total / y_items.size();

	for (auto y : y_items) {
		double avg = isY1 ? y.y1_avg : y.y2_avg;
		var_sum += pow(avg - mean, 2);
	}

	return var_sum / y_items.size();
}

/**
 * \brief Determine out-of-focus situation.
 * \param context The IPA context.
 *
 * Out-of-focus means that the variance change rate for a focused and a new
 * variance is greater than a threshold.
 *
 * \return True if the variance threshold is crossed indicating lost focus,
 *         false otherwise.
 */
bool Af::afIsOutOfFocus(IPAContext context)
{
	const uint32_t diff_var = std::abs(currentVariance_ -
					   context.activeState.af.maxVariance);
	const double var_ratio = diff_var / context.activeState.af.maxVariance;

	LOG(IPU3Af, Debug) << "Variance change rate: "
			   << var_ratio
			   << " Current VCM step: "
			   << context.activeState.af.focus;

	if (var_ratio > kMaxChange)
		return true;
	else
		return false;
}

/**
 * \brief Determine the max contrast image and lens position.
 * \param[in] context The IPA context.
 * \param[in] frameContext The current frame context
 * \param[in] stats The statistics buffer of IPU3.
 *
 * Ideally, a clear image also has a relatively higher contrast. So, every
 * image for each focus step should be tested to find an optimal focus step.
 *
 * The Hill Climbing Algorithm[1] is used to find the maximum variance of the
 * AF statistics which is the AF output of IPU3. The focus step is increased
 * then the variance of the AF statistics are estimated. If it finds the
 * negative derivative we have just passed the peak, and we infer that the best
 * focus is found.
 *
 * [1] Hill Climbing Algorithm, https://en.wikipedia.org/wiki/Hill_climbing
 */
void Af::process(IPAContext &context, [[maybe_unused]] IPAFrameContext *frameContext,
		 const ipu3_uapi_stats_3a *stats)
{
	/* Evaluate the AF buffer length */
	uint32_t afRawBufferLen = context.configuration.af.afGrid.width *
				  context.configuration.af.afGrid.height;

	ASSERT(afRawBufferLen < IPU3_UAPI_AF_Y_TABLE_MAX_SIZE);

	Span<const y_table_item_t> y_items(reinterpret_cast<const y_table_item_t *>(&stats->af_raw_buffer.y_table),
					   afRawBufferLen);

	/*
	 * Calculate the mean and the variance of AF statistics for a given grid.
	 * For coarse: y1 are used.
	 * For fine: y2 results are used.
	 */
	currentVariance_ = afEstimateVariance(y_items, !coarseCompleted_);

	if (!context.activeState.af.stable) {
		afCoarseScan(context);
		afFineScan(context);
	} else {
		if (afIsOutOfFocus(context))
			afReset(context);
		else
			afIgnoreFrameReset();
	}
}

} /* namespace ipa::ipu3::algorithms */

} /* namespace libcamera */