use crate::bitset;
use crate::packed::PackedTask;
use crate::types::{eval_numpre, AssignOp, CompOp, NExpr, NumEff, NumPre};
const LAYER_CAP: u32 = 2000;
const INF: u32 = u32::MAX;
pub struct Scratch {
reached: Vec<bool>,
fact_layer: Vec<u32>,
op_layer: Vec<u32>,
gen: u32,
op_stamp: Vec<u32>,
applicable: Vec<u32>,
lb: Vec<f64>,
ub: Vec<f64>,
selected: Vec<u32>,
need_fact: Vec<u32>,
queue: Vec<u32>,
num_applied: Vec<u32>,
cond_ops: Vec<u32>,
helpful: Vec<u32>,
}
impl Scratch {
pub fn new(task: &PackedTask) -> Self {
let nfl = task.fv0.len();
Scratch {
reached: vec![false; task.n_facts],
fact_layer: vec![INF; task.n_facts],
op_layer: vec![INF; task.n_ops],
gen: 0,
op_stamp: vec![0; task.n_ops],
applicable: Vec::with_capacity(task.n_ops),
lb: vec![0.0; nfl],
ub: vec![0.0; nfl],
selected: vec![0; task.n_ops],
need_fact: vec![0; task.n_facts],
queue: Vec::with_capacity(task.n_facts),
num_applied: Vec::with_capacity(task.n_ops),
cond_ops: Vec::new(),
helpful: Vec::new(),
}
}
fn reset(&mut self, task: &PackedTask, bits: &[u64], fv: &[f64]) {
for f in 0..task.n_facts {
self.reached[f] = bitset::test(bits, f);
}
self.fact_layer.iter_mut().enumerate().for_each(|(f, l)| {
*l = if self.reached[f] { 0 } else { INF };
});
self.gen = self.gen.wrapping_add(1);
if self.gen == 0 {
self.op_stamp.fill(0);
self.selected.fill(0);
self.need_fact.fill(0);
self.gen = 1;
}
self.lb.copy_from_slice(fv);
self.ub.copy_from_slice(fv);
self.queue.clear();
self.num_applied.clear();
self.cond_ops.clear();
self.helpful.clear();
}
}
fn widen(
neffs: &[NumEff],
relevant: &[bool],
lb: &mut [f64],
ub: &mut [f64],
def: &[bool],
) -> bool {
let mut changed = false;
for ne in neffs {
let t = ne.target as usize;
if !relevant[t] {
continue;
}
if let Some((vl, vu)) = eval_iv(&ne.value, lb, ub, def) {
let before = (lb[t], ub[t]);
match ne.op {
AssignOp::Increase => {
ub[t] += vu.max(0.0);
lb[t] += vl.min(0.0);
}
AssignOp::Decrease => {
lb[t] -= vu.max(0.0);
ub[t] -= vl.min(0.0);
}
AssignOp::Assign => {
lb[t] = lb[t].min(vl);
ub[t] = ub[t].max(vu);
}
AssignOp::ScaleUp => ub[t] *= vu.max(1.0),
AssignOp::ScaleDown => lb[t] /= vu.max(1.0),
}
if (lb[t], ub[t]) != before {
changed = true;
}
}
}
changed
}
fn op_has_relevant_neff(task: &PackedTask, oi: usize) -> bool {
task.num_eff
.slice(oi)
.iter()
.any(|ne| task.relevant_fluent[ne.target as usize])
}
fn eval_iv(e: &NExpr, lb: &[f64], ub: &[f64], def: &[bool]) -> Option<(f64, f64)> {
Some(match e {
NExpr::Num(n) => (*n, *n),
NExpr::Fluent(i) => {
let i = *i as usize;
if !def[i] {
return None;
}
(lb[i], ub[i])
}
NExpr::Neg(a) => {
let (l, u) = eval_iv(a, lb, ub, def)?;
(-u, -l)
}
NExpr::Add(a, b) => {
let (al, au) = eval_iv(a, lb, ub, def)?;
let (bl, bu) = eval_iv(b, lb, ub, def)?;
(al + bl, au + bu)
}
NExpr::Sub(a, b) => {
let (al, au) = eval_iv(a, lb, ub, def)?;
let (bl, bu) = eval_iv(b, lb, ub, def)?;
(al - bu, au - bl)
}
NExpr::Mul(a, b) => {
let (al, au) = eval_iv(a, lb, ub, def)?;
let (bl, bu) = eval_iv(b, lb, ub, def)?;
let c = [al * bl, al * bu, au * bl, au * bu];
(
c.iter().cloned().fold(f64::INFINITY, f64::min),
c.iter().cloned().fold(f64::NEG_INFINITY, f64::max),
)
}
NExpr::Div(a, b) => {
let (al, au) = eval_iv(a, lb, ub, def)?;
let (bl, bu) = eval_iv(b, lb, ub, def)?;
if bl <= 0.0 && bu >= 0.0 {
(f64::NEG_INFINITY, f64::INFINITY)
} else {
let c = [al / bl, al / bu, au / bl, au / bu];
(
c.iter().cloned().fold(f64::INFINITY, f64::min),
c.iter().cloned().fold(f64::NEG_INFINITY, f64::max),
)
}
}
})
}
fn num_sat(np: &NumPre, lb: &[f64], ub: &[f64], def: &[bool]) -> bool {
let l = match eval_iv(&np.lhs, lb, ub, def) {
Some(x) => x,
None => return false,
};
let r = match eval_iv(&np.rhs, lb, ub, def) {
Some(x) => x,
None => return false,
};
match np.op {
CompOp::Lt => l.0 < r.1,
CompOp::Le => l.0 <= r.1,
CompOp::Gt => l.1 > r.0,
CompOp::Ge => l.1 >= r.0,
CompOp::Eq => l.0 <= r.1 && r.0 <= l.1,
}
}
fn goal_done(
goal_pos: &[u32],
goal_num: &[NumPre],
reached: &[bool],
lb: &[f64],
ub: &[f64],
def: &[bool],
) -> bool {
goal_pos.iter().all(|&f| reached[f as usize])
&& goal_num.iter().all(|np| num_sat(np, lb, ub, def))
}
fn build_rpg(
task: &PackedTask,
sc: &mut Scratch,
goal_pos: &[u32],
goal_num: &[NumPre],
def: &[bool],
to_fixpoint: bool,
) {
let mut layer: u32 = 0;
loop {
if !to_fixpoint && goal_done(goal_pos, goal_num, &sc.reached, &sc.lb, &sc.ub, def) {
break;
}
let mut changed = false;
for idx in 0..sc.num_applied.len() {
let oi = sc.num_applied[idx] as usize;
if widen(
task.num_eff.slice(oi),
&task.relevant_fluent,
&mut sc.lb,
&mut sc.ub,
def,
) {
changed = true;
}
}
for idx in 0..sc.cond_ops.len() {
let oi = sc.cond_ops[idx] as usize;
for ce in task.cond_effs(oi) {
let pos_ok = ce.cond_pos.iter().all(|&c| sc.reached[c as usize]);
let num_ok = ce
.cond_num
.iter()
.all(|np| num_sat(np, &sc.lb, &sc.ub, def));
if pos_ok && num_ok {
for &f in &ce.add {
let f = f as usize;
if !sc.reached[f] {
sc.reached[f] = true;
sc.fact_layer[f] = layer + 1;
changed = true;
}
}
if !ce.num.is_empty()
&& widen(&ce.num, &task.relevant_fluent, &mut sc.lb, &mut sc.ub, def)
{
changed = true;
}
}
}
}
sc.applicable.clear();
for oi in 0..task.n_ops {
if sc.op_stamp[oi] == sc.gen {
continue; }
let ok = task
.pre_pos
.slice(oi)
.iter()
.all(|&f| sc.reached[f as usize])
&& task
.pre_num
.slice(oi)
.iter()
.all(|np| num_sat(np, &sc.lb, &sc.ub, def));
if ok {
sc.op_stamp[oi] = sc.gen;
sc.op_layer[oi] = layer;
sc.applicable.push(oi as u32);
changed = true;
}
}
for k in 0..sc.applicable.len() {
let oi = sc.applicable[k] as usize;
for &f in task.add.slice(oi) {
let f = f as usize;
if !sc.reached[f] {
sc.reached[f] = true;
sc.fact_layer[f] = layer + 1;
changed = true;
}
}
if op_has_relevant_neff(task, oi) {
if widen(
task.num_eff.slice(oi),
&task.relevant_fluent,
&mut sc.lb,
&mut sc.ub,
def,
) {
changed = true;
}
sc.num_applied.push(oi as u32);
}
if task.n_cond_effs(oi) > 0 {
sc.cond_ops.push(oi as u32);
}
}
layer += 1;
if !changed || layer > LAYER_CAP {
break;
}
}
}
pub fn reachability_layers(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
) -> (Vec<u32>, Vec<u32>) {
sc.reset(task, bits, fv);
build_rpg(task, sc, &[], &[], def, true);
let op_layer: Vec<u32> = (0..task.n_ops)
.map(|oi| {
if sc.op_stamp[oi] == sc.gen {
sc.op_layer[oi]
} else {
u32::MAX
}
})
.collect();
(sc.fact_layer.clone(), op_layer)
}
pub fn relaxed_to(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
goal_pos: &[u32],
goal_num: &[NumPre],
) -> Option<i32> {
sc.reset(task, bits, fv);
build_rpg(task, sc, goal_pos, goal_num, def, false);
if !goal_done(goal_pos, goal_num, &sc.reached, &sc.lb, &sc.ub, def) {
return None;
}
let mut count: i32 = 0;
let mut head = 0usize;
for &g in goal_pos {
let f = g as usize;
if sc.need_fact[f] != sc.gen {
sc.need_fact[f] = sc.gen;
sc.queue.push(g);
}
}
while head < sc.queue.len() {
let f = sc.queue[head] as usize;
head += 1;
if bitset::test(bits, f) {
continue;
}
if let Some(oi) = achiever(task, &sc.op_layer, &sc.op_stamp, sc.gen, &sc.fact_layer, f) {
select(task, sc, oi, 1, &mut count);
queue_cond_for(task, sc, oi, f);
}
}
for np in goal_num {
if eval_numpre(np, fv, def).unwrap_or(false) {
continue;
}
if let Some((oi, reps)) = numeric_achiever(task, np, fv, def, &sc.op_stamp, sc.gen) {
select(task, sc, oi, reps, &mut count);
while head < sc.queue.len() {
let f = sc.queue[head] as usize;
head += 1;
if bitset::test(bits, f) {
continue;
}
if let Some(o2) =
achiever(task, &sc.op_layer, &sc.op_stamp, sc.gen, &sc.fact_layer, f)
{
select(task, sc, o2, 1, &mut count);
queue_cond_for(task, sc, o2, f);
}
}
}
}
Some(count)
}
pub fn relaxed(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
) -> Option<i32> {
relaxed_to(task, sc, bits, fv, def, &task.goal_pos, &task.goal_num)
}
pub fn relaxed_helpful(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
goal_pos: &[u32],
goal_num: &[NumPre],
) -> Option<(i32, Vec<u32>)> {
let h = relaxed_to(task, sc, bits, fv, def, goal_pos, goal_num)?;
let applicable = |oi: usize| {
task.pre_pos
.slice(oi)
.iter()
.all(|&f| bitset::test(bits, f as usize))
&& task
.pre_num
.slice(oi)
.iter()
.all(|np| eval_numpre(np, fv, def) == Some(true))
};
let mut helpful: Vec<u32> = sc
.helpful
.iter()
.copied()
.filter(|&oi| applicable(oi as usize))
.collect();
if helpful.is_empty() && h > 0 {
let mut wanted = vec![false; fv.len()];
let mut any = false;
let mut tmp = Vec::new();
for oi in 0..task.n_ops {
if sc.selected[oi] != sc.gen {
continue;
}
for np in task.pre_num.slice(oi) {
if eval_numpre(np, fv, def) == Some(true) {
continue;
}
tmp.clear();
np.lhs.collect_fluents(&mut tmp);
np.rhs.collect_fluents(&mut tmp);
for &fl in &tmp {
wanted[fl as usize] = true;
any = true;
}
}
}
if any {
for oi in 0..task.n_ops {
if applicable(oi)
&& task
.num_eff
.slice(oi)
.iter()
.any(|ne| wanted[ne.target as usize])
{
helpful.push(oi as u32);
}
}
}
if helpful.is_empty() {
for oi in 0..task.n_ops {
if applicable(oi) {
helpful.push(oi as u32);
}
}
}
}
Some((h, helpful))
}
#[allow(clippy::too_many_arguments)]
pub fn relaxed_plan_cost(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
goal_pos: &[u32],
goal_num: &[NumPre],
cost_fluent: usize,
) -> Option<f64> {
relaxed_to(task, sc, bits, fv, def, goal_pos, goal_num)?;
Some(selected_increase_sum(task, sc, fv, def, cost_fluent))
}
fn selected_increase_sum(
task: &PackedTask,
sc: &Scratch,
fv: &[f64],
def: &[bool],
cost_fluent: usize,
) -> f64 {
let mut cost = 0.0;
for oi in 0..task.n_ops {
if sc.selected[oi] != sc.gen {
continue;
}
for ne in task.num_eff.slice(oi) {
if ne.target as usize == cost_fluent && ne.op == AssignOp::Increase {
if let Some(v) = ne.value.eval(fv, def) {
cost += v.max(0.0);
}
}
}
}
cost
}
#[allow(clippy::too_many_arguments)]
pub fn relaxed_costed(
task: &PackedTask,
sc: &mut Scratch,
bits: &[u64],
fv: &[f64],
def: &[bool],
goal_pos: &[u32],
goal_num: &[NumPre],
cost_fluent: usize,
) -> Option<i32> {
let count = relaxed_to(task, sc, bits, fv, def, goal_pos, goal_num)?;
let cost = selected_increase_sum(task, sc, fv, def, cost_fluent);
Some(count.saturating_add(cost.min(1e9).round() as i32))
}
fn achiever(
task: &PackedTask,
op_layer: &[u32],
op_stamp: &[u32],
gen: u32,
fact_layer: &[u32],
f: usize,
) -> Option<usize> {
let fl = fact_layer[f];
if fl == INF || fl == 0 {
return None;
}
let mut best = None;
let mut best_layer = INF;
for &oi in task.add_by_fact.slice(f) {
let oi = oi as usize;
if op_stamp[oi] == gen && op_layer[oi] < fl && op_layer[oi] < best_layer {
best_layer = op_layer[oi];
best = Some(oi);
}
}
best
}
fn queue_cond_for(task: &PackedTask, sc: &mut Scratch, oi: usize, f: usize) {
if task.add.slice(oi).iter().any(|&x| x as usize == f) {
return; }
let mut best_layer = INF;
let mut best: Option<&crate::packed::CondEff> = None;
for ce in task.cond_effs(oi) {
if ce.add.iter().any(|&x| x as usize == f) {
let cl = ce
.cond_pos
.iter()
.map(|&c| sc.fact_layer[c as usize])
.max()
.unwrap_or(0);
if cl != INF && cl < best_layer {
best_layer = cl;
best = Some(ce);
}
}
}
if let Some(ce) = best {
for &cf in &ce.cond_pos {
let c = cf as usize;
if sc.need_fact[c] != sc.gen {
sc.need_fact[c] = sc.gen;
sc.queue.push(cf);
}
}
}
}
fn select(task: &PackedTask, sc: &mut Scratch, oi: usize, reps: i32, count: &mut i32) {
if sc.selected[oi] == sc.gen {
return;
}
sc.selected[oi] = sc.gen;
if sc.op_stamp[oi] == sc.gen && sc.op_layer[oi] == 0 {
sc.helpful.push(oi as u32);
}
*count += reps.max(1);
for &pf in task.pre_pos.slice(oi) {
let f = pf as usize;
if sc.need_fact[f] != sc.gen {
sc.need_fact[f] = sc.gen;
sc.queue.push(pf);
}
}
}
fn numeric_achiever(
task: &PackedTask,
np: &NumPre,
fv: &[f64],
def: &[bool],
op_stamp: &[u32],
gen: u32,
) -> Option<(usize, i32)> {
let target = match &np.lhs {
NExpr::Fluent(i) => *i,
_ => return None,
};
let want = match &np.rhs {
NExpr::Num(n) => *n,
_ => return None,
};
let cur = if def[target as usize] {
fv[target as usize]
} else {
0.0
};
let need_raise = cur < want;
let mut best: Option<(usize, i32)> = None;
for &oi in task.neff_by_fluent.slice(target as usize) {
let oi = oi as usize;
if op_stamp[oi] != gen {
continue;
}
for ne in task.num_eff.slice(oi) {
if ne.target != target {
continue;
}
let delta = match ne.value.eval(fv, def) {
Some(v) => v,
None => continue,
};
let helps = match ne.op {
AssignOp::Increase => need_raise && delta > 0.0,
AssignOp::Decrease => !need_raise && delta > 0.0,
_ => false,
};
if helps {
let reps = (((want - cur).abs() / delta.abs().max(1e-9)).ceil() as i32).max(1);
if best.map(|(_, r)| reps < r).unwrap_or(true) {
best = Some((oi, reps));
}
}
}
}
best
}