%% Sloving optimzation problem to determine the polynomial coefficents taking into account the constraints
clc; clear all
addpath(genpath("/home/msoliman/YALMIP-master/"))

numberofSidesObstacle=4;Np=10;safety_margin = 2;M_big=1000;
load safe_path.mat;
%%
O_set=[22 40 90;22 38 70];
o_uas_i = binvar(length(O_set(1,:)),numberofSidesObstacle);
constraints = [];
x=xhist(1,:);
x = sdpvar(1);
[p,a,v] = polynomial(x,6);
for c = 1:length(O_set(1,:))
    
 Model = [replace(p,x,x) <=...
 ObstacleArray(3,c) + M_big* o_uas_i(c,3)];

 Model = [replace(p,x,x)<=...
 -ObstacleArray(4,c) + M_big* o_uas_i(c,4)];

 end

optimize(Model)