# CSDP_issue_malloc.jl. This example was run with JuMP v0.19.0 and CSDP v0.4.1
using JuMP, LinearAlgebra, Test, CSDP

function generate_problem_data()
  A = [
    -2.7300    0.0900    0.3500    0.2100    0.1500
     0.5400   -2.8800    0.9500    0.7800    0.8500
     0.7000    0.6300   -2.4200    0.9100    0.7800
     0.9600    0.8000    0.9500   -2.1600    0.2700
     0.4400    0.6900    0.0700    0.3000   -2.7100
   ]
  B1  = [
    0.3200    0.2900    0.4700
    0.8300    0.7000    0.2800
    0.8200    0.8000    0.6800
    0.5700    0.4400    0.9000
    0.5700    0.4500    0.9100
  ]
  B2  = [
    0.7500
    0.2600
    0.6900
    0.1300
    0.1200
  ]
  C1  = [
    0.1900    0.8200    0.6500    0.9800    0.3700
    0.1500    0.7200    0.8900    0.0400    0.3100
    0.5900    0.9300    0.5400    0.3300    0.1200
    0.0700    0.4900    0.2800    0.9700    0.9200
  ]
  C2  = [
    0.1400    0.9000    0.6200    0.7000    0.5300
    0.3300    0.5000    0.5800    0.0300    0.0300
  ]
  D11 = [
    0.8300    0.5800    0.0200
    0.3400    0.9400    0.6800
    0.8500    0.0500    0.6000
    0.2500    0.0500    0.1100
  ]
  D12 = [
    0.8000
    0.6200
    0.0700
    0.0700
  ]
  D21 = [
    0.1400    0.0900    0.2400
    0.7900    0.2400    0.1000
  ]
  D22 = zeros(2,1)
  return A, B1, B2, C1, C2, D11, D12, D21, D22
end


function solve_hinfinity_LMI(A,B1,B2,C1,C2,D11,D12,D21,D22)

  model = Model(with_optimizer(CSDP.Optimizer))

  n        = size(A,1)
  (p1, m1) = size(D11)
  (p2, m2) = size(D22)

  @variable(model, R[1:n, 1:n], PSD)
  @variable(model, S[1:n, 1:n], PSD)
  @variable(model, G[1:1, 1:1], PSD)

  NR = nullspace(Array([ B2' D12']))
  NS = nullspace(Array([ C2  D21 ]))

  Inn   = Matrix{Float64}(I,  n,  n)
  Im1m1 = Matrix{Float64}(I, m1, m1)
  Ip1p1 = Matrix{Float64}(I, p1, p1)

  MR = [NR                    zeros(size(NR,1),m1);
        zeros(m1, size(NR,2))                Im1m1];
  MS = [NS                    zeros(size(NS,1),p1);
        zeros(p1, size(NS,2))                Ip1p1];

  F1 = [  R  Inn;
        Inn    S]

  GIp1p1 = Matrix(Diagonal(vec(ones(p1,1)*G)))
  GIm1m1 = Matrix(Diagonal(vec(ones(m1,1)*G)))

  # Continuous time LMI formulation
  F2 = MR' * [
    A*R+R*A'          R*C1'              B1;
    C1*R            -GIp1p1             D11;
    B1'                D11'          -GIm1m1] * MR

  F3 = MS' * [
    A'*S+S*A           S*B1             C1';
    B1'*S           -GIm1m1            D11';
    C1                  D11          -GIp1p1] * MS

  @SDconstraint(model, F1 >= zeros(size(F1)))
  @SDconstraint(model, F2 <= zeros(size(F2)))
  @SDconstraint(model, F3 <= zeros(size(F3)))
  @SDconstraint(model, G >=0)
  @objective(model, Min, G[1,1])
  JuMP.optimize!(model)

  # For some reason, the latest version of JuMP does not allow direct
  # parametrization of matrix variables through JuMP.value(). This is
  # a temporary workaround
  Ropt = zeros(n,n)
  Sopt = zeros(n,n)
  Gopt = JuMP.value(G[1,1])
  for ii = 1:n
    for jj = 1:n
      Ropt[ii,jj] = JuMP.value(R[ii,jj])
      Sopt[ii,jj] = JuMP.value(S[ii,jj])
    end
  end
  return Ropt, Sopt, Gopt
end

# This function solves an H-infinity optimization program. It has been tested in
# Matlab using the hinfsyn() function [1], and also by a CVX-implementation
# calling the SDPT3 4.0 solver [2]. The problem is solvable, and the solution is
# known. We should expect a gain γ≈1.4528 and that R and S are positive definite.
#
# [1] https://www.mathworks.com/help/robust/ref/hinfsyn.html
# [2] http://www.math.nus.edu.sg/~mattohkc/sdpt3.html
#
# This problem is solved repeatedly below, running the test until failing.

A, B1, B2, C1, C2, D11, D12, D21, D22 = generate_problem_data()

γ = 1.4528

for ii = 1:1000
  println(string("~~~ Run ", ii, " ~~~"))
  R,S,G, = solve_hinfinity_LMI(A, B1, B2, C1, C2, D11, D12, D21, D22)
  @test all(eigvals(R).>=0)
  @test all(eigvals(S).>=0)
  @test abs(G - γ) <= 1e-3
end