omni = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');

A = kron(eye(3), [zeros(size(omni.origin.syscl.a)) eye(size(omni.origin.syscl.a)); zeros(size(omni.origin.syscl.a)) omni.origin.syscl.a]);
B = kron(eye(3),kron([0;1],omni.origin.syscl.b));
R = @(the) [[cos(deg2rad(the)) -sin(deg2rad(the)) 0; sin(deg2rad(the)) cos(deg2rad(the)) 0; [0 0 1];] zeros(size(omni.origin.syscl.a)); zeros(size(omni.origin.syscl.a)) zeros(size(omni.origin.syscl.a))];
C = @(the1,the2, the3) kron(diag([1 0 0]), R(the1)) + kron(diag([0 1 0]), R(the2)) + kron(diag([0 0 1]), R(the3));
fvref_cw = fncSpeedRef('cw',4,6,3);
fvref_ysin = fncSpeedRef('sysin',8,6,3);
fvref_xsin = fncSpeedRef('sxsin',8,6,3);
finput = @(t) [fvref_cw(t)+[0 0 90*sin(deg2rad(5*t))]'; fvref_xsin(t); fvref_ysin(t);]

sys = @(st, input) A*st + B* input;
dydt = @(t, y) sys(y, finput(t));
RX = @(x,y,the) x*cos(deg2rad(the))-y*sin(deg2rad(the));
RY = @(x,y,the) y*cos(deg2rad(the))+x*sin(deg2rad(the));

init = [0 0 0 0 0 0 5 7 45 0 0 0 -8 4 270 0 0 0]';
tSpan = 1:.1:50;
[t, y] = ode45(dydt,tSpan,init);
for i=1:size(t,1)
    out(i,:) = C(y(i,3),y(i,9)+y(i,3),y(i,15)+y(i,3))*y(i,:)';
    u(i,:)   = finput(t(i))';
    d1(i,1)  = hypot(out(i,7)-out(i,1),out(i,8)-out(i,2));
    d2(i,1)  = hypot(out(i,13)-out(i,1),out(i,14)-out(i,2));
endfor

close all
figure(1)
plot(out(:,1), out(:,2), out(:,7), out(:,8), out(:,13), out(:,14));
figure(2)
plot(t(:,1), d1(:,1), t(:,1), d2(:,1));
figure(3)
plot(t(:,1), u(:,1), t(:,1), u(:,2), t(:,1), u(:,3));
% figure(4)
% plot(t(:,1), u(:,4), t(:,1), u(:,5),t(:,1), u(:,6));
% figure(5)
% plot(t(:,1), u(:,7), t(:,1), u(:,8),t(:,1), u(:,9));