#include "atu_control.hpp"

namespace atu {
namespace {
constexpr double pi = 3.14159265358979323846;
double abs(double value) { return value < 0 ? -value : value; }
bool finite(double value) { return value == value && value > -1e200 && value < 1e200; }
double root(double value) {
    if (value <= 0) return 0;
    double answer = value >= 1 ? value : 1;
    for (unsigned n = 0; n < 96; ++n) answer = 0.5 * (answer + value / answer);
    return answer;
}
Complex add(Complex a, Complex b) { return {a.real + b.real, a.imag + b.imag}; }
Complex inverse(Complex value) {
    const double norm = value.real * value.real + value.imag * value.imag;
    return norm > 0 ? Complex{value.real / norm, -value.imag / norm} : Complex{};
}
double bank_value(const double* values, unsigned count, unsigned mask) {
    double value = 0;
    for (unsigned bit = 0; bit < count; ++bit) if (mask & (1u << bit)) value += values[bit];
    return value;
}
void nearest(const double* values, unsigned count, double requested, unsigned* masks) {
    double distances[4] = {1e199, 1e199, 1e199, 1e199};
    for (unsigned mask = 0; mask < (1u << count); ++mask) {
        const double distance = abs(bank_value(values, count, mask) - requested);
        for (unsigned place = 0; place < 4; ++place) if (distance < distances[place]) {
            for (unsigned other = 3; other > place; --other) { distances[other] = distances[other - 1]; masks[other] = masks[other - 1]; }
            distances[place] = distance; masks[place] = mask; break;
        }
    }
}
bool healthy_power(double watts) { return finite(watts) && watts >= 0; }
bool interpolate(unsigned raw, const Calibration& curve, double& watts) {
    if (raw < curve.points[0].raw || raw > curve.points[curve.count - 1].raw) return false;
    for (unsigned i = 1; i < curve.count; ++i) if (raw <= curve.points[i].raw) {
        const auto& low = curve.points[i - 1]; const auto& high = curve.points[i];
        watts = low.watts + (high.watts - low.watts) * (raw - low.raw) / (high.raw - low.raw);
        return true;
    }
    return false;
}
bool configuration_valid(const Configuration& config) {
    return config.commissioned && config.confirmed_actuator_interface && config.power.rated_w > 0 && profile(config.power.rated_w).rated_w > 0
        && config.actuator_mode == ActuatorMode::LatchingRelayBanks && config.power.mode == ActuatorMode::LatchingRelayBanks && finite(config.cold_rf_w)
        && config.cold_rf_w >= 0 && config.cold_rf_w < config.power.minimum_tune_w
        && config.power.minimum_tune_w > 0 && config.power.maximum_tune_w >= config.power.minimum_tune_w
        && config.power.maximum_tune_w < config.power.rated_w && finite(config.arm_swr)
        && finite(config.trip_swr) && config.arm_swr >= 1 && config.trip_swr >= config.arm_swr
        && config.inhibit_timeout_ms > config.cold_dwell_ms && config.release_ms > 0
        && config.operate_settle_ms > 0 && config.actuator_timeout_ms > config.release_ms
        && config.actuator_timeout_ms > config.operate_settle_ms && config.stable_samples >= 2
        && config.sample_interval_ms > 0 && config.probe_timeout_ms > config.sample_interval_ms * config.stable_samples;
}
}

bool equal(Network a, Network b) { return a.l == b.l && a.c == b.c && a.capacitor_at_input == b.capacitor_at_input && a.bypass == b.bypass; }
bool valid(Network state) { return state.l <= 2047 && state.c <= 2047 && (!state.bypass || (state.l == 0 && state.c == 0)); }
Word output_word(Network state) {
    if (!valid(state)) return 0;
    if (state.bypass) return 1u << 25;
    // KL contacts SHUNT each inductor. A closed contact removes that L cell;
    // Network::l denotes INCLUDED inductors, so physical contact intents invert it.
    return (Word(state.l) ^ 2047u) | (Word(state.c) << 11) | (1u << (state.capacitor_at_input ? 22 : 23)) | (1u << 24) | (1u << 26);
}
Banks nominal_banks() {
    Banks banks;
    for (unsigned bit = 0; bit < 11; ++bit) banks.inductance_h[bit] = 20e-9 * (1u << bit);
    for (unsigned bit = 0; bit < 11; ++bit) banks.capacitance_f[bit] = 2e-12 * (1u << bit);
    return banks;
}
bool valid(const Banks& banks) {
    for (double value : banks.inductance_h) if (!finite(value) || value <= 0 || value > 1e-3) return false;
    for (double value : banks.capacitance_f) if (!finite(value) || value <= 0 || value > 1e-6) return false;
    return true;
}
ImpedanceResult impedance(double frequency_hz, Complex load, Network state, const Banks& banks) {
    if (!valid(state) || !valid(banks) || !finite(frequency_hz) || frequency_hz < 1 || frequency_hz > 1e9
        || !finite(load.real) || !finite(load.imag) || load.real <= 0 || abs(load.real) > 1e9 || abs(load.imag) > 1e9) return {};
    if (state.bypass) return {true, load};
    const double omega = 2 * pi * frequency_hz;
    const double xl = omega * bank_value(banks.inductance_h, 11, state.l);
    const double bc = omega * bank_value(banks.capacitance_f, 11, state.c);
    const Complex input = state.capacitor_at_input
        ? inverse(add(inverse(add(load, {0, xl})), {0, bc}))
        : add(inverse(add(inverse(load), {0, bc})), {0, xl});
    return {finite(input.real) && finite(input.imag) && input.real > 0, input};
}
double swr(Complex input, double reference_ohm) {
    if (!finite(input.real) || !finite(input.imag) || input.real <= 0 || !finite(reference_ohm) || reference_ohm <= 0) return 0;
    const double denominator = (input.real + reference_ohm) * (input.real + reference_ohm) + input.imag * input.imag;
    const double numerator = (input.real - reference_ohm) * (input.real - reference_ohm) + input.imag * input.imag;
    if (!finite(denominator) || denominator <= 0) return 0;
    const double gamma = root(numerator / denominator);
    return gamma < 1 ? (1 + gamma) / (1 - gamma) : 0;
}
Prediction predict(double frequency_hz, Complex load, const Banks& banks, StateAllowed allowed, void* context) {
    Prediction result;
    const auto consider = [&](Network state) {
        if (allowed && !allowed(state, frequency_hz, context)) return;
        const auto z = impedance(frequency_hz, load, state, banks);
        if (!z.valid) return;
        const double score = swr(z.value); ++result.evaluated;
        if (score >= 1 && (!result.valid || score < result.swr)) { result.valid = true; result.network = state; result.swr = score; }
    };
    consider({0, 0, false, true});
    if (!result.valid) {
        const auto test = impedance(frequency_hz, load, {}, banks);
        if (!test.valid) return result;
    }
    const double omega = 2 * pi * frequency_hz;
    double target_l[2] = {}, target_c[2] = {};
    bool possible[2] = {};
    if (load.real <= 50) {
        const double t = root(load.real * (50 - load.real));
        target_l[0] = (t - load.imag) / omega;
        target_c[0] = t / (50 * load.real * omega);
        possible[0] = target_l[0] >= 0 && target_c[0] >= 0;
    }
    const double norm = load.real * load.real + load.imag * load.imag;
    const double g = load.real / norm;
    if (g > 0 && g <= 1.0 / 50) {
        const double b = root(g / 50 - g * g);
        target_l[1] = 50 * b / (g * omega);
        target_c[1] = (b + load.imag / norm) / omega;
        possible[1] = target_l[1] >= 0 && target_c[1] >= 0;
    }
    for (unsigned topology = 0; topology < 2; ++topology) if (possible[topology]) {
        unsigned l_masks[4] = {}, c_masks[4] = {};
        nearest(banks.inductance_h, 11, target_l[topology], l_masks);
        nearest(banks.capacitance_f, 11, target_c[topology], c_masks);
        for (unsigned l : l_masks) for (unsigned c : c_masks) consider({static_cast<unsigned short>(l), static_cast<unsigned short>(c), topology == 0, false});
    }
    return result;
}

bool valid(const Calibration& curve) {
    if (!curve.commissioned || curve.version == 0 || curve.count < 2 || curve.count > 12 || curve.adc_max < 100
        || curve.rail_guard == 0 || curve.rail_guard * 2 >= curve.adc_max) return false;
    for (unsigned n = 0; n < curve.count; ++n) {
        const auto& point = curve.points[n];
        if (point.raw < curve.rail_guard || point.raw > curve.adc_max - curve.rail_guard || !healthy_power(point.watts)) return false;
        if (n && (point.raw <= curve.points[n - 1].raw || point.watts <= curve.points[n - 1].watts)) return false;
    }
    return true;
}
Measurement measure(unsigned forward_raw, unsigned reflected_raw, const Calibration& forward, const Calibration& reflected, double minimum_forward_w) {
    Measurement result;
    if (!valid(forward) || !valid(reflected) || forward.version != reflected.version || !finite(minimum_forward_w) || minimum_forward_w <= 0) return result;
    if (forward_raw <= forward.rail_guard || forward_raw >= forward.adc_max - forward.rail_guard
        || reflected_raw <= reflected.rail_guard || reflected_raw >= reflected.adc_max - reflected.rail_guard) { result.status = MeasurementStatus::AdcRail; return result; }
    if (!interpolate(forward_raw, forward, result.forward_w) || !interpolate(reflected_raw, reflected, result.reflected_w)) {
        result.status = MeasurementStatus::OutsideCalibration; return result;
    }
    if (result.forward_w < minimum_forward_w) { result.status = MeasurementStatus::InsufficientForward; return result; }
    if (result.reflected_w >= result.forward_w) { result.status = MeasurementStatus::InconsistentPowers; return result; }
    const double gamma = root(result.reflected_w / result.forward_w);
    result.swr = (1 + gamma) / (1 - gamma);
    result.status = MeasurementStatus::Ok;
    return result;
}
Profile profile(unsigned rated_w) {
    switch (rated_w) {
    case 20: return {20, 0.2, 2};
    case 50: return {50, 0.2, 3};
    case 150: return {150, 0.5, 5};
    case 500: case 1000: case 1500: case 2000: case 2500: case 3000: return {rated_w, 0.5, 10, ActuatorMode::Motorized};
    default: return {};
    }
}
Controller::Controller(Configuration config) : config_(config) {
    if (config.power.mode != ActuatorMode::LatchingRelayBanks || config.actuator_mode != ActuatorMode::LatchingRelayBanks
        || profile(config.power.rated_w).mode != ActuatorMode::LatchingRelayBanks) fail(Fault::UnsupportedActuator);
    else if (!configuration_valid(config_)) fail(Fault::Configuration);
}
bool Controller::health(const Inputs& inputs) const { return inputs.supply_ok && inputs.guard_ok && inputs.temperature_ok && inputs.driver_ok; }
bool Controller::cold(const Inputs& inputs) const { return inputs.sensor_ok && healthy_power(inputs.forward_w) && healthy_power(inputs.reflected_w)
    && inputs.forward_w <= config_.cold_rf_w && inputs.reflected_w <= config_.cold_rf_w && inputs.hardware_inhibited; }
bool Controller::confirmed(const Inputs& inputs) const { return inputs.applied_state_known && inputs.actuator_idle && inputs.applied_state_word == outputs_.relays; }
void Controller::fail(Fault reason) {
    state_ = State::Fault; fault_ = reason;
    outputs_.pa_permit = false; outputs_.low_power_request = false; outputs_.actuator_request = false;
    result_available_ = false;
    // Cancel new movement; hold contact-state intent. GPIO low is NOT an open
    // contact on a bistable relay. The external pulse HAL must also inhibit RF.
}
bool Controller::request(Network network, Tick now) {
    if (state_ == State::Fault || (state_ != State::Idle && state_ != State::Ready && state_ != State::Armed)) return false;
    outputs_.pa_permit = false; outputs_.low_power_request = false; outputs_.actuator_request = false;
    if (!valid(network)) { fail(Fault::InvalidNetwork); return false; }
    pending_ = network; started_ = phase_ = now; samples_ = 0; result_available_ = false;
    state_ = State::Inhibiting; return true;
}
void Controller::tick(Tick now, const Inputs& inputs) {
    if (state_ == State::Fault) return;
    if (!health(inputs)) { fail(Fault::Interlock); return; }
    if (!inputs.sensor_ok || !healthy_power(inputs.forward_w) || !healthy_power(inputs.reflected_w)) { fail(Fault::Sensor); return; }
    if ((state_ == State::Armed || state_ == State::Ready || state_ == State::Probing || state_ == State::StoppingProbe)
        && !confirmed(inputs)) { fail(Fault::ActuatorFeedback); return; }
    if (state_ == State::Armed) {
        if (inputs.forward_w > config_.power.rated_w) fail(Fault::OverPower);
        else if (inputs.forward_w >= config_.power.minimum_tune_w && (!inputs.measurement_valid || !finite(inputs.measured_swr) || inputs.measured_swr < 1)) fail(Fault::Sensor);
        else if (inputs.measurement_valid && inputs.measured_swr > config_.trip_swr) fail(Fault::Mismatch);
        return;
    }
    if (state_ == State::Inhibiting || state_ == State::ColdDwell) {
        if (now - started_ > config_.inhibit_timeout_ms) { fail(Fault::InhibitTimeout); return; }
        if (!cold(inputs)) { state_ = State::Inhibiting; phase_ = now; return; }
        if (state_ == State::Inhibiting) { phase_ = now; state_ = State::ColdDwell; return; }
        if (now - phase_ >= config_.cold_dwell_ms) { outputs_.relays = 0; outputs_.actuator_request = true; phase_ = now; state_ = State::Releasing; }
        return;
    }
    if (state_ == State::Releasing || state_ == State::Settling) {
        if (!cold(inputs)) { fail(Fault::UnexpectedRf); return; }
        if (now - phase_ > config_.actuator_timeout_ms) { fail(Fault::ActuatorTimeout); return; }
        if (state_ == State::Releasing && confirmed(inputs) && now - phase_ >= config_.release_ms) {
            outputs_.relays = output_word(pending_); phase_ = now; state_ = State::Settling;
        } else if (state_ == State::Settling && confirmed(inputs) && now - phase_ >= config_.operate_settle_ms) {
            outputs_.actuator_request = false; outputs_.low_power_request = true; phase_ = now; last_sample_ = now; state_ = State::Probing;
        }
        return;
    }
    if (state_ == State::Probing) {
        if (inputs.forward_w > config_.power.maximum_tune_w) { fail(Fault::OverPower); return; }
        if (now - phase_ > config_.probe_timeout_ms) { fail(Fault::ProbeTimeout); return; }
        if (inputs.measurement_valid && inputs.forward_w >= config_.power.minimum_tune_w
            && finite(inputs.measured_swr) && inputs.measured_swr >= 1 && now - last_sample_ >= config_.sample_interval_ms) {
            if (samples_ && abs(inputs.measured_swr - result_swr_) > 0.05) samples_ = 0;
            result_swr_ = inputs.measured_swr; last_sample_ = now;
            if (++samples_ >= config_.stable_samples) {
                outputs_.low_power_request = false; phase_ = started_ = now; state_ = State::StoppingProbe;
            }
        }
        return;
    }
    if (state_ == State::StoppingProbe) {
        if (now - started_ > config_.inhibit_timeout_ms) { fail(Fault::InhibitTimeout); return; }
        if (!cold(inputs)) { phase_ = now; return; }
        if (now - phase_ >= config_.cold_dwell_ms) { result_available_ = true; state_ = State::Ready; }
    }
}
bool Controller::reset(Tick now, const Inputs& inputs) {
    if (state_ != State::Fault || !configuration_valid(config_) || !health(inputs) || !cold(inputs)) return false;
    fault_ = Fault::None; state_ = State::Idle; started_ = phase_ = now; samples_ = 0; result_available_ = false;
    outputs_.pa_permit = false; outputs_.low_power_request = false; return true;
}
bool Controller::arm(const Inputs& inputs) {
    if (state_ != State::Ready || !result_available_ || result_swr_ > config_.arm_swr || !health(inputs) || !cold(inputs) || !confirmed(inputs)) return false;
    state_ = State::Armed; outputs_.pa_permit = true; return true;
}

void ScalarSearch::begin(unsigned budget, double target_swr) {
    budget_ = budget > 256 ? 256 : budget; target_ = target_swr >= 1 ? target_swr : 1.3;
    proposed_ = coarse_ = direction_ = 0; step_l_ = 256; step_c_ = 256;
    refining_ = has_best_ = waiting_ = false; score_ = 0; best_ = {};
}
bool ScalarSearch::next(Network& proposal) {
    if (waiting_ || proposed_ >= budget_ || (has_best_ && score_ <= target_)) return false;
    constexpr unsigned l_grid[7] = {0, 128, 256, 512, 1024, 1536, 2047};
    constexpr unsigned c_grid[7] = {0, 128, 256, 512, 1024, 1536, 2047};
    while (true) {
        if (!refining_ && coarse_ < 98) {
            const unsigned index = coarse_++;
            proposal = {static_cast<unsigned short>(l_grid[(index / 7) % 7]), static_cast<unsigned short>(c_grid[index % 7]), index < 49, false};
        } else {
            refining_ = true;
            if (!has_best_ || (step_l_ == 0 && step_c_ == 0)) return false;
            constexpr int dl[8] = {-1, 1, 0, 0, -1, -1, 1, 1};
            constexpr int dc[8] = {0, 0, -1, 1, -1, 1, -1, 1};
            const unsigned direction = direction_++;
            const int l = int(best_.l) + dl[direction] * step_l_;
            const int c = int(best_.c) + dc[direction] * step_c_;
            if (direction_ == 8) { direction_ = 0; step_l_ /= 2; step_c_ /= 2; }
            if (l < 0 || l > 2047 || c < 0 || c > 2047) continue;
            proposal = {static_cast<unsigned short>(l), static_cast<unsigned short>(c), best_.capacitor_at_input, false};
            if (equal(proposal, best_)) continue;
        }
        outstanding_ = proposal; waiting_ = true; ++proposed_; return true;
    }
}
bool ScalarSearch::observe(Network proposal, double measured_swr, bool verified) {
    if (!waiting_ || !equal(proposal, outstanding_)) return false;
    waiting_ = false;
    if (!verified || !finite(measured_swr) || measured_swr < 1) return false;
    if (!has_best_ || measured_swr < score_) { best_ = proposal; score_ = measured_swr; has_best_ = true; }
    return true;
}
bool TuneCache::store(CacheKey key, Network network, double measured_swr, bool verified) {
    if (!verified || !valid(network) || !finite(measured_swr) || measured_swr < 1 || measured_swr > 1.3
        || key.frequency_hz == 0 || key.span_hz == 0 || key.calibration_version == 0 || profile(key.power_w).rated_w == 0) return false;
    entries_[cursor_] = {key, network, true}; cursor_ = (cursor_ + 1) % 16; return true;
}
bool TuneCache::propose(CacheKey query, Network& network) const {
    for (const auto& entry : entries_) if (entry.valid && query.antenna_id == entry.key.antenna_id
        && query.calibration_version == entry.key.calibration_version && query.power_w == entry.key.power_w) {
        const unsigned difference = query.frequency_hz > entry.key.frequency_hz ? query.frequency_hz - entry.key.frequency_hz : entry.key.frequency_hz - query.frequency_hz;
        if (difference <= entry.key.span_hz) { network = entry.network; return true; }
    }
    return false;
}
bool valid(MotorTarget target) {
    return finite(target.inductance_h) && target.inductance_h >= 0.05e-6 && target.inductance_h <= 40e-6
        && finite(target.fine_capacitance_f) && target.fine_capacitance_f >= 15e-12 && target.fine_capacitance_f <= 1000e-12
        && target.coarse_capacitance_mask <= 7 && target.calibration_version > 0
        && target.frequency_hz >= 1800000 && target.frequency_hz <= 30000000;
}
bool motor_at_target(MotorTarget target, MotorFeedback feedback, Tick now, double tolerance_l, double tolerance_c, Tick maximum_age) {
    return valid(target) && feedback.homed && feedback.encoder_ok && feedback.watchdog_ok
        && !feedback.moving && !feedback.limit_fault && feedback.calibration_version == target.calibration_version
        && feedback.frequency_hz == target.frequency_hz && now >= feedback.sampled_at_ms
        && maximum_age > 0 && now - feedback.sampled_at_ms <= maximum_age
        && finite(tolerance_l) && tolerance_l > 0 && tolerance_l <= 0.05e-6
        && finite(tolerance_c) && tolerance_c > 0 && tolerance_c <= 5e-12
        && finite(feedback.inductance_h) && finite(feedback.fine_capacitance_f)
        && abs(target.inductance_h - feedback.inductance_h) <= tolerance_l
        && abs(target.fine_capacitance_f - feedback.fine_capacitance_f) <= tolerance_c
        && feedback.coarse_capacitance_mask == target.coarse_capacitance_mask
        && feedback.capacitor_at_input == target.capacitor_at_input
        && feedback.capacitor_disconnected == target.capacitor_disconnected
        && feedback.series_capacitor_enabled == target.series_capacitor_enabled;
}
}