clear; echo on;

pvar  x1 x2 u1 u2 p;
%pvar a1 a2 a3 a4 a5;

vars = [x1; x2; u1; u2;];

f = [x2;
    -9.8*u1;
    u2*x2;
    -u1*x2]

prog = sosprogram(vars,p);

%prog = sosdecvar(prog,[a1;a2;a3;a4;a5])

%V = a1*x2^2 + a2*x3^2 + a3*x4^2 + a4*x4 + a5

[prog,V] = sospolyvar(prog,[x2^2; u1^2; u2^2; u2; 1] ,'wscoeff');

%Should this constraint be here? Gives primal infeasible
%prog = soseq(prog, x3^2 + x4^2 - 1)

expr = coeff_2+coeff_3+coeff_5
%expr = a3 + a4 + a5

prog = soseq(prog,expr)

% Set the epsilons to 1
prog = sosineq(prog,V-0.1*(1-u2)-0.1*x2^2 + p*(u1^2+u2^2-1))

%Possible problem here, discards all monomials in addextrasosvar
%Commented out "discarding unecessary monomials" in sossolve.m lines
%207-211

expr2 = -(diff(V,x1)*f(1)+diff(V,x2)*f(2)+diff(V,u1)*f(3)+diff(V,u2)*f(4))+p*(u1^2+u2^2-1)
prog = sosineq(prog,expr2)

%expr3 = expr2 + p*(x3^2+x4^2-1)

%prog = soseq(prog,expr3)

prog = sossolve(prog)

SOLV = sosgetsol(prog,V)