function res = BR_test_bounce
%

p0 = [0,0;0,6;-6,6;-6,5;-6,6;-6,0;-1,-6;6,0;-6,-6;-5,-6;6,-6;6,-5;6,5;6,6];
v0 = [1,1;-2,2;-2,2;-2,2;-2,0;-1,-1;1,-1;1,1;-1,-1;-1,-1;1,-1;1,-1;1,1;1,1];
del_t = ones(14,1);
data = [p0,v0,del_t];

[num_tests,dummy] = size(data);
box = [-6.25,6.25; 6.25,6.25; 6.25,-6.25; -6.25,-6.25];
res = zeros(num_tests,4);

for t = 1:num_tests
    [pos,vel] = BR_bounded_motion(box,data(t,1:2),data(t,3:4),data(t,5));
    res(t,1:2) = pos;
    res(t,3:4) = vel;
end

clf
hold on
plot([box(1,1),box(2,1)],[box(1,2),box(2,2)]);
plot([box(2,1),box(3,1)],[box(2,2),box(3,2)]);
plot([box(3,1),box(4,1)],[box(3,2),box(4,2)]);
plot([box(4,1),box(1,1)],[box(4,2),box(1,2)]);
quiver(data(:,1),data(:,2),data(:,3),data(:,4));

for p = 1:num_tests
    clf
    hold on
    plot([box(1,1),box(2,1)],[box(1,2),box(2,2)]);
    plot([box(2,1),box(3,1)],[box(2,2),box(3,2)]);
    plot([box(3,1),box(4,1)],[box(3,2),box(4,2)]);
    plot([box(4,1),box(1,1)],[box(4,2),box(1,2)]);
    quiver(p0(p,1),p0(p,2),v0(p,1),v0(p,2),'g');
    quiver(res(p,1),res(p,2),res(p,3),res(p,4),'r');
    tch = 0;
end

return

[pos,vel] = BR_bounded_motion(box,[0,0],[1,1],1);
res(1,1:2) = pos;
res(1,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[0,6],[-2,2],1);
res(2,1:2) = pos;
res(2,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-6,6],[-2,2],1);
res(3,1:2) = pos;
res(3,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-6,5],[-2,2],1);
res(4,1:2) = pos;
res(4,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-6,6],[-2,0],1);
res(5,1:2) = pos;
res(5,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,0],[-1,-1],1);
res(6,1:2) = pos;
res(6,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-1,-6],[1,-1],1);
res(7,1:2) = pos;
res(7,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,0],[1,1],1);
res(8,1:2) = pos;
res(8,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-6,-6],[-1,-1],1);
res(9,1:2) = pos;
res(9,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[-5,-6],[-1,-1],1);
res(10,1:2) = pos;
res(10,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,-6],[1,-1],1);
res(11,1:2) = pos;
res(11,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,-5],[1,-1],1);
res(12,1:2) = pos;
res(12,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,5],[1,1],1);
res(13,1:2) = pos;
res(13,3:4) = vel;

[pos,vel] = BR_bounded_motion(box,[6,6],[1,1],1);
res(14,1:2) = pos;
res(14,3:4) = vel;
