///|
/// The knobs of the model-predictive controller.
///
/// `penalty` is the ADMM step of the quadratic program, `input` the box on the
/// command and `rate` the box on its change per second. `iterations` bounds the
/// solve, so a period can never take longer than the budget says.
#derive(Debug)
pub(all) struct MpcConfig {
  horizon : Int32
  iterations : Int32
  penalty : Double
  input : Output
  rate : Output
}

///|
/// The preset the model-predictive controller runs with: a horizon of twenty
/// periods, no limit on the command and no limit on its rate.
pub let mpc_config : MpcConfig = {
  horizon: 20,
  iterations: 600,
  penalty: 2.0,
  input: output_unbounded,
  rate: output_unbounded,
}

///|
/// A receding-horizon controller for a discrete linear plant with box limits on
/// the input and on its rate of change.
///
/// The prediction is condensed into one quadratic program over the stacked
/// inputs, which is what makes the period predictable: the matrices are built
/// and factorised once per call, and the solve is a fixed number of ADMM
/// iterations rather than a search whose length depends on the active set.
pub(all) struct Mpc {
  model : @contract.StateSpace
  state_weight : @linear.Mat
  input_weight : @linear.Mat
  config : MpcConfig
  mut previous : @linear.Vec
}

///|
/// A model-predictive controller for `model` at the given weights.
///
/// The plant must be discrete: a continuous record would need the period it is
/// to be discretised at, which the controller cannot guess.
pub fn Mpc::new(
  model~ : @contract.StateSpace,
  state_weight~ : @linear.Mat,
  input_weight~ : @linear.Mat,
  config~ : MpcConfig = mpc_config,
) -> Mpc raise @linear.Refused {
  if !model.is_discrete() {
    raise @linear.Refused::Unreachable
  }
  if config.horizon < 1 {
    raise @linear.Refused::Range(
      got=config.horizon.to_double(),
      low=1.0,
      high=1.0e9,
    )
  }
  let n = model.a.rows
  if state_weight.rows != n || state_weight.cols != n {
    raise @linear.Refused::Dimension(got=state_weight.rows, want=n)
  }
  let m = model.b.cols
  if input_weight.rows != m || input_weight.cols != m {
    raise @linear.Refused::Dimension(got=input_weight.rows, want=m)
  }
  {
    model,
    state_weight,
    input_weight,
    config,
    previous: @linear.Vec::zeros(m),
  }
}

///|
/// The command for this period, after which the horizon shifts by one.
pub fn Mpc::control(
  self : Mpc,
  state~ : @linear.Vec,
  target~ : @linear.Vec,
) -> @linear.Vec raise @linear.Refused {
  let n = self.model.a.rows
  let m = self.model.b.cols
  let horizon = self.config.horizon
  if state.len() != n {
    raise @linear.Refused::Dimension(got=state.len(), want=n)
  }
  if target.len() != n {
    raise @linear.Refused::Dimension(got=target.len(), want=n)
  }
  let dt = match self.model.dt {
    Some(value) => value
    None => raise @linear.Refused::Unreachable
  }
  // The predicted states: x = sx * x0 + su * u.
  let sx = @linear.Mat::zeros(n * horizon, n)
  let su = @linear.Mat::zeros(n * horizon, m * horizon)
  let powers = Array::make(horizon + 1, @linear.Mat::identity(n))
  for i in 1..<=horizon {
    powers[i] = self.model.a.mul(powers[i - 1])
  }
  for i in 0.. @linear.Mat {
  let size = block.rows
  let out = @linear.Mat::zeros(size * count, size * count)
  for i in 0.. @linear.Mat {
  let out = @linear.Mat::zeros(width * horizon, width * horizon)
  for k in 0.. 0 {
        out.set(k * width + c, (k - 1) * width + c, -1.0)
      }
    }
  }
  out
}

///|
/// The minimiser of `0.5 z' h z + f' z` under a box on `z` and a box on the
/// differences `difference * z`.
///
/// Two constraint sets, so ADMM keeps two dual variables and one factorisation
/// of `h + p1 i + p2 difference' difference`; both projections are clamps.
fn constrained_qp(
  h : @linear.Mat,
  f : @linear.Vec,
  lower : @linear.Vec,
  upper : @linear.Vec,
  difference : @linear.Mat,
  dlower : @linear.Vec,
  dupper : @linear.Vec,
  config : MpcConfig,
) -> @linear.Vec raise @linear.Refused {
  let n = f.len()
  let penalty = if config.penalty > 0.0 { config.penalty } else { 1.0 }
  let dt = difference.transpose().mul(difference)
  let system = h
    .add(@linear.Mat::identity(n).scale(penalty))
    .add(dt.scale(penalty))
  let factor = @linear.cholesky(system)
  let z1 = @linear.Vec::zeros(n)
  let mut z2 = @linear.Vec::zeros(n)
  let dual1 = @linear.Vec::zeros(n)
  let dual2 = @linear.Vec::zeros(n)
  for i in 0.. residual {
        residual = (x.get(i) - candidate1).abs()
      }
      if (dz.get(i) - candidate2).abs() > residual {
        residual = (dz.get(i) - candidate2).abs()
      }
      z1.set(i, candidate1)
      z2.set(i, candidate2)
      dual1.set(i, dual1.get(i) + x.get(i) - candidate1)
      dual2.set(i, dual2.get(i) + dz.get(i) - candidate2)
    }
    if residual <= 1.0e-8 && penalty * z1.sub(previous).max_abs() <= 1.0e-8 {
      break
    }
  }
  z1
}

///|
pub extend MpcConfig with @debug.Debug::{to_repr}