function f = hover_test()
% EN530.678 HW#5 supplementary 
% simulation of a hovercraft model

% size of body frame of hovercraft
hover_frame = .5;

% model parameters
S.m = 5;
S.J = .5;
S.r = 0.2;

% initial state 
x0 = [4; 3; 0; 0.5; -.3; 0];

% desired state
S.xd = [1; 1; -pi; 0; 0; 0];

% desired accelration -- only applicable for trajectory tracking
S.ad = [0; 0; 0];

T = 10; % simulation time

% propotional control gains
S.kq = [1; 1; 1];

% derivative control gains
S.kv = [2; 2; 2];

% simulate dynamics for T seconds
[ts, xs] = ode45(@hover_ode, [0 T], x0, [], S);

% plot position path
figure
n_prints = 20;
step_size = int64(length(ts)/n_prints);
for i=1:step_size:length(ts)
    vecs = get_frame(xs(i,3));
    xs_ = [xs(i,1); xs(i,1)+hover_frame*vecs(1,1)];
    ys_ = [xs(i,2); xs(i,2)+hover_frame*vecs(2,1)];
    l = line(xs_, ys_, 'Color','red');
    l.Annotation.LegendInformation.IconDisplayStyle = 'off';
    
    hold on
    xs_ = [xs(i,1); xs(i,1)+hover_frame*vecs(1,2)];
    ys_ = [xs(i,2); xs(i,2)+hover_frame*vecs(2,2)];
    l = line(xs_, ys_, 'Color','red');
    l.Annotation.LegendInformation.IconDisplayStyle = 'off';
    
    hold on
end
plot(xs(:,1), xs(:,2),'.');
xlabel('x')
ylabel('y')
xlim([-2 5])
ylim([-2 5])


function u = hover_ctrl(t, x, S)
% control law: u = k(t,x), where x=(q,v) is the state
% S is a structure containing all model parameters 
% see main file for their listing

% current
p = x(1:3);  % position
v = x(4:6);  % velocity

% desired
pd = S.xd(1:3);
vd = S.xd(4:6);

r = S.r;

% required spatial force
b1 = [cos(x(3)), -sin(x(3)), 0;
      sin(x(3)),  cos(x(3)), 0;
              0,          0, 1];

b2 = [ 1, 1, 0;
       0, 0, 1;
      -r, r, 0];

B = b1 * b2;

D = diag([0.01, 0.1, 0.02]);
M = diag([S.m, S.m, S.J]);

u = inv(B) * (M*(S.ad -S.kq.* (p-pd) - S.kv.* (v-vd)) + D*v);


function dx = hover_ode(t, x, S)
% defines the ODE dx = f(t, x) for the fully-actuated hover ODE
% x = (q,v) denotes the full state, i.e. q=(px,py,theta), v=(vx,vy,w)

% position and orientation
p = x(1:3);

% control inputs from control law
u = hover_ctrl(t, x, S);

% set derivative
r = S.r;

b1 = [cos(x(3)), -sin(x(3)), 0;
      sin(x(3)),  cos(x(3)), 0;
              0,          0, 1];

b2 = [ 1, 1, 0;
       0, 0, 1;
      -r, r, 0];

  
B = b1 * b2;

D = diag([0.01, 0.1, 0.02]);
M = diag([S.m, S.m, S.J]);

dx2 = inv(M) * ((B*u) - D*x(4:6));

dx = [x(4:6);
      dx2];

% function to plot the body frame of hovercraft
function vecs = get_frame(theta)
    E = [[cos(theta),  sin(theta), 0];
         [-sin(theta), cos(theta), 0];
         [         0,           0, 1]];
     
    orig = [[1, 0];
            [0, 1];
            [0, 0]];
        
    pos = E * orig;
    
    vecs = pos(1:2, 1:2);
