///|
/// Saturation limits for an actuator or control input.
pub struct ControlLimits {
  lower : Array[Double]
  upper : Array[Double]
}

///|
pub fn ControlLimits::new(
  lower : Array[Double],
  upper : Array[Double],
) -> ControlLimits {
  if lower.length() != upper.length() {
    { lower: [], upper: [] }
  } else {
    { lower: lower.copy(), upper: upper.copy() }
  }
}

///|
pub fn ControlLimits::dimension(self : ControlLimits) -> Int {
  self.lower.length()
}

///|
pub fn ControlLimits::lower(self : ControlLimits) -> Array[Double] {
  self.lower.copy()
}

///|
pub fn ControlLimits::upper(self : ControlLimits) -> Array[Double] {
  self.upper.copy()
}

///|
pub fn ControlLimits::apply(
  self : ControlLimits,
  command : Array[Double],
) -> Array[Double] {
  if command.length() != self.dimension() {
    return []
  }
  Array::makei(command.length(), index => {
    let low = self.lower[index]
    let high = if self.upper[index] < low { low } else { self.upper[index] }
    if command[index].is_nan() {
      0.0
    } else if command[index] < low {
      low
    } else if command[index] > high {
      high
    } else {
      command[index]
    }
  })
}

///|
pub fn ControlLimits::contains(
  self : ControlLimits,
  command : Array[Double],
) -> Bool {
  if command.length() != self.dimension() {
    return false
  }
  for i in 0.. self.upper[i] ||
      command[i].is_nan() {
      return false
    }
  }
  true
}

///|
pub struct ControlCommand {
  timestamp : Int
  values : Array[Double]
  duration : Double
} derive(Debug)

///|
pub fn ControlCommand::new(
  timestamp : Int,
  values : Array[Double],
  duration : Double,
) -> ControlCommand {
  {
    timestamp,
    values: values.copy(),
    duration: if duration < 0.0 {
      0.0
    } else {
      duration
    },
  }
}

///|
pub fn ControlCommand::timestamp(self : ControlCommand) -> Int {
  self.timestamp
}

///|
pub fn ControlCommand::values(self : ControlCommand) -> Array[Double] {
  self.values.copy()
}

///|
pub fn ControlCommand::duration(self : ControlCommand) -> Double {
  self.duration
}

///|
pub fn ControlCommand::is_valid(self : ControlCommand) -> Bool {
  self.values.length() > 0 && vector_is_finite(self.values)
}

///|
/// A bounded command sequence for offline control replay.
pub struct ControlSequence {
  commands : Array[ControlCommand]
  capacity : Int
  mut rejected : Int
}

///|
pub fn ControlSequence::new(capacity : Int) -> ControlSequence {
  {
    commands: [],
    capacity: if capacity < 0 {
      0
    } else {
      capacity
    },
    rejected: 0,
  }
}

///|
pub fn ControlSequence::push(
  self : ControlSequence,
  command : ControlCommand,
) -> Bool {
  if self.capacity == 0 || !command.is_valid() {
    self.rejected = self.rejected + 1
    return false
  }
  if self.commands.length() > 0 &&
    command.timestamp() < self.commands[self.commands.length() - 1].timestamp() {
    self.rejected = self.rejected + 1
    return false
  }
  if self.commands.length() >= self.capacity {
    for i in 1.. Array[ControlCommand] {
  self.commands.copy()
}

///|
pub fn ControlSequence::length(self : ControlSequence) -> Int {
  self.commands.length()
}

///|
pub fn ControlSequence::rejected(self : ControlSequence) -> Int {
  self.rejected
}

///|
pub fn ControlSequence::clear(self : ControlSequence) -> Unit {
  self.commands.clear()
  self.rejected = 0
}

///|
/// Integrate a first-order control response with a bounded acceleration.
pub struct ControlIntegrator {
  dimension : Int
  mut state : Array[Double]
  limits : ControlLimits
  response : Double
}

///|
pub fn ControlIntegrator::new(
  dimension : Int,
  limits : ControlLimits,
  response : Double,
) -> ControlIntegrator {
  let size = if dimension < 0 { 0 } else { dimension }
  let safe_response = if response <= 0.0 { 1.0 } else { response }
  {
    dimension: size,
    state: Array::make(size, 0.0),
    limits,
    response: safe_response,
  }
}

///|
pub fn ControlIntegrator::step(
  self : ControlIntegrator,
  command : Array[Double],
  dt : Double,
) -> Array[Double] {
  if command.length() != self.dimension {
    return self.state.copy()
  }
  let safe_dt = if dt < 0.0 { 0.0 } else { dt }
  let bounded = self.limits.apply(command)
  for i in 0.. Array[Double] {
  self.state.copy()
}

///|
pub fn ControlIntegrator::dimension(self : ControlIntegrator) -> Int {
  self.dimension
}

///|
pub fn ControlIntegrator::response(self : ControlIntegrator) -> Double {
  self.response
}

///|
pub fn ControlIntegrator::reset(self : ControlIntegrator) -> Unit {
  self.state = Array::make(self.dimension, 0.0)
}

///|
/// Generate a control vector that moves a state toward a target under limits.
pub fn proportional_control(
  state : Array[Double],
  target : Array[Double],
  gain : Double,
  limits : ControlLimits,
) -> Array[Double] {
  if state.length() != target.length() || state.length() != limits.dimension() {
    return []
  }
  limits.apply(
    vector_scale(vector_sub(target, state), if gain < 0.0 { 0.0 } else { gain }),
  )
}

///|
pub fn control_energy(commands : Array[ControlCommand]) -> Double {
  let mut energy = 0.0
  for command in commands {
    energy = energy +
      vector_dot(command.values(), command.values()) * command.duration()
  }
  energy
}

///|
pub fn control_peak(commands : Array[ControlCommand]) -> Double {
  let mut peak = 0.0
  for command in commands {
    let value = vector_linf_norm(command.values())
    if value > peak {
      peak = value
    }
  }
  peak
}

///|
pub fn control_interpolate(
  left : ControlCommand,
  right : ControlCommand,
  timestamp : Int,
) -> ControlCommand {
  if left.values().length() != right.values().length() {
    return left
  }
  let span = right.timestamp() - left.timestamp()
  if span <= 0 {
    return left
  }
  let amount = (timestamp - left.timestamp()).to_double() / span.to_double()
  ControlCommand::new(
    timestamp,
    vector_lerp(left.values(), right.values(), amount),
    left.duration() + amount * (right.duration() - left.duration()),
  )
}

///|
pub fn control_schedule_energy(sequence : ControlSequence) -> Double {
  control_energy(sequence.commands())
}

///|
pub fn control_schedule_peak(sequence : ControlSequence) -> Double {
  control_peak(sequence.commands())
}