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

% Last Modified by GUIDE v2.5 25-Apr-2005 14:53:13

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

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

handles.A = [];
handles.B = [];
handles.C = [];
handles.D = [];
handles.E = [];

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


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

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

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

% Update handles structure
guidata(hObject, handles);

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


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



% Parameter Functions

function q11_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
Calculate(hObject,handles);
guidata(hObject,handles);

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

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

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

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

function q32_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
Calculate(hObject,handles);
guidata(hObject,handles);


% Calculate Function
function Calculate_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
Calculate(hObject,handles);
guidata(hObject,handles);

function Calculate(hObject,handles)

eval('RiseParameters');

q12=str2num(get(handles.q12,'String'))*(pi/180);
q22=str2num(get(handles.q22,'String'))*(pi/180);
q32=str2num(get(handles.q32,'String'))*(pi/180);
qCrank = [q12 q22 q32];



guidata(hObject,handles);
plotModel(hObject,handles);
plotLeg1(hObject,handles);
plotLeg2(hObject,handles);
plotLeg3(hObject,handles);

guidata(hObject,handles);

%Clear Function
function Clear_Callback(hObject, eventdata, handles)
handles=guidata(hObject);
axes(handles.Leg1)
cla
axes(handles.Leg2)
cla
axes(handles.Leg3)
cla

axes(handles.Model)
cla

guidata(hObject,handles);

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

function plotLeg1(hObject,handles)
handles=guidata(hObject);
axes(handles.Leg1)

guidata(hObject,handles);

function plotLeg2(hObject,handles)
handles=guidata(hObject);
axes(handles.Leg2)

guidata(hObject,handles);

function plotLeg3(hObject,handles)
handles=guidata(hObject);
axes(handles.Leg3)

guidata(hObject,handles);

function plotModel(hObject,handles)
handles=guidata(hObject);
axes(handles.Model)
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]);
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]);
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]);
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]);
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]);
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]);


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]);


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 on
else
    rotate3d off
end
guidata(hObject,handles);

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

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



