%==========================================================================
% BraitenBug Simulator (bbsim)
% version 0.3
% Feb 25, 2004
%
% Educational software developed for Neuro 317
% Methods in Computational Neurobiology
% University of Illinois, Urbana-Champaign
%
% Copyright (c) 2004 
% Board of Trustees of the University of Illinois
% All rights reserved
%
% For more information contact: 
% Prof. Mark E. Nelson (m-nelson@uiuc.edu)
%
% Programming and development support: 
% Eugene Kang (mandoo@uiuc.edu)
%==========================================================================
function bbsim(action)

global sensor_location sensors light_x light_y robot score light;
global htrail xtrail ytrail trailCnt maxTrail;
persistent t DT Hndl arena_size vel_left vel_right;

if nargin<1
    action='initialize';
end;

if strcmp(action,'initialize')
    %===================================
    % Get rid of previous figure windows (if any)
    % (this should be made more sophisticated in the future)
    try
        close(1:2);
    end
    
    %===================================
    % Information for all buttons
    labelColor=[.5 .7 .9];
    btnWidth=0.10;
    btnHeight=0.05;
    % Spacing between the buttons/labels
    btnSpacing=0.05;
    xInitPos=btnSpacing;
    frmBorder=0.01;
    yPos=0.06;
    scrSize = get(0,'ScreenSize');
    figSize = fix(0.5*scrSize(4));
    figNumber = 1;
    figure(figNumber);
    set(gcf,...
        'Name','BB Simulation', ...
        'NumberTitle','off', ...
        'DoubleBuffer','on', ...
        'Menubar','none', ...
        'Visible','off', ...
        'Color',[.4 .7 .5], ...
        'Position',[50 scrSize(4)-figSize-50 figSize figSize],...
        'BackingStore','off');
    axes( ...
        'Units','normalized', ...
        'Position',[0.1 2*btnHeight+7*frmBorder 0.8 0.8], ...
        'Visible','off', ...
        'DrawMode','fast', ...
        'NextPlot','add');
    
    % dummy axis, fill in arena parameters later
    axis([-10 10 -10 10]);
    axis square;
    grid off;
    
    %====================================
    % The CONSOLE frame
    
    xPos=xInitPos;
    frmPos=[0 0 1 2*btnHeight+3*frmBorder];
    h=uicontrol( ...
        'Style','frame', ...
        'Units','normalized', ...
        'Position',frmPos, ...
        'BackgroundColor',[0.20 0.40 0.30]);
    
    %====================================
    % The START button
    btnNumber=1;
    xPos=xInitPos+(btnNumber-1)*(btnWidth+btnSpacing);
    labelStr='Start';
    callbackStr='bbsim(''start'');';
    
    % Generic button information
    btnPos=[xPos yPos-btnSpacing btnWidth btnHeight];
    Hndl.start=uicontrol( ...
        'Style','toggle', ...
        'BackgroundColor',labelColor, ...
        'Units','normalized', ...
        'Position',btnPos, ...
        'String',labelStr, ...
        'Interruptible','on', ...
        'Callback',callbackStr);
    
    %====================================
    % The RESET button
    btnNumber=2;
    xPos=xInitPos+(btnNumber-1)*(btnWidth+btnSpacing);
    labelStr='Reset';
    callbackStr='bbsim(''reset'');';
    
    % Generic button information
    btnPos=[xPos yPos-btnSpacing btnWidth btnHeight];
    Hndl.reset=uicontrol( ...
        'Style','push', ...
        'BackgroundColor',labelColor, ...
        'Units','normalized', ...
        'Position',btnPos, ...
        'Enable','on', ...
        'String',labelStr, ...
        'Callback',callbackStr, ...
        'Interruptible','on');
    
    %====================================
    % The PRINT button
    btnNumber=3;
    labelStr='Print';
    callbackStr='figure(1); caxis([-10 1]); print bbsim.ps; disp(''printed to bbsim.ps''); caxis([0 1])';
    xPos=xInitPos+(btnNumber-1)*(btnWidth+btnSpacing);
    btnPos=[xPos yPos-btnSpacing btnWidth btnHeight];
    Hndl.print=uicontrol( ...
        'Style','push', ...
        'BackgroundColor',labelColor, ...
        'Units','normalized', ...
        'Position',btnPos, ...
        'Enable','on', ...
        'String',labelStr, ...
        'Callback',callbackStr);
    
    %====================================
    % The CLOSE button
    btnNumber=4;
    labelStr='Close';
    callbackStr='try, close(1:2); end; disp(''done'');';
    xPos=xInitPos+(btnNumber-1)*(btnWidth+btnSpacing);
    btnPos=[xPos yPos-btnSpacing btnWidth btnHeight];
    Hndl.close=uicontrol( ...
        'Style','push', ...
        'BackgroundColor',labelColor, ...
        'Units','normalized', ...
        'Position',btnPos, ...
        'String',labelStr, ...
        'Callback',callbackStr);
    
    %====================================
    % The Score 
    labelStr='Score';
    xPos=xInitPos;
    btnPos=[xPos yPos+0.01 btnWidth btnHeight];
    Hndl.score=uicontrol( ...
        'Style','text', ...
        'Units','normalized', ...
        'BackgroundColor',[1 1 .9], ...
        'Position',btnPos, ...
        'String',labelStr);
    
    %====================================
    % The Timer 
    labelStr='Timer';
    xPos=xInitPos+ (btnWidth+btnSpacing);
    btnPos=[xPos yPos+0.01 btnWidth btnHeight];
    Hndl.timer=uicontrol( ...
        'Style','text', ...
        'Units','normalized', ...
        'BackgroundColor',[1 1 .9], ...
        'Position',btnPos, ...
        'String',labelStr);
    
    %====================================
    % Neural controller
    labelStr='Brain:';
    xPos=xInitPos+2*(btnWidth+btnSpacing);
    btnPos=[xPos yPos+0.01 btnWidth btnHeight];
    uicontrol( ...
        'Style','text', ...
        'BackgroundColor',[.8 .8 .6], ...
        'Units','normalized', ...
        'Position',btnPos, ...
        'String',labelStr);
    xPos=xInitPos+3*btnWidth+2*btnSpacing;
    btnPos=[xPos yPos+0.01 2*btnWidth btnHeight];
    Hndl.filename=uicontrol( ...
        'Style','edit', ...
        'Units','normalized', ...
        'BackgroundColor',[1 1 .9], ...
        'Position',btnPos, ...
        'String','brain_avoid');
    
    % Uncover the figure
    set(figNumber,'Visible','on');
    
    %================================================
    axis([-10 10 -10 10]);  % create dummy arena
    set(gca,'color','k');
    axis square
    set(gca,'Visible','on');
    
    bbsim('reset');
    return
    
elseif strcmp(action,'reset')
    
    disp('reset');
    
    robot.x = 0;
    robot.y = 0;
    robot.heading_angle = 0; 
    robot.v = 0;
    robot.vmax = 30;
    robot.radius = 0.6;
    
    arena_size = 10;
    
    score = 0;
    
    t = 0;
    DT = 1/30;
    vel_left = 0;
    vel_right = 0;
    
    deg2rad = pi/180;
    sensors.left.angle = 30*deg2rad;
    sensors.right.angle = -30*deg2rad;
    sensors.left.angle_range = 90*deg2rad;
    sensors.right.angle_range = 90*deg2rad;
    
    light_x = rand(1)*2*(arena_size)-(arena_size);
    light_y = rand(1)*2*(arena_size)-(arena_size);
    
    timeStr = sprintf('%3.2f', t);
    set(Hndl.timer,'String',timeStr);
    set(Hndl.score,'String',score);
    
    figure(1); cla;
    draw_light(light_x, light_y, arena_size);
    sensor_location = draw_robot('init');       % create the robot
    
    return
    
elseif strcmp(action,'start')
    
    set([Hndl.reset Hndl.print Hndl.close],'Enable','off');
    if get(Hndl.start,'Value')==1
        set(Hndl.start,'String','Pause');
    else
        set(Hndl.start,'String','Start');
    end
    
    % duplicate lights for arena wrap around purpose
    x_mult = [-arena_size*2 0 arena_size*2; -arena_size*2 0 arena_size*2; -arena_size*2 0 arena_size*2];
    y_mult = [arena_size*2 arena_size*2 arena_size*2; 0 0 0; -arena_size*2 -arena_size*2 -arena_size*2];
    light.x = light_x + x_mult;
    light.y = light_y + y_mult;
    
    while get(Hndl.start,'Value')==1
        
        % where is the light?
        light.left_distance = sqrt( (sensor_location.left.x - light.x).^2 + (sensor_location.left.y - light.y).^2  );
        light.right_distance = sqrt( (sensor_location.right.x - light.x).^2 + (sensor_location.right.y - light.y).^2 ) ;   
        for index=1:9
            if light.left_distance(index) < 1 & light.right_distance(index) < 1
                
                % update score
                score = score + 1;
                
                % reset trail
                trailCnt = 0;
                xtrail = robot.x*ones(maxTrail,1);
                ytrail = robot.y*ones(maxTrail,1);
                
                % new light position
                light_x = rand(1)*2*(arena_size)-(arena_size);
                light_y = rand(1)*2*(arena_size)-(arena_size);
                draw_light(light_x, light_y);
                
                % duplicate lights for arena wrap around purpose
                light.x = light_x + x_mult;
                light.y = light_y + y_mult;
            end            
        end
        
        % left sensor
        sensorL = 0;
        robotToSensor.dx = sensor_location.left.x - robot.x;
        robotToSensor.dy = sensor_location.left.y - robot.y;
        sensorToTarget.dx = light.x - sensor_location.left.x;
        sensorToTarget.dy = light.y - sensor_location.left.y;
        
        for index=1:9
            targetDist = sqrt(sensorToTarget.dx(index)^2 + sensorToTarget.dy(index)^2);
            targetAngle = acos(sum([sensorToTarget.dx(index) sensorToTarget.dy(index)] .* ...
                [robotToSensor.dx robotToSensor.dy])/(targetDist*robot.radius));
            if(targetAngle < sensors.left.angle_range/2)
                sensorL = sensorL + 10/targetDist^2;
            end
        end
        
        % right sensor
        sensorR = 0;
        robotToSensor.dx = sensor_location.right.x - robot.x;
        robotToSensor.dy = sensor_location.right.y - robot.y;
        sensorToTarget.dx = light.x - sensor_location.right.x;
        sensorToTarget.dy = light.y - sensor_location.right.y;
        
        for index=1:9
            targetDist = sqrt(sensorToTarget.dx(index)^2 + sensorToTarget.dy(index)^2);
            targetAngle = acos(sum([sensorToTarget.dx(index) sensorToTarget.dy(index)] .* ...
                [robotToSensor.dx robotToSensor.dy])/(targetDist*robot.radius));
            if(targetAngle < sensors.right.angle_range/2)
                sensorR = sensorR + 10/targetDist^2;
            end
        end
        
        % sensors saturate at 1
        sensorL = min(sensorL,1);
        sensorR = min(sensorR,1);
        
        % update controller
        try
            [motorL motorR] = feval(get(Hndl.filename,'String'), sensorL, sensorR, DT);
            motorL = min(motorL, 1);
            motorL = max(motorL,-1);
            motorR = min(motorR, 1);
            motorR = max(motorR,-1);
        catch
            errString = sprintf('Error using function %s\n',get(Hndl.filename,'String'))
            error(errString);
        end
        
        % lowpass filter 
        TAU_MOTOR = 10;
        vel_left = vel_left + (-vel_left + robot.vmax*motorL)/TAU_MOTOR;
        vel_right = vel_right + (-vel_right + robot.vmax*motorR)/TAU_MOTOR;
        
        robot.v = (vel_left + vel_right)/2;
        
        robot.x = robot.x - robot.v * sin(robot.heading_angle) * DT;
        robot.y = robot.y + robot.v * cos(robot.heading_angle) * DT;
        
        % robot dynamics
        robot.heading_angle = robot.heading_angle - (vel_left - vel_right)/(2*robot.radius) * DT;
        
        if robot.x>arena_size 
            robot.x=-arena_size;
        end
        if robot.y>arena_size
            robot.y=-arena_size;
        end
        if robot.x<-arena_size 
            robot.x=arena_size;
        end
        if robot.y<-arena_size
            robot.y=arena_size;
        end
        
        
        robot.heading_angle = rem(robot.heading_angle,2*pi);
        if robot.heading_angle < 0
            robot.heading_angle =robot.heading_angle + 2*pi;    
        end
        
        % graphics update
        sensor_location = draw_robot(robot.x, robot.y, robot.heading_angle);
        
        t = t + DT;
        timeStr = sprintf('%3.2f', t);
        set(Hndl.timer,'String',timeStr);
        set(Hndl.score,'String',score);
        drawnow
    end
    set([Hndl.reset Hndl.start Hndl.print Hndl.close ],'Enable','on');
end


%==========================================================================
% end of robot
%==========================================================================




%--------------------------------------------------------------------------
% draw_light
%--------------------------------------------------------------------------
function draw_light(x, y, arena_size_init)
persistent hlight arena_size himage temp nx ny mx my
if nargin >2
    arena_size=arena_size_init;
    nx = 200;
    ny = 200;
    
    [mx,my] = meshgrid(1:nx, 1:ny);
end

figure(1);

x_mult = [-arena_size*2 0 arena_size*2; -arena_size*2 0 arena_size*2; -arena_size*2 0 arena_size*2];
y_mult = [arena_size*2 arena_size*2 arena_size*2; 0 0 0; -arena_size*2 -arena_size*2 -arena_size*2];

intensity = zeros(ny,nx);
for index = 1:9
    sx=((arena_size+x+x_mult(index))/arena_size/2*nx);
    sy=((arena_size+y+y_mult(index))/arena_size/2*ny);
    intensity = intensity + 3./sqrt( (mx-sx).^2 + (my-sy).^2);
end

if (isempty(himage) | ~ishandle(himage))
    himage=imagesc([-arena_size arena_size],[-arena_size arena_size], intensity);
else
    set(himage,'CData', intensity);
end

mymap = gray;

% turn off blue to make everything yellow
mymap(:,3) = 0;
colormap(mymap)

caxis([0 1])
hold on;

%--------------------------------------------------------------------------
% end of draw_light
%--------------------------------------------------------------------------


%--------------------------------------------------------------------------
% draw_robot
%--------------------------------------------------------------------------
function sensors = draw_robot(x,y,heading_angle)
global robot htrail xtrail ytrail trailCnt maxTrail;
persistent nverts verts linewidth color hrobot  

maxTrail = 1000;

if(nargin < 3)
    % initialize
    [verts linewidth color] = robot_coords(robot.radius);
    nverts = size(verts,1);
    figure(1); 
    for i=1:nverts
        hrobot(i) = plot(verts(i,1:2), verts(i,3:4),'color',color(i,:),'linewidth',linewidth(i)); 
        hold on;
    end
    
    sensors.left.x = verts(41,1);
    sensors.left.y = verts(41,3);  % left side
    sensors.right.x = verts(43,1);
    sensors.right.y = verts(43,3);
    
    % initilize robot trail
    xtrail = zeros(maxTrail,1);
    ytrail = zeros(maxTrail,1);
    htrail = plot(xtrail, ytrail,'w.');
    trailCnt = 0;
else 
    % redraw the robot in the new configuration
    tmp_verts = verts;
    
    % adjust heading - rotate the entire robot
    cosa = cos(heading_angle);
    sina = sin(heading_angle);
    new_verts(:,1:2) =  x + cosa * tmp_verts(:,1:2) - sina * tmp_verts(:,3:4); 
    new_verts(:,3:4) =  y + sina * tmp_verts(:,1:2) + cosa * tmp_verts(:,3:4);
    
    figure(1); 
    for i=1:nverts
        set(hrobot(i),'xdata',new_verts(i,1:2),'ydata',new_verts(i,3:4));
    end
    sensors.left.x = new_verts(41,1);
    sensors.left.y = new_verts(41,3);  % left side
    sensors.right.x = new_verts(43,1);
    sensors.right.y = new_verts(43,3);
    
    % update the trail
    trailCnt = trailCnt + 1;
    if(trailCnt == (maxTrail+1))trailCnt = 1; end
    xtrail(trailCnt) = x;
    ytrail(trailCnt) = y;
    set(htrail,'xdata',xtrail,'ydata',ytrail);
    
end
%--------------------------------------------------------------------------
% end of draw_robot
%--------------------------------------------------------------------------



%--------------------------------------------------------------------------
% robot_coords
%--------------------------------------------------------------------------
function [verts, width, color] = robot_coords(radius)
global sensors

c = 1;  % counter

angle = (0:.025:1)*pi*2;
xplot = radius * cos(angle);
yplot = radius * sin(angle);
for i=1:40
    verts(c,1) = xplot(i);
    verts(c,3) = yplot(i);
    verts(c,2) = xplot(i+1);
    verts(c,4) = yplot(i+1);
    width(c) = 1; color(c,:) = [1 1 0]; c=c+1;
end

% left sensor
l_sensor_origin_x = radius * cos(sensors.left.angle + pi/2);
l_sensor_origin_y = radius * sin(sensors.left.angle + pi/2);

verts(c,1) = l_sensor_origin_x;
verts(c,2) = radius*cos(sensors.left.angle + pi/2 - sensors.left.angle_range/2) + l_sensor_origin_x;
verts(c,3) = l_sensor_origin_y; 
verts(c,4) = radius*sin(sensors.left.angle + pi/2 - sensors.left.angle_range/2) + l_sensor_origin_y;
width(c) = 1; color(c,:) = [1 1 1]; c=c+1;

verts(c,1) = l_sensor_origin_x;
verts(c,2) = radius*cos(sensors.left.angle + pi/2 + sensors.left.angle_range/2) + l_sensor_origin_x;
verts(c,3) = l_sensor_origin_y; 
verts(c,4) = radius*sin(sensors.left.angle + pi/2 + sensors.left.angle_range/2) + l_sensor_origin_y;
width(c) = 1; color(c,:) = [1 1 1]; c=c+1;

% right sensor
r_sensor_origin_x = radius * cos(sensors.right.angle + pi/2);
r_sensor_origin_y = radius * sin(sensors.right.angle + pi/2);

verts(c,1) = r_sensor_origin_x;
verts(c,2) = radius*cos(sensors.right.angle + pi/2 - sensors.right.angle_range/2) + r_sensor_origin_x;
verts(c,3) = r_sensor_origin_y; 
verts(c,4) = radius*sin(sensors.right.angle + pi/2 - sensors.right.angle_range/2) + r_sensor_origin_y;
width(c) = 1; color(c,:) = [1 1 1]; c=c+1;

verts(c,1) = r_sensor_origin_x;
verts(c,2) = radius*cos(sensors.right.angle + pi/2 + sensors.right.angle_range/2) + r_sensor_origin_x;
verts(c,3) = r_sensor_origin_y; 
verts(c,4) = radius*sin(sensors.right.angle + pi/2 + sensors.right.angle_range/2) + r_sensor_origin_y;
width(c) = 1; color(c,:) = [1 1 1]; c=c+1;

%--------------------------------------------------------------------------
% end of robot_coords
%--------------------------------------------------------------------------
