jasontron2012()

function jasontron2012()
% Currently makes a movie of NUMROBOTS moving to attack an older sister and
% pelt her with rubber suction cup darts.

global FrameCount MOVIE_NAME MAKE_MOVIE NUMROBOTS
MAKE_MOVIE = 0; % if 1, records png images to make into a movie

NUMROBOTS=25;
NUMMOVES = NUMROBOTS*10;%'img/%s%06d.png'
dTHETA = pi/4;
%ffmpeg -r 50 -f image2 -i img/JasonTron2012%06d.png -b 8000k -r 30 JasonTron2012.mp4
MOVIE_NAME = 'JasonTron2012';
FrameCount = 0;


%TODO: use Prof Bretl's minimization routine instead?

figure(1)
close all
figure(1)
set(gcf, 'position',[-98          57        1774         965]);
set(gca, 'XTick',[],'YTick',[])
axis1 = [-13.2403   11.0205   -6.1577   12.9771];
axis2 = [ -0.0529   10.1984    0.9210    9.0063];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%% plot the game area
%yr = 15;
%xr = 15;
%plot([0,xr,xr,0,0],[0,0,yr,yr,0],'r')
hold on
axis equal

targetRange = 4;
tx = 5;
ty = 5;
numHits = 0;


% solve the solution
yp = mod((0:NUMROBOTS-1),10)';
xp = -5 - floor((0:NUMROBOTS-1)/10)';
tp = zeros(NUMROBOTS,1);

delta = 1/2;
epsilon = linspace(1-delta, 1+delta, NUMROBOTS);

% Goal position.
tg = NUMMOVES*dTHETA*epsilon;
xg = tx+ targetRange*cos(tg+pi)';
yg = ty+ targetRange*sin(tg+pi)';

%Create C matrix:
angs = dTHETA*repmat(epsilon', 1,NUMMOVES).*repmat((1:NUMMOVES),NUMROBOTS,1);
C = [cos(angs);sin(angs)];

dq = [xg;yg]-[xp;yp];
% X = A\B is the solution in the least squares sense to the
% under- or overdetermined system of equations AX = B
%C.u = dq
u = C\dq;

%plot the path
% for i = 1:NUMROBOTS
%       plot(xp(i) +[0,cumsum(cos(dTHETA*epsilon(i)*(1:NUMMOVES)).*u')],...
%        yp(i) +[0,cumsum(sin(dTHETA*epsilon(i)*(1:NUMMOVES)).*u')],'color',[.7,.7,1]); 
%    hold on
% end


%%%%%%%%%%%%%% draw the arrows %%%%%%%%%%%%%%%%%%%%
xArr = [ -2,  2,2, 4, 0,-4,-2, -2]/10;
yArr = [  0,  0,5, 5,10, 5, 5,  0]/10;
xArrOff = 9; yArrOff = -3;
au = patch(xArrOff+xArr,yArrOff+ 1/2+yArr,'g');
ad = patch(xArrOff+xArr,yArrOff+-1/2-yArr,'g');

ar = patch(xArrOff+ 1/2+yArr,yArrOff+xArr,'g');
al = patch(xArrOff+-1/2-yArr,yArrOff+xArr,'g');


%%%%%%%%%%%%%% draw the robots %%%%%%%%%%%%%%%%%%%%

%rob = zeros{NUMROBOTS,1};
tstrcut = struct;
for i = 1:NUMROBOTS
    if i == 1
        rob = tstrcut;
        rob = plotRobot(xp(i),yp(i),tp(i),1,rob);
    else
        rob(i) = rob(1);
        rob(i) = plotRobot(xp(i),yp(i),tp(i),1,rob(i));
    end
end

plotGoal(tx,ty,targetRange);
targetOutlinex =[    4.4019    4.3600    4.3233    4.3076    4.3024    4.2605    4.2553    4.2553    4.3338    4.4280    4.5694    4.7212    4.8155    4.7998    4.7998    4.8626    4.9673    5.0459    5.1349    5.1872    5.1977    5.1872    5.4333    5.5799    5.6741    5.7160    5.7422    5.7422    5.6951    5.6951    5.6689    5.6375    5.6061    5.5904    5.5799    5.5642    5.5956    5.6061    5.5694    5.5118    5.4176    5.3495    5.2553    5.2082    5.2029    5.2082    5.1715    5.1296    5.0982    5.1035    5.0563    4.9830    4.8993    4.8940    4.8888    4.8574    4.8102    4.7945    4.7998    4.7736    4.7160    4.6479    4.5013    4.4228    4.4019    4.4123    4.4333    4.4071    4.4019    4.4071    4.4071];
targetOutliney =[    4.4015    4.4015    4.4643    4.5795    5.1188    5.1607    5.2130    5.7837    5.9146    5.9617    5.9931    5.9984    5.9984    6.1031    6.2497    6.4382    6.5429    6.5377    6.4277    6.2549    6.0926    5.9984    5.9984    5.9617    5.9094    5.8518    5.7994    5.2130    5.1188    4.5638    4.4277    4.4015    4.3910    3.9774    3.8779    3.8203    3.7365    3.5795    3.4329    3.3072    3.2496    3.2653    3.3596    3.4905    3.6423    3.6999    3.7365    3.8308    3.9826    4.0769    4.0716    4.0716    4.1240    4.0193    3.9093    3.7889    3.7156    3.6894    3.5638    3.4329    3.3229    3.2548    3.2915    3.4381    3.5742    3.7313    3.8255    3.9826    4.1135    4.3910    4.3910];

%  plot(targetOutlinex,targetOutliney,'g') %debugging: plot outline of target

% %% trace a line
% bit = 1;x = NaN;y = NaN;
% while bit == 1
%     [xn,yn,bit] = ginput(1);
%     x = [x;xn];
%     y = [y;yn];
%     plot(x,y,'-bx')
% end
% 
axis(axis1);
updateDrawing();
%u = [1,2];  %todo: add this as an introductory video?
%NUMMOVES = numel(u);

% maxVel = .01;
% NOMEGA = 30;
% maxVel = .5;
% NOMEGA = 2;
% c =0;
%implement solution
for j = 1:NUMMOVES
    if j < 8
        maxVel = .05;
        NOMEGA = 10;
    else
        maxVel = .5;
        NOMEGA = 2;
    end
    
    % turn all the robots
    for k = 1:NOMEGA
        tp = tp+epsilon'*dTHETA/NOMEGA;
        rob = drawRobots(xp,yp,tp,rob, NUMROBOTS);
        set(au,'FaceColor','w');
        set(ad,'FaceColor','w');
        set(al,'FaceColor','g');
        set(ar,'FaceColor','w');
        axis(axis1);
        updateDrawing();
    end
    % fwd/bkwd
    %title(['u(',num2str(j),') = ',num2str(u(NUMMOVES))])
    iters = ceil(abs(u(j))/maxVel)+3;
    for k = 1:iters
        xp = xp + cos(tp)*u(j)/iters;
        yp = yp + sin(tp)*u(j)/iters;
        rob = drawRobots(xp,yp,tp,rob, NUMROBOTS);
        if u(j)>0
            set(au,'FaceColor','g');
            set(ad,'FaceColor','w');
        else
            set(au,'FaceColor','w');
            set(ad,'FaceColor','g');
        end
        set(al,'FaceColor','w');
        set(ar,'FaceColor','w');
       axis(axis1);
       if abs(u(j)) > 0 
           updateDrawing();
       end
    end
end

for c = 1:30
    axis(axis1+c/30*(axis2-axis1));
    updateDrawing();
end

% fire the arrows!
arSteps = 60;
dstep = targetRange/arSteps; 
for j = 1:arSteps
    for i = 1:NUMROBOTS
        [rob(i), numHits] = plotArrow(dstep,rob(i), targetOutlinex,targetOutliney, numHits);
    end
    %title(['Num Hits = ',num2str(numHits)])
    updateDrawing();
end %fire arrows

for i = 1:30  % save final position
    updateDrawing
end

 plot(targetOutlinex,targetOutliney,'g')
end % end the main function


function rob = drawRobots(xp,yp,tp,rob, NUMROBOTS)
    for i = 1:NUMROBOTS
        rob(i) = plotRobot(xp(i),yp(i),tp(i),0,rob(i));
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function rb = plotRobot(xp,yp,tp,init,rb)
% plot robots
pink  = [1,.8,.8];
darkgreen = [0,.6,0];

% darts
xdi = [0.20    0.28    0.30    0.33,   0.33,   .30,  0.28];
ydi = [0.16    0.16    0.19    0.2  0.12   0.13,  0.16];

rb.xd1 =cos(tp)*xdi-sin(tp)*ydi;
rb.yd1 =sin(tp)*xdi+cos(tp)*ydi;
rb.xd2 =cos(tp)*xdi-sin(tp)*-ydi;
rb.yd2 =sin(tp)*xdi+cos(tp)*-ydi;

[rb.xd1, rb.yd1] = poly2cw(rb.xd1, rb.yd1);
[rb.xd2, rb.yd2] = poly2cw(rb.xd2, rb.yd2);

%legs
xli = [0.05    0.05    -0.05    -.05  ];
yli = [-.14,.14,.14,-.14];
xl =cos(tp)*xli-sin(tp)*yli;
yl =sin(tp)*xli+cos(tp)*yli;
% arms
xai = [0.1618    0.0571   0    0.0571 0.1618];
yai = [0.1539    0.1322   0    -0.1322  -0.1539];
xa =cos(tp)*xai-sin(tp)*yai;
ya =sin(tp)*xai+cos(tp)*yai;
% glasses
xgi = [0.0096    0.11      0.11       0.105    0.105   0.105    0.105   0.11 0.11 0.0096];
ygi = [0.0815    0.065    0.0143    0.0143    0.065  -0.065  -0.0143   -0.0143 -0.065  -0.0815];
xg =cos(tp)*xgi-sin(tp)*ygi;
yg =sin(tp)*xgi+cos(tp)*ygi;
%[x,y] = ginputExtra(5)
%guns
xgni = [0.14    0.14    0.25    0.25  ];
ygni = [.17,.14,.14,.17];
xgn =cos(tp)*xgni-sin(tp)*ygni;
ygn =sin(tp)*xgni+cos(tp)*ygni;
xgn2 =cos(tp)*xgni-sin(tp)*-ygni;
ygn2 =sin(tp)*xgni+cos(tp)*-ygni;
% run solution


if init
    rb.d1 = patch(xp+rb.xd1,yp+rb.yd1,pink); %,'edgecolor',pink
    rb.d2 = patch(xp+rb.xd2,yp+rb.yd2,pink);
    rb.legs = patch(xp+xl,yp+yl,'b');
    rb.arms = line(xp+xa,yp+ya,'color','k');
    rb.head = rectangle('position',[xp-.1,yp-.1,.2,.2],'curvature',[1,1],'facecolor','y');
    rb.glasses = line(xp+xg,  yp+yg,'color','k');
    rb.g1 = patch(xp+xgn,yp+ygn,darkgreen,'edgecolor',darkgreen);
    rb.g2 = patch(xp+xgn2,yp+ygn2,darkgreen,'edgecolor',darkgreen);
else
    set(rb.d1,'XData',xp+rb.xd1,'YData',yp+rb.yd1); %,'edgecolor',pink
    set(rb.d2,'XData',xp+rb.xd2,'YData',yp+rb.yd2);
    set(rb.legs,'XData',xp+xl,'YData',yp+yl);
    set(rb.arms,'XData',xp+xa,'YData',yp+ya);
    set(rb.head,'position',[xp-.1,yp-.1,.2,.2]);
    set(rb.glasses,'XData',xp+xg,'YData',yp+yg);
    set(rb.g1,'XData',xp+xgn,'YData',yp+ygn);
    set(rb.g2,'XData',xp+xgn2,'YData',yp+ygn2);
end
rb.dartorient = tp;
rb.dartstate1 = 1; %1 = unfired, 2 = flying, 3 = hit, 4 = fallen.
rb.dartx1 = xp;
rb.darty1 = yp;
rb.dist1 = 0;
rb.dartstate2 = 1; %1 = unfired, 2 = flying, 3 = hit, 4 = fallen.
rb.dartx2 = xp;
rb.darty2 = yp;
rb.dist2 = 0;

end

function [rb,numHits] = plotArrow(dstep,rb, targetOutlinex,targetOutliney,numHits)
    %todo: different state for each dart
    if rb.dartstate1 == 1 %1 = unfired, 2 = flying, 3 = hit, 4 = fallen.
        rb.dartstate1 = 2;
    end
    if rb.dartstate1 == 2
        rb.dartx1 = rb.dartx1+dstep*cos(rb.dartorient);
        rb.darty1 = rb.darty1+dstep*sin(rb.dartorient);
        rb.dist1 = rb.dist1+dstep;
        
        set(rb.d1,'XData',rb.dartx1+rb.xd1,'YData',rb.darty1+rb.yd1); %,'edgecolor',pink
        uistack(rb.d1,'top')
    end
    
    if rb.dartstate1 == 2
        b = polybool('intersection',rb.dartx1+rb.xd1,rb.darty1+rb.yd1,targetOutlinex,targetOutliney);
        if ~isempty(b)
             rb.dartstate1 = 3;
             set(rb.d1,'edgecolor','r');
             numHits = numHits + 1;
        end
    end
    
    if rb.dartstate2 == 1 %1 = unfired, 2 = flying, 3 = hit, 4 = fallen.
        rb.dartstate2 = 2;
    end
    if rb.dartstate2 == 2
        rb.dartx2 = rb.dartx2+dstep*cos(rb.dartorient);
        rb.darty2 = rb.darty2+dstep*sin(rb.dartorient);
        rb.dist2 = rb.dist2+dstep;
        
        set(rb.d2,'XData',rb.dartx2+rb.xd2,'YData',rb.darty2+rb.yd2); %,'edgecolor',pink
        uistack(rb.d2,'top')
    end
    
    if rb.dartstate2 == 2
        b = polybool('intersection',rb.dartx2+rb.xd2,rb.darty2+rb.yd2,targetOutlinex,targetOutliney);
        if ~isempty(b)
             rb.dartstate2 = 3;
             set(rb.d2,'edgecolor','r');
             numHits = numHits + 1;
        end
    end
     
end

function target = plotGoal(tx,ty,targetRange)
% Plot goal/target
% chair  34"W x 39"D x 42"H
th = linspace(0,2*pi,50);
rangearea = patch(tx+ targetRange*cos(th),ty + targetRange*sin(th), [1,.6,1]);
uistack(rangearea, 'bottom');
purple  = [.5,0,.5];
pink  = [1,.8,.8];
patch(tx+[-1,1,1,-1,-1],ty+[-1,-1,1,1,-1],purple);
patch(tx+[-1.5,-1,-1,-1.5,-1.5],ty+[-1,-1,1,1,-1],purple);
patch(tx+[1,1.5,1.5,1,1],ty+[-1,-1,1,1,-1],purple);
patch(tx+[-1.5,1.5,1.5,-1.5,-1.5],ty+[1,1,1.75,1.75,1],purple);
%Paige

%feet
target.lFoot = rectangle('position',[tx+-.6,ty+-1.75,.4,.75],'curvature',[1,1],'facecolor','w');
target.rFoot = rectangle('position',[tx+ .2,ty+-1.75,.4,.75],'curvature',[1,1],'facecolor','w');
%pants
target.lLeg = rectangle('position',[tx+-.6,ty+-1.3,.5,1.7],'curvature',[.5,.5],'facecolor','c');
target.rLeg = rectangle('position',[tx+ .1,ty+-1.3,.5,1.7],'curvature',[.5,.5],'facecolor','c');
%arms
target.lArm = rectangle('position',[tx+-.7,ty+-.6,.3,1.],'curvature',[.5,.5],'facecolor',pink);
target.rArm = rectangle('position',[tx+ .4,ty+-.6,.3,1.],'curvature',[.5,.5],'facecolor',pink);

%shirt
target.shirt1 = rectangle('position',[tx+-.75,ty+0, 1.5,1],'curvature',[.5,.5],'facecolor',pink);
target.shirt2 = patch(tx+[-0.6275   -0.5910    0.0336    0.5281    0.5801    0.1117   -0.0185   -0.0185],...
ty+[-0.5505   -0.5089   -0.8212   -0.5037   -0.6338   -0.9305   -0.9357   -0.9357],'w');
%hair
target.hair = rectangle('position',[tx+-.2,ty+.8,.4,.75],'curvature',[1,1],'facecolor','y');
target.ponytail = rectangle('position',[tx+-.5,ty+-.5,1,1.5],'curvature',[1,1],'facecolor','y');
end

function updateDrawing
    global FrameCount MOVIE_NAME MAKE_MOVIE
    drawnow
    if(MAKE_MOVIE)
        FrameCount=FrameCount+1;
        F = getframe(gca);
        fname = sprintf('img/%s%06d.png',MOVIE_NAME,FrameCount);
        imwrite(F.cdata, fname,'png'); 
        while(FrameCount < 10)
            updateDrawing
        end
    end
end