solveQuadraticSubproblem :: Problem
                         -> Vector Double
                         -- convex cost
                         -> (Matrix Double, Vector Double, Double)
                         -- convexified inequality constraints
                         -> (Matrix Double, Vector Double)
                         -- convexified equality constraints
                         -> (Matrix Double, Vector Double)
                         -> Double -- trust region size
                         -> Double -- constraint penalty parameter
                         -> (Vector Double, Double) -- new x, model merit
solveQuadraticSubproblem
  problem x (costMatrix, costVector, costConstant)
  (approxIneqMatrix, approxIneqVector) (approxEqMatrix, approxEqVector)
  trustSize penaltyParam =
  -- We approximate each nonlinear inequality as |ax + b|^+. For each
  -- of these, we introduce a new optimization variable t (i.e., a
  -- slack variable) that comes with two inequalities:
  --
  -- 0 <= t
  --
  -- ax + b <= t
  --
  -- Each nonlinear equality as |ax + b|. For each of these, two new
  -- optimization variables s and t with following properties:
  --
  -- -s <= 0
  -- -t <= 0
  -- s - t = ax + b
  let numIneqs = _numIneqs problem
      numEqs = _numEqs problem
      numVariables = _numVariables problem

      costMatrixWithSlacks =
        diagBlock [ costMatrix
                  , konst 0.0 (numIneqs, numIneqs)
                  , konst 0.0 (2 * numEqs, 2 * numEqs)]

      -- LET'S MAKE COST SPD
      (l, v) = eigSH costMatrixWithSlacks
      positiveEigVals = cmap (max 1e-12) l
      augmentedCostMatrix = v <> diag positiveEigVals <> tr v

      augmentedCostVector = vjoin [costVector
                                  , konst 1.0 numIneqs
                                  , konst 1.0 $ 2 * numEqs]

      augmentedIneqMatrix =
        ((scalar penaltyParam * approxIneqMatrix) |||
         negate (ident numIneqs) |||
         konst 0.0 (numIneqs, 2 * numEqs)) ===
         (konst 0.0 (numIneqs + 2 * numEqs, numVariables) |||
          negate (ident $ numIneqs + 2 * numEqs))
      augmentedIneqVector =
        vjoin [ scalar penaltyParam * approxIneqVector
              , konst 0.0 $ numIneqs + 2 * numEqs ]

      ineqMatrixWithTrustConstraints =
        augmentedIneqMatrix ===
        (ident numVariables ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs)) ===
        (negate (ident numVariables) ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs))
      ineqVectorWithTrustConstraints =
        vjoin [ augmentedIneqVector
              , negate (x + scalar trustSize)
              , x - scalar trustSize ]

      augmentedEqMatrix =
        scalar penaltyParam * approxEqMatrix |||
        konst 0.0 (numEqs, numIneqs) |||
        negate (ident numEqs) |||
        ident numEqs
      augmentedEqVector = scalar penaltyParam * approxEqVector

      -- inequalities are given as ax + b <= 0, but quadprog++ wants
      -- a'x + b' >= 0
      result = solveQuadProg
        (augmentedCostMatrix, augmentedCostVector)
        (Just (augmentedEqMatrix, augmentedEqVector)) $
        Just ((-ineqMatrixWithTrustConstraints),
              (-ineqVectorWithTrustConstraints))

  in  case result of
        Left e -> error $ show e
        Right (xNewAugmented, newMeritHomogenous) ->
          let xNew = subVector 0 numVariables xNewAugmented
              newMerit = newMeritHomogenous + costConstant

              -- Compute the merit of the approximation without regularization
              cost = evalQuadratic (costMatrix, costVector, costConstant) xNew
              ineqViolations =
                evalLinear (approxIneqMatrix, approxIneqVector) xNew
              ineqPenalty =
                penaltyParam * (sum $ map (max 0.0) $ toList ineqViolations)
              eqViolations = evalLinear (approxEqMatrix, approxEqVector) xNew
              eqPenalty =
                penaltyParam * (sum $ map abs $ toList eqViolations)
              meritNoReg = cost + ineqPenalty + eqPenalty

              -- DEBUG
              meritError = abs $ newMerit - meritNoReg
          in  if meritError / (abs meritNoReg) > 0.5
              then error $ "merits don't match!" ++ show (meritNoReg, newMerit)
              else (xNew, meritNoReg)
solveQuadraticSubproblem :: Problem
                         -> Vector Double
                         -- convex cost
                         -> (Matrix Double, Vector Double, Double)
                         -- convexified inequality constraints
                         -> (Matrix Double, Vector Double)
                         -- convexified equality constraints
                         -> (Matrix Double, Vector Double)
                         -> Double -- trust region size
                         -> Double -- constraint penalty parameter
                         -> (Vector Double, Double)
solveQuadraticSubproblem
  problem x (costMatrix, costVector, costConstant)
  (approxIneqMatrix, approxIneqVector) (approxEqMatrix, approxEqVector)
  trustSize penaltyParam =
  -- We approximate each nonlinear inequality as |ax + b|^+. For each
  -- of these, we introduce a new optimization variable t (i.e., a
  -- slack variable) that comes with two inequalities:
  --
  -- 0 <= t
  --
  -- ax + b <= t
  --
  -- Each nonlinear equality as |ax + b|. For each of these, two new
  -- optimization variables s and t with following properties:
  --
  -- -s <= 0
  -- -t <= 0
  -- s - t = ax + b
  let numIneqs = _numIneqs problem
      numEqs = _numEqs problem
      numVariables = _numVariables problem

      costMatrixWithSlacks =
        diagBlock [ costMatrix
                  , konst 0.0 (numIneqs, numIneqs)
                  , konst 0.0 (2 * numEqs, 2 * numEqs)]

      -- LET'S MAKE COST SPD
      (l, v) = eigSH costMatrixWithSlacks
      positiveEigVals = cmap (max 1e-12) l
      augmentedCostMatrix = v <> diag positiveEigVals <> tr v

      augmentedCostVector = vjoin [costVector
                                  , konst 1.0 numIneqs
                                  , konst 1.0 $ 2 * numEqs]

      augmentedIneqMatrix =
        ((scalar penaltyParam * approxIneqMatrix) |||
         negate (ident numIneqs) |||
         konst 0.0 (numIneqs, 2 * numEqs)) ===
         (konst 0.0 (numIneqs + 2 * numEqs, numVariables) |||
          negate (ident $ numIneqs + 2 * numEqs))
      augmentedIneqVector =
        vjoin [ scalar penaltyParam * approxIneqVector
              , konst 0.0 $ numIneqs + 2 * numEqs ]

      ineqMatrixWithTrustConstraints =
        augmentedIneqMatrix ===
        (ident numVariables ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs)) ===
        (negate (ident numVariables) ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs))
      ineqVectorWithTrustConstraints =
        vjoin [ augmentedIneqVector
              , negate (x + scalar trustSize)
              , x - scalar trustSize ]

      augmentedEqMatrix =
        scalar penaltyParam * approxEqMatrix |||
        konst 0.0 (numEqs, numIneqs) |||
        negate (ident numEqs) |||
        ident numEqs
      augmentedEqVector = scalar penaltyParam * approxEqVector

      -- inequalities are given as ax + b <= 0, but quadprog++ wants
      -- a'x + b' >= 0
      result = solveQuadProg
        (augmentedCostMatrix, augmentedCostVector)
        (Just (augmentedEqMatrix, augmentedEqVector)) $
        Just ((-ineqMatrixWithTrustConstraints),
              (-ineqVectorWithTrustConstraints))

  in  case result of
        Left e -> error $ show e
        Right (xNewAugmented, newMeritHomogenous) ->
          let xNew = subVector 0 numVariables xNewAugmented
              newMerit = newMeritHomogenous + costConstant

              -- Compute the merit of the approximation without regularization
              cost = evalQuadratic (costMatrix, costVector, costConstant) xNew
              ineqViolations =
                evalLinear (approxIneqMatrix, approxIneqVector) xNew
              ineqPenalty =
                penaltyParam * (sum $ map (max 0.0) $ toList ineqViolations)
              eqViolations = evalLinear (approxEqMatrix, approxEqVector) xNew
              eqPenalty =
                penaltyParam * (sum $ map abs $ toList eqViolations)
              meritNoReg = cost + ineqPenalty + eqPenalty

              -- DEBUG
              meritError = abs $ newMerit - meritNoReg
          in  if meritError / (abs meritNoReg) > 0.5
              then error $ "merits don't match!" ++ show (meritNoReg, newMerit)
              else (xNew, meritNoReg)
solveQuadraticSubproblem
  problem x (costMatrix, costVector, costConstant)
  (approxIneqMatrix, approxIneqVector) (approxEqMatrix, approxEqVector)
  trustSize penaltyParam =
  -- We approximate each nonlinear inequality as |ax + b|^+. For each
  -- of these, we introduce a new optimization variable t (i.e., a
  -- slack variable) that comes with two inequalities:
  --
  -- 0 <= t
  --
  -- ax + b <= t
  --
  -- Each nonlinear equality as |ax + b|. For each of these, two new
  -- optimization variables s and t with following properties:
  --
  -- -s <= 0
  -- -t <= 0
  -- s - t = ax + b
  let numIneqs = _numIneqs problem
      numEqs = _numEqs problem
      numVariables = _numVariables problem

      costMatrixWithSlacks =
        diagBlock [ costMatrix
                  , konst 0.0 (numIneqs, numIneqs)
                  , konst 0.0 (2 * numEqs, 2 * numEqs)]

      -- LET'S MAKE COST SPD
      (l, v) = eigSH costMatrixWithSlacks
      positiveEigVals = cmap (max 1e-12) l
      augmentedCostMatrix = v <> diag positiveEigVals <> tr v

      augmentedCostVector = vjoin [costVector
                                  , konst 1.0 numIneqs
                                  , konst 1.0 $ 2 * numEqs]

      augmentedIneqMatrix =
        ((scalar penaltyParam * approxIneqMatrix) |||
         negate (ident numIneqs) |||
         konst 0.0 (numIneqs, 2 * numEqs)) ===
         (konst 0.0 (numIneqs + 2 * numEqs, numVariables) |||
          negate (ident $ numIneqs + 2 * numEqs))
      augmentedIneqVector =
        vjoin [ scalar penaltyParam * approxIneqVector
              , konst 0.0 $ numIneqs + 2 * numEqs ]

      ineqMatrixWithTrustConstraints =
        augmentedIneqMatrix ===
        (ident numVariables ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs)) ===
        (negate (ident numVariables) ||| konst 0.0 (numVariables, numIneqs + 2 * numEqs))
      ineqVectorWithTrustConstraints =
        vjoin [ augmentedIneqVector
              , negate (x + scalar trustSize)
              , x - scalar trustSize ]

      augmentedEqMatrix =
        scalar penaltyParam * approxEqMatrix |||
        konst 0.0 (numEqs, numIneqs) |||
        negate (ident numEqs) |||
        ident numEqs
      augmentedEqVector = scalar penaltyParam * approxEqVector

      -- inequalities are given as ax + b <= 0, but quadprog++ wants
      -- a'x + b' >= 0
      result = solveQuadProg
        (augmentedCostMatrix, augmentedCostVector)
        (Just (augmentedEqMatrix, augmentedEqVector)) $
        Just ((-ineqMatrixWithTrustConstraints),
              (-ineqVectorWithTrustConstraints))

  in  case result of
        Left e -> error $ show e
        Right (xNewAugmented, newMeritHomogenous) ->
          let xNew = subVector 0 numVariables xNewAugmented
              newMerit = newMeritHomogenous + costConstant

              -- Compute the merit of the approximation without regularization
              cost = evalQuadratic (costMatrix, costVector, costConstant) xNew
              ineqViolations =
                evalLinear (approxIneqMatrix, approxIneqVector) xNew
              ineqPenalty =
                penaltyParam * (sum $ map (max 0.0) $ toList ineqViolations)
              eqViolations = evalLinear (approxEqMatrix, approxEqVector) xNew
              eqPenalty =
                penaltyParam * (sum $ map abs $ toList eqViolations)
              meritNoReg = cost + ineqPenalty + eqPenalty

              -- DEBUG
              meritError = abs $ newMerit - meritNoReg
          in  if meritError / (abs meritNoReg) > 0.5
              then error $ "merits don't match!" ++ show (meritNoReg, newMerit)
              else (xNew, meritNoReg)