/* SPDX-License-Identifier: LGPL-2.1-or-later */
/*
 * Copyright (C) 2021, Red Hat
 * Copyright (C) 2022, Ideas On Board
 * Copyright (C) 2022, Theobroma Systems
 *
 * af_hill_climbing.h - AF Hill Climbing common algorithm
 */

#pragma once

#include <libcamera/base/log.h>

#include "af_algorithm.h"

namespace libcamera {

LOG_DECLARE_CATEGORY(Af)

namespace ipa::common::algorithms {

template<typename Module>
class AfHillClimbing : public AfAlgorithm<Module>
{
public:
	AfHillClimbing()
		: mode_(controls::AfModeAuto), state_(controls::AfStateIdle),
		  pauseState_(controls::AfPauseStateRunning),
		  lensPosition_(0), bestPosition_(0), currentContrast_(0.0),
		  previousContrast_(0.0), maxContrast_(0.0), maxStep_(0),
		  coarseCompleted_(false), fineCompleted_(false),
		  lowStep_(0), highStep_(kMaxFocusSteps), framesToSkip_(0)
	{
	}

	virtual ~AfHillClimbing() {}

	void setMode(controls::AfModeEnum mode) final
	{
		if (mode != mode_) {
			LOG(Af, Debug) << "Switched AF mode from " << mode_
				       << " to " << mode;
			pauseState_ = libcamera::controls::AfPauseStateRunning;
			mode_ = mode;
		}
	}

	void setRange([[maybe_unused]] controls::AfRangeEnum range) final
	{
		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
	}

	void setSpeed([[maybe_unused]] controls::AfSpeedEnum speed) final
	{
		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
	}

	void setTrigger(controls::AfTriggerEnum trigger) final
	{
		LOG(Af, Debug) << "Trigger called in mode " << mode_
			       << " with " << trigger;
		if (mode_ == libcamera::controls::AfModeAuto) {
			if (trigger == libcamera::controls::AfTriggerStart)
				afReset();
			else
				state_ = libcamera::controls::AfStateIdle;
		}
	}

	void setPause(controls::AfPauseEnum pause) final
	{
		/* \todo: add the AfPauseDeferred mode */
		if (mode_ == libcamera::controls::AfModeContinuous) {
			if (pause == libcamera::controls::AfPauseImmediate)
				pauseState_ = libcamera::controls::AfPauseStatePaused;
			else if (pause == libcamera::controls::AfPauseResume)
				pauseState_ = libcamera::controls::AfPauseStateRunning;
		}
	}

	void setLensPosition([[maybe_unused]] float lensPosition) final
	{
		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
	}

	/* These methods should be implemented by derived class */
	virtual void setMetering(controls::AfMeteringEnum metering) = 0;
	virtual void setWindows(Span<const Rectangle> windows) = 0;

protected:
	uint32_t processAutofocus(double currentContrast)
	{
		currentContrast_ = currentContrast;

		if (shouldSkipFrame())
			return lensPosition_;

		/* If we are in a paused state, we won't process the stats */
		if (pauseState_ == libcamera::controls::AfPauseStatePaused)
			return lensPosition_;

		/* Depending on the mode, we may or may not process the stats */
		if (state_ == libcamera::controls::AfStateIdle)
			return lensPosition_;

		if (state_ != libcamera::controls::AfStateFocused) {
			afCoarseScan();
			afFineScan();
		} else {
			/* We can re-start the scan at any moment in AfModeContinuous */
			if (mode_ == libcamera::controls::AfModeContinuous)
				if (afIsOutOfFocus())
					afReset();
		}

		return lensPosition_;
	}

	void setFramesToSkip(uint32_t n)
	{
		if (n > framesToSkip_)
			framesToSkip_ = n;
	}

private:
	void afCoarseScan()
	{
		if (coarseCompleted_)
			return;

		if (afScan(kCoarseSearchStep)) {
			coarseCompleted_ = true;
			maxContrast_ = 0;
			lensPosition_ = lensPosition_ - (lensPosition_ * kFineRange);
			previousContrast_ = 0;
			maxStep_ = std::clamp(lensPosition_ + static_cast<uint32_t>((lensPosition_ * kFineRange)),
					      0U, highStep_);
		}
	}

	void afFineScan()
	{
		if (!coarseCompleted_)
			return;

		if (afScan(kFineSearchStep)) {
			LOG(Af, Debug) << "AF found the best focus position !";
			state_ = libcamera::controls::AfStateFocused;
			fineCompleted_ = true;
		}
	}

	bool afScan(uint32_t minSteps)
	{
		if (lensPosition_ + minSteps > maxStep_) {
			/* If the max step is reached, move lens to the position. */
			lensPosition_ = bestPosition_;
			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 ((currentContrast_ - maxContrast_) >= -(maxContrast_ * 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.
				*/
				bestPosition_ = lensPosition_;
				lensPosition_ += minSteps;
				maxContrast_ = currentContrast_;
			} 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.
				*/
				lensPosition_ = bestPosition_;
				return true;
			}
		}

		previousContrast_ = currentContrast_;
		LOG(Af, Debug) << " Previous step is " << bestPosition_
			       << " Current step is " << lensPosition_;
		return false;
	}

	void afReset()
	{
		LOG(Af, Debug) << "Reset AF parameters";
		lensPosition_ = lowStep_;
		maxStep_ = highStep_;
		state_ = libcamera::controls::AfStateScanning;
		previousContrast_ = 0.0;
		coarseCompleted_ = false;
		fineCompleted_ = false;
		maxContrast_ = 0.0;
		setFramesToSkip(1);
	}

	bool afIsOutOfFocus()
	{
		const uint32_t diff_var = std::abs(currentContrast_ -
						   maxContrast_);
		const double var_ratio = diff_var / maxContrast_;
		LOG(Af, Debug) << "Variance change rate: " << var_ratio
			       << " Current VCM step: " << lensPosition_;
		if (var_ratio > kMaxChange)
			return true;
		else
			return false;
	}

	bool shouldSkipFrame()
	{
		if (framesToSkip_ > 0) {
			framesToSkip_--;
			return true;
		}

		return false;
	}

	controls::AfModeEnum mode_;
	controls::AfStateEnum state_;
	controls::AfPauseStateEnum pauseState_;

	/* VCM step configuration. It is the current setting of the VCM step. */
	uint32_t lensPosition_;
	/* The best VCM step. It is a local optimum VCM step during scanning. */
	uint32_t bestPosition_;

	/* Current AF statistic contrast. */
	double currentContrast_;
	/* It is used to determine the derivative during scanning */
	double previousContrast_;
	double maxContrast_;
	/* The designated maximum range of focus scanning. */
	uint32_t maxStep_;
	/* If the coarse scan completes, it is set to true. */
	bool coarseCompleted_;
	/* If the fine scan completes, it is set to true. */
	bool fineCompleted_;

	uint32_t lowStep_;
	uint32_t highStep_;
	uint32_t framesToSkip_;

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

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

} /* namespace ipa::common::algorithms */
} /* namespace libcamera */