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

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

% Last Modified by GUIDE v2.5 05-Apr-2012 19:34:57

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

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

% Update handles structure
guidata(hObject, handles);

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


% --- Outputs from this function are returned to the command line.
function varargout = trajectory_GSP_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 checkbox_Px.
function checkbox_Px_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Px (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 checkbox_Px


% --- Executes on button press in checkbox_Py.
function checkbox_Py_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Py (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 checkbox_Py


% --- 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)


% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (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 checkbox_Pz.
function checkbox_Pz_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Pz (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 checkbox_Pz


% --- Executes on button press in checkbox_Alpha.
function checkbox_Alpha_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Alpha (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 checkbox_Alpha


% --- Executes on button press in checkbox_Beta.
function checkbox_Beta_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Beta (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 checkbox_Beta


% --- Executes on button press in checkbox_Gamma.
function checkbox_Gamma_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_Gamma (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 checkbox_Gamma


% --- Executes on button press in pb_Moving.
function pb_Moving_Callback(hObject, eventdata, handles)
%PATH PLANNING = LINEAR PATH PLANNING
% step_size = 50;

step_size = getappdata(0, 'step_size' );
total_step_size = getappdata( 0, 'total_step_size' );
time = getappdata( 0, 'time' );

pop_Moving = get(handles.pop_Moving, 'Value' );
if ( pop_Moving == 1 ) 
    %%%%  Position and Orientation 
    DATA1 = getappdata(0, 'trajectory_path');
    DATA2 = getappdata(0, 'trajectory_angles');

else if ( pop_Moving == 2 ) 
        %%%%   Velocity
        DATA1 = getappdata( 0 , 'trajectory_path_velocity');
        DATA2 = getappdata( 0, 'trajectory_angle_acceleration');
    end
        
        if (pop_Moving == 3 ) 
            %%%%   Acceleration
            DATA1 = getappdata( 0, 'trajectory_path_acceleration');
            DATA2 = getappdata( 0, 'trajectory_angle_acceleration');
        end
end    


resolution = getappdata(0,'Resolution');
time = getappdata(0,'Time');

set(handles.axes11, 'visible', 'on' ); 
axes(handles.axes11);
cla;
title('Moving Platform Trajectory');
plot( 1:total_step_size , DATA1(:,1) );
grid on;xlabel('Time'); ylabel('X');
xlim_label = str2num(get(handles.axes11,'XTickLabel')) / resolution;
set(handles.axes11,'XTickLabel',xlim_label)

axes(handles.axes21);
cla;
plot( 1:total_step_size , DATA1(:,2));
grid on;xlabel('Time'); ylabel('Y');
xlim_label = str2num(get(handles.axes21,'XTickLabel')) / resolution;
set(handles.axes21,'XTickLabel',xlim_label)

axes(handles.axes31);
cla;
plot( 1:total_step_size , DATA1(:,3));
grid on;xlabel('Time'); ylabel('Z');
xlim_label = str2num(get(handles.axes31,'XTickLabel')) / resolution;
set(handles.axes31,'XTickLabel',xlim_label)

axes(handles.axes12);
cla;
plot( 1:total_step_size , DATA2(:,1));
grid on;xlabel('Time'); ylabel('\alpha');
xlim_label = str2num(get(handles.axes12,'XTickLabel')) / resolution;
set(handles.axes12,'XTickLabel',xlim_label)

axes(handles.axes22);
cla;
plot( 1:total_step_size , DATA2(:,2));
grid on;xlabel('Time'); ylabel('\beta');
xlim_label = str2num(get(handles.axes22,'XTickLabel')) / resolution;
set(handles.axes22,'XTickLabel',xlim_label)

axes(handles.axes32);
cla;
plot( 1:total_step_size , DATA2(:,3));
grid on;xlabel('Time'); ylabel('\gamma');
xlim_label = str2num(get(handles.axes32,'XTickLabel')) / resolution;
set(handles.axes32,'XTickLabel',xlim_label)

% --- Executes on button press in pb_Legs.
function pb_Legs_Callback(hObject, eventdata, handles)
%PATH PLANNING = LINEAR PATH PLANNING
% step_size = 50;

resolution = getappdata(0,'Resolution');
time = getappdata(0,'Time');


step_size = getappdata(0, 'step_size' );
total_step_size = getappdata( 0, 'total_step_size' );
time = getappdata( 0, 'time' );

pop_Legs = get(handles.pop_Legs, 'Value' );
if ( pop_Legs == 1 ) 
    %%%%  Position and Orientation 
    DATA = getappdata(0, 'trajectory_leg_length');

else if ( pop_Legs == 2 ) 
        %%%%   Velocity
        DATA = getappdata(0, 'q_velocity');
    end
        
        if (pop_Legs == 3 ) 
            %%%%   Acceleration
            DATA = getappdata(0, 'q_acceleration');
        end
end    


axes(handles.axes11);
cla;    
title('Moving Platform Trajectory');
plot( 1:total_step_size , DATA(:,1));
grid on;xlabel('Time'); ylabel('D1');
xlim_label = str2num(get(handles.axes11,'XTickLabel')) / resolution;
set(handles.axes11,'XTickLabel',xlim_label)

axes(handles.axes21);
cla;
plot( 1:total_step_size , DATA(:,2));
grid on;xlabel('Time'); ylabel('D2');
xlim_label = str2num(get(handles.axes21,'XTickLabel')) / resolution;
set(handles.axes21,'XTickLabel',xlim_label)

axes(handles.axes31);
cla;
plot( 1:total_step_size , DATA(:,3));
grid on;xlabel('Time'); ylabel('D3');
xlim_label = str2num(get(handles.axes31,'XTickLabel')) / resolution;
set(handles.axes31,'XTickLabel',xlim_label)

axes(handles.axes12);
cla;
plot( 1:total_step_size , DATA(:,4));
grid on;xlabel('Time'); ylabel('D4');
xlim_label = str2num(get(handles.axes12,'XTickLabel')) / resolution;
set(handles.axes12,'XTickLabel',xlim_label)

axes(handles.axes22);
cla;
plot( 1:total_step_size , DATA(:,5));
grid on;xlabel('Time'); ylabel('D5');
xlim_label = str2num(get(handles.axes22,'XTickLabel')) / resolution;
set(handles.axes22,'XTickLabel',xlim_label)

axes(handles.axes32);
cla;
plot( 1:total_step_size , DATA(:,6));
grid on;xlabel('Time'); ylabel('D6');
xlim_label = str2num(get(handles.axes32,'XTickLabel')) / resolution;
set(handles.axes32,'XTickLabel',xlim_label)


% --- Executes on button press in checkbox_D1.
function checkbox_D1_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D1 (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 checkbox_D1


% --- Executes on button press in checkbox_D2.
function checkbox_D2_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D2 (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 checkbox_D2


% --- Executes on button press in checkbox_D3.
function checkbox_D3_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D3 (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 checkbox_D3


% --- Executes on button press in checkbox_D4.
function checkbox_D4_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D4 (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 checkbox_D4


% --- Executes on button press in checkbox_D5.
function checkbox_D5_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D5 (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 checkbox_D5


% --- Executes on button press in checkbox_D6.
function checkbox_D6_Callback(hObject, eventdata, handles)
% hObject    handle to checkbox_D6 (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 checkbox_D6


% --- Executes on button press in pushbutton_OKP.
function pushbutton_OKP_Callback(hObject, eventdata, handles)
%PATH PLANNING = LINEAR PATH PLANNING
step_size = 50;
%step_size = getappdata(0, 'step_size' );

% trajectory_path = getappdata(0, 'trajectory_path');
% trajectory_angles = getappdata(0, 'trajectory_angles');
% trajectory_leg_length = getappdata(0, 'trajectory_leg_length');

pop_Moving = get(handles.pop_Moving, 'Value' );
if ( pop_Moving == 1 ) 
    %%%%  Position and Orientation 
    DATA1 = getappdata(0, 'trajectory_path');
    DATA2 = getappdata(0, 'trajectory_angles');

else if ( pop_Moving == 2 ) 
        %%%%   Velocity
        DATA1 = zeros(step_size,3);
        DATA2 = zeros(step_size,3);
    end
        
        if (pop_Moving == 3 ) 
            %%%%   Acceleration
            DATA1 = zeros(step_size,3);
            DATA2 = zeros(step_size,3);
        end
end    

set(handles.axes11, 'visible', 'on' ); 
axes(handles.axes11);
cla;
title('Moving Platform Trajectory');
plot( 1:step_size , DATA1(:,1) );
grid on;xlabel('time'); ylabel('X');


axes(handles.axes21);
cla;
plot( 1:step_size , DATA1(:,2));
grid on;xlabel('time'); ylabel('Y');

axes(handles.axes31);
cla;
plot( 1:step_size , DATA1(:,3));
grid on;xlabel('time'); ylabel('Z');

axes(handles.axes12);
cla;
plot( 1:step_size , DATA2(:,1));
grid on;xlabel('time'); ylabel('\alpha');

axes(handles.axes22);
cla;
plot( 1:step_size , DATA2(:,2));
grid on;xlabel('time'); ylabel('\Beta');

axes(handles.axes32);
cla;
plot( 1:step_size , DATA2(:,3));
grid on;xlabel('time'); ylabel('\Gamma');


% --- Executes on button press in pb_Moving_Position_Orientation.
function pb_Moving_Position_Orientation_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Moving_Position_Orientation (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 pushbutton_OKD.
function pushbutton_OKD_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton_OKD (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 pb_Moving_Velocity.
function pb_Moving_Velocity_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Moving_Velocity (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 pb_Moving_Acceleration.
function pb_Moving_Acceleration_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Moving_Acceleration (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 pb_Legs_Velocity.
function pb_Legs_Velocity_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Legs_Velocity (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 pb_Legs_Acceleration.
function pb_Legs_Acceleration_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Legs_Acceleration (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 pb_Legs_Position.
function pb_Legs_Position_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Legs_Position (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


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

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


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

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


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

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

% --- Executes during object creation, after setting all properties.
function pop_Moving_CreateFcn(hObject, eventdata, handles)
% hObject    handle to pop_Moving (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 && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- 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


% --- Executes on button press in togglebutton2.
function togglebutton2_Callback(hObject, eventdata, handles)
% hObject    handle to togglebutton2 (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 togglebutton2


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

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


% --- Executes during object creation, after setting all properties.
function pop_Legs_CreateFcn(hObject, eventdata, handles)
% hObject    handle to pop_Legs (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 && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes when user attempts to close figure1.
function figure1_CloseRequestFcn(hObject, eventdata, handles)
% hObject    handle to figure1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: delete(hObject) closes the figure
delete(hObject);
