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

% Last Modified by GUIDE v2.5 12-May-2005 16:43:12

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

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

handles.ConfigFile = 'Rise_Config';
handles.SaveFile = 'Rise_Save';
handles.q = linspace(0,pi/2,100)';
handles.F = 0;
handles.N = 0;
handles.L = 0;
handles.Lp = 0;
handles.limit = 0;
handles.A = [];
handles.B = [];
handles.C = [];
handles.D = [];
handles.E = [];

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 RiseForceControlGUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = RiseForceControlGUI_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 q11_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function q12_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function q21_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function q22_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function q31_CreateFcn(hObject, eventdata, handles)
set(hObject,'BackgroundColor','white');
function q32_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 q11_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
plotModel(hObject,handles);
guidata(hObject,handles);

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

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

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

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

function q32_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.q11,'String',num2str(q11*(180/pi)));
set(handles.q12,'String',num2str(q12*(180/pi)));
set(handles.q21,'String',num2str(q21*(180/pi)));
set(handles.q22,'String',num2str(q22*(180/pi)));
set(handles.q31,'String',num2str(q31*(180/pi)));
set(handles.q32,'String',num2str(q32*(180/pi)));
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]=RiseForceControl(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);

%Update coordinates
eval(handles.ConfigFile);
[Ar Br Cr Dr Er]=RiseKinematics(q11,q12,q21,q22,q31,q32);
p1=Er(:,1); p2=Er(:,2); p3=Er(:,3);
x1=sprintf('%.2f',p1(1));
y1=sprintf('%.2f',p1(2));
z1=sprintf('%.2f',p1(3));
x2=sprintf('%.2f',p2(1));
y2=sprintf('%.2f',p2(2));
z2=sprintf('%.2f',p2(3));
x3=sprintf('%.2f',p3(1));
y3=sprintf('%.2f',p3(2));
z3=sprintf('%.2f',p3(3));
set(handles.x1,'String',x1);
set(handles.y1,'String',y1);
set(handles.z1,'String',z1);
set(handles.x2,'String',x2);
set(handles.y2,'String',y2);
set(handles.z2,'String',z2);
set(handles.x3,'String',x3);
set(handles.y3,'String',y3);
set(handles.z3,'String',z3);
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,'q11=%d;\r\n',str2num(get(handles.q11,'String'))*(pi/180));
fprintf(File,'q12=%d;\r\n',str2num(get(handles.q12,'String'))*(pi/180));
fprintf(File,'q21=%d;\r\n',str2num(get(handles.q21,'String'))*(pi/180));
fprintf(File,'q22=%d;\r\n',str2num(get(handles.q22,'String'))*(pi/180));
fprintf(File,'q31=%d;\r\n',str2num(get(handles.q31,'String'))*(pi/180));
fprintf(File,'q32=%d;\r\n',str2num(get(handles.q32,'String'))*(pi/180));
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);
axes(handles.Model)
[az, el]=view;
cla

eval('RiseParameters');

xvertices=[-w/2; -w/2; -w/2; -w/2;  w/2;  w/2;  w/2;  w/2];
yvertices=[-l/2; -l/2;  l/2;  l/2;  l/2;  l/2; -l/2; -l/2];
zvertices=[-h/2;  h/2;  h/2; -h/2; -h/2;  h/2;  h/2; -h/2];
xlabel('X');
ylabel('Y');
zlabel('Z');
patch([-w/2 -w/2 -w/2 -w/2],[-l/2 -l/2 l/2 l/2],[-h/2 h/2 h/2 -h/2],[.75 .75 .75],'FaceAlpha',.5);
patch([w/2 w/2 w/2 w/2],[-l/2 -l/2 l/2 l/2],[-h/2 h/2 h/2 -h/2],[.75 .75 .75],'FaceAlpha',.5);
patch([-w/2 w/2 w/2 -w/2],[l/2 l/2 l/2 l/2],[-h/2 -h/2 h/2 h/2],[.75 .75 .75],'FaceAlpha',.5);
patch([-w/2 w/2 w/2 -w/2],[-l/2 -l/2 -l/2 -l/2],[-h/2 -h/2 h/2 h/2],[.75 .75 .75],'FaceAlpha',.5);
patch([-w/2 w/2 w/2 -w/2],[-l/2 -l/2 l/2 l/2],[h/2 h/2 h/2 h/2],[.75 .75 .75],'FaceAlpha',.5);
patch([-w/2 w/2 w/2 -w/2],[-l/2 -l/2 l/2 l/2],[-h/2 -h/2 -h/2 -h/2],[.75 .75 .75],'FaceAlpha',.5);

q11=str2num(get(handles.q11,'String'))*(pi/180);
q12=str2num(get(handles.q12,'String'))*(pi/180);
q21=str2num(get(handles.q21,'String'))*(pi/180);
q22=str2num(get(handles.q22,'String'))*(pi/180);
q31=str2num(get(handles.q31,'String'))*(pi/180);
q32=str2num(get(handles.q32,'String'))*(pi/180);

J1=RiseLegJacobian('Right',q11,q12);
J2=RiseLegJacobian('Left',q21,q22);
J3=RiseLegJacobian('Right',q31,q32);
[A B C D E]=RiseKinematics(q11,q12,q21,q22,q31,q32);

A1=A(:,1);
B1=B(:,1);
C1=C(:,1);
D1=D(:,1);
E1=E(:,1);

A2=A(:,2);
B2=B(:,2);
C2=C(:,2);
D2=D(:,2);
E2=E(:,2);

A3=A(:,3);
B3=B(:,3);
C3=C(:,3);
D3=D(:,3);
E3=E(:,3);

s=1;

line([A1(1) B1(1)],[A1(2) B1(2)],[A1(3) B1(3)],'Color',[0 0 1],'LineWidth',2);
line([B1(1) C1(1)],[B1(2) C1(2)],[B1(3) C1(3)],'Color',[1 0 0],'LineWidth',2);
line([C1(1) D1(1)],[C1(2) D1(2)],[C1(3) D1(3)],'Color',[0 1 0],'LineWidth',2);
line([C1(1) E1(1)],[C1(2) E1(2)],[C1(3) E1(3)],'Color',[0 0 1],'LineWidth',2);
line([D1(1) A1(1)],[D1(2) A1(2)],[D1(3) A1(3)],'Color',[1 0 0],'LineWidth',2);

line([E1(1) E1(1)+(J1(1,1)*s)],[E1(2) E1(2)+(J1(2,1)*s)],[E1(3) E1(3)+(J1(3,1)*s)],'Color',[0 0 0],'LineWidth',3);
line([E1(1) E1(1)+(J1(1,2)*s)],[E1(2) E1(2)+(J1(2,2)*s)],[E1(3) E1(3)+(J1(3,2)*s)],'Color',[0 0 0],'LineWidth',3);

line([A2(1) B2(1)],[A2(2) B2(2)],[A2(3) B2(3)],'Color',[0 0 1],'LineWidth',2);
line([B2(1) C2(1)],[B2(2) C2(2)],[B2(3) C2(3)],'Color',[1 0 0],'LineWidth',2);
line([C2(1) D2(1)],[C2(2) D2(2)],[C2(3) D2(3)],'Color',[0 1 0],'LineWidth',2);
line([C2(1) E2(1)],[C2(2) E2(2)],[C2(3) E2(3)],'Color',[0 0 1],'LineWidth',2);
line([D2(1) A2(1)],[D2(2) A2(2)],[D2(3) A2(3)],'Color',[1 0 0],'LineWidth',2);

line([E2(1) E2(1)+(J2(1,1)*s)],[E2(2) E2(2)+(J2(2,1)*s)],[E2(3) E2(3)+(J2(3,1)*s)],'Color',[0 0 0],'LineWidth',3);
line([E2(1) E2(1)+(J2(1,2)*s)],[E2(2) E2(2)+(J2(2,2)*s)],[E2(3) E2(3)+(J2(3,2)*s)],'Color',[0 0 0],'LineWidth',3);

line([A3(1) B3(1)],[A3(2) B3(2)],[A3(3) B3(3)],'Color',[0 0 1],'LineWidth',2);
line([B3(1) C3(1)],[B3(2) C3(2)],[B3(3) C3(3)],'Color',[1 0 0],'LineWidth',2);
line([C3(1) D3(1)],[C3(2) D3(2)],[C3(3) D3(3)],'Color',[0 1 0],'LineWidth',2);
line([C3(1) E3(1)],[C3(2) E3(2)],[C3(3) E3(3)],'Color',[0 0 1],'LineWidth',2);
line([D3(1) A3(1)],[D3(2) A3(2)],[D3(3) A3(3)],'Color',[1 0 0],'LineWidth',2);

line([E3(1) E3(1)+(J3(1,1)*s)],[E3(2) E3(2)+(J3(2,1)*s)],[E3(3) E3(3)+(J3(3,1)*s)],'Color',[0 0 0],'LineWidth',3);
line([E3(1) E3(1)+(J3(1,2)*s)],[E3(2) E3(2)+(J3(2,2)*s)],[E3(3) E3(3)+(J3(3,2)*s)],'Color',[0 0 0],'LineWidth',3);

axis([-.2 .2 -.2 .2 -.2 .2]);

view(az, el);

guidata(hObject,handles);

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

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
