%% Defining YALMIP decision variables and YALMIP binary variables %%
 % decision and manupilated variables
  u_i = sdpvar(repmat(nu,1,Np2+1),repmat(1,1,Np2+1));     %Control vector
  x_i = sdpvar(repmat(nx,1,Np2+1),repmat(1,1,Np2+1)); %State vector
  r   = sdpvar(4,1); % ref vector
  region_expo     = sdpvar(4,2); % explored region
  lidar_old  = sdpvar(4,2); % previous lidar measurement 
  lidar_read      = sdpvar(4,2); % current lidar measurement 
  b1 = binvar(1,2);
%   b2 = binvar(1,Np2);

%% System dynamics and discrtization
A = [0 0 1 0;
 0 0 0 1;
 0 0 0 0;
 0 0 0 0];

 B = [0 0;
 0 0;
 1 0;
 0 1];

 Ad = [];
 Bd = [];
 %Discretisation
 [Ad,Bd] = c2d(A,B,dtp2);
%% Define the unexplored region
constraints=[]; objective=0;
  for k = 1:Np2

 constraints = constraints + [x_i{k+1}(1) >=...
 region_expo(1,1)];

 constraints = constraints + [-x_i{k+1}(1)>=...
 -region_expo(2,1)];

 constraints = constraints + [x_i{k+1}(2) >=...
 region_expo(1,2)];

 constraints = constraints + [-x_i{k+1}(2)>=...
 -region_expo(3,2)];

  end
 
 
  
 %% Define the complete vector for lidar FOV (% previous lidar measurement, current lidar )
 Lidar_all=horzcat([lidar_old(3,1);lidar_old(1,1);lidar_old(2,2);lidar_old(1,2)],[lidar_read(3,1);lidar_read(1,1);lidar_read(2,2);lidar_read(1,2)]);
 %%     
   for k = 1:Np2
       constraints = constraints + [x_i{k+1}(1)   >=    Lidar_all(1,k) *b1(k) + Lidar_all(1,k+1) *b1(k+1) ];
       constraints = constraints + [-x_i{k+1}(1)  >=   -Lidar_all(2,k) *b1(k) - Lidar_all(2,k+1) *b1(k+1) ];
       constraints = constraints + [x_i{k+1}(2)   >=    Lidar_all(3,k) *b1(k) + Lidar_all(3,k+1) *b1(k+1) ];
       constraints = constraints + [-x_i{k+1}(2)  >=   -Lidar_all(4,k) *b1(k) - Lidar_all(4,k+1) *b1(k+1) ];
       constraints = constraints + [b1(k)+b1(k+1)==1];
  
   end


 
 %% Defining the objective function and constraints 
 
 %Stage Cost
  for k = 1:Np2
  objective = objective + (C2*x_i{k+1}-r)'*Q2*(C2*x_i{k+1}-r) + (u_i{k}'*R2*u_i{k});
  constraints = [constraints, x_i{k+1} == Ad*x_i{k} + Bd*u_i{k}];
  constraints = [constraints, 0<=x_i{k+1}(3:4)<=1.0];
                              
  end
  
  % Terminal cost
  objective = objective + (C2*x_i{Np2+1}-r)'*(C2*x_i{Np2+1}-r);
  constraints = [constraints, 0<=x_i{Np2+1}(3:4)<=1.0]; 
                             
  
parameters_in2 = {x_i{1},[r],[lidar_old],[lidar_read],region_expo};
solutions_out2 = {[u_i{:}], [x_i{:}],[objective]};

controller2 = optimizer(constraints, objective,sdpsettings('solver','gurobi'),parameters_in2,solutions_out2);