%% See more demos here:
% https://yalmip.github.io/tutorial/sumofsquaresprogramming/

%Three quick demos to get started on doing SOS optimization on YALMIP:

%% Check if a given polynomial is SOS
clear all
sdpvar x1 x2 x3

%p=x1^2*x2^2-2*x1*x2^3+x2^4-9*x1^2*x3^2-2*x1*x2*x3^2+x2^2*x3^2+25*x1^4+x3^4;

%Motzkin:
%p=x1^2*x2^4+x2^2*x1^4-3*x1^2*x2^2*x3^2+x3^6;
%Motzkin times sum x_i^2
p=(x1^2*x2^4+x2^2*x1^4-3*x1^2*x2^2*x3^2+x3^6)*(x1^2+x2^2+x3^2);

F = [sos(p)];
options = sdpsettings('verbose',2,'solver','mosek'); %choose your solver (optional)

[info,z,Q]=solvesos(F,0,options) %constraints, objective (default is minimize), options

sol_Q=Q{1} %get the Gram matrix
sdisplay(z{1}) %get the monomial vector
eig(sol_Q) %check Gram matrix is psd



%% Unconstrained minmization of a polynomial using the SOS relaxation
clear all
sdpvar x y t
p = (1+x)^4 + (1-y)^2+x*y;

F = [sos(p-t)];
options = sdpsettings('verbose',2,'solver','sdpt3');
optimize(F,-t, options)

sol_t=double(t) %lower bound on p (must be tight in this case because we have a bivariate quartic)



%% Demo showing how to parameterize an unkown polynomial and impose arbitrary linear, semidefinite, 
%or sos constraints on affine combinations of its coefficients
clear all
sdpvar x1 x2 
x=[x1;x2];

d=5;
[p,cp,mp]=polynomial(x,d); %p equals cp'*mp; cp is the coefficient vector and mp is the vector of monomials.

g=x1^4-x1^2;

F=[replace(p,[x1,x2],[4,7])<20]; %p evaluated at (4,7) should be less than 20. This is a linear constraint
%in the coefficients of p.
grad_p=jacobian(p,x)'; %taking the gradient of p
F=F+[sos(grad_p(1)-g)];


options = sdpsettings('verbose',2,'solver','sdpt3');
optimize(F,replace(grad_p(1),[x1,x2],[0,0]), options)


%sol_p=double(cp)'*mp
%sdisplay(sol_p) %show solution
%replace(sol_p,[x1,x2],[4,7]) %sanity check on the constraint
%sol_p=clean(sol_p,1e-3) %get rid of all terms with coefficient less than
%10^-3


