function tutorial()
% 1. Run Backward Reachable Set (BRS) with a goal
%     uMode = 'min' <-- goal
%     minWith = 'none' <-- Set (not tube)
%     compTraj = false <-- no trajectory
% 2. Run BRS with goal, then optimal trajectory
%     uMode = 'min' <-- goal
%     minWith = 'none' <-- Set (not tube)
%     compTraj = true <-- compute optimal trajectory
% 3. Run Backward Reachable Tube (BRT) with a goal, then optimal trajectory
%     uMode = 'min' <-- goal
%     minWith = 'minVOverTime' <-- Tube (not set)
%     compTraj = true <-- compute optimal trajectory
% 4. Add disturbance
%     dStep1: define a dMax (dMax = [.25, .25, 0];)
%     dStep2: define a dMode (opposite of uMode)
%     dStep3: input dMax when creating your DubinsCar
%     dStep4: add dMode to schemeData
% 5. Change to an avoid BRT rather than a goal BRT
%     uMode = 'max' <-- avoid
%     dMode = 'min' <-- opposite of uMode
%     minWith = 'minVOverTime' <-- Tube (not set)
%     compTraj = false <-- no trajectory
% 6. Change to a Forward Reachable Tube (FRT)
%     add schemeData.tMode = 'forward'
%     note: now having uMode = 'max' essentially says "see how far I can
%     reach"
% 7. Add obstacles
%     add the following code:
%     obstacles = shapeCylinder(g, 3, [-1.5; 1.5; 0], 0.75);
%     HJIextraArgs.obstacles = obstacles;
% 8. Add random disturbance (white noise)
%     add the following code:
%     HJIextraArgs.addGaussianNoiseStandardDeviation = [0; 0; 0.5];


%% Should we compute the trajectory?
compTraj = false;

%global variables
clear dx_global
global u_optimal
global dx_global

%% Grid
grid_min = [-1.5; -1.5; 0.0]; % Lower corner of computation domain
grid_max = [8.0; 7.0; 6.0];    % Upper corner of computation domain
N = [25; 25; 25];         % Number of grid points per dimension
%pdDims = 3;               % 3rd dimension is periodic
g = createGrid(grid_min, grid_max, N)
% Use "g = createGrid(grid_min, grid_max, N);" if there are no periodic
% state space dimensions

%compute optimal u
xyz = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\controller_matlab_air_1000_600.mat');
W1 = xyz.fc1_weight;
b1 = xyz.fc1_bias';
W2 = xyz.fc_2_weight;
b2 = xyz.fc_2_bias';


for k = 1:length(g.xs{1,1,1})
    k
    for i = 1:length(g.xs{2,1,1})
        for j = 1:length(g.xs{3,1,1})
            s_inp = [g.xs{1,1,1}(i, j, k); g.xs{2, 1, 1}(i, j, k); g.xs{3, 1, 1}(i, j, k); 1];
            l1 = vpa(tanh([W1, b1]*s_inp));
            temp = [W2, b2]*[l1; 1];
            sig = vpa((1./(1 + exp(-temp))));
            tn = vpa(tanh(temp));
            uOpt{1,1,1}(i, j, k) = vpa( sig(1) )*5;
            uOpt{2,1,1}(i, j, k) = vpa( tn(2) )*3.14;
            uOpt{3,1,1}(i, j, k) = vpa( tn(3) )*3.14;
        end
    end
end

u_optimal = uOpt

%compute dynamics
abc = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\model_matlab_air_1000.mat');
W1 = abc.fc1_weight;
b1 = abc.fc1_bias';
W2 = abc.fc_state_weight;
b2 = abc.fc_state_bias';

dx_global = u_optimal;
for k = 1:length(u_optimal{3, 1, 1})
    k
    for i = 1:length(u_optimal{1, 1, 1})
        for j = 1:length(u_optimal{2, 1, 1})
            t1 = vpa(tanh([W1, b1] * [u_optimal{1, 1, 1}(i, j, k); u_optimal{2, 1, 1}(i, j, k); u_optimal{3, 1, 1}(i, j, k);1]));
            t2 = vpa( tanh([W2, b2] * [t1; 1]) ).*0.5;
            
            dx_global{1,1,1}(i,j,k) = t2(1);
            dx_global{2,1,1}(i,j,k) = t2(2);
            dx_global{3,1,1}(i,j,k) = t2(3);
        end
    end
end

dx_global

%% target set
R = 0.3;
% data0 = shapeCylinder(grid,ignoreDims,center,radius)
%data0 = shapeCylinder(g, 3, [0; 0; 0], R);
% also try shapeRectangleByCorners, shapeSphere, etc.
%data0 = shapeSphere(g, [6; 6], R);
%data0 = shapeRectangleByCenter(g, [1.5, 4.5], [3, 1])
%data0 = shapeRectangleByCenter(g, [4.0, 1.5], [1, 3])
%data0 = shapeSphere(g, [0.0; 0.0; 0.0], 1.0);
data0 = shapeCylinder(g, 3, [2.0; 4.0], 0.5);

%% time vector
t0 = 0;
tMax = 24;
dt = 1;
tau = t0:dt:tMax;

%% problem parameters

% input bounds
speed = 5;
wMax = 3.14;
% do dStep1 here

% control trying to min or max value function?
uMode = 'max';
uMax = [speed, wMax, wMax]; 
% do dStep2 here
dMode = 'min';
dMax = [3*0.00211, 3*0.00259, 3*0.00201];

minWith = 'minVOverTime';
%% Pack problem parameters

% Define dynamic system
% obj = DubinsCar(x, wMax, speed, dMax)
dCar = ToyExample5([0, 0, 0], uMax, dMax); %do dStep3 here

% Put grid and dynamic systems into schemeData
schemeData.grid = g;
schemeData.dynSys = dCar;
schemeData.accuracy = 'high'; %set accuracy
schemeData.uMode = uMode;
schemeData.dMode = dMode;

%do dStep4 here

%% additive random noise
%do Step8 here
%HJIextraArgs.addGaussianNoiseStandardDeviation = [0; 0; 0.5];
% Try other noise coefficients, like:
%    [0.2; 0; 0]; % Noise on X state
%    [0.2,0,0;0,0.2,0;0,0,0.5]; % Independent noise on all states
%    [0.2;0.2;0.5]; % Coupled noise on all states
%    {zeros(size(g.xs{1})); zeros(size(g.xs{1})); (g.xs{1}+g.xs{2})/20}; % State-dependent noise

%% If you have obstacles, compute them here
%obstacle1 = shapeSphere(g, [0.0, 0.0], 0.7);
%HJIextraArgs.obstacleFunction = obstacle1;
%% Compute value function

%HJIextraArgs.visualize = true; %show plot
HJIextraArgs.visualize.valueSet = 1;
HJIextraArgs.visualize.initialValueSet = 1;
HJIextraArgs.visualize.figNum = 1; %set figure number
HJIextraArgs.visualize.deleteLastPlot = true; %delete previous plot as you update
HJIextraArgs.visualize.holdOn = true;
HJIextraArgs.visualize.lineWidth = 2.0;
%HJIextraArgs.visualize.plotColorTS = 'y';
%HJIextraArgs.visualize.plotColorTF = 'y';
%HJIextraArgs.visualize.plotColorVS0 = 'b';
%HJIextraArgs.visualize.plotColorVF0 = 'b';
%HJIextraArgs.visualize.plotColorVS = 'r';
%HJIextraArgs.visualize.plotColorVF = 'r';
%HJIextraArgs.visualize.plotLineStyleVS = ':';
HJIextraArgs.visualize.plotAlphaVF = 0.2;
HJIextraArgs.visualize.xTitle = 'x';
HJIextraArgs.visualize.yTitle = 'y';
HJIextraArgs.visualize.fontSize = 20;

% uncomment if you want to see a 2D slice
%HJIextraArgs.visualize.plotData.plotDims = [1 1 0]; %plot x, y
%HJIextraArgs.visualize.plotData.projpt = [0]; %project at theta = 0
%HJIextraArgs.visualize.viewAngle = [0,90]; % view 2D

%[data, tau, extraOuts] = ...
% HJIPDE_solve(data0, tau, schemeData, minWith, extraArgs)
[data, tau2, ~] = ...
  HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);

data0 = shapeCylinder(g, 3, [4.0; 3.0], 0.5);

[data, tau2, ~] = ...
  HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);

tau = t0:dt:1;
data0 = shapeRectangleByCenter(g, [3.8, 4.5, 4.5], [0.8, 0.8, 1.0]);

[data, tau2, ~] = ...
  HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);

data0 = shapeRectangleByCenter(g, [0.0, 0.0, 0.0], [2.0, 2.0, 0.4]);
[data, tau2, ~] = ...
  HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);


init_states = csvread('3D_init_1000_600.csv');
sc = 1;
uc = 1;
for i = 1:length(init_states)
    if(init_states(i,4)==1)
        safe_idx(sc) = i;
        sc = sc+1;
    else
        unsafe_idx(uc) = i;
        uc = uc+1;
    end
end

%Plot safe states
if sc>1
    scatter3( init_states(safe_idx(1:sc-1),1), init_states(safe_idx(1:sc-1),2), init_states(safe_idx(1:sc-1),3),  'r.');
end
if uc>1
    scatter3( init_states(unsafe_idx(1:uc-1),1), init_states(unsafe_idx(1:uc-1),2), init_states(unsafe_idx(1:uc-1),3),  'g.');
end

%compute optimal u for second controller
% xyz = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\controller_matlab_400.mat');
% W1 = xyz.fc1_weight;
% b1 = xyz.fc1_bias';
% W2 = xyz.fc_2_weight;
% b2 = xyz.fc_2_bias';
% 
% for i = 1:length(g.xs{1,1})
%     for j = 1:length(g.xs{2,1})
%         s_inp = [g.xs{1,1}(i, j); g.xs{2, 1}(i, j); 1];
%         l1 = vpa(tanh([W1, b1]*s_inp));
%         temp = [W2, b2]*[l1; 1];
%         sig = vpa((1./(1 + exp(-temp))));
%         tn = vpa(tanh(temp));
%         uOpt{1,1}(i, j) = vpa( sig(1) )*5;
%         uOpt{2,1}(i, j) = vpa( tn(2) )*3.14;
%     end
% end
% 
% u_optimal = uOpt

%compute dynamics for second controller
% abc = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\model_matlab.mat');
% W1 = abc.fc1_weight;
% b1 = abc.fc1_bias';
% W2 = abc.fc_state_weight;
% b2 = abc.fc_state_bias';
% 
% for i = 1:length(u_optimal{1,1})
%     for j = 1:length(u_optimal{2,1})
%         t1 = vpa(tanh([W1, b1] * [u_optimal{1, 1}(i, j); u_optimal{2, 1}(i, j); 1]));
%         t2 = vpa( tanh([W2, b2] * [t1; 1]) ).*0.5;
%         dx_t(i, j) = t2(1);
%         dy_t(i, j) = t2(2);
%     end
% end
% dx_global{1} = dx_t;
% dx_global{2} = dy_t;
% 
% %Plot settings for second controller
% HJIextraArgs.visualize.plotColorVS = 'm';
% HJIextraArgs.visualize.plotLineStyleVS = '--';
% [data, tau2, ~] = ...
%   HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);
% 
% 
% %compute optimal u for third controller
% xyz = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\controller_matlab_600.mat');
% W1 = xyz.fc1_weight;
% b1 = xyz.fc1_bias';
% W2 = xyz.fc_2_weight;
% b2 = xyz.fc_2_bias';
% 
% for i = 1:length(g.xs{1,1})
%     for j = 1:length(g.xs{2,1})
%         s_inp = [g.xs{1,1}(i, j); g.xs{2, 1}(i, j); 1];
%         l1 = vpa(tanh([W1, b1]*s_inp));
%         temp = [W2, b2]*[l1; 1];
%         sig = vpa((1./(1 + exp(-temp))));
%         tn = vpa(tanh(temp));
%         uOpt{1,1}(i, j) = vpa( sig(1) )*5;
%         uOpt{2,1}(i, j) = vpa( tn(2) )*3.14;
%     end
% end
% 
% u_optimal = uOpt
% 
% %compute dynamics for second controller
% abc = load('C:\Users\admin\Downloads\helperOC-master\helperOC-master\model_matlab.mat');
% W1 = abc.fc1_weight;
% b1 = abc.fc1_bias';
% W2 = abc.fc_state_weight;
% b2 = abc.fc_state_bias';
% 
% for i = 1:length(u_optimal{1,1})
%     for j = 1:length(u_optimal{2,1})
%         t1 = vpa(tanh([W1, b1] * [u_optimal{1, 1}(i, j); u_optimal{2, 1}(i, j); 1]));
%         t2 = vpa( tanh([W2, b2] * [t1; 1]) ).*0.5;
%         dx_t(i, j) = t2(1);
%         dy_t(i, j) = t2(2);
%     end
% end
% dx_global{1} = dx_t;
% dx_global{2} = dy_t;
% 
% %Plot settings for second controller
% HJIextraArgs.visualize.plotColorVS = 'c';
% HJIextraArgs.visualize.plotLineStyleVS = '-.';
% [data, tau2, ~] = ...
%   HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);


%Add obstacles
%data0 = shapeSphere(g, [6; 4.5], 0.5);
%data0 = shapeRectangleByCenter(g, [1.5, 4.5], [3, 1])
%data0 = shapeRectangleByCenter(g, [4.0, 1.5], [1, 3])

%data0 = shapeRectangleByCenter(g, [1.5, 4.5], [3, 1]);
%[data, tau2, ~] = ...
%  HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs);
%% Compute optimal trajectory from some initial state
if compTraj
  pause
  
  %set the initial state
  xinit = [2, 2, -pi];
  
  figure(6)
  clf
  h = visSetIm(g, data(:,:,:,end));
  h.FaceAlpha = .3;
  hold on
  s = scatter3(xinit(1), xinit(2), xinit(3));
  s.SizeData = 70;
  
  %check if this initial state is in the BRS/BRT
  %value = eval_u(g, data, x)
  value = eval_u(g,data(:,:,:,end),xinit);
  
  if value <= 0 %if initial state is in BRS/BRT
    % find optimal trajectory
    
    dCar.x = xinit; %set initial state of the dubins car

    TrajextraArgs.uMode = uMode; %set if control wants to min or max
    TrajextraArgs.visualize = true; %show plot
    TrajextraArgs.fig_num = 2; %figure number
    
    %we want to see the first two dimensions (x and y)
    TrajextraArgs.projDim = [1 1 0]; 
    
    %flip data time points so we start from the beginning of time
    dataTraj = flip(data,4);
    
    % [traj, traj_tau] = ...
    % computeOptTraj(g, data, tau, dynSys, extraArgs)
    [traj, traj_tau] = ...
      computeOptTraj(g, dataTraj, tau2, dCar, TrajextraArgs);
  else
    error(['Initial state is not in the BRS/BRT! It have a value of ' num2str(value,2)])
  end
end
end