function varargout = Cbot3DGUI(varargin)
% CBOT3DGUI M-file for Cbot3DGUI.fig
%      CBOT3DGUI, by itself, creates a new CBOT3DGUI or raises the existing
%      singleton*.
%
%      H = CBOT3DGUI returns the handle to a new CBOT3DGUI or the handle to
%      the existing singleton*.
%
%      CBOT3DGUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in CBOT3DGUI.M with the given input arguments.
%
%      CBOT3DGUI('Property','Value',...) creates a new CBOT3DGUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before Cbot3DGUI_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to Cbot3DGUI_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 Cbot3DGUI

% Last Modified by GUIDE v2.5 11-May-2005 18:49:02

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @Cbot3DGUI_OpeningFcn, ...
                   'gui_OutputFcn',  @Cbot3DGUI_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 Cbot3DGUI is made visible.
function Cbot3DGUI_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 Cbot3DGUI (see VARARGIN)

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

handles.ConfigFile = 'Cbot3D_Config';
handles.SaveFile = 'Cbot3D_Save';
handles.q = linspace(0,pi/2,100)';
handles.F = 0;
handles.N = 0;
handles.L = 0;
handles.Lp = 0;
handles.limit = 0;

axes(handles.Model)
axis auto
hold on
title('Model')

axes(handles.Body)
axis auto
hold on
title('Body Forces')
set(handles.Body,'Color',[.5 .5 .5]);

axes(handles.Stability)
axis auto
hold on
title('Stability')
set(handles.Stability,'Color',[.5 .5 .5]);

axes(handles.Lateral)
axis auto
hold on
title('Lateral')
set(handles.Lateral,'Color',[.5 .5 .5]);

axes(handles.Tangential)
axis auto
hold on
title('Tangential')
set(handles.Tangential,'Color',[.5 .5 .5]);

axes(handles.Normal)
axis auto
hold on
title('Normal')
set(handles.Normal,'Color',[.5 .5 .5]);

% Update handles structure
guidata(hObject, handles);

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


% --- Outputs from this function are returned to the command line.
function varargout = Cbot3DGUI_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;

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

% Hints: get(hObject,'String') returns contents of m as text
%        str2double(get(hObject,'String')) returns contents of m as a
%        double

% Creation Functions
function m_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function h_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function x1_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function y1_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function x2_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function y2_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function x3_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function y3_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function mu1_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function a1_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function mu2_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function a2_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function mu3_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function a3_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function Load_text_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function Save_text_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function angle_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function Save2_text_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');


% Parameter Functions
function m_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function h_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function x1_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function y1_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function x2_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function y2_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function x3_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function y3_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function mu1_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function a1_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function mu2_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function a2_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function mu3_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

function a3_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

%Load and Save Functions

function Load_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
rehash
string=strcat(get(handles.Load_text,'String'),'.m');
if(fopen(string,'r')==-1)
    makeConfigFile(hObject,handles);
end
rehash
eval(get(handles.Load_text,'String'));
set(handles.m,'String',num2str(m));
set(handles.h,'String',num2str(-z1));
set(handles.x1,'String',num2str(x1));
set(handles.y1,'String',num2str(y1));
set(handles.x2,'String',num2str(x2));
set(handles.y2,'String',num2str(y2));
set(handles.x3,'String',num2str(x3));
set(handles.y3,'String',num2str(y3));
set(handles.mu1,'String',num2str(mu1));
set(handles.a1,'String',num2str(alpha1));
set(handles.mu2,'String',num2str(mu2));
set(handles.a2,'String',num2str(alpha2));
set(handles.mu3,'String',num2str(mu3));
set(handles.a3,'String',num2str(alpha3));
handles.ConfigFile = get(handles.Load_text,'String');
guidata(hObject,handles);
plotModel(hObject,handles);
guidata(hObject,handles);

function Save_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
clear handles.ConfigFile;
handles.ConfigFile=get(handles.Save_text,'String');
guidata(hObject,handles);
makeConfigFile(hObject,handles);
plotModel(hObject,handles);
guidata(hObject,handles);

function Load_text_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
guidata(hObject,handles);

function Save_text_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
guidata(hObject,handles);

function Save2_text_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
guidata(hObject,handles);


% Calculate Function
function Calculate_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
handles.ConfigFile = get(handles.Save_text,'String');
handles.SaveFile = strcat(get(handles.Save2_text,'String'),'.m');
guidata(hObject,handles);
makeConfigFile(hObject,handles);
rehash
[handles.F,handles.L,handles.N,handles.Lp]=Cbot3D(handles.q,handles.ConfigFile,handles.SaveFile);
handles.limit=NaN;
for i=2:size(handles.L(4,:),2)
    if(handles.L(4,i)<0)
        m = (handles.L(4,i)-handles.L(4,i-1))/(handles.q(i)-handles.q(i-1));
        handles.limit=handles.q(i-1)+((0-handles.L(4,i-1))/m);
        break;
    end
end
guidata(hObject,handles);
plotModel(hObject,handles);
plotLateral(hObject,handles);
plotTangential(hObject,handles);
plotNormal(hObject,handles);
plotStability(hObject,handles);
plotBody(hObject,handles);
guidata(hObject,handles);

%Clear Function
function Clear_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
axes(handles.Lateral)
cla
axes(handles.Tangential)
cla
axes(handles.Normal)
cla
axes(handles.Stability)
cla
axes(handles.Model)
cla
axes(handles.Body)
cla
set(handles.angle,'Value',0);
guidata(hObject,handles);

%-----------------------
%HELPER FUNCTIONS
%-----------------------

function makeConfigFile(hObject,handles)
string=strcat(handles.ConfigFile,'.m');
if(fopen(string,'r')~=-1)
    delete(string);
end
File=fopen(string,'w+');
fprintf(File,'m=%d;\r\n',str2num(get(handles.m,'String')));
fprintf(File,'x1=%d;\r\n',str2num(get(handles.x1,'String')));
fprintf(File,'y1=%d;\r\n',str2num(get(handles.y1,'String')));
fprintf(File,'z1=%d;\r\n',-str2num(get(handles.h,'String')));
fprintf(File,'x2=%d;\r\n',str2num(get(handles.x2,'String')));
fprintf(File,'y2=%d;\r\n',str2num(get(handles.y2,'String')));
fprintf(File,'z2=%d;\r\n',-str2num(get(handles.h,'String')));
fprintf(File,'x3=%d;\r\n',str2num(get(handles.x3,'String')));
fprintf(File,'y3=%d;\r\n',str2num(get(handles.y3,'String')));
fprintf(File,'z3=%d;\r\n',-str2num(get(handles.h,'String')));
fprintf(File,'mu1=%d;\r\n',str2num(get(handles.mu1,'String')));
fprintf(File,'alpha1=%d;\r\n',str2num(get(handles.a1,'String')));
fprintf(File,'mu2=%d;\r\n',str2num(get(handles.mu2,'String')));
fprintf(File,'alpha2=%d;\r\n',str2num(get(handles.a2,'String')));
fprintf(File,'mu3=%d;\r\n',str2num(get(handles.mu3,'String')));
fprintf(File,'alpha3=%d;\r\n',str2num(get(handles.a3,'String')));
fclose(File);

function plotLateral(hObject,handles)
handles=guidata(hObject);
axes(handles.Lateral)
axis auto
set(handles.Lateral,'XLim',[handles.q(1) handles.q(size(handles.q,1))]);
plot(handles.q,handles.F(1,:),'b-',handles.q,handles.F(4,:),'r-.',handles.q,handles.F(7,:),'y-.','LineWidth',2)
y=get(handles.Lateral,'YLim');
line([handles.limit handles.limit],[y(1) y(2)],'Color',[0 0 0])
line([handles.q(1) handles.q(size(handles.q,1))],[0 0],'Color',[0 0 0])
guidata(hObject,handles);

function plotTangential(hObject,handles)
handles=guidata(hObject);
axes(handles.Tangential)
axis auto
set(handles.Tangential,'XLim',[handles.q(1) handles.q(size(handles.q,1))]);
plot(handles.q,handles.F(2,:),'b-',handles.q,handles.F(5,:),'r-.',handles.q,handles.F(8,:),'y-.','LineWidth',2)
y=get(handles.Tangential,'YLim');
line([handles.limit handles.limit],[y(1) y(2)],'Color',[0 0 0])
line([handles.q(1) handles.q(size(handles.q,1))],[0 0],'Color',[0 0 0])
guidata(hObject,handles);

function plotNormal(hObject,handles)
handles=guidata(hObject);
axes(handles.Normal)
axis auto
set(handles.Normal,'XLim',[handles.q(1) handles.q(size(handles.q,1))]);
plot(handles.q,handles.F(3,:),'b-',handles.q,handles.F(6,:),'r-.',handles.q,handles.F(9,:),'y-.','LineWidth',2)
y=get(handles.Normal,'YLim');
line([handles.limit handles.limit],[y(1) y(2)],'Color',[0 0 0])
line([handles.q(1) handles.q(size(handles.q,1))],[0 0],'Color',[0 0 0])
guidata(hObject,handles);

function plotStability(hObject,handles)
handles=guidata(hObject);
axes(handles.Stability)
axis auto
set(handles.Stability,'XLim',[handles.q(1) handles.q(size(handles.q,1))]);
plot(handles.q,handles.L(4,:),'b-','LineWidth',2)
string=sprintf('Stability, Limit = %.2f deg',handles.limit*(180/pi));
title(string)
line([handles.q(1) handles.q(size(handles.q,1))],[0 0],'Color',[0 0 0])
y=get(handles.Stability,'YLim');
line([handles.limit handles.limit],[y(1) y(2)],'Color',[0 0 0])
guidata(hObject,handles);

function plotModel(hObject,handles)
handles=guidata(hObject);

rehash
eval(handles.ConfigFile);
axes(handles.Model)
xmax=1.5*max([x1 x2 x3]);
xmin=1.5*min([x1 x2 x3]);
ymax=1.5*max([y1 y2 y3]);
ymin=1.5*min([y1 y2 y3]);
patch([xmax; xmax; xmin; xmin],[ymax; ymin; ymin; ymax],[0; 0; 0; 0;],'FaceColor',[.5 .5 .5]);
plot3(0,0,-z1,'ko','MarkerSize',15,'MarkerFaceColor',[0 0 0]);
plot3(x1,y1,0,'bo','MarkerSize',15,'MarkerFaceColor',[0 0 1]);
plot3(x2,y2,0,'ro','MarkerSize',15,'MarkerFaceColor',[1 0 0]);
plot3(x3,y3,0,'yo','MarkerSize',15,'MarkerFaceColor',[1 1 0]);
line([0 0 0; x1 x2 x3],[0 0 0; y1 y2 y3],[-z1 -z2 -z3; 0 0 0],'Color',[0 0 0],'LineWidth',2);
axis equal
axis tight
view(45,45);

guidata(hObject,handles);

function plotBody(hObject,handles)
handles=guidata(hObject);


% rehash
% eval(handles.ConfigFile);
% axes(handles.Body);
% plot(x1,y1,'bo','MarkerSize',15,'MarkerFaceColor',[0 0 1]);
% plot(x2,y2,'ro','MarkerSize',15,'MarkerFaceColor',[1 0 0]);
% plot(x3,y3,'go','MarkerSize',15,'MarkerFaceColor',[0 1 0]);
% index=find(handles.q<=handles.angle,1,'last');
% 
% if(handles.Lp(1,index)>0)
%     line([x1; x2],[y1; y2],'LineWidth',2,'Color',[0 0 1]);
% elseif(handles.Lp(1,index)<0)
%     line([x1; x2],[y1; y2],'LineWidth',2,'Color',[1 0 0]);
% else
%     line([x1; x2],[y1; y2],'LineWidth',2,'Color',[0 1 0]);
% end
% if(handles.Lp(2,index)>0)
%     line([x2; x3],[y2; y3],'LineWidth',2,'Color',[0 0 1]);
% elseif(handles.Lp(2,index)<0)
%     line([x2; x3],[y2; y3],'LineWidth',2,'Color',[1 0 0]);
% else
%     line([x2; x3],[y2; y3],'LineWidth',2,'Color',[0 1 0]);
% end
% if(handles.Lp(3,index)>0)
%     line([x3; x1],[y3; y1],'LineWidth',2,'Color',[0 0 1]);
% elseif(handles.Lp(3,index)<0)
%     line([x3; x1],[x3; y1],'LineWidth',2,'Color',[1 0 0]);
% else
%     line([x3; x1],[y3; y1],'LineWidth',2,'Color',[0 1 0]);
% end

axes(handles.Body)
axis auto
set(handles.Body,'XLim',[handles.q(1) handles.q(size(handles.q,1))]);
plot(handles.q,handles.Lp(1,:),'Color',[.75 0 .75],'LineStyle','-','LineWidth',2)
plot(handles.q,handles.Lp(2,:),'Color',[1 .5 0],'LineStyle','-.','LineWidth',2)
plot(handles.q,handles.Lp(3,:),'Color',[0 1 0],'LineStyle','-.','LineWidth',2)
y=get(handles.Body,'YLim');
line([handles.limit handles.limit],[y(1) y(2)],'Color',[0 0 0])
line([handles.q(1) handles.q(size(handles.q,1))],[0 0],'Color',[0 0 0])

guidata(hObject,handles);

% Plot Manipulation
function Z_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
z=get(hObject,'Value');
if(z)
    zoom on
else
    zoom off
end
guidata(hObject,handles);

function R_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
r=get(hObject,'Value');
if(r)
    rotate3d(handles.Model);
else
    rotate3d off
end
guidata(hObject,handles);

function angle_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
persistent angle;
if( isempty(angle) )
    angle=get(hObject,'Value');
end
axes(handles.Model);
rotate(get(handles.Model,'Children'),[1 0 0],-angle+get(hObject,'Value'),[0 0 0]);
angle=get(hObject,'Value');
axis equal
view(45,45);

guidata(hObject,handles);

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

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
