clear 
format short g
cvx_solver mosek
%cvx_solver_settings('MSK_DPAR_MIO_MAX_TIME', 200)

rng(1);
rng('shuffle');

n = 5;
m = 2;
K = 4;
T = 20;
x_max = 3;
B = .1*randn(n, m);
C = [eye(n); -eye(n)];
d = [x_max*ones(2*n,1)];
g = @(x) sum_square(x, 1);
p = @(x, s) quad_over_lin(x, s);

num_iters = 1;
for iter = 1:num_iters

  iter

  for i = 1:K
    A{i} = eye(n) + .1*randn(n);
    v{i} = randn(n,1);
    x0 = randn(n, 1);
  end
  [G, h] = voronoi(v);

  % GLOBAL OPTIMAL (us)
  if 1
    [J_star(iter), z, s, cpu_time] = pwa_ctrl(A, B, x0, T, C, d, G, h, g, p, 1);
    J_star
    [J_star(iter), z, s, cpu_time] = pwa_ctrl(A, B, x0, T, C, d, G, h, g, p, 0);
    J_star
    x_opt = squeeze(sum(z(:,:,:), 2));
    bb_time(iter) = cputime;
  end

  % OUR APPROACH - SHRINKING HORIZON
  if 0
    x_sim = x0;
    my_ub_sh(iter) = g(x0);
    for t_sim = 1:T-1
      [lb, z, s, cpu_time] = sw_ctrl(A, b, x_sim(:,t_sim), T - t_sim + 1, C, d, g, p, 0);
      if t_sim == 1
        my_lb(iter) = lb;
        my_time(iter) = cputime;
      end
      [~, s_app] = max(s(:, 1));
      x_sim(:,t_sim+1) = A{s_app}*x_sim(:,t_sim) + b{s_app};
      my_ub_sh(iter) = my_ub_sh(iter) + g(x_sim(:, t_sim+1));
    end
    if norm(x_sim(:,t_sim), 'inf') > x_max
      my_ub_sh(iter) = inf;
    end
  end

  % OUR APPROACH - RELAX AND ROUND
  if 0
    [lb, z, s, cpu_time] = sw_ctrl(A, b, x0, T, C, d, g, p, 0);
    [~, s_app] = max(s);
    x_sim = x0;
    my_ub_rr(iter) = g(x0);
    for t_sim = 1:T-1
      if t_sim == 1
        my_lb(iter) = lb;
        my_time(iter) = cputime;
      end
      x_sim(:,t_sim+1) = A{s_app(t_sim)}*x_sim(:,t_sim) + b{s_app(t_sim)};
      my_ub_rr(iter) = my_ub_rr(iter) + g(x_sim(:, t_sim+1));
      if norm(x_sim(:,t_sim), 'inf') > x_max
        my_ub_rr(iter) = inf;
      end
    end
  end

  
  % SAVE DATA
  if 0
  save('data.mat', ...
    'J_star', ...
    'my_lb', ...
    'my_ub_sh', ...
    'my_ub_rr', ...
    'mld_lb', ...
    'mld_ub_sh', ...
    'mld_ub_rr', ...
    'bb_time', ...
    'my_time', ...
    'mld_time', ...
    'n', ...
    'K', ...
    'T', ...
    'x_max');
  end

end

%fprintf('MLD lower bound is   : %d\n', mld_lb(iter))
%fprintf('My lower bound is    : %d\n', my_lb(iter))
%%fprintf('The optimal value is : %d\n', J_star)
%fprintf('My upper bound is    : %d\n', my_ub(iter))
%fprintf('MLD upper bound is   : %d\n', hybrid_ub(iter))