#include "internal.hpp"
#include <algorithm>
#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;
    std::optional<double> normalized_gray;
    std::optional<TimestampUs> direction_since;
    int timed_direction = 0;
    void resetTemporal() {
        normalized_gray.reset(); direction_since.reset(); timed_direction = 0;
    }
};

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

void ContinuousExposureControl::reset(FeedbackResetReason reason) {
    impl_->controller.reset();
    impl_->resetTemporal();
    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 (input.predicted_gray && (!std::isfinite(*input.predicted_gray) ||
        *input.predicted_gray < 1 || *input.predicted_gray > 254 ||
        !std::isfinite(input.prediction_max_step_ratio) ||
        input.prediction_max_step_ratio <= 0 || input.prediction_max_step_ratio >= 1))
        out.errors.emplace_back("invalid prediction gray or step limit");
    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;
    out.raw_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;
    const bool preserveIdleConfirmation = !s.pending_reset &&
        out.reset_reason == FeedbackResetReason::IdleTimeout && config.preserve_confirmation_on_idle;
    if (out.reset_reason && !preserveIdleConfirmation) {
        s.controller.reset(); s.resetTemporal();
    }

    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;
    const double rawGray = *measurement->gray;
    const double rawError = (controller.target_gray - rawGray) / controller.target_gray;
    feedback.control.current_error_ratio = rawError;
    const bool rawDeadband = std::abs(rawError) < controller.deadband_ratio || rawError == 0;
    const bool rawHolding = rawDeadband || (s.controller.deadband_latched &&
        std::abs(rawError) + 1e-12 < controller.deadband_ratio + controller.hysteresis_ratio);
    const bool suppressIntegral = input.suppress_integral || preserveIdleConfirmation;
    double integralScale = 1.0;
    bool confirmationReady = true;
    if (config.time_aware) {
        auto& trace = feedback.control;
        trace.time_aware = true;
        trace.elapsed_us = s.watermark ? input.timestamp_us - *s.watermark : config.nominal_interval_us;
        const double dt = static_cast<double>(trace.elapsed_us);
        integralScale = suppressIntegral ? 0.0 : std::min(1.0, dt / config.nominal_interval_us);
        trace.integral_time_scale = integralScale;
        trace.effective_max_step_ratio = std::min(controller.max_step_ratio,
            config.max_step_ratio_per_second * (dt / 1'000'000.0));
        controlConfig.controller.max_step_ratio = trace.effective_max_step_ratio;
        controlConfig.controller.same_direction_frames = std::max(2, controller.same_direction_frames);

        // Filter scene response per exposure, then project to THIS frame's exposure.
        // Gain changes require caller reset: gain units and response are not assumed.
        const double normalized = rawGray / input.actual_exposure_us;
        const bool resetFilter = !s.normalized_gray || !std::isfinite(normalized) || suppressIntegral || out.reset_reason ||
            config.smoothing_tau_us == 0 || std::abs(rawError) >= controller.large_error_ratio || rawHolding;
        trace.filter_alpha = resetFilter ? 1.0 : -std::expm1(-dt / config.smoothing_tau_us);
        double filtered = resetFilter ? normalized :
            *s.normalized_gray + trace.filter_alpha * (normalized - *s.normalized_gray);
        double gray = resetFilter ? rawGray : std::clamp(filtered * input.actual_exposure_us, 0.0, 255.0);
        // Past dark frames must never command brightening an already-bright frame,
        // nor hide a return into the raw deadband behind a delayed filtered error.
        if (!std::isfinite(gray) || (controller.target_gray - gray) * rawError <= 0) {
            gray = rawGray; filtered = normalized; trace.filter_alpha = 1.0;
        }
        if (std::isfinite(filtered)) s.normalized_gray = filtered;
        else s.normalized_gray.reset();
        out.control_gray = gray;
        trace.filtered_gray = gray;
    }
    const double timeStepRatio = controlConfig.controller.max_step_ratio;
    out.feedback_control_gray = out.control_gray;
    if (input.predicted_gray && selected != FeedbackSource::Global && rawHolding &&
        !suppressIntegral && !out.reset_reason) {
        out.prediction_used = true;
        out.control_gray = input.predicted_gray;
        out.feedback_reason = rawDeadband ? Reason::Deadband : Reason::Hysteresis;
        out.feedback_suggested_exposure_us = input.actual_exposure_us;
        controlConfig.controller.max_step_ratio = std::min(timeStepRatio, input.prediction_max_step_ratio);
        feedback.control.effective_max_step_ratio = controlConfig.controller.max_step_ratio;
    }
    if (config.time_aware) {
        auto& trace = feedback.control;
        const double error = (controller.target_gray - *out.control_gray) / controller.target_gray;
        const bool holding = std::abs(error) < controller.deadband_ratio || error == 0 ||
            (s.controller.deadband_latched &&
             std::abs(error) + 1e-12 < controller.deadband_ratio + controller.hysteresis_ratio);
        const int sign = holding ? 0 : (error > 0 ? 1 : -1);
        if (sign == 0) { s.direction_since.reset(); s.timed_direction = 0; }
        else if (sign != s.timed_direction || !s.direction_since) {
            s.direction_since = input.timestamp_us; s.timed_direction = sign;
        }
        trace.same_direction_duration_us = s.direction_since ? input.timestamp_us - *s.direction_since : 0;
        confirmationReady = trace.same_direction_duration_us >= config.confirmation_us;
        trace.confirmation_time_satisfied = confirmationReady;
    }
    s.controller.update(*out.control_gray, input.actual_exposure_us, controlConfig, feedback,
                        suppressIntegral, integralScale, confirmationReady);
    if (config.time_aware) feedback.control.time_rate_limited = feedback.control.step_limited &&
        timeStepRatio < controller.max_step_ratio &&
        feedback.control.effective_max_step_ratio == timeStepRatio;
    if (!out.prediction_used) {
        out.feedback_control_gray = out.control_gray;
        out.feedback_reason = feedback.reason;
        out.feedback_suggested_exposure_us = feedback.suggested_exposure_us;
    }
    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