#include "power_setpoint_policy.h"

PowerSetpointAction_t PowerSetpoint_Decide(
	const PowerSetpoint_t *applied,
	const PowerSetpoint_t *next,
	const PowerPathContext_t *ctx)
{
#if !PSD_ENABLE
	(void)applied;
	(void)next;
	(void)ctx;
	return POWER_SETPOINT_NORMAL;
#else
	uint16_t delta_v;

	if (applied == 0 || next == 0 || ctx == 0) {
		return POWER_SETPOINT_NORMAL;
	}

	if (next->voltage_V >= applied->voltage_V) {
		return POWER_SETPOINT_NORMAL;
	}

	delta_v = (uint16_t)(applied->voltage_V - next->voltage_V);
	if (delta_v < PSD_MIN_VOLTAGE_STEP_V) {
		return POWER_SETPOINT_NORMAL;
	}

	if (applied->current_0p1A > PSD_MAX_CMD_CURRENT_0P1A ||
	    next->current_0p1A > PSD_MAX_CMD_CURRENT_0P1A) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->measured_current_0p1A >= PSD_MAX_MEASURED_CURRENT_0P1A) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->measured_voltage_V <= (uint16_t)(next->voltage_V + PSD_DISCHARGE_MARGIN_V)) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->power_delivery_active) {
		return POWER_SETPOINT_NORMAL;
	}

	if (!ctx->enable_output) {
		return POWER_SETPOINT_NORMAL;
	}

	if (!ctx->psu_state_connected) {
		return POWER_SETPOINT_NORMAL;
	}

	if (!ctx->dc_contactor_closed) {
		return POWER_SETPOINT_NORMAL;
	}

	if (!ctx->psu_hv_enabled) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->psu_cont_fault || ctx->psu_fault) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->fast_discharge_count >= PSD_MAX_PER_SESSION) {
		return POWER_SETPOINT_NORMAL;
	}

	if (ctx->last_fast_discharge_ms != 0u &&
	    (ctx->now_ms - ctx->last_fast_discharge_ms) < PSD_DEBOUNCE_MS) {
		return POWER_SETPOINT_NORMAL;
	}

	return POWER_SETPOINT_FAST_DISCHARGE;
#endif
}