use std::collections::BTreeMap;
use continuous_blocks::StateSpace;
use general_spice_core::Dialect;
use pwl_devices::{IdealDiode, IdealSwitch};
use crate::{
classify_segments, step_with_fallback, DaeError, GateState, OperatingPoint, Segment,
RINGING_COOLDOWN_STEPS,
};
pub fn sawtooth_carrier(t: f64, freq_hz: f64) -> f64 {
let phase = t * freq_hz;
phase - phase.floor()
}
#[allow(clippy::too_many_arguments)]
pub fn simulate_closed_loop(
source: &str,
dialect: Dialect,
diodes: &BTreeMap<String, IdealDiode>,
ideal_switches: &BTreeMap<String, IdealSwitch>,
controller: &StateSpace,
reference: impl Fn(f64) -> f64,
measure: impl Fn(&OperatingPoint) -> f64,
pwm: impl Fn(f64, f64) -> BTreeMap<String, GateState>,
output_clamp: (f64, f64),
shared_r_on: f64,
x_initial: Option<&[f64]>,
t_final: f64,
dt: f64,
) -> Result<Vec<(f64, OperatingPoint, f64)>, DaeError> {
let statements = general_mna::parse_and_flatten(source, dialect).map_err(DaeError::Parse)?;
let initial_states: BTreeMap<String, (IdealSwitch, GateState)> = ideal_switches
.iter()
.map(|(name, m)| (name.clone(), (*m, GateState::Off)))
.collect();
let (system0, all_diodes0) = crate::build_with_ideal_switches(
&statements,
dialect,
diodes,
&initial_states,
shared_r_on,
)?;
let mut x_prev = crate::resolve_initial_state(&system0, &all_diodes0, x_initial)?;
let (mut point_prev, _) = step_with_fallback(
&system0,
&statements,
&all_diodes0,
None,
&x_prev,
dt,
&BTreeMap::new(),
&None,
true,
0.0,
&BTreeMap::new(),
&BTreeMap::new(),
)?;
let mut x_prev_prev: Option<Vec<f64>> =
Some(std::mem::replace(&mut x_prev, point_prev.x.clone()));
let mut controller_x = vec![0.0; controller.states()];
let mut prev_diode_raw_ioff: BTreeMap<String, f64> = BTreeMap::new();
let mut prev_segments: Option<Vec<Segment>> = None;
let mut prev_gate_states: Option<BTreeMap<String, GateState>> = None;
let mut ringing_cooldown: u32 = 0;
let steps = (t_final / dt).round() as usize;
let mut trace = Vec::with_capacity(steps);
let mut t = 0.0;
for step_index in 0..steps {
t += dt;
let measured = measure(&point_prev);
let error = [reference(t) - measured];
let (lo, hi) = output_clamp;
let tentative_x = controller.rk4_step(&controller_x, &error, dt);
let tentative_output = controller.output(&tentative_x, &error)[0];
let saturating_further = (tentative_output >= hi && error[0] > 0.0)
|| (tentative_output <= lo && error[0] < 0.0);
let controller_output = if saturating_further {
controller.output(&controller_x, &error)[0].clamp(lo, hi)
} else {
controller_x = tentative_x;
tentative_output
};
let gate_states = pwm(controller_output, t);
let states: BTreeMap<String, (IdealSwitch, GateState)> = ideal_switches
.iter()
.map(|(name, m)| {
(
name.clone(),
(*m, *gate_states.get(name).unwrap_or(&GateState::Off)),
)
})
.collect();
let gate_changed = prev_gate_states.as_ref() != Some(&gate_states);
let forced = step_index == 0 || gate_changed || ringing_cooldown > 0;
let (system, all_diodes) =
crate::build_with_ideal_switches(&statements, dialect, diodes, &states, shared_r_on)?;
let (point, used_backward_euler) = step_with_fallback(
&system,
&statements,
&all_diodes,
x_prev_prev.as_deref(),
&x_prev,
dt,
&prev_diode_raw_ioff,
&prev_segments,
forced,
t,
&BTreeMap::new(),
&BTreeMap::new(),
)?;
if used_backward_euler && !forced {
ringing_cooldown = RINGING_COOLDOWN_STEPS;
} else if ringing_cooldown > 0 {
ringing_cooldown = ringing_cooldown.saturating_sub(1);
}
prev_segments = Some(classify_segments(&point));
prev_gate_states = Some(gate_states);
prev_diode_raw_ioff = point
.diode_names
.iter()
.cloned()
.zip(point.diode_raw_ioff.iter().copied())
.collect();
x_prev_prev = Some(std::mem::replace(&mut x_prev, point.x.clone()));
point_prev = point.clone();
trace.push((t, point, controller_output));
}
Ok(trace)
}