function rungreenfield = greenfield3D(simdata)
% greenfield3D
%
% DJH Bruijnen
% Tech United
% Eindhoven 2008

%initialization
global data rungreenfield
if ~nargin % run greenfield3D by calling m-file
    !/usr/local/matlab75/bin/matlab -nojvm -r show_gf3D </dev/null >/dev/null &
%     task_id_greenfield3D=task_start('show_gf3D');
    return
elseif isequal(simdata,'replay') %run greenfield3D in replay mode when input argument is replay
    data = initfield('replay');
    global replay
    while rungreenfield
        greenfield3D(replay.data(:,floor(replay.cnt-replay.offset)));
        global replay
        if replay.play==0
            replay.cnt = replay.offset+mod(replay.cnt-replay.offset+replay.direction-1,replay.max)+1;
            replay.direction = 0;
        else
            replay.cnt = replay.offset+mod(replay.cnt-replay.offset+replay.play-1,replay.max)+1;
            replay.play = min(max(replay.play,-1),1);
        end
    end
    return
elseif ~simdata(1) %if first element of simdata is equal to zero then a new field is initiated
    data = initfield;
end

%update field with new data
if rungreenfield==1
    if length(simdata)==data.simdatalength
        updatefield(data,simdata);
    else
        disp(['received data has not the correct size ' int2str(length(simdata)) ' ' int2str(data.simdatalength)])
        pause(.2)
    end
elseif rungreenfield==2
    rungreenfield=0;
    closefield(data)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function gotopathofmfile
k = mfilename('fullpath');
ind = findstr(k,'/');
cd(k(1:ind(end)));

function closefield(data)

%get camera view parameters
cameratarget = [0 0 0];
cameraupvector = [0 0 1];
cameraposition = get(data.field.handle,'cameraposition');
cameraviewangle = get(data.field.handle,'cameraviewangle');
activepositionproperty = get(data.field.handle,'activepositionproperty');
position = get(gcf,'position');

%save camera view data
curdir = cd;
gotopathofmfile;
save cameradata cameraposition cameratarget cameraviewangle position cameraupvector activepositionproperty

%close the greenfield figure
close(data.figure.handle)

%plot the average frame rate
global framecnt
disp(['average frame rate: ' num2str(framecnt/toc) ' Hz'])

%save replay if
global replay
if ~replay.active
    if replay.cnt>replay.max
        k = mod(replay.cnt-1,replay.max)+1;
        replay.data = replay.data(:,[k+1:replay.max 1:k]);
    else
        replay.data = replay.data(:,1:replay.cnt);
        replay.max = replay.cnt;
    end
    save replaydata replay
end
cd(curdir);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function updatefield(data,simdata)

homegoal = simdata([data.turtle.idx_blueishomegoal]);

%% update camera view
global cam

%rotate camera after start
cam.ncnt = 50;
if cam.cnt<=cam.ncnt
    cam.orbit(1)=360/cam.ncnt;
    cam.cnt = cam.cnt+1;
    if cam.cnt==cam.ncnt+1
        cam.orbit(1)=0;
    end
end

%rotate camera around target
if any(cam.orbit~=0)
    try
        camorbit(data.field.handle,cam.orbit(1),cam.orbit(2));
    catch
    end
end

%move across the field in the XY-plane
if any(cam.dolly~=0)
    try
        [AZ,EL]=view(data.field.handle);
        AZ = AZ/180*pi;
        camdolly(data.field.handle,cam.dolly(1)*cos(AZ)-cam.dolly(2)*sin(AZ),cam.dolly(1)*sin(AZ)+cam.dolly(2)*cos(AZ),cam.dolly(3),'movetarget','data');
    catch
    end
end

%move towards the target
if cam.zoom~=0
    try
        camdolly(data.field.handle,0,0,cam.zoom,'fixtarget')
    catch
    end
end

%change the camera view angle
if cam.viewangle~=1
    try
        camzoom(data.field.handle,cam.viewangle)
    catch
    end
end

%follow cpb
global currentturtle
if cam.followcpb
    if ~simdata(data.turtle(cam.turtlecpb).idx_active)
        for i=1:data.nturtles
            if simdata(data.turtle(i).idx_active)
                cam.turtlecpb = i;
                currentturtle = i;
                break
            end
        end
    end
    
    if simdata(data.turtle(cam.turtlecpb).idx_CPB)<.5
        for i=1:data.nturtles
            if simdata(data.turtle(i).idx_active) && simdata(data.turtle(i).idx_CPB)>.5
                cam.turtlecpb = i;
                currentturtle = i;
                break
            end
        end
    end
    cam.target = flipxy(homegoal(cam.turtlecpb),[simdata(data.turtle(cam.turtlecpb).idx_pose(1:2)); .4]);
    set(data.field.handle,'cameratarget',cam.target)
    set(data.field.handle,'cameraupvector',[0 0 1]);
    cam.turtlecpb = currentturtle;
end

%follow turtle
if cam.followturtle & simdata(data.turtle(currentturtle).idx_active)
    cam.target = flipxy(homegoal(currentturtle),[simdata(data.turtle(currentturtle).idx_pose(1:2)); .4]);
    set(data.field.handle,'cameratarget',cam.target)
    set(data.field.handle,'cameraupvector',[0 0 1]);
end

%follow ball
if cam.followball & simdata(data.turtle(currentturtle).idx_active)
    cam.target = flipxy(homegoal(currentturtle),[simdata(data.turtle(currentturtle).idx_ball)]);
    set(data.field.handle,'cameratarget',cam.target)
    set(data.field.handle,'cameraupvector',[0 0 1]);
end

%first person
if cam.firstperson & simdata(data.turtle(currentturtle).idx_active)
    pose = flipxyo(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_pose));
    cam.position = [pose(1) pose(2) .6]+.2*[-sin(pose(3)) cos(pose(3)) 0];
    cam.target = [pose(1) pose(2) .6]+3*[-sin(pose(3)) cos(pose(3)) 0];
    set(data.field.handle,'cameraposition',cam.position);
    set(data.field.handle,'cameratarget',cam.target);
    set(data.field.handle,'cameraupvector',[0 0 1]);
end

%side view
if cam.sideview
    cam.position = [-12 0 8];
    cam.target = [0 0 0];
    set(data.field.handle,'cameraposition',cam.position);
    set(data.field.handle,'cameratarget',cam.target);
    set(data.field.handle,'cameraupvector',[0 0 1]);
end

%side view
if cam.topview
    cam.position = [.01 0 20];
    cam.target = [0 0 0];
    set(data.field.handle,'cameraposition',cam.position);
    set(data.field.handle,'cameratarget',cam.target);
    set(data.field.handle,'cameraupvector',[0 0 1]);
end

%reset camera parameters if keyreleasefcn is not supported by current Matlab release
if ~cam.keyreleasefcn
    cam.orbit = [0 0];
    cam.dolly = [0 0 0];
    cam.zoom = 0;
    cam.viewangle = 1;
end

%% update turtle information
global currentteamcolor
for i = 1:data.nturtles
    newlocalization = sum(simdata(data.turtle(i).idx_lineposrecog));
    if simdata(data.turtle(i).idx_active)
        %set localization info for all turtles
        if newlocalization==2
            set(data.locturtle(i).handle,'facecolor', [0 1 0])
        elseif newlocalization==1
            set(data.locturtle(i).handle,'facecolor', [1 1 1])
        else
            set(data.locturtle(i).handle,'facecolor', [1 0 0])
        end

        if i==currentturtle
            %set localization info in the textbox for the selected turtle
            if newlocalization==2
                set(data.localization.handle,'color',[0 1 0])
            elseif newlocalization==1
                set(data.localization.handle,'color',[1 1 1])
            else
                set(data.localization.handle,'color',[1 0 0])
            end
        end
        
    else
        set(data.locturtle(i).handle,'facecolor', [0 0 0])
        set(data.localization.handle,'color',[0 0 0])
    end

    %set teamcolor for all turtletops
    newteamcolor = simdata(data.turtle(i).idx_teamcolor);
    if newteamcolor==0 && currentteamcolor(i)==1
        data.turtletop(i).color = [1 0 1];
        set(data.turtletop(i).handle,'facecolor',[1 0 1])
        currentteamcolor(i) = 0;
    end
    if newteamcolor==1 && currentteamcolor(i)==0
        data.turtletop(i).color = [0 1 1];
        set(data.turtletop(i).handle,'facecolor',[0 1 1])
        currentteamcolor(i) = 1;
    end
end

%set teamcolor in the textbox for the selected turtle
if currentteamcolor(currentturtle)
    set(data.info.currentcolor.handle,'string','cyan','color',[0 1 1])
else
    set(data.info.currentcolor.handle,'string','magenta','color',[1 0 1])
end

%set turtle id
set(data.info.currentturtle.handle,'string',['turtle ' int2str(currentturtle) ': ']);

%set the home goal in the textbox for the selected turtle
if simdata(data.turtle(currentturtle).idx_blueishomegoal)
    set(data.info.homegoal.handle,'string','blue','color',[0 0 1])
else
    set(data.info.homegoal.handle,'string','yellow','color',[1 1 0])
end

%game roles
global   ROLE_ATTACKER_MAIN;
global   ROLE_ATTACKER_ASSIST;
global   ROLE_DEFENDER_MAIN;
global   ROLE_DEFENDER_ASSIST1;
global   ROLE_DEFENDER_ASSIST2;
global   ROLE_GOALKEEPER;

%max refbox roles
global   MAX_REFBOX_ROLES;

%set the left textbox info
if simdata(data.turtle(currentturtle).idx_active),
    switch simdata(data.turtle(currentturtle).idx_roleId(1)),
        case ROLE_ATTACKER_MAIN;
            roleString = 'attacker main';
        case ROLE_ATTACKER_ASSIST;
            roleString = 'attacker assist';
        case ROLE_DEFENDER_MAIN;
            roleString = 'defender main';
        case ROLE_DEFENDER_ASSIST1;
            roleString = 'defender assist 1';
        case ROLE_DEFENDER_ASSIST2;
            roleString = 'defender assist 2';
        case ROLE_GOALKEEPER;
            roleString = 'goalkeeper';
        otherwise
            roleString = '-';
    end
    set(data.info.roleId.handle,'string',['cur. role: ' roleString]);

    if (simdata(data.turtle(currentturtle).idx_refboxRole)>0 & simdata(data.turtle(currentturtle).idx_refboxRole)<=MAX_REFBOX_ROLES),
        set(data.info.refboxRoleId.handle,'string',['ref. role: ' num2str(simdata(data.turtle(currentturtle).idx_refboxRole))]);
    else set(data.info.refboxRoleId.handle,'string','ref. role: -');
    end

    set(data.info.restartCount.handle,'string',['v: ' num2str(simdata(data.turtle(currentturtle).idx_restartCountVision)) ', m: ' num2str(simdata(data.turtle(currentturtle).idx_restartCountMotion)) ', w: ' num2str(simdata(data.turtle(currentturtle).idx_restartCountWorldmodel)) ])

    %set executable info
    if simdata(data.turtle(currentturtle).idx_vision_status),
        set(data.info.exeState(1).handle,'facecolor',[0 1 0]);
        %set ball found info
        if simdata(data.turtle(currentturtle).idx_ballFound),
            set(data.info.ballFound.handle,'facecolor',[1 0 0]);
        else set(data.info.ballFound.handle,'facecolor',[0 1 0]);
        end
    else set(data.info.exeState(1).handle,'facecolor',[0 0 0]);
        set(data.info.ballFound.handle,'facecolor',[0 0 0]);
    end
    if simdata(data.turtle(currentturtle).idx_motion_status),
        set(data.info.exeState(2).handle,'facecolor',[0 1 0]);
    else set(data.info.exeState(2).handle,'facecolor',[0 0 0]);
    end
    if simdata(data.turtle(currentturtle).idx_worldmodel_status),
        set(data.info.exeState(3).handle,'facecolor',[0 1 0]);
    else set(data.info.exeState(3).handle,'facecolor',[0 0 0]);
    end
    
    %battery voltage
    set(data.info.batteryVoltage.handle,'string',['V: ' sprintf('%.0f',simdata(data.turtle(currentturtle).idx_batteryVoltage)) '%,'])

    %CPU load
    set(data.info.CPUload.handle,'string',['cpu0: ' sprintf('%.0f',simdata(data.turtle(currentturtle).idx_cpu0Load)) '%, cpu1: ' sprintf('%.0f',simdata(data.turtle(currentturtle).idx_cpu1Load)) '%'])
    
    %in field
    if simdata(data.turtle(currentturtle).idx_robotInField),
        set(data.info.robotInField.handle,'facecolor',[0 1 0]);
    else set(data.info.robotInField.handle,'facecolor',[1 0 0]);
    end

    %set skill id
    skillId = simdata(data.turtle(currentturtle).idx_skillid);
    switch skillId,
        case 1, skillstring = 'move';
        case 2, skillstring = 'dribble';
        case 3, skillstring = 'aim';
        case 4, skillstring = 'kick straight';
        case 5, skillstring = 'kick lob';
        case 6, skillstring = 'frame left';
        case 7, skillstring = 'frame right';
        case 8, skillstring = 'frame up';
        case 9, skillstring = 'flat pas';
        otherwise, skillstring = '-';
    end
    set(data.info.skillId.handle,'string',['skill id: ' skillstring]);
    
    %set ball source
    if simdata(data.turtle(currentturtle).idx_ball_confidence)>0 & simdata(data.turtle(currentturtle).idx_ballFound),
        set(data.info.ballSource.handle,'string','ball: omnivision');
    elseif simdata(data.turtle(currentturtle).idx_ball_confidence)==0 & simdata(data.turtle(currentturtle).idx_ballFound),
        set(data.info.ballSource.handle,'string','ball: communicated');
    else set(data.info.ballSource.handle,'string','ball: not found');
    end
    
    %set CPB team
    if simdata(data.turtle(currentturtle).idx_CPBteam),
        CPBteam='yes';
    else CPBteam='no';
    end
    set(data.info.CPBteam.handle,'string',['CPB team: ' CPBteam]);
     
else
    set(data.info.roleId.handle,'string','current role: ');
    set(data.info.refboxRoleId.handle,'string','refbox role: ');

    set(data.info.restartCount.handle,'string','m: -, v: -, w: -');

    %set executable active boxes
    d=.15;
    for i=1:3,
        set(data.info.exeState(i).handle,'facecolor',[0 0 0]);
    end
    set(data.info.ballFound.handle,'facecolor',[0 0 0]);

    %set battery voltage
    set(data.info.batteryVoltage.handle,'string','V: - %');

    %set CPU load
    set(data.info.CPUload.handle,'string','cpu0: - %, cpu1: - %');

    %set in field
    set(data.info.robotInField.handle,'facecolor',[0 0 0]);
    
    %set skill id
    set(data.info.skillId.handle,'string','skill id: -');
    
    %set ball source
    set(data.info.ballSource.handle,'string','ball: -');
    
    %set CPB team
    set(data.info.CPBteam.handle,'string','CPB team: -');
end

%% set visibility turtle information
global setvisibilityturtleinfo
if isempty(setvisibilityturtleinfo),
    setvisibilityturtleinfo = 'off';
end
setvisibility(data.info,setvisibilityturtleinfo);

%% replay handling
global replay
if replay.active
    if replay.play==0
        set(data.replay.handle,'string',['REPLAY: halt, FRAME: ' int2str(floor(replay.cnt))])
    elseif replay.play==-1
        set(data.replay.handle,'string',['REPLAY: reverse, FRAME: ' int2str(floor(replay.cnt))])
    else
        set(data.replay.handle,'string',['REPLAY: forward, FRAME: ' int2str(floor(replay.cnt))])
    end
else
    global framecnt
    set(data.counter.handle,'string',['FRAME: ' int2str(framecnt)])

    %% capture replaydata
    replay.cnt = replay.cnt+1;
    replay.data(:,mod(replay.cnt-1,replay.max)+1)=simdata;
end

%% update turtle positions

%get orientation of camera view
[AZ,EL]=view(data.field.handle);
AZ = AZ/180*pi-pi/2;
EL = EL/180*pi;

for i = 1:data.nturtles
    pose = flipxyo(homegoal(i),simdata(data.turtle(i).idx_pose));
    cx = data.turtleshape.x*cos(pose(3))-data.turtleshape.y*sin(pose(3));
    cy = data.turtleshape.x*sin(pose(3))+data.turtleshape.y*cos(pose(3));
    if simdata(data.turtle(i).idx_active)
        %move turtle
        set(data.turtle(i).handle,'xdata',cx+pose(1),'ydata',cy+pose(2))
        set(data.turtletop(i).handle,'xdata',data.turtletopshape.x+pose(1),'ydata',data.turtletopshape.y+pose(2))
        if simdata(data.turtle(i).idx_CPB)>.5
            set(data.turtlenumber(i).handle,'color',[1 0 0])
        else
            set(data.turtlenumber(i).handle,'color',[0 0 0])
        end
            
        %draw number above turtle
        if i==currentturtle %a larger number for the selected turtle
            [cx,cy,cz] = shapenumber(i,AZ,EL,.3);
            set(data.turtlenumber(i).handle,'xdata',pose(1)+cx,'ydata',pose(2)+cy,'zdata',.8+cz,'linewidth',5)
        else
            [cx,cy,cz] = shapenumber(i,AZ,EL,.2);
            set(data.turtlenumber(i).handle,'xdata',pose(1)+cx,'ydata',pose(2)+cy,'zdata',.8+cz,'linewidth',2)
        end
    else
        set(data.turtle(i).handle,'xdata',cx+100,'ydata',cy)
        set(data.turtletop(i).handle,'xdata',cx+100,'ydata',cy)
        [cx,cy,cz] = shapenumber(i,AZ,EL,.3);
        set(data.turtlenumber(i).handle,'xdata',cx+100,'ydata',cy,'zdata',cz)
    end
end

%% update target/subtarget

%data of current turtle
target = flipxyo(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_target));
subtarget = flipxy(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_subtarget));
pose = flipxyo(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_pose));

% set position of target/subtarget/targetline/targetheading
if simdata(data.turtle(currentturtle).idx_active)
    set(data.target.handle,'xdata',data.target.x+target(1),'ydata',data.target.y+target(2));
    set(data.subtarget.handle,'xdata',data.subtarget.x+subtarget(1),'ydata',data.subtarget.y+subtarget(2));
    set(data.targetline.handle,'xdata',[pose(1) subtarget(1) target(1)],...
        'ydata',[pose(2) subtarget(2) target(2)]);
    if abs(simdata(data.turtle(currentturtle).idx_skillid)-2)<.5
        set(data.targetheading.handle,'xdata',pose(1)+[0 0],...
            'ydata',pose(2)+[0 0])
    else
        set(data.targetheading.handle,'xdata',pose(1)+[0 -sin(target(3))],...
            'ydata',pose(2)+[0 cos(target(3))])
    end
else
    set(data.target.handle,'xdata',100,'ydata',0);
    set(data.subtarget.handle,'xdata',100,'ydata',0);
    set(data.targetline.handle,'xdata',100,'ydata',0);
    set(data.targetheading.handle,'xdata',100,'ydata',0);
end

%% update waypoints
NPATH_MAX=8;

%data of current turtle
waypoints = flipxyo(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_waypoints));
pose = flipxyo(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_pose));
pathlength = min(8,simdata(data.turtle(currentturtle).idx_pathlength));

%set position of waypoints
if simdata(data.turtle(currentturtle).idx_active)
    for i=1:pathlength;
        set(data.wp(i).handle,'xdata',data.wp(i).x+waypoints(2*(i-1)+1),'ydata',data.wp(i).y+waypoints(2*(i-1)+2));
    end
    for i=pathlength+1:NPATH_MAX,
        set(data.wp(i).handle,'xdata',100,'ydata',0);
    end
    set(data.wpline.handle,'xdata',[pose(1) waypoints(1:2:2*pathlength)'],...
        'ydata',[pose(2) waypoints(2:2:2*pathlength)']);
else
    for i=1:pathlength,
        set(data.wp(i).handle,'xdata',100,'ydata',0);
    end
    set(data.wpline.handle,'xdata',100,'ydata',0);
end

%plot setpoint
sp = simdata(data.turtle(currentturtle).idx_setpoint);
set(data.sp.handle,'xdata',sp(1:2:63),'ydata',sp(2:2:64))

%% update ball positions

%update ball history
global balls
set(data.ball(1).handle,'edgecolor',[0 0 0],'linewidth',.5);
if simdata(data.turtle(currentturtle).idx_CPB)>.5
    curball = [pose(1);pose(2);0]+.2*[-sin(pose(3));cos(pose(3));0];
else
    curball = flipxy(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_ball));
    commball = flipxy(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_ballWM));
    if sum(abs(commball(1:2)-pose(1:2)))>1e-10 && sum(abs(curball(1:2)-pose(1:2)))<1e-10
        set(data.ball(1).handle,'edgecolor',[1 0 0],'linewidth',2)
    end
end
balls = [curball balls(:,1:end-1)];

%move balls
cx = data.ballshape.x;
cy = data.ballshape.y;
cz = data.ballshape.z;
if simdata(data.turtle(currentturtle).idx_active)
    for i = 1:size(balls,2)
        set(data.ball(i).handle,'xdata',balls(1,i)+cx,'ydata',balls(2,i)+cy,'zdata',balls(3,i)+cz)
    end
    %move shadow of ball
    set(data.ballshadow.handle,'xdata',data.ballshadow.x+balls(1,1),'ydata',data.ballshadow.y+balls(2,1))
else
    for i = 1:size(balls,2)
        set(data.ball(i).handle,'xdata',100+cx,'ydata',cy,'zdata',cz)
    end
    %move shadow of ball
    set(data.ballshadow.handle,'xdata',data.ballshadow.x+100,'ydata',data.ballshadow.y+0)
end


%% update obstacle positions
global obstacleactive
global obstaclesvisibility

obs_vis_setting = mod(obstaclesvisibility,3); % obstaclevisibility is increased by one during a keypress

for i = 1:get_global_par('MAXNOBJ_LOCAL'),
    %plot obstacle if its radius is larger than zero
    r = simdata(data.turtle(currentturtle).idx_obstacles(i*3));
    obstacle = flipxy(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_obstacles(i*3-2:i*3-1)));
    if r>0 & simdata(data.turtle(currentturtle).idx_active) & obs_vis_setting < 2
        set(data.obstacle(i).handle,'xdata',data.obstacleshape.x*r+obstacle(1),'ydata',data.obstacleshape.y*r+obstacle(2))
        obstacleactive(i)=1;
        %remove the obstacle from the field if it is not an object anymore
        %afterwhich it will not be updated anymore until it becomes an obstacle again
    elseif obstacleactive(i) | ~simdata(data.turtle(currentturtle).idx_active)
        set(data.obstacle(i).handle,'xdata',-100+data.obstacleshape.x*r+obstacle(1),'ydata',data.obstacleshape.y*r+obstacle(2))
        obstacleactive(i)=0;
    end
end

global opponentactive
%% update opponent positions
for i = 1:get_global_par('MAX_OPPONENTS'),
    [cx,cy,cz] = shapenumber(i,AZ,EL,.2);
    %plot obstacle if its radius is larger than zero
    r = simdata(data.turtle(currentturtle).idx_opponent(i*3));
    opponent = flipxy(homegoal(currentturtle),simdata(data.turtle(currentturtle).idx_opponent(i*3-2:i*3-1)));
    if r>0 & simdata(data.turtle(currentturtle).idx_active) & obs_vis_setting > 0
        set(data.opponent(i).handle,'xdata',data.obstacleshape.x*r+opponent(1),'ydata',data.obstacleshape.y*r+opponent(2))
        opponentactive(i)=1;
        % draw number (label) above opponent
        set(data.opponentlabel(i).handle,'xdata',opponent(1)+cx,'ydata',opponent(2)+cy,'zdata',.8+cz,'linewidth',2)
        %remove the opponent from the field if it is not an object anymore
        %afterwhich it will not be updated anymore until it becomes an opponent again
    elseif opponentactive(i) | ~simdata(data.turtle(currentturtle).idx_active)
        set(data.opponent(i).handle,'xdata',-100+data.obstacleshape.x*r+opponent(1),'ydata',data.obstacleshape.y*r+opponent(2))
        opponentactive(i)=0;
        set(data.opponentlabel(i).handle,'xdata',cx+100,'ydata',cy,'zdata',cz)
    end
end


%% update figure
drawnow
global framecnt
framecnt = framecnt+1;

%% set visibility info boxes
function setvisibility(infohandlestruct,onoff)
infofieldnames = fieldnames(infohandlestruct);
for i1=1:length(infofieldnames),
    if isstruct(eval(['infohandlestruct.',infofieldnames{i1}])),
        for i2=1:length(eval(['infohandlestruct.',infofieldnames{i1}])),
            setvisibility(eval(['infohandlestruct.',infofieldnames{i1},'(i2)']),onoff);
        end
    else set(eval(['infohandlestruct.',infofieldnames{i1}]),'visible',onoff);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function x=flipxy(b,x)

if b
    x(1:2)=-x(1:2);          
end

function x=flipxyo(b,x)

if b
    x(1:2)=-x(1:2);  
    x(3)=mod(x(3)+2*pi,2*pi)-pi;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function data = initfield(replayflag)

%% determine data indices of simulink input for each turtle

%field names and sizes in simdata
data = setgreenfield3Dparameters;

%determine indices for all turtles for all fieldnames
for i=1:data.nturtles
    z=(i-1)*sum([data.simdatafields{:,2}])+2;
    for j=1:size(data.simdatafields,1)
        eval(['data.turtle(i).idx_' data.simdatafields{j,1} '=z:z-1+' int2str(data.simdatafields{j,2}) ';'])
        z = z+data.simdatafields{j,2};
    end
end

%% field dependent parameters
fieldwidth = get_global_par('FIELDWIDTH');
fieldlength = get_global_par('FIELDLENGTH');
fieldborder = 1;
linewidth = .1;
circleradius = get_global_par('CIRCLERADIUS');
cornerradius = get_global_par('CORNERRADIUS');
penaltyareawidth = get_global_par('PENALTYAREAWIDTH');
penaltyarealength = get_global_par('PENALTYAREALENGTH');
penaltyspot = get_global_par('PENALTYSPOT');
goalareawidth = get_global_par('GOALAREAWIDTH');
goalarealength = get_global_par('GOALAREALENGTH');
goalwidth = get_global_par('GOALWIDTH');
goalheight = 1;
goaldepth = get_global_par('GOALDEPTH');
ballradius = get_global_par('BALLRADIUS');

%% roles
% roles
global   ROLE_ATTACKER_MAIN;
global   ROLE_ATTACKER_ASSIST;
global   ROLE_DEFENDER_MAIN;
global   ROLE_DEFENDER_ASSIST1;
global   ROLE_DEFENDER_ASSIST2;
global   ROLE_GOALKEEPER;

ROLE_ATTACKER_MAIN =    get_global_par('ROLE_ATTACKER_MAIN');
ROLE_ATTACKER_ASSIST =  get_global_par('ROLE_ATTACKER_ASSIST');
ROLE_DEFENDER_MAIN =    get_global_par('ROLE_DEFENDER_MAIN');
ROLE_DEFENDER_ASSIST1 = get_global_par('ROLE_DEFENDER_ASSIST1');
ROLE_DEFENDER_ASSIST2 = get_global_par('ROLE_DEFENDER_ASSIST2');
ROLE_GOALKEEPER =       get_global_par('ROLE_GOALKEEPER');

%refbox roles
global MAX_REFBOX_ROLES;
MAX_REFBOX_ROLES = get_global_par('MAX_REFBOX_ROLES');

%% set global parameters

%camera settings
global cam
cam.orbit = [0 0];
cam.dolly = [0 0 0];
cam.zoom = 0;
cam.viewangle = 1;
cam.cnt = 0;
cam.followturtle = 0;
cam.followball = 0;
cam.firstperson = 0;
cam.sideview = 0;
cam.topview = 0;
cam.followcpb = 0;
cam.turtlecpb = 1;

%current turtle
global currentturtle
currentturtle = 1;

%run greenfield flag
global rungreenfield
rungreenfield = 1;

%% create figure and put grass on the field
data.figure.handle = figure;

% place greenfield3D next to trc
% get screen resolution
screen_size = get(0,'ScreenSize');
% define rect : [x lower left, y lower left, width, height]
% WIDTH OF TRC IS 390
rect = [390, screen_size(4)-(screen_size(3)-390)/screen_size(3), screen_size(3)-390, (screen_size(3)-390)/screen_size(3)*screen_size(4)];
% set sizes to figure
set(data.figure.handle,'Position',rect);

data.field.handle = gca;
if 0
    patch([1 1 -1 -1]*(.5*fieldwidth+fieldborder),[1 -1 -1 1]*(.5*fieldlength+fieldborder),-.01*[1 1 1 1],[0 .7 0]);
else
    A = imread('grass.jpg');
    surface((.5*fieldwidth+fieldborder)*[-1 1;-1 1],...
        (.5*fieldlength+fieldborder)*[1 1;-1 -1],...
        -.01*ones(2),A,'facecolor','texturemap','edgecolor',[0 0 0],'linewidth',3,'cdatamapping','direct')
end
hold on
set(gca,'projection','perspective')
set(gca,'position',[0 0 1 1])
set(gca,'color','none')
set(gcf,'keypressfcn',@keypressfcn)

if findobj(gcf,'-property','keyreleasefcn')
    set(gcf,'keyreleasefcn',@keyreleasefcn)
    cam.keyreleasefcn = 1;
else
    cam.keyreleasefcn = 0;
end

set(gcf,'name','  quit: esc  rotate: a,d,s,w  zoom: +,-,q,e  move: arrows/pageup/down  select turtle: 1-7  follow object: t,c,b,f  side/topview: g,v  replay: p,[,]  info: \,/')
set(gcf,'numbertitle','off')
axis image
axis([-fieldwidth*2 fieldwidth*2 -fieldlength*2 fieldlength*2 -.1 6])
axis off

%% set rendering settings
set(gcf,'renderer','opengl')
set(gcf,'doublebuffer','on')
% opengl hardware
% opengl('OpenGLBitmapZbufferBug',1)
% opengl('OpenGLWobbleTesselatorBug',1)
% opengl('OpenGLLineSmoothingBug',1)
% opengl('OpenGLClippedImageBug',1)
% opengl('OpenGLEraseModeBug',1)
% try opengl('OpenGLDockingBug',1), catch, end %not supported by Matlab v7.04

%% set previous camera view settings if available
curdir = cd;
gotopathofmfile;
if exist('cameradata.mat','file')
    load cameradata
    set(gca,'cameraposition',cameraposition)
    set(gca,'cameratarget',cameratarget)
    set(gca,'cameraviewangle',cameraviewangle)
    set(gca,'cameraupvector',cameraupvector);
    set(gca,'activepositionproperty',activepositionproperty);
    drawnow
else
    position = get(gcf,'position');
end
cd(curdir)

%% plot fieldlines

%outer fieldlines
patch([[1 1 -1 -1 1]*.5*(fieldwidth+linewidth) [1 -1 -1 1 1]*.5*(fieldwidth-linewidth) .5*(fieldwidth+linewidth)],...
    [[-1 1 1 -1 -1]*.5*(fieldlength+linewidth) [-1 -1 1 1 -1]*.5*(fieldlength-linewidth) -.5*(fieldlength+linewidth)],...
    zeros(1,11),[1 1 1],'linestyle','none')

%middle fieldline
patch([1 1 -1 -1]*.5*fieldwidth,[-1 1 1 -1]*.5*linewidth,zeros(1,4),[1 1 1],'linestyle','none')

%penaltyarea lines
patch([[1 1 -1 -1]*.5*(penaltyareawidth+linewidth) [-1 -1 1 1]*.5*(penaltyareawidth-linewidth)],...
    -.5*fieldlength+[[0 1 1 0]*(penaltyarealength+.5*linewidth) [0 1 1 0]*(penaltyarealength-.5*linewidth)],...
    zeros(1,8),[1 1 1],'linestyle','none')
patch([[1 1 -1 -1]*.5*(penaltyareawidth+linewidth) [-1 -1 1 1]*.5*(penaltyareawidth-linewidth)],...
    .5*fieldlength-[[0 1 1 0]*(penaltyarealength+.5*linewidth) [0 1 1 0]*(penaltyarealength-.5*linewidth)],...
    zeros(1,8),[1 1 1],'linestyle','none')

%goalarea lines
patch([[1 1 -1 -1]*.5*(goalareawidth+linewidth) [-1 -1 1 1]*.5*(goalareawidth-linewidth)],...
    -.5*fieldlength+[[0 1 1 0]*(goalarealength+.5*linewidth) [0 1 1 0]*(goalarealength-.5*linewidth)],...
    zeros(1,8),[1 1 1],'linestyle','none')
patch([[1 1 -1 -1]*.5*(goalareawidth+linewidth) [-1 -1 1 1]*.5*(goalareawidth-linewidth)],...
    .5*fieldlength-[[0 1 1 0]*(goalarealength+.5*linewidth) [0 1 1 0]*(goalarealength-.5*linewidth)],...
    zeros(1,8),[1 1 1],'linestyle','none')

%center circle
phi = linspace(0,2*pi,40);
patch([cos(phi)*(circleradius+linewidth*.5) cos(2*pi-phi)*(circleradius-linewidth*.5)],...
    [sin(phi)*(circleradius+linewidth*.5) sin(2*pi-phi)*(circleradius-linewidth*.5)],...
    zeros(1,80),[1 1 1],'linestyle','none')

%corner circles
phi = linspace(0,pi/2,10);
cx = [-1 -1 1 1]*.5*fieldwidth;
cy = [-1 1 1 -1]*.5*fieldlength;
for i=1:4
    patch(cx(i)-[cos(phi+i*pi/2)*(cornerradius+linewidth*.5) cos(pi/2-phi+i*pi/2)*(cornerradius-linewidth*.5)],...
        cy(i)+[sin(phi+i*pi/2)*(cornerradius+linewidth*.5) sin(pi/2-phi+i*pi/2)*(cornerradius-linewidth*.5)],...
        zeros(1,20),[1 1 1],'linestyle','none')
end

%penalty spots
phi = linspace(0,2*pi,8);
patch(sin(phi)*linewidth*.5,cos(phi)*linewidth*.5-fieldlength*.5+penaltyspot,zeros(1,8),[1 1 1],'linestyle','none')
patch(sin(phi)*linewidth*.5,cos(phi)*linewidth*.5+fieldlength*.5-penaltyspot,zeros(1,8),[1 1 1],'linestyle','none')

%% plot goals
data.goal.home.handle = patch([1 1 1 1;1 1 -1 -1;-1 -1 -1 -1;1 1 -1 -1]'*.5*goalwidth,...
    [-1 -1 0 0;-1 0 0 -1;-1 -1 0 0;-1 -1 -1 -1]'*goaldepth-fieldlength*.5,...
    [0 1 1 0;1 1 1 1;0 1 1 0;0 .5 .5 0]'*goalheight,[1 1 0])
data.goal.opponent.handle = patch([1 1 1 1;1 1 -1 -1;-1 -1 -1 -1;1 1 -1 -1]'*.5*goalwidth,...
    -[-1 -1 0 0;-1 0 0 -1;-1 -1 0 0;-1 -1 -1 -1]'*goaldepth+fieldlength*.5,...
    [0 1 1 0;1 1 1 1;0 1 1 0;0 .5 .5 0]'*goalheight,[0 0 1])

%% plot subtarget/target
phi = linspace(0,2*pi,16);
data.target.x = .3*cos(phi);
data.target.y = .3*sin(phi);
data.target.handle = plot(0,0,'r','linewidth',3);
data.subtarget.x = .2*cos(phi);
data.subtarget.y = .2*sin(phi);
data.subtarget.handle = plot(0,0,'r','linewidth',3);
data.targetline.handle = plot(0,0,'--r','linewidth',3);
data.targetheading.handle = plot(0,0,'-k','linewidth',3);

%% plot waypoints
phi = linspace(0,2*pi,16);
NPATH_MAX = 8;
for i=1:NPATH_MAX,
    data.wp(i).x = .05*cos(phi);
    data.wp(i).y = .05*sin(phi);
    data.wp(i).handle = plot(0,0,'b','linewidth',3);
end
data.wpline.handle = plot(0,0,'--b','linewidth',1);

%% plot setpoint
data.sp.handle = plot(0,0,'y','linewidth',2);

%% plot turtle

%plot turtle body
[data.turtleshape.x,data.turtleshape.y,data.turtleshape.z] = shapeturtle;
for i=1:data.nturtles
    data.turtle(i).handle = surface(data.turtleshape.x,data.turtleshape.y,data.turtleshape.z,'facecolor',[.2 .2 .2],'edgecolor',[0 0 0]);
end

% plot turtletop
global currentteamcolor
currentteamcolor = zeros(data.nturtles,1);
phi = [0:10]/11*2*pi;
data.turtletopshape.x = cos(phi)*.2;
data.turtletopshape.y = sin(phi)*.2;
data.turtletopshape.z = .8*ones(size(phi));
for i=1:data.nturtles
    data.turtletop(i).color = [1 0 1];
    data.turtletop(i).handle = patch(data.turtletopshape.x,data.turtletopshape.y,data.turtletopshape.z,[1 0 1]);
end

%plot turtle number
for i=1:data.nturtles
    data.turtlenumber(i).handle = plot3(0,0,0,'color',[0 0 0],'linewidth',2);
end

%% plot balls

%plot balls
global balls
nballs = 5;
balls = zeros(3,nballs);
[data.ballshape.x,data.ballshape.y,data.ballshape.z] = shapeball;
for i = nballs:-1:2;
    data.ball(i).handle = surface(data.ballshape.x,data.ballshape.y,data.ballshape.z,'facecolor',[1 1 1]-(nballs+1-i)/nballs*[0 .5 1],'linestyle','none');
end
data.ball(1).handle = surface(data.ballshape.x,data.ballshape.y,data.ballshape.z,'facecolor',[1 1 1]-(nballs+1-i)/nballs*[0 .5 1],'edgecolor',[0 0 0]);

%plot ball shadow
[data.ballshadow.x data.ballshadow.y data.ballshadow.z] = shapeballshadow;
data.ballshadow.handle = patch(data.ballshadow.x,data.ballshadow.y,data.ballshadow.z,[0 0 0]);

%% plot obstacles
global obstaclesvisibility
obstaclesvisibility = 0

global obstacleactive
obstacleactive = ones(get_global_par('MAXNOBJ_LOCAL'),1);
[data.obstacleshape.x, data.obstacleshape.y, data.obstacleshape.z] = shapeobstacle;
for i=1:get_global_par('MAXNOBJ_LOCAL'),
    data.obstacle(i).handle = surface(-50+data.obstacleshape.x, data.obstacleshape.y, .5*data.obstacleshape.z,'facecolor',[.5 .5 .5],'edgecolor',[0 0 0]);
end

%% plot wm opponents
global opponentactive
opponentactive = ones(get_global_par('MAX_OPPONENTS'),1);
for i=1:get_global_par('MAX_OPPONENTS'),
    data.opponent(i).handle = surface(-50+data.obstacleshape.x, data.obstacleshape.y, .5*data.obstacleshape.z,'facecolor',[0 0 1],'edgecolor',[0 0 0]);
end
%% plot wm opponent label
for i=1:data.nturtles
    data.opponentlabel(i).handle = plot3(0,0,0,'color',[0 0 1],'linewidth',2);
end


%% plot arena
plotarena(fieldwidth,fieldlength,fieldborder);

%% plot textbox info right
%create right textbox axes
axes('position',[.7 .8 .3 .2]);
axis([-.5 1 -.5 1])
axis off

%background
data.info.rightpatch = patch([0.05 .97 .97 0.05],[-.5 -.5 .93 .93],-.1*[1 1 1 1],[1 1 1],'facealpha',.7);

%set localization boxes
for i=1:data.nturtles
    data.locturtle(i).handle = patch((1+[i-1 i i i-1])/9,[.4 .4 .6 .6],[0 0 0 0],[0 1 0]);
end
data.localization.handle = text(.5,.65,'localization','fontsize',14,'horizontalalignment','center');

%set right textbox strings
data.info.currentturtle.handle = text(.6,-.05,'turtle 1: ','fontsize',14,'horizontalalignment','right');
data.info.currentcolor.handle = text(.9,-.05,' ','fontsize',14,'horizontalalignment','right');
data.info.homegoaltext = text(.6,-.25,'home goal: ','fontsize',14,'horizontalalignment','right');
data.info.homegoal.handle = text(.9,-.25,' ','fontsize',14,'horizontalalignment','right');

%% replay counter
global replay
replay=[];
if nargin==1
    curdir = cd;
    gotopathofmfile;
    load replaydata
    cd(curdir)
    replay.active = 1;
    if replay.cnt>replay.max
        replay.offset = replay.cnt-replay.max;
    else
        replay.offset = 0;
    end
    replay.cnt = replay.offset+1;
    replay.data(1,1)=1e-10;
    replay.max = size(replay.data,2);
    replay.play = 1;
    replay.direction = 1;
    data.replay.handle = text(.9,.2,'REPLAY:','fontsize',14','horizontalalignment','right','color',[1 0 0]);
else
    replay.play = 1;
    replay.direction = 1;
    replay.active = 0;
    replay.max = 10000;
    replay.cnt = 0;
    replay.data = zeros(data.nturtles*sum([data.simdatafields{:,2}])+1,replay.max);
    data.counter.handle = text(.9,.2,'FRAME: 1','fontsize',14','horizontalalignment','right','color',[1 0 0]);
end

%% plot textbox info left
%create left textbox axes
axes('position',[.005 .6 .3 .4]);
axis([-.5 1 -.5 1]);
axis off

%background
data.info.leftpatch = patch([-.49 .25 .25 -.49],[-.15 -.15 .95 .95],-.1*[1 1 1 1],[1 1 1],'facealpha',.7);

%set left textbox strings
data.info.roleId.handle = text(-.45,.8,'current role: -','fontsize',14,'horizontalalignment','left');
data.info.refboxRoleId.handle = text(-.45,.7,'refbox role: -','fontsize',14,'horizontalalignment','left');

data.info.restartCount.handle = text(-.45,.55,'m: -, v: -, w: -','fontsize',14,'horizontalalignment','left');

%set executable active boxes
d=.15;
for i=1:3,
    data.info.exeState(i).handle = patch(-.45+d*(i-1)+[0 d d 0],[.45 .45 .5 .5],[0 0 0 0],[0 0 0]);
end
data.info.ballFound.handle = patch(-.45+d/3*[1 2 2 1],[.45 .45 .5 .5],[0 0 0 0],[0 0 0]);

%set CPU load
data.info.CPUload.handle = text(-.45,.3,'cpu0: - %, cpu1: - %','fontsize',14,'horizontalalignment','left');

%set battery voltage
data.info.batteryVoltage.handle = text(-.45,.2,'V: - %,','fontsize',14,'horizontalalignment','left');

%set in field state
data.info.infieldtext = text(-.2,.2,'in field:','fontsize',14,'horizontalalignment','left');
data.info.robotInField.handle = patch(.05+[0 .1 .1 0],[.2 .2 .25 .25],[0 0 0 0],[0 0 0]);

%set skill id
data.info.skillId.handle = text(-.45,.1,'skill id: -','fontsize',14,'horizontalalignment','left');

%set ball source
data.info.ballSource.handle = text(-.45,0,'ball: -','fontsize',14,'horizontalalignment','left');

%set CPB (team)
data.info.CPBteam.handle = text(-.45,-.1,'CPB team: -','fontsize',14,'horizontalalignment','left');

%% play sound
% [y,fs,nbits]=wavread('greenfield3D.wav');
% y = resample(y,8192,fs);
% sound(y);

% set(gcf,'position',position)

%% initialize tictoc timer
global framecnt
framecnt = 0;
tic

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [cx, cy, cz] = shapeturtle
cx = [.3 0 -.3 0 .3;
    .3 0 -.3 0 .3;
    .1 0 -.1 0 .1;
    0 0 0 0 0];
cy = [.2 -.3 .2 .1 .2;
    .2 -.3 .2 .1 .2;
    .1 -.15 .1 .1 .1;
    0 0 0 0 0];
cz = [0 0 0 0 0;
    .2 .2 .2 .2 .2;
    .2 .2 .2 .2 .2;
    .8 .8 .8 .8 .8];

function [cx, cy, cz] = shapeball
ballradius = .11;
[cx cy cz] = sphere(8);
cx = cx*ballradius;
cy = cy*ballradius;
cz = ballradius+cz*ballradius;

function [cx, cy, cz] = shapeballshadow
ballradius = .11;
phi = linspace(0,2*pi,8);
cx = cos(phi)*ballradius;
cy = sin(phi)*ballradius;
cz = zeros(size(phi));

function [cx,cy,cz] = shapeobstacle
[cx,cy,cz] = cylinder(1,10);

function [cx,cy,cz] = shapenumber(nr,az,el,a)
if nr==1
    x = [0 0];
    y = [0 0];
    z = [0 2*a];
elseif nr==2
    x = [0 0 0 0 0 0];
    y = .5*[a -a -a a a -a];
    z = [0 0 a a 2*a 2*a];
elseif nr==3
    x = [0 0 0 0 0 0 0];
    y = .5*[-a a a -a a a -a];
    z = [0 0 a a a 2*a 2*a];
elseif nr==4
    x = [0 0 0 0 0];
    y = .5*[a a a -a -a];
    z = [0 2*a a a 2*a];
elseif nr==5
    x = [0 0 0 0 0 0];
    y = .5*[-a a a -a -a a];
    z = [0 0 a a 2*a 2*a];
elseif nr==6
    x = [0 0 0 0 0 0];
    y = .5*[-a a a -a -a a];
    z = [a a 0 0 2*a 2*a];
else
    x = zeros(1,4);
    y = .5*[a a -a -a];
    z = [0 2*a 2*a a];
end
cz = sin(el)*x+cos(el)*z;
xx = cos(el)*x-sin(el)*z;
cx = cos(az)*xx-sin(az)*y;
cy = sin(az)*xx+cos(az)*y;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function keypressfcn(src,evnt)
global cam currentturtle rungreenfield replay setvisibilityturtleinfo obstaclesvisibility
if isequal(evnt.Key,'rightarrow')
    cam.dolly(1)=.3;
elseif isequal(evnt.Key,'leftarrow')
    cam.dolly(1)=-.3;
elseif isequal(evnt.Key,'uparrow')
    cam.dolly(2)=.3;
elseif isequal(evnt.Key,'downarrow')
    cam.dolly(2)=-.3;
elseif isequal(evnt.Key,'pagedown')
    cam.dolly(3)=-.3;
elseif isequal(evnt.Key,'pageup')
    cam.dolly(3)=.3;
elseif isequal(evnt.Key,'hyphen') || isequal(evnt.Key,'subtract')
    cam.viewangle=1/1.04;
elseif isequal(evnt.Key,'equal') || isequal(evnt.Key,'add')
    cam.viewangle=1.04;
elseif isequal(evnt.Key,'a')
    cam.orbit(1)=3;
elseif isequal(evnt.Key,'d')
    cam.orbit(1)=-3;
elseif isequal(evnt.Key,'w')
    cam.orbit(2)=3;
elseif isequal(evnt.Key,'s')
    cam.orbit(2)=-3;
elseif isequal(evnt.Key,'q')
    cam.zoom=.05;
elseif isequal(evnt.Key,'e')
    cam.zoom=-.05;
elseif isequal(evnt.Key,'1')
    currentturtle=1;
elseif isequal(evnt.Key,'2')
    currentturtle=2;
elseif isequal(evnt.Key,'3')
    currentturtle=3;
elseif isequal(evnt.Key,'4')
    currentturtle=4;
elseif isequal(evnt.Key,'5')
    currentturtle=5;
elseif isequal(evnt.Key,'6')
    currentturtle=6;
elseif isequal(evnt.Key,'7')
    currentturtle=7;
elseif isequal(evnt.Key,'escape')
    rungreenfield=2;
elseif isequal(evnt.Key,'p')
    if replay.play==0
        replay.play=1;
    else
        replay.play=0;
    end
    replay.direction=0;
elseif isequal(evnt.Key,'rightbracket')
    if replay.play==0
        replay.direction = 1;
    else
        replay.play=100;
    end
elseif isequal(evnt.Key,'leftbracket')
    if replay.play==0
        replay.direction = -1;
    else
        replay.play=-100;
    end
elseif isequal(evnt.Key,'t')
    cam.followturtle=1-cam.followturtle;
    if cam.followturtle
        cam.followball=0;
        cam.firstperson=0;
        cam.sideview=0;
        cam.topview=0;
        cam.followcpb=0
    end
elseif isequal(evnt.Key,'b')
    cam.followball=1-cam.followball;
    if cam.followball
        cam.followturtle=0;
        cam.firstperson=0;
        cam.sideview=0;
        cam.topview=0;
        cam.followcpb=0
    end
elseif isequal(evnt.Key,'f')
    cam.firstperson = 1-cam.firstperson;
    if cam.firstperson
        cam.followturtle=0;
        cam.followball=0;
        cam.sideview=0;
        cam.topview=0;
        cam.followcpb=0
    end
elseif isequal(evnt.Key,'g')
    cam.sideview = 1-cam.sideview;
    if cam.sideview
        cam.followturtle=0;
        cam.followball=0;
        cam.firstperson=0;
        cam.topview=0;
        cam.followcpb=0
    end
elseif isequal(evnt.Key,'v')
    cam.topview = 1-cam.topview;
    if cam.topview
        cam.followturtle=0;
        cam.followball=0;
        cam.firstperson=0;
        cam.sideview=0;
        cam.followcpb=0
    end
elseif isequal(evnt.Key,'c')
    cam.followcpb = 1-cam.followcpb;
    if cam.followcpb
        cam.followturtle=0;
        cam.followball=0;
        cam.firstperson=0;
        cam.sideview=0;
        cam.topview=0;
    end
elseif isequal(evnt.Key,'slash')
    setvisibilityturtleinfo = 'on';
elseif isequal(evnt.Key,'backslash')
    setvisibilityturtleinfo = 'off';
elseif isequal(evnt.Key,'o')
    obstaclesvisibility = obstaclesvisibility + 1;
end

function keyreleasefcn(src,evnt)
global cam
if isequal(evnt.Key,'rightarrow')
    cam.dolly(1)=0;
elseif isequal(evnt.Key,'leftarrow')
    cam.dolly(1)=0;
elseif isequal(evnt.Key,'uparrow')
    cam.dolly(2)=0;
elseif isequal(evnt.Key,'downarrow')
    cam.dolly(2)=0;
elseif isequal(evnt.Key,'pagedown')
    cam.dolly(3)=0;
elseif isequal(evnt.Key,'pageup')
    cam.dolly(3)=0;
elseif isequal(evnt.Key,'hyphen') || isequal(evnt.Key,'subtract')
    cam.viewangle=1;
elseif isequal(evnt.Key,'equal') || isequal(evnt.Key,'add')
    cam.viewangle=1;
elseif isequal(evnt.Key,'a')
    cam.orbit(1)=0;
elseif isequal(evnt.Key,'d')
    cam.orbit(1)=0;
elseif isequal(evnt.Key,'w')
    cam.orbit(2)=0;
elseif isequal(evnt.Key,'s')
    cam.orbit(2)=0;
elseif isequal(evnt.Key,'q')
    cam.zoom=0;
elseif isequal(evnt.Key,'e')
    cam.zoom=0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function plotarena(fieldwidth,fieldlength,fieldborder);

%plot wall
wallsize = 1;
crowdx = 3;
crowdz = 2;
cx = [fieldwidth/2+fieldborder+crowdx+[0 wallsize wallsize];
    fieldwidth/2+fieldborder+crowdx+[0 wallsize wallsize];
    fieldwidth/2+fieldborder+[0 0 0];
    -fieldwidth/2-fieldborder+[0 0 0];
    -fieldwidth/2-fieldborder-crowdx-[0 wallsize wallsize];
    -fieldwidth/2-fieldborder-crowdx-[0 wallsize wallsize];
    -fieldwidth/2-fieldborder+[0 0 0];
    fieldwidth/2+fieldborder+[0 0 0];
    fieldwidth/2+fieldborder+crowdx+[0 wallsize wallsize]];
cy = [fieldlength/2+fieldborder+[0 0 0];
    -fieldlength/2-fieldborder+[0 0 0];
    -fieldlength/2-fieldborder-crowdx-[0 wallsize wallsize];
    -fieldlength/2-fieldborder-crowdx-[0 wallsize wallsize];
    -fieldlength/2-fieldborder+[0 0 0];
    fieldlength/2+fieldborder+[0 0 0];
    fieldlength/2+fieldborder+crowdx+[0 wallsize wallsize];
    fieldlength/2+fieldborder+crowdx+[0 wallsize wallsize];
    fieldlength/2+fieldborder+[0 0 0]];
cz = ones(9,1)*[crowdz crowdz 0];
surface(cx,cy,cz,'facecolor',[.5 .3 .1])

%plot crowd at sides
A = imread('greenfield3D.jpg');
surface(fieldwidth/2+fieldborder+[crowdx crowdx;0 0],...
    [-1 1;-1 1]*(fieldlength/2+fieldborder),...
    [crowdz crowdz;0 0],A,'facecolor','texturemap','edgecolor','none','cdatamapping','direct')
surface(-fieldwidth/2-fieldborder-[crowdx crowdx;0 0],...
    [1 -1;1 -1]*(fieldlength/2+fieldborder),...
    [crowdz crowdz;0 0],A,'facecolor','texturemap','edgecolor','none','cdatamapping','direct')
surface([1 -1;1 -1]*(fieldwidth/2+fieldborder),...
    fieldlength/2+fieldborder+[crowdx crowdx;0 0],...
    [crowdz crowdz;0 0],A,'facecolor','texturemap','edgecolor','none','cdatamapping','direct')
surface([-1 1;-1 1]*(fieldwidth/2+fieldborder),...
    -fieldlength/2-fieldborder-[crowdx crowdx;0 0],...
    [crowdz crowdz;0 0],A,'facecolor','texturemap','edgecolor','none','cdatamapping','direct')

%plot black corners
stadiumcolor = [0,0,0];
patch(-fieldwidth/2-fieldborder-[0 0 crowdx],...
    fieldlength/2+fieldborder+[0 crowdx 0],...
    [0 crowdz crowdz],stadiumcolor)
patch(fieldwidth/2+fieldborder+[0 0 crowdx],...
    fieldlength/2+fieldborder+[0 crowdx 0],...
    [0 crowdz crowdz],stadiumcolor)
patch(-fieldwidth/2-fieldborder-[0 0 crowdx],...
    -fieldlength/2-fieldborder-[0 crowdx 0],...
    [0 crowdz crowdz],stadiumcolor)
patch(fieldwidth/2+fieldborder+[0 0 crowdx],...
    -fieldlength/2-fieldborder-[0 crowdx 0],...
    [0 crowdz crowdz],stadiumcolor)