#include "speed_supervisor.h"

#include <limits.h>
#include <stddef.h>
#include <string.h>

#define Q16_ONE INT64_C(65536)
#define DS_MAX_SERVICE_GAP_MS 1000u
#define DS_MAX_CURRENT_LIMIT_MA 10000000u

static uint32_t magnitude_i32(int32_t value) {
    int64_t wide = value;
    if (wide < 0) wide = -wide;
    return (uint32_t)wide;
}

static int8_t sign_i32(int32_t value) {
    if (value > 0) return 1;
    if (value < 0) return -1;
    return 0;
}

static bool target_valid(const ds_supervisor *s, int32_t target) {
    return s && magnitude_i32(target) <= s->cfg.max_rpm;
}

static uint32_t active_speed_limit(const ds_supervisor *s) {
    if (s->beginner_mode && s->cfg.max_rpm > DS_BEGINNER_MAX_RPM)
        return DS_BEGINNER_MAX_RPM;
    return s->cfg.max_rpm;
}

static int32_t limit_target(const ds_supervisor *s, int32_t target) {
    uint32_t limit = active_speed_limit(s);
    if (magnitude_i32(target) <= limit) return target;
    return target < 0 ? -(int32_t)limit : (int32_t)limit;
}

static bool fresh_inputs(const ds_supervisor *s, uint32_t now,
                         const ds_inputs *in) {
    return s && in && in->power_ok && in->driver_ready &&
           in->observer_locked && in->current_calibrated &&
           in->handpiece_sensor_ok && !in->handpiece_parked &&
           !in->hardware_fault && !in->overcurrent_fault &&
           (uint32_t)(now - in->measured_at_ms) <= s->cfg.feedback_max_age_ms &&
           magnitude_i32(in->measured_rpm) <= s->cfg.max_rpm;
}

static void zero_output(ds_supervisor *s) {
    s->ramped_target_rpm = 0;
    s->q_current_ref_ma = 0;
    s->integral_q16_ma = 0;
    s->ramp_remainder = 0;
}

static void latch(ds_supervisor *s, ds_fault fault) {
    zero_output(s);
    s->target_rpm = 0;
    s->state = DS_FAULT;
    s->fault = fault;
    s->zero_timer_active = false;
}

static ds_output snapshot(const ds_supervisor *s) {
    ds_output out = {0};
    out.state = DS_FAULT;
    out.fault = DS_FAULT_CONFIG;
    if (!s) return out;
    out.state = s->state;
    out.fault = s->fault;
    out.run_enable = s->state == DS_RUNNING || s->state == DS_REVERSAL_WAIT;
    out.beginner_mode = s->beginner_mode;
    out.active_speed_limit_rpm = active_speed_limit(s);
    out.ramped_target_rpm = s->ramped_target_rpm;
    out.q_current_ref_ma = s->q_current_ref_ma;
    return out;
}

static bool config_valid(const ds_config *cfg) {
    return cfg && cfg->max_rpm > 0u && cfg->max_rpm <= DS_LISTING_MAX_RPM &&
           cfg->current_limit_ma > 0u &&
           cfg->current_limit_ma <= DS_MAX_CURRENT_LIMIT_MA &&
           cfg->feedback_max_age_ms > 0u &&
           cfg->feedback_max_age_ms < INT32_MAX &&
           cfg->max_service_gap_ms > 0u &&
           cfg->max_service_gap_ms <= DS_MAX_SERVICE_GAP_MS &&
           cfg->max_service_gap_ms < INT32_MAX &&
           cfg->ramp_rpm_per_s > 0u && cfg->ramp_rpm_per_s <= 100000000u &&
           cfg->zero_speed_rpm <= cfg->max_rpm &&
           cfg->zero_speed_dwell_ms > 0u &&
           cfg->zero_speed_dwell_ms < INT32_MAX &&
           cfg->kp_q16_ma_per_rpm > 0u;
}

static bool input_fault(const ds_supervisor *s, uint32_t now,
                        const ds_inputs *in, ds_fault *fault) {
    if (!in) {
        *fault = DS_FAULT_INTERLOCK;
        return true;
    }
    if (!in->power_ok || !in->driver_ready || !in->current_calibrated ||
        !in->handpiece_sensor_ok) {
        *fault = DS_FAULT_INTERLOCK;
        return true;
    }
    if (in->hardware_fault || in->overcurrent_fault) {
        *fault = DS_FAULT_DRIVER;
        return true;
    }
    if (!in->observer_locked ||
        (uint32_t)(now - in->measured_at_ms) > s->cfg.feedback_max_age_ms) {
        *fault = DS_FAULT_FEEDBACK_STALE;
        return true;
    }
    if (magnitude_i32(in->measured_rpm) > s->cfg.max_rpm) {
        *fault = DS_FAULT_OVERSPEED;
        return true;
    }
    return false;
}

bool ds_init(ds_supervisor *s, const ds_config *cfg, uint32_t now) {
    if (!s) return false;
    memset(s, 0, sizeof(*s));
    s->state = DS_FAULT;
    s->fault = DS_FAULT_CONFIG;
    if (!config_valid(cfg)) return false;
    s->cfg = *cfg;
    s->state = DS_DISABLED;
    s->fault = DS_FAULT_NONE;
    s->configured = true;
    s->last_service_ms = now;
    return true;
}

bool ds_set_beginner_mode(ds_supervisor *s, bool enabled) {
    if (!s || !s->configured || s->state != DS_DISABLED ||
        s->fault != DS_FAULT_NONE) return false;
    s->beginner_mode = enabled;
    return true;
}

bool ds_start(ds_supervisor *s, int32_t target, uint32_t now,
              const ds_inputs *in) {
    if (!s || !s->configured || s->state != DS_DISABLED ||
        s->fault != DS_FAULT_NONE || !target_valid(s, target) || target == 0 ||
        !fresh_inputs(s, now, in)) return false;
    int8_t direction = sign_i32(target);
    if (magnitude_i32(in->measured_rpm) > s->cfg.zero_speed_rpm &&
        sign_i32(in->measured_rpm) != direction) return false;
    s->direction = direction;
    s->target_rpm = limit_target(s, target);
    s->state = DS_RUNNING;
    s->last_service_ms = now;
    s->zero_timer_active = false;
    zero_output(s);
    return true;
}

static int32_t ramp_toward(ds_supervisor *s, int32_t target, uint32_t dt_ms) {
    uint64_t scaled = (uint64_t)s->cfg.ramp_rpm_per_s * dt_ms +
                      s->ramp_remainder;
    uint64_t max_step = scaled / 1000u;
    s->ramp_remainder = (uint32_t)(scaled % 1000u);
    int64_t delta = (int64_t)target - s->ramped_target_rpm;
    uint64_t magnitude = delta < 0 ? (uint64_t)(-delta) : (uint64_t)delta;
    if (magnitude <= max_step) {
        s->ramp_remainder = 0;
        return target;
    }
    if (max_step == 0u) return s->ramped_target_rpm;
    return s->ramped_target_rpm +
           (delta < 0 ? -(int32_t)max_step : (int32_t)max_step);
}

static int32_t clamp_current(const ds_supervisor *s, int64_t value) {
    int64_t limit = s->cfg.current_limit_ma;
    if (value > limit) value = limit;
    if (value < -limit) value = -limit;
    return (int32_t)value;
}

static int64_t q16_to_integer(int64_t value) {
    return value < 0 ? -((-value) / Q16_ONE) : value / Q16_ONE;
}

static int32_t speed_pi(ds_supervisor *s, int32_t measured, uint32_t dt_ms,
                        bool regen_permitted) {
    int64_t error = (int64_t)s->ramped_target_rpm - measured;
    int64_t p_q16 = error * s->cfg.kp_q16_ma_per_rpm;
    int64_t integral_delta = error * (int64_t)dt_ms *
                             s->cfg.ki_q16_ma_per_rpm_s / 1000;
    int64_t integral_limit = (int64_t)s->cfg.current_limit_ma * Q16_ONE;
    int64_t candidate_integral = s->integral_q16_ma + integral_delta;
    if (candidate_integral > integral_limit) candidate_integral = integral_limit;
    if (candidate_integral < -integral_limit) candidate_integral = -integral_limit;

    int64_t requested = q16_to_integer(p_q16 + candidate_integral);
    int32_t limited = clamp_current(s, requested);
    bool saturated = requested != limited;
    bool drives_out_of_saturation =
        (requested > limited && error < 0) || (requested < limited && error > 0);
    if (!saturated || drives_out_of_saturation) {
        s->integral_q16_ma = candidate_integral;
        requested = q16_to_integer(p_q16 + s->integral_q16_ma);
        limited = clamp_current(s, requested);
    }

    if (!regen_permitted && (int64_t)limited * measured < 0) {
        /* The power path has not approved regenerative energy absorption. */
        if ((int64_t)requested * measured < 0) s->integral_q16_ma = 0;
        return 0;
    }
    return limited;
}

ds_output ds_update(ds_supervisor *s, int32_t target, uint32_t now,
                    const ds_inputs *in) {
    ds_output out = snapshot(s);
    if (!s || !s->configured || s->state == DS_FAULT ||
        s->state == DS_DISABLED) return out;

    uint32_t dt_ms = now - s->last_service_ms;
    if (dt_ms == 0u) return snapshot(s);
    if (dt_ms > s->cfg.max_service_gap_ms) {
        latch(s, DS_FAULT_SERVICE_GAP);
        return snapshot(s);
    }
    s->last_service_ms = now;

    ds_fault fault = DS_FAULT_NONE;
    if (input_fault(s, now, in, &fault)) {
        latch(s, fault);
        return snapshot(s);
    }
    if (in->handpiece_parked) {
        /* Docking is a normal stop; picking up the handpiece never auto-restarts. */
        ds_stop(s);
        return snapshot(s);
    }
    if (!target_valid(s, target)) {
        latch(s, DS_FAULT_TARGET);
        return snapshot(s);
    }

    int32_t effective_target = limit_target(s, target);
    int8_t desired_direction = sign_i32(effective_target);
    if (desired_direction != 0 && desired_direction != s->direction) {
        s->state = DS_REVERSAL_WAIT;
        s->ramped_target_rpm = 0;
        s->q_current_ref_ma = 0;
        s->integral_q16_ma = 0;
        s->ramp_remainder = 0;
        if (magnitude_i32(in->measured_rpm) <= s->cfg.zero_speed_rpm) {
            if (!s->zero_timer_active) {
                s->zero_timer_active = true;
                s->zero_since_ms = now;
            } else if ((uint32_t)(now - s->zero_since_ms) >=
                       s->cfg.zero_speed_dwell_ms) {
                s->direction = desired_direction;
                s->state = DS_RUNNING;
                s->zero_timer_active = false;
            }
        } else {
            s->zero_timer_active = false;
        }
        s->target_rpm = effective_target;
        if (s->state == DS_REVERSAL_WAIT) return snapshot(s);
    }

    if (s->state == DS_REVERSAL_WAIT) {
        /* A request back toward the current direction cancels the coast wait. */
        s->state = DS_RUNNING;
        s->zero_timer_active = false;
    }

    s->target_rpm = effective_target;
    s->ramped_target_rpm = ramp_toward(s, effective_target, dt_ms);
    if (effective_target == 0 && s->ramped_target_rpm == 0 &&
        magnitude_i32(in->measured_rpm) <= s->cfg.zero_speed_rpm) {
        ds_stop(s);
        return snapshot(s);
    }
    s->q_current_ref_ma = speed_pi(s, in->measured_rpm, dt_ms,
                                   in->regen_permitted);
    return snapshot(s);
}

void ds_stop(ds_supervisor *s) {
    if (!s || s->state == DS_FAULT) return;
    zero_output(s);
    s->target_rpm = 0;
    s->state = DS_DISABLED;
    s->zero_timer_active = false;
}

bool ds_clear_fault(ds_supervisor *s, uint32_t now, const ds_inputs *in) {
    if (!s || !s->configured || s->state != DS_FAULT ||
        !fresh_inputs(s, now, in) ||
        magnitude_i32(in->measured_rpm) > s->cfg.zero_speed_rpm) return false;
    s->fault = DS_FAULT_NONE;
    s->state = DS_DISABLED;
    s->last_service_ms = now;
    s->direction = 0;
    s->zero_timer_active = false;
    zero_output(s);
    return true;
}
