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

% Last Modified by GUIDE v2.5 09-Apr-2012 11:13:09

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

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

selected = getappdata(0,'Selected_Mechanism');

if ( isempty(selected)==1 ) 
    %load mechanism Base, Platform and Connection Matrix from mat file.
    mechanism_file = uigetfile('*.mat'); 
    load(mechanism_file);  
    
    %DO NOT Use global.... GET&SET Appdata 
    setappdata( 0 , 'Base' , Base );
    setappdata( 0 , 'Platform' ,Platform );
    setappdata( 0 , 'ConMatrix' , ConMatrix );
    setappdata( 0 , 'LegLimits' , LegLimits );
    setappdata( 0 , 'JointLimits' , JointLimits );
    setappdata( 0 , 'Thickness_Base', Thickness_Base );
    setappdata( 0 , 'Thickness_Moving', Thickness_Moving );
    setappdata( 0 , 'Limitation' , Limitation);

else load(selected);
end

filename= 'trajectory_file2.xlsx';
[num,txt,raw] = xlsread(filename);

px =num(:,1);
py =num(:,2);
pz =num(:,3);
alpha=num(:,4);
beta=num(:,5);
gamma=num(:,6);

% data = get(handles.uitable_trajectory,'Data');
% data = { px , py , pz , alpha , beta, gamma }
set( handles.uitable_trajectory, 'Data', num )
width = get(handles.uitable_trajectory,'ColumnWidth');

Platform.Rotation = Rotation( Platform.alpha, Platform.beta, Platform.gamma , 'RPY' ) ;
Platform.Translate  = [ Platform.Px ; Platform.Py  ; Platform.Pz ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
[ Leg, Platform.PBase, ErrorMsg ]= inverse_kinematic(Base, Platform, ConMatrix, LegLimits, JointLimits );
%if ( strcmp( ErrorMsg, ' 'No ERROR' ) ~= 0 ) return end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
setappdata( 0 , 'Platform' ,Platform );
setappdata( 0 , 'Leg' , Leg );
cla;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
handles = Plot_Mechanism( handles );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Platform.currentPx = Platform.Px;
Platform.currentPy = Platform.Py;
Platform.currentPz = Platform.Pz;
Platform.currentAlpha = Platform.alpha;
Platform.currentBeta = Platform.beta;
Platform.currentGamma = Platform.gamma;

set( handles.edit_Px, 'String', Platform.Px );
set( handles.edit_Py, 'String', Platform.Py );
set( handles.edit_Pz, 'String', Platform.Pz );
set( handles.edit_Alpha, 'String', Platform.alpha );
set( handles.edit_Beta, 'String', Platform.beta );
set( handles.edit_Gamma, 'String', Platform.gamma );

axes(handles.axes1);

% Update handles structure
guidata(hObject, handles);

% --- Executes on button press in pb_OK.
function pb_OK_Callback(hObject, eventdata, handles)
% hObject    handle to pb_OK (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

view(40,34);

Base = getappdata( 0 , 'Base' );
Platform = getappdata( 0 , 'Platform' );
ConMatrix = getappdata( 0 , 'ConMatrix' );
LegLimits = getappdata( 0 , 'LegLimits' );
JointLimits = getappdata( 0 , 'JointLimits' );
Thickness_Base = getappdata( 0 , 'Thickness_Base' );
Thickness_Moving = getappdata( 0 , 'Thickness_Moving' );

num = get( handles.uitable_trajectory, 'Data' );

time = 1;
step_size = 2;
total_step_size = time * step_size * length( num ) ;
setappdata( 0, 'step_size' , step_size );
setappdata( 0, 'total_step_size' , total_step_size );
setappdata( 0, 'time' , time );
setappdata( 0, 'Resolution', step_size * time );

trajectory_path = zeros(total_step_size,3);
trajectory_angles = zeros(total_step_size,3);
trajectory_leg_length = zeros(total_step_size,6);
q_velocity = zeros( total_step_size , 6 ) ;
q_acceleration = zeros( total_step_size , 6 ) ;

width = get(handles.uitable_trajectory,'ColumnWidth');

Platform.currentPx      = num(1,1);
Platform.currentPy      = num(1,2);
Platform.currentPz      = num(1,3);
Platform.currentAlpha   = num(1,4);
Platform.currentBeta    = num(1,5);
Platform.currentGamma   = num(1,6);
index = 0 ; 
index2 = 1;

for row = 1:1:length( num ) 
        Platform.targetPx =num(row,1);
        Platform.targetPy =num(row,2);
        Platform.targetPz =num(row,3);
        Platform.targetAlpha=num(row,4);
        Platform.targetBeta=num(row,5);
        Platform.targetGamma=num(row,6);     

[step_size_Tx,step_size_Tx_dt,step_size_Tx_ddt] = jtraj(Platform.currentPx, Platform.targetPx, step_size);
[step_size_Ty,step_size_Ty_dt,step_size_Ty_ddt] = jtraj(Platform.currentPy, Platform.targetPy, step_size);
[step_size_Tz,step_size_Tz_dt,step_size_Tz_ddt] = jtraj(Platform.currentPz, Platform.targetPz, step_size);
[step_size_Alpha,step_size_Alpha_dt,step_size_Alpha_ddt] = jtraj(Platform.currentAlpha, Platform.targetAlpha, step_size);
[step_size_Beta,step_size_Beta_dt,step_size_Beta_ddt] = jtraj(Platform.currentBeta, Platform.targetBeta, step_size);
[step_size_Gamma,step_size_Gamma_dt,step_size_Gamma_ddt] = jtraj(Platform.currentGamma, Platform.targetGamma, step_size);

for i = 1:1:step_size
        Platform.currentPx = step_size_Tx(i);
        Platform.currentPy = step_size_Ty(i);
        Platform.currentPz = step_size_Tz(i);       
        Platform.Translate = [Platform.currentPx; Platform.currentPy;Platform.currentPz; ];

        Platform.currentAlpha = step_size_Alpha(i);
        Platform.currentBeta  = step_size_Beta(i);
        Platform.currentGamma = step_size_Gamma(i);
   
        Platform.Rotation = Rotation( Platform.targetAlpha, Platform.targetBeta, Platform.targetGamma , 'RPY' ) ;
        Platform.Translate  = [ Platform.targetPx ; Platform.targetPy  ; Platform.targetPz ];

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %           Inverse Kinematic of this position
        [ Leg, Platform.PBase, ErrorMsg ]= inverse_kinematic(Base, Platform, ConMatrix, LegLimits, JointLimits );
        if ( ErrorMsg ~= 0 ) 
            NonReachable_path(index2,: ) =  [ Platform.currentPx , Platform.currentPy, Platform.currentPz ];
            NonReachable_angles(index2,:) = [ Platform.currentAlpha , Platform.currentBeta, Platform.currentGamma ];
            index2 = index2 + 1 ;
        end
        
        %if ( strcmp( ErrorMsg, ' 'No ERROR' ) ~= 0 ) return end;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %           PLOT Mechanism on GUI
        %      what if the step_size is too big 
        if ( mod( i , (step_size/55) ) == 0 ) 
            Platform.Px = Platform.currentPx;
            Platform.Py = Platform.currentPy;
            Platform.Pz = Platform.currentPz;
            Platform.alpha = Platform.currentAlpha;
            Platform.beta = Platform.currentBeta;
            Platform.gamma = Platform.currentGamma;
       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%            
            setappdata( 0 , 'Platform' ,Platform );
            setappdata( 0 , 'Leg' , Leg );
%             cla;       
            handles = Plot_Mechanism( handles );
            drawnow;
       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%            
            pause(0.00000001);
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
     
        trajectory_path(index+i,:) = [ Platform.currentPx , Platform.currentPy, Platform.currentPz ];
        trajectory_angles(index+i,:) = [ Platform.currentAlpha , Platform.currentBeta, Platform.currentGamma ];
        trajectory_leg_length(index+i,:)  = Leg.L;        
   
end
        index = index + step_size;

        Platform.currentPx = Platform.targetPx;
        Platform.currentPy = Platform.targetPy;
        Platform.currentPz = Platform.targetPz;
        Platform.currentAlpha = Platform.targetAlpha;
        Platform.currentBeta = Platform.targetBeta;
        Platform.currentGamma = Platform.targetGamma;

        set( handles.edit_Px, 'String', Platform.Px );
        set( handles.edit_Py, 'String', Platform.Py );
        set( handles.edit_Pz, 'String', Platform.Pz );
        set( handles.edit_Alpha, 'String', Platform.alpha );
        set( handles.edit_Beta, 'String', Platform.beta );
        set( handles.edit_Gamma, 'String', Platform.gamma );
                
        trajectory_path(row,:) = [ Platform.currentPx , Platform.currentPy, Platform.currentPz ];
        trajectory_angles(row,:) = [ Platform.currentAlpha , Platform.currentBeta, Platform.currentGamma ];
        trajectory_leg_length(row,:)  = Leg.L;        

%         pause(.1);
end
% axes(handles.axes1);

  set( handles.edit_Px, 'String', Platform.Px );
        set( handles.edit_Py, 'String', Platform.Py );
        set( handles.edit_Pz, 'String', Platform.Pz );
        set( handles.edit_Alpha, 'String', Platform.alpha );
        set( handles.edit_Beta, 'String', Platform.beta );
        set( handles.edit_Gamma, 'String', Platform.gamma );
        
setappdata(0, 'trajectory_path', trajectory_path );
setappdata(0, 'trajectory_angles', trajectory_angles );
setappdata(0, 'trajectory_leg_length', trajectory_leg_length );
setappdata( 0 , 'NonReachable_path',NonReachable_path ) ;


newf = figure(2);
axes( gca );

hold on;
line( 'Xdata', trajectory_path(:,1) , 'YData', trajectory_path(:,2), ...
    'Zdata', trajectory_path(:,3) , 'LineWidth',4);




% plot3(  NonReachable_path(:,1) , NonReachable_path(:,2), NonReachable_path(:,3) , 'red' );
% scatter3(  NonReachable_path(:,1) , NonReachable_path(:,2), NonReachable_path(:,3) , 'red' , 'LineWidth', 5 ,'filled');
scatter3(  NonReachable_path(:,1) , NonReachable_path(:,2), NonReachable_path(:,3) , 'red' , 'filled','LineWidth',5);



grid on; rotate3d;
view(-50,40);

% scatter3(  trajectory_path(:,1) , trajectory_path(:,2), trajectory_path(:,3) );


% trajectory_GSP;

% Update handles structure
guidata(hObject, handles);



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


function edit_Px_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Px (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 edit_Px as text
%        str2double(get(hObject,'String')) returns contents of edit_Px as a double


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

% Hint: edit 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



function edit_Pz_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Pz (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 edit_Pz as text
%        str2double(get(hObject,'String')) returns contents of edit_Pz as a double


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

% Hint: edit 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



function edit_Alpha_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Alpha (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 edit_Alpha as text
%        str2double(get(hObject,'String')) returns contents of edit_Alpha as a double


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

% Hint: edit 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



function edit_Beta_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Beta (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 edit_Beta as text
%        str2double(get(hObject,'String')) returns contents of edit_Beta as a double


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

% Hint: edit 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



function edit_Gamma_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Gamma (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 edit_Gamma as text
%        str2double(get(hObject,'String')) returns contents of edit_Gamma as a double


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

% Hint: edit 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



function edit_Py_Callback(hObject, eventdata, handles)
% hObject    handle to edit_Py (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 edit_Py as text
%        str2double(get(hObject,'String')) returns contents of edit_Py as a double


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

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


% --- Executes during object creation, after setting all properties.
function popupmenu_Jacobian_Method_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu_Jacobian_Method (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 during object creation, after setting all properties.
function figure1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to figure1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called


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


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




% --- 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 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 during object creation, after setting all properties.
function pb_OK_CreateFcn(hObject, eventdata, handles)
% hObject    handle to pb_OK (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called


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


% --- Executes when selected cell(s) is changed in uitable_trajectory.
function uitable_trajectory_CellSelectionCallback(hObject, eventdata, handles)
% hObject    handle to uitable_trajectory (see GCBO)
% eventdata  structure with the following fields (see UITABLE)
%	Indices: row and column indices of the cell(s) currently selecteds
% handles    structure with handles and user data (see GUIDATA)

view(40,34);

Base = getappdata( 0 , 'Base' );
Platform = getappdata( 0 , 'Platform' );
ConMatrix = getappdata( 0 , 'ConMatrix' );
LegLimits = getappdata( 0 , 'LegLimits' );
JointLimits = getappdata( 0 , 'JointLimits' );
Thickness_Base = getappdata( 0 , 'Thickness_Base' );
Thickness_Moving = getappdata( 0 , 'Thickness_Moving' );


selection = eventdata.Indices;
if ( selection(1) == 0 ) 
    return;
end
row = selection(1);

num = get( handles.uitable_trajectory, 'Data' );

px =num(row,1);
py =num(row,2);
pz =num(row,3);
alpha=num(row,4);
beta=num(row,5);
gamma=num(row,6);

Platform.Px = px;
Platform.Py = py;
Platform.Pz = pz;

Platform.alpha = alpha;
Platform.beta = beta;
Platform.gamma = gamma;

% data = get(handles.uitable_trajectory,'Data');
% data = { px , py , pz , alpha , beta, gamma }

width = get(handles.uitable_trajectory,'ColumnWidth');

Platform.Rotation = Rotation( Platform.alpha, Platform.beta, Platform.gamma , 'RPY' ) ;
Platform.Translate  = [ Platform.Px ; Platform.Py  ; Platform.Pz ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
[ Leg, Platform.PBase, ErrorMsg ]= inverse_kinematic(Base, Platform, ConMatrix, LegLimits, JointLimits );
%if ( strcmp( ErrorMsg, ' 'No ERROR' ) ~= 0 ) return end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
setappdata( 0 , 'Platform' ,Platform );
setappdata( 0 , 'Leg' , Leg );
cla;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
handles = Plot_Mechanism( handles );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Platform.currentPx = Platform.Px;
Platform.currentPy = Platform.Py;
Platform.currentPz = Platform.Pz;
Platform.currentAlpha = Platform.alpha;
Platform.currentBeta = Platform.beta;
Platform.currentGamma = Platform.gamma;

set( handles.edit_Px, 'String', Platform.Px );
set( handles.edit_Py, 'String', Platform.Py );
set( handles.edit_Pz, 'String', Platform.Pz );
set( handles.edit_Alpha, 'String', Platform.alpha );
set( handles.edit_Beta, 'String', Platform.beta );
set( handles.edit_Gamma, 'String', Platform.gamma );

% axes(handles.axes1);

% Update handles structure
guidata(hObject, handles);



% --- Executes when entered data in editable cell(s) in uitable_trajectory.
function uitable_trajectory_CellEditCallback(hObject, eventdata, handles)
% hObject    handle to uitable_trajectory (see GCBO)
% eventdata  structure with the following fields (see UITABLE)
%	Indices: row and column indices of the cell(s) edited
%	PreviousData: previous data for the cell(s) edited
%	EditData: string(s) entered by the user
%	NewData: EditData or its converted form set on the Data property. Empty if Data was not changed
%	Error: error string when failed to convert EditData to appropriate value for Data
% handles    structure with handles and user data (see GUIDATA)


num = get( handles.uitable_trajectory, 'Data' );

px =num(:,1);
py =num(:,2);
pz =num(:,3);
alpha=num(:,4);
beta=num(:,5);
gamma=num(:,6);

% data = get(handles.uitable_trajectory,'Data');
% data = { px , py , pz , alpha , beta, gamma }

width = get(handles.uitable_trajectory,'ColumnWidth');

Platform.Rotation = Rotation( Platform.alpha, Platform.beta, Platform.gamma , 'RPY' ) ;
Platform.Translate  = [ Platform.Px ; Platform.Py  ; Platform.Pz ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
[ Leg, Platform.PBase, ErrorMsg ]= inverse_kinematic(Base, Platform, ConMatrix, LegLimits, JointLimits );
%if ( strcmp( ErrorMsg, ' 'No ERROR' ) ~= 0 ) return end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
setappdata( 0 , 'Platform' ,Platform );
setappdata( 0 , 'Leg' , Leg );
cla;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Inverse Kinematic of this position
handles = Plot_Mechanism( handles );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Platform.currentPx = Platform.Px;
Platform.currentPy = Platform.Py;
Platform.currentPz = Platform.Pz;
Platform.currentAlpha = Platform.alpha;
Platform.currentBeta = Platform.beta;
Platform.currentGamma = Platform.gamma;

set( handles.edit_Px, 'String', Platform.Px );
set( handles.edit_Py, 'String', Platform.Py );
set( handles.edit_Pz, 'String', Platform.Pz );
set( handles.edit_Alpha, 'String', Platform.alpha );
set( handles.edit_Beta, 'String', Platform.beta );
set( handles.edit_Gamma, 'String', Platform.gamma );

% [handles.String_Pz,h_text_Pz]=uibutton('Style','text','Unit','characters','Position',[52.2 3.30769 10 1.69231],'String','Pz','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% handles.h_text_Pz=uibutton('Style','text','Unit','characters','Position',[52.2 3.30769 10 1.69231],'String','Pz','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% handles.h_text_Py=uibutton('Style','text','Unit','characters','Position',[52.2 4.61538 10 1.69231],'String','Py','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% handles.h_text_Px=uibutton('Style','text','Unit','characters','Position',[52.2 5.92308 10 1.69231],'String','Px','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% handles.h_text_Alpha = uibutton('Style','text','Unit','characters','Position',[52.12,11,10,1.692],'String','\alpha','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% handles.h_text_Beta = uibutton('Style','text','Unit','characters','Position',[52.2 9 10 1.69231],'String','\beta','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);                     
% handles.h_text_Gamma = uibutton('Style','text','Unit','characters','Position',[52.2 7.23077 10 1.69231],'String','\gamma','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12);
% uibutton('Style','text','Unit','characters','Position',[0 3 1 .69231],'String','\gamma','BackgroundColor' , [0.839216 0.909804 0.85098],'Fontsize',12,'visible','on');

axes(handles.axes1);

% Update handles structure
guidata(hObject, handles);
