CommonLibraries/src/continuous_control.cpp

#include "internal.hpp"
#include <cmath>

namespace target_exposure {
namespace {
bool usable(const Brightness& value) {
    const auto gray = [](double v) { return std::isfinite(v) && v >= 0 && v <= 255; };
    const auto fraction = [](double v) { return std::isfinite(v) && v >= 0 && v <= 1; };
    return value.valid() && gray(*value.gray) &&
        (!value.arithmetic_gray || gray(*value.arithmetic_gray)) && value.pixels > 0 &&
        value.retained_pixels > 0 && value.retained_pixels <= value.pixels &&
        fraction(value.near_black_fraction) && fraction(value.saturated_fraction) &&
        value.near_black_fraction + value.saturated_fraction <= 1.0 + 1e-12;
}
bool validSource(FeedbackSource source) {
    return source == FeedbackSource::Global || source == FeedbackSource::ManualRoi ||
        source == FeedbackSource::YoloRegions;
}
}

struct ContinuousExposureControl::Impl {
    detail::Controller controller;
    std::optional<ContinuousConfig> config;
    std::optional<FeedbackSource> source;
    std::optional<TimestampUs> watermark;
    std::optional<FeedbackResetReason> pending_reset = FeedbackResetReason::Startup;
};

ContinuousExposureControl::ContinuousExposureControl() : impl_(std::make_unique<Impl>()) {}
ContinuousExposureControl::~ContinuousExposureControl() = default;

void ContinuousExposureControl::reset(FeedbackResetReason reason) {
    impl_->controller.reset();
    impl_->source.reset();
    impl_->pending_reset = reason;
    if (reason == FeedbackResetReason::ClockChanged) impl_->watermark.reset();
}

ContinuousFrameResult ContinuousExposureControl::update(const ContinuousFrameInput& input,
                                                        const ContinuousConfig& config) {
    auto& s = *impl_;
    ContinuousFrameResult out;
    out.requested_source = input.source;
    out.actual_exposure_us = input.actual_exposure_us;
    out.metering = input.metering;
    out.control.integral = s.controller.integral;
    out.control.same_direction_count = s.controller.count;
    out.errors = validate(config);
    if (!validSource(input.source)) out.errors.emplace_back("invalid measurement source");
    if (!out.errors.empty()) { out.reason = Reason::InvalidConfiguration; return out; }
    if (input.timestamp_us < 0) { out.reason = Reason::InvalidTimestamp; return out; }
    if (s.watermark && input.timestamp_us <= *s.watermark) {
        out.reason = input.timestamp_us == *s.watermark ? Reason::DuplicateFrame : Reason::OutOfOrderFrame;
        return out;
    }
    if (!std::isfinite(input.actual_exposure_us) || input.actual_exposure_us < config.min_exposure_us ||
        input.actual_exposure_us > config.max_exposure_us) {
        out.reason = Reason::InvalidActualExposure; return out;
    }

    FeedbackSource selected = input.source;
    const Brightness* measurement = &input.metering.global;
    if (input.source != FeedbackSource::Global) {
        if (usable(input.metering.target)) measurement = &input.metering.target;
        else {
            selected = FeedbackSource::Global;
            const auto status = input.metering.target.status;
            out.fallback_reason = status == MeterStatus::NoTarget || status == MeterStatus::EmptyRoi ||
                status == MeterStatus::InsufficientPixels ? FeedbackFallbackReason::RegionUnavailable :
                FeedbackFallbackReason::InvalidRegionMeasurement;
        }
    }
    if (!usable(*measurement)) {
        out.reason = Reason::InvalidMetering;
        out.errors.emplace_back("selected measurement has invalid status, gray, pixel counts or fractions");
        return out;
    }
    out.effective_source = selected;
    const auto& controller = selected == FeedbackSource::Global ? config.global_controller : config.region_controller;
    out.target_gray = controller.target_gray;
    out.control_gray = measurement->gray;
    if (s.pending_reset) out.reset_reason = s.pending_reset;
    else if (!s.config || config != *s.config) out.reset_reason = FeedbackResetReason::ConfigurationChanged;
    else if (!s.source || selected != *s.source) out.reset_reason = FeedbackResetReason::SourceChanged;
    else if (s.watermark && input.timestamp_us - *s.watermark > config.idle_timeout_us)
        out.reset_reason = FeedbackResetReason::IdleTimeout;
    if (out.reset_reason) s.controller.reset();

    Config controlConfig;
    controlConfig.min_exposure_us = config.min_exposure_us;
    controlConfig.max_exposure_us = config.max_exposure_us;
    controlConfig.controller = controller;
    FrameResult feedback;
    feedback.suggested_exposure_us = input.actual_exposure_us;
    feedback.control.current_error_ratio = (controller.target_gray - *measurement->gray) / controller.target_gray;
    s.controller.update(*measurement->gray, input.actual_exposure_us, controlConfig, feedback);
    out.accepted = true;
    out.reason = feedback.reason;
    out.suggested_exposure_us = feedback.suggested_exposure_us;
    out.adjustment_suggested = feedback.adjustment_suggested;
    out.control = feedback.control;
    out.state = feedback.reason == Reason::ExposureLimit ? FeedbackState::Limited :
        feedback.adjustment_suggested ? FeedbackState::Adjusting : FeedbackState::Holding;
    s.config = config;
    s.source = selected;
    s.watermark = input.timestamp_us;
    s.pending_reset.reset();
    return out;
}
} // namespace target_exposure