function [pos,vel] = BR_bounded_motion(box,p0,v0,del_t)
%

LINE_EPS = 0.001;
V_VEL = [-10,0];
DAMPING = 0.9;
MAX_VEL = 2000;
%MAX_VEL = norm(V_VEL);

x_min = box(1,1);
x_max = box(2,1);
y_min = box(3,2);
y_max = box(1,2);
line12 = [box(1,:),0; box(2,:),0];
line23 = [box(2,:),0; box(3,:),0];
line34 = [box(3,:),0; box(4,:),0];
line41 = [box(4,:),0; box(1,:),0];

p_new = p0 + del_t*v0;

% if new position is on box, use it
if p_new(1)>=x_min&p_new(1)<=x_max&p_new(2)>=y_min&p_new(2)<=y_max
    pos = p_new;
    vel = v0;
    return
end

% new position outside box
dir = p_new - p0;
dir = dir/norm(dir);
pt2 = p0 + dir;
line = [p0,0; pt2,0];

[ip12,st12] = cv_int_line_line(line,line12,LINE_EPS);
[ip23,st23] = cv_int_line_line(line,line23,LINE_EPS);
[ip34,st34] = cv_int_line_line(line,line34,LINE_EPS);
[ip41,st41] = cv_int_line_line(line,line41,LINE_EPS);

if st12==1  % if line intersects line12
    d12 = cv_dist_pt_line([p0,0],line12);  % p0 on line12?
    if d12<LINE_EPS
        dists(1) = d12;  % if p0 on line12, set that dist
    else
        dir12 = ip12(1:2) - p0;  % else get direction to intersection
        dir12 = dir12/norm(dir12);
        dir_dot = dot(dir,dir12);
        if dir_dot>0.9
            dists(1) = norm(ip12(1:2)-p0);  % if in direction of motion
        else
            dists(1) = Inf;  % if in opposite direction
        end
    end
elseif st12==-1 % line is line12
    dists(1) = 0;
else
    dists(1) = Inf;  % if lines do not intersect
end
if st23==1
    d23 = cv_dist_pt_line([p0,0],line23);
    if d23<LINE_EPS
        dists(2) = d23;
    else
        dir23 = ip23(1:2) - p0;
        dir23 = dir23/norm(dir23);
        dir_dot = dot(dir,dir23);
        if dir_dot>0.9
            dists(2) = norm(ip23(1:2)-p0);
        else
            dists(2) = Inf;
        end
    end
elseif st23==-1
    dists(2) = 0;
else
    dists(2) = Inf;
end
if st34==1
    d34 = cv_dist_pt_line([p0,0],line34);
    if d34<LINE_EPS
        dists(3) = d34;
    else
        dir34 = ip34(1:2) - p0;
        dir34 = dir34/norm(dir34);
        dir_dot = dot(dir,dir34);
        if dir_dot>0.9
            dists(3) = norm(ip34(1:2)-p0);
        else
            dists(3) = Inf;
        end
    end
elseif st34==-1
    dists(3) = 0;
else
    dists(3) = Inf;
end
if st41==1
    d41 = cv_dist_pt_line([p0,0],line41);
    if d41<LINE_EPS
        dists(4) = d41;
    else
        dir41 = ip41(1:2) - p0;
        dir41 = dir41/norm(dir41);
        dir_dot = dot(dir,dir41);
        if dir_dot>0.9
            dists(4) = norm(ip41(1:2)-p0);
        else
            dists(4) = Inf;
        end
    end
elseif st41==-1
    dists(4) = 0;
else
    dists(4) = Inf;
end

% determine if one bounce or two
[val,index] = min(dists);
one_bounce = 0;
if ((p_new(1)>=x_min&p_new(1)<=x_max)&...
        (p_new(2)>y_max|p_new(2)<y_min))...
        |((p_new(2)>=y_min&p_new(2)<=y_max)&...
        (p_new(1)>x_max|p_new(1)<x_min))
    one_bounce = 1;
end
if one_bounce==1
    switch index
        case 1
            pos(1) = p_new(1);
            pos(2) = y_max - (p_new(2)-y_max);
            dir = pos - ip12(1:2);
            dir = dir/norm(dir);
            vel = norm(v0)*dir + V_VEL;
        case 2
            pos(1) = x_max - (p_new(1)-x_max);
            pos(2) = p_new(2);
            dir = pos - ip23(1:2);
            dir = dir/norm(dir);
            vel = norm(v0)*DAMPING*dir;
        case 3
            pos(1) = p_new(1);
            pos(2) = y_min + (y_min-p_new(2));
            dir = pos - ip34(1:2);
            dir = dir/norm(dir);
            vel = norm(v0)*DAMPING*dir;
        case 4
            pos(1) = x_min + (x_min-p_new(1));
            pos(2) = p_new(2);
            dir = pos - ip41(1:2);
            dir = dir/norm(dir);
            vel = norm(v0)*DAMPING*dir;
    end
    if norm(vel)>MAX_VEL
        vel = MAX_VEL*vel/norm(vel);
    end
    pos(1) = max(x_min,min(x_max,pos(1)));
    pos(2) = max(y_min,min(y_max,pos(2)));
    return
end

% Two bounce case
if p_new(1)<x_min&p_new(2)>y_max  % up left
    pos(1) = x_min + (x_min-p_new(1));
    pos(2) = y_max - (p_new(2)-y_max);
    dir = p0 - p_new;
    dir = dir/norm(dir);
    vel = norm(v0)*dir + V_VEL;
elseif p_new(1)>x_max&p_new(2)>y_max % up right
    pos(1) = x_max - (p_new(2)-x_max);
    pos(2) = y_max - (p_new(2)-y_max);
    dir = p0 - p_new;
    dir = dir/norm(dir);
    vel = norm(v0)*dir + V_VEL;
elseif p_new(1)>x_max&p_new(2)<y_min % down right
    pos(1) = x_max - (p_new(1)-x_max);
    pos(2) = y_min + (y_min-p_new(2));
    dir = p0 - p_new;
    dir = dir/norm(dir);
    vel = norm(v0)*DAMPING*dir;
else % down left
    pos(1) = x_min + (x_min-p_new(1));
    pos(2) = y_min + (y_min-p_new(2));
    dir = p0 - p_new;
    dir = dir/norm(dir);
    vel = norm(v0)*DAMPING*dir;
end
if norm(vel)>MAX_VEL
    vel = MAX_VEL*vel/norm(vel);
end
