function varargout = hw2gui(varargin)
%clear all; close all;
% HW2GUI M-file for hw2gui.fig
%      HW2GUI, by itself, creates a new HW2GUI or raises the existing
%      singleton*.
%
%      H = HW2GUI returns the handle to a new HW2GUI or the handle to
%      the existing singleton*.
%
%      HW2GUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in HW2GUI.M with the given input arguments.
%
%      HW2GUI('Property','Value',...) creates a new HW2GUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before hw2gui_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to hw2gui_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Copyright 2002-2003 The MathWorks, Inc.

% Edit the above text to modify the response to help hw2gui

% Last Modified by GUIDE v2.5 19-Sep-2007 16:43:39

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @hw2gui_OpeningFcn, ...
                   'gui_OutputFcn',  @hw2gui_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before hw2gui is made visible.
function hw2gui_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to hw2gui (see VARARGIN)

% Choose default command line output for hw2gui
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% This sets up the initial plot - only do when we are invisible
% so window can get raised using hw2gui.
if strcmp(get(hObject,'Visible'),'off')
    %plot(rand(5));
end

% UIWAIT makes hw2gui wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = hw2gui_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
axes(handles.axes1);
cla;

popup_sel_index = get(handles.popupmenu1, 'Value');
switch popup_sel_index
    case 1
        plot(rand(5));
    case 2
        plot(sin(1:0.01:25.99));
    case 3
        bar(1:.5:10);
    case 4
        plot(membrane);
    case 5
        surf(peaks);
end


% --------------------------------------------------------------------
function FileMenu_Callback(hObject, eventdata, handles)
% hObject    handle to FileMenu (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --------------------------------------------------------------------
function OpenMenuItem_Callback(hObject, eventdata, handles)
% hObject    handle to OpenMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global obstacles; global startloc; global endloc;
[file,path] = uigetfile('*.mat','Choose Environment File');
filename = strcat(path, file);
load(filename);
hold on;
fill([0,0,10,10],[0,10,10,0],'w');
for m = 1:length(obstacles)
    xVals = obstacles(m).xVals;
    yVals = obstacles(m).yVals;
    fill(xVals,yVals,'b');
end
plot(startloc(1),startloc(2),'go');
plot(endloc(1),endloc(2),'rx');
set(handles.pp,'Enable','on');
set(handles.en,'Enable','on');
set(handles.pushbutton4,'Enable','off');
set(handles.togglebutton1,'Value',0);


% --------------------------------------------------------------------
function PrintMenuItem_Callback(hObject, eventdata, handles)
% hObject    handle to PrintMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
printdlg(handles.figure1)

% --------------------------------------------------------------------
function CloseMenuItem_Callback(hObject, eventdata, handles)
% hObject    handle to CloseMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
selection = questdlg(['Close ' get(handles.figure1,'Name') '?'],...
                     ['Close ' get(handles.figure1,'Name') '...'],...
                     'Yes','No','Yes');
if strcmp(selection,'No')
    return;
end

delete(handles.figure1)
clear all; close all;

% --- Executes on selection change in popupmenu1.
function popupmenu1_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = get(hObject,'String') returns popupmenu1 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu1


% --- Executes during object creation, after setting all properties.
function popupmenu1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

set(hObject, 'String', {'plot(rand(5))', 'plot(sin(1:0.01:25))', 'bar(1:.5:10)', 'plot(membrane)', 'surf(peaks)'});



% --------------------------------------------------------------------
function nse_Callback(hObject, eventdata, handles)
% hObject    handle to nse (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.text1, 'String','Choose start and end locations');
change_se;

function change_se
global startloc; global endloc;
plot(startloc(1),startloc(2),'wo')
plot(endloc(1),endloc(2),'wx');
[startloc(1),startloc(2),button] = ginput(1);
plot(startloc(1),startloc(2),'go');
[endloc(1),endloc(2),button] = ginput(1);
plot(endloc(1),endloc(2),'rx');

% --------------------------------------------------------------------
function pp_Callback(hObject, eventdata, handles)
% hObject    handle to pp (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)




% --- Executes on button press in pushbutton4.
function pushbutton4_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
hold on
set(handles.pp,'Enable','on');
set(handles.en,'Enable','on');
set(handles.pushbutton4,'Enable','off');
set(handles.text1, 'String', 'Choose start and end locations');
global startloc; global endloc; global obstacles; global obstacles_nobloat;
obstacles = []; obstacles_nobloat = [];
HW2b



% --------------------------------------------------------------------
function ao_Callback(hObject, eventdata, handles)
% hObject    handle to ao (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.text1,'String','Left click to select vertices. Right click to select last vertex');
addObstacle;

% --------------------------------------------------------------------
function bg2_Callback(hObject, eventdata, handles)
% hObject    handle to bg2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.text1,'String','Bug 2 Planning');
status = bug_2;
set(handles.text1, 'String', status);



% --------------------------------------------------------------------
function ce_Callback(hObject, eventdata, handles)
% hObject    handle to ce (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global obstacles;
fill([0,0,10,10],[0,10,10,0],'w');
obstacles = [];
set(handles.text1, 'String','Choose Start and End locations');
change_se;


% --------------------------------------------------------------------
function pot_path_Callback(hObject, eventdata, handles)
% hObject    handle to pot_path (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.text1,'String','Potential Path Planning...');
success = potentialPath;
if success == 1
    set(handles.text1,'String','Success!');
else
    set(handles.text1,'String','Pathfinding Failed');
end


% --------------------------------------------------------------------
function SaveMenuOption_Callback(hObject, eventdata, handles)
% hObject    handle to SaveMenuOption (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global startloc; global endloc; global obstacles;
[file,path] = uiputfile('*.mat','Save Environment As');
filename = strcat(path, file);
save(filename, 'obstacles','startloc','endloc');



% --- Executes on button press in togglebutton1.
function togglebutton1_Callback(hObject, eventdata, handles)
% hObject    handle to togglebutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of togglebutton1
global startloc; global endloc; global obstacles; global obstacles_nobloat;
bot_vertices = [.25 .5 .25 0 .25; .5 .25 0 .25 .5];
xVals = bot_vertices(1,:);
yVals = bot_vertices(2,:);
bot_edges = [];
for n = 1:length(xVals)-1
    cur_x_dir = xVals(n) - xVals(n+1);
    cur_y_dir = yVals(n) - yVals(n+1);
        
    dir = 1;
    %support for counter-clockwise obstacles missing!
        
    cur_angle = atan2(cur_y_dir,cur_x_dir);
    cur_angle_deg = ((cur_angle+pi)/(pi)*180);
    cur_norm_angle = cur_angle_deg + (dir*90);
    if cur_norm_angle >= 360
        cur_norm_angle = cur_norm_angle - 360;
    end
    if cur_norm_angle < 0
        cur_norm_angle = cur_norm_angle + 360;
    end
    edge = struct('vertex1',[xVals(n);yVals(n)],'vertex2',[xVals(n+1);yVals(n+1)],...
            'translation',[xVals(n)-xVals(n+1);yVals(n)-yVals(n)],'number',n,...
            'angle',cur_angle_deg,'normal',cur_norm_angle,'type',-1);
    bot_edges = [bot_edges; edge];
end
switch get(hObject,'Value')
    case 1
        %button on: polygon robot.  bloat obstacles.
        %save obstacles to obstacles_nonbloat
        obstacles_nobloat = obstacles;
        set(handles.ao,'Enable','off');
        for m = 1:length(obstacles)
            xVals = obstacles(m).xVals;
            yVals = obstacles(m).yVals;
            fill(xVals,yVals,'w');
        end
        %generate bloated obstacles and save
        b_obstacles = bloat_obstacles(obstacles,bot_edges);
        obstacles
        b_obstacles
        obstacles = b_obstacles;
    case 0
        %button off: point robot.  unbloat obstacles.
        %erase bloated obstacles.
        for m = 1:length(obstacles)
            xVals = obstacles(m).xVals;
            yVals = obstacles(m).yVals;
            fill(xVals,yVals,'w');
        end
        %set obstacles variable to original unbloated obstacles.
        obstacles = obstacles_nobloat;
        obstacles_nobloat = [];
        set(handles.ao,'Enable','on');
end
%draw start/end locations
plot(startloc(1),startloc(2),'go');
plot(endloc(1),endloc(2),'rx');
%draw obstacles
for m = 1:length(obstacles)
    xVals = obstacles(m).xVals;
    yVals = obstacles(m).yVals;
    fill(xVals,yVals,'b');
end