%% Sloving optimzation problem to determine the polynomial coefficents taking into account the constraints
clc; clear all;close all
addpath(genpath("/home/msoliman/YALMIP-master/"))
load safe_path.mat
%%
% O_set=[15 40 40 90;22 38 75 70]; %box center
% ObstacleArray=[];
% 
% ObstacleArray=[20 38 0 88;24 42 80 92;20 36 50 68;24 40 100 72]; %box sizes

xhist = xhist(:,1:10:end);
xhist=xhist(:,1:140);
xdata = xhist(1,:);
ydata = xhist(2,:);

sdpvar t
[x,a] = polynomial(t,6);
[y,b] = polynomial(t,6);

objective = 0;
t_i = linspace(0,1,length(xdata));
X = [];
Y = [];
Avoid = [];     
for i = 1:length(xdata)
    x_i = replace(x,t,t_i(i));X = [X x_i];
    y_i = replace(y,t,t_i(i));Y = [Y y_i];
    objective = objective + 1000*(x_i-xdata(i))^2 + 1000*(y_i - ydata(i))^2 ;
    
    Avoid = [Avoid,norm([x_i;y_i] - [22;22],inf) >= 1,...
        norm([x_i;y_i] - [40;75],inf) >= [40;20],norm([x_i;y_i] - [90;70],inf) >= 1];
end
% objective= objective+1000*abs(Y(55)-ydata(55));
Match = [X(1)==xdata(1),
         X(end)==xdata(end),
         Y(1)==ydata(1),
         Y(end)==ydata(end)];

optimize([Match,Avoid],objective);
plot(value(X),value(Y))
hold on
plot(xdata,ydata)
sdpvar z(2,3);
plot(norm(z(:,1)-[22;22],inf)<=1)
hold on

plot(norm(z(:,2)-[40;75],inf)<=[80;25])
hold on
plot(norm(z(:,3)-[90;70],inf)<=1)


