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 28-Dec-2011 21:25:42

% 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)
% hObject    handle to pb_Moving (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% for i=1:1:6
%      eval( sprintf( 'set(handles.checkbox_D%d, ''visible'', ''off'')',i ) );
% end
% set(handles.checkbox_Px,    'visible', 'on', 'Value', 0);
% set(handles.checkbox_Py,    'visible', 'on', 'Value', 0);
% set(handles.checkbox_Pz,    'visible', 'on', 'Value', 0);
% set(handles.checkbox_Alpha, 'visible', 'on', 'Value', 0);
% set(handles.checkbox_Beta,  'visible', 'on', 'Value', 0);
% set(handles.checkbox_Gamma, 'visible', 'on', 'Value', 0);
% 
% set(handles.text_select_parameters, 'visible', 'on');
% set(handles.frame1, 'visible', 'on');
% set(handles.pushbutton_OKP, 'visible', 'on');
% set(handles.pushbutton_OKD, 'visible', 'off');
% 
% 
% set(handles.pb_Moving_Position_Orientation,    'visible', 'on');
% set(handles.pb_Moving_Velocity,    'visible', 'on');
% set(handles.pb_Moving_Acceleration,    'visible', 'on');
% 
% set(handles.pb_Legs_Position,    'visible', 'off');
% set(handles.pb_Legs_Velocity,    'visible', 'off');
% set(handles.pb_Legs_Acceleration,    'visible', 'off');


%PATH PLANNING = LINEAR PATH PLANNING
step_size = 50;
%step_size = getappdata(0, 'step_size' );

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

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



axes(handles.axes21);
title('Moving Platform Trajectory');
cla;
plot( 1:step_size , trajectory_path(:,2));
grid on;xlabel('time'); ylabel('Y');

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

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

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

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

% --- Executes on button press in pb_Legs.
function pb_Legs_Callback(hObject, eventdata, handles)

%PATH PLANNING = LINEAR PATH PLANNING
step_size = 50;
%step_size = getappdata(0, 'step_size' );

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



axes(handles.axes11);
cla;    
title('Moving Platform Trajectory');
plot( 1:step_size , trajectory_leg_length(:,1));
grid on;xlabel('time'); ylabel('D1');

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

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

axes(handles.axes12);
cla;
plot( 1:step_size , trajectory_leg_length(:,4));
grid on;xlabel('time'); ylabel('D4');

axes(handles.axes22);
cla;
plot( 1:step_size , trajectory_leg_length(:,5));
grid on;xlabel('time'); ylabel('D5');

axes(handles.axes32);
cla;
plot( 1:step_size , trajectory_leg_length(:,6));
grid on;xlabel('time'); ylabel('D6');


% --- 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)
% hObject    handle to pushbutton_OKP (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
Px = get(handles.checkbox_Px, 'Value');
Py = get(handles.checkbox_Py, 'Value');
Pz = get(handles.checkbox_Pz, 'Value');
Alpha = get(handles.checkbox_Alpha,'Value');
Beta = get(handles.checkbox_Beta,'Value');
Gamma = get(handles.checkbox_Gamma,'Value');

Moving = [ Px , Py , Pz , Alpha , Pz , Gamma ];
if ( Moving == 0 ) 
    warndlg('Please select one of the parameters');
end

NselectedCheckbox = sum(Moving);

%PATH PLANNING = LINEAR PATH PLANNING
step_size = 50;
%step_size = getappdata(0, 'step_size' );

hcf = gcf;

set( hcf, 'Unit' , 'pixel' );
posF = get( hcf , 'Position');
left  = posF(1);
bottom  = posF(2);
width  = posF(3);
height  = posF(4);

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

if ( Px ) 
    set(handles.axes11, 'visible', 'on' ); 
    axes(handles.axes11);
    title('Moving Platform Trajectory');
    plot( 1:step_size , trajectory_path(:,1));
    grid on;xlabel('time'); ylabel('X');
else
    axes(handles.axes11);
    cla;
    set(handles.axes11, 'visible', 'off' );
end
if ( Py ) 
    axes(handles.axes21);
    h21 = plot( 1:step_size , trajectory_path(:,2));
    grid on;xlabel('time'); ylabel('Y');
else 
    axes(handles.axes21);
    cla;
    set(handles.axes21, 'visible', 'off' );  
end
if ( Pz )     
    axes(handles.axes31);
    h31 = plot( 1:step_size , trajectory_path(:,3));
    grid on;xlabel('time'); ylabel('Z');
else
     axes(handles.axes31);
     cla;         
     set(handles.axes31, 'visible', 'off' );
end
if ( Alpha )     
    axes(handles.axes12);
    h12 = plot( 1:step_size , trajectory_angles(:,1));
    grid on;xlabel('time'); ylabel('\alpha');
else 
    axes(handles.axes12);
    cla;
    set(handles.axes12, 'visible', 'off' );      
end
if ( Beta )
    axes(handles.axes22);
    hp22 = plot( 1:step_size , trajectory_angles(:,2));
    grid on;xlabel('time'); ylabel('\Beta');
else
    axes(handles.axes22);    
    cla;    
    set(handles.axes22, 'visible', 'off' );
end
if ( Gamma )    
    axes(handles.axes32);
    hp32 = plot( 1:step_size , trajectory_angles(:,3));
    grid on;xlabel('time'); ylabel('\Gamma');
else
    axes(handles.axes32);
    cla;
    set(handles.axes32, 'visible', 'off' );
end
                            
% Lim2 = find( xor(Moving,[1 1 1 1 1 1]) )

% --- 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 popupmenu2.
function popupmenu2_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu2 (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 popupmenu2 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu2


% --- Executes during object creation, after setting all properties.
function popupmenu2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu2 (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
