% Clauses for SAT representation of a graph plan problem.
% Each row of a matrix is a clause; +ve or -ve entries are positive or
% negated literals. Zero entries are ignored.
% The clauses containing only one or two literals are padded with 0s to make it
% easier to specify this matrix. The file to_lp.m does the appropriate
% unpacking.

nVars = 48;
% initial state
initial = [1 0 0; 50 0 0; 51 0 0; 52 0 0; 53 0 0];

% goals 
goal = [47 0 0; 48 0 0];

% maintenance action preconditions
mpre = [54 1 0; 56 50 0; 57 51 0; 58 52 0; 59 53 0]; %t=1
mpre = [mpre; 65 12 0; 67 60 0; 69 61 0; 71 62 0; 72 63 0; 73 64 0]; %t=2
mpre = [mpre; 79 26 0; 81 74 0; 84 75 0; 85 28 0; 87 76 0; 88 29 0; 90 77 0; 91 78 0]; % t=3;

% maintenance action postconditions
mpost = [54 12 0; 56 61 0; 57 62 0; 58 63 0; 59 64 0]; % t=1
mpost = [mpost; 65 26 0; 67 74 0; 69 75 0; 71 76 0; 72 77 0; 73 78 0]; % t=2
mpost = [mpost; 79 44 0; 81 92 0; 82 45 0; 84 93 0; 85 46 0; 87 94 0; 88 47 0; 90 95 0; 91 96 0]; %t=3

% other action preconditions
pre = [55 1 0; 66 12 0; 68 13 0; 70 13 0; 80 26 0; 83 27 0; 86 27 0; 89 28 0];

% other action postconditions
post = [55 60 0; 55 13 0; 66 74 0; 66 27 0; 68 75 0; 68 28 0; 70 29 0]; % t=1,2
post = [post; 80 92 0; 80 45 0; 83 93 0; 83 46 0; 86 47 0; 89 48 0]; % t=3

% Must select actions to achieve predicates
act = [60 6 0; 12 7 0; 61 7 0; 13 8 0; 14 9 0; 15 10 0; 16 11 0]; % t=1
act = [act; 74 17 0; 26 18 19; 75 18 0; 27 20 21; 76 20 0; 28 23 0; 77 22 0; 29 24 0; 30 25 0]; % t=2
act = [act; 92 31 0; 44 32 33; 93 32 34; 45 35 36; 94 35 37; 46 39 0; 95 38 40; 95 42 0; 96 41 0; 48 43 0]; % t=3

% mutex constraints
mutex = [54 55 0; 55 56 0; 60 61 0; 6 7 0; 7 8 0; 12 13 0]; % t=1

mutex = [mutex; 65 66 0; 65 68 0; 65 70 0; 66 67 0; 66 68 0; 66 69 0; 66 70 0; 68 70 0; 70 72 0]; % t=2 act
mutex = [mutex; 74 75 0; 74 76 0; 74 77 0; 75 76 0; 76 77 0]; % t=2 pred

mutex = [mutex; 79 80 0; 79 83 0; 80 81 0; 80 82 0; 80 83 0; 80 86 0; 80 84 0; 80 89 0; 83 86 0; 83 89 0; 84 86 0; 86 89 0; 86 91 0; 87 89 0; 89 91 0]; % t=3 act
mutex = [mutex; 92 93 0; 92 94 0; 92 95 0; 92 96 0; 93 94 0; 93 96 0; 94 95 0; 46 96 0; 95 96 0]; % t=3 pred

% Planning actions and their associated costs
costlyActions = [7 0 0; 18 0 0; 22 0 0; 32 0 0; 35 0 0; 95 0 0; 96 0 0];
costOfActions = [10; 10; 15; 10; 15; 20; 40];

% Turn all of the clauses into linear inequality constraints 
clauses = [initial; mpre; mpost; pre; post; act; mutex; goal; costlyActions];

inEqConstr = zeros(2 * nVars);
n = size(clauses,1);
m = size(clauses,2);
for i = 1:n
  for j = 1:m
    if clauses(i,j) > 0
      inEqConstr(i,clauses(i,j)) = 1;
    end
  end
end
nConstr = size(inEqConstr,1);

% We add a slack variable to each constraint. This variable is 0 when the
% constraint is satisfied, and 1 otherwise.
inEqConstr = [inEqConstr eye(nConstr)];
inEqRHS = ones(nConstr,1);

% We add the constraint x_i + x_{n+i} = 1
eqConstr = [eye(nVars) eye(nVars) zeros(nVars,nConstr)];
eqRHS = ones(nVars,1);

% Give a high cost to all constraints, since we want them all to hold. Impose
% additional cost values on particular assignments of the 'goal' and 'move'
% variables, to bias the search towards obtaining a solution where these clauses
% are satisfied, and the true reward is maximized i.e. the cost is minimized.

bigCost = 100;
cost = [zeros(2*nVars,1); bigCost * ones(nConstr-length(costOfActions),1); costOfActions];

% We now use linprog to solve the LP relaxation
nVariables = 2*nVars + nConstr;
LB = zeros(nVariables,1);
UB = ones(nVariables,1);

% Our inequality constraints are of the form: inEqConstr >= inEqRHS   
% But linprog needs constraints of the form Ax <= b
% So to use linprog, we multiply our constraints by -1 to obtain: 
%   -1*inEqConstr <= -1*inEqRHS

[currentSol, currentSolVal, exitFlag] = linprog(cost, -1*inEqConstr, -1*inEqRHS, eqConstr, eqRHS, LB, UB);

disp(currentSolVal)
figure(1); plot(currentSol, 'x')