function dx = dynamics(obj, ~, x, u, d)
% Dynamics of the Dubins Car
%    \dot{x}_1 = v * cos(x_3)
%    \dot{x}_2 = v * sin(x_3)
%    \dot{x}_3 = w
%   Control: u = w;
%
% Mo Chen, 2016-06-08

if nargin < 5
  d = [0; 0; 0];
end

if iscell(x)
  dx = cell(length(obj.dims), 1);
  
  for i = 1:length(obj.dims)
    dx{i} = dynamics_cell_helper(obj, x, u, d, obj.dims, obj.dims(i));
  end
else
%   dx = zeros(obj.nx, 1);
%   
%   dx(1) = obj.speed * cos(x(3)) + d(1);
%   dx(2) = obj.speed * sin(x(3)) + d(2);
%   dx(3) = u + d(3);
end
end

function dx_t = dynamics_cell_helper(obj, x, u, d, dims, dim)

global dx_global;

% 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';

%u_inp = [u{1,1}; u{2,1}; ones(41, 41)];
%l1 = vpa(tanh([W1, b1].*[u_inp]));
%l2 = vpa(tanh([W2, b2]*[l1; 1])).*0.5;
switch dim
  case 1
    %dx = l2(1)*0.5 +d{1};  
    %dx = u{2} * cos(u{1}) + d{1};
    %dx = l2(1) + d{1};
       
%    dx_t = dx_t + d{1};
    dx_t = dx_global{1} + d{1};
  case 2
    %dx = l2(2)*0.5 +d{2};
    %dx = u{2} * sin(u{1}) + d{2};
    %dx = l2(2) + d{2};
    
%     for i = 1:length(u{1,1})
%         for j = 1:length(u{2,1})
%             t1 = vpa(tanh([W1, b1] * [u{1, 1}(i, j); u{2, 1}(i, j); 1]));
%             t2 = vpa( tanh([W2, b2] * [t1;1]) ).*0.5;
%             dx_t(i, j) = t2(2);
%         end
%     end
%     
%     dx_t = dx_t + d{2};
    dx_t = dx_global{2} + d{2};
  case 3
  %  dx = u + d{3};
    dx_t = dx_global{3} + d{3};
  otherwise
    error('Only dimension 1-2 are defined for dynamics of DubinsCar!')
end
end