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

% Last Modified by GUIDE v2.5 27-Feb-2012 04:42:17

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

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

% selected = getappdata(0,'Selected_Mechanism');

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

if ( isempty(Base) == 1 )
    hWorkspace = gcf;
    helpdlg( 'Before workspace calculation, you must start Analysis GSP');
    uiwait();
    close(hWorkspace);
    AnalysisGSP;
    return;
    
end    






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 );
view(-23,18);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%display Jacobian equation on axes
% axes(handles.axes_Jacobian_equation);
% imshow('jacobian_equation3.tif');

% axes(handles.axes1);

index2=0;
for i=1:1:6

    for k = 1:1:6
       
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        txt_handle = ['set(handles.' ... 
            'text_JX_' num2str(i) num2str(k) ', ''String'' ,' ...
            num2str( 0 ) ')'  ];
        
        eval ( txt_handle );

        txt_handle = ['set(handles.' ...
            'text_Q_' num2str(k+index2) ', ''String'' ,' ...
            num2str( 0 ) ')'  ];
        
        eval ( txt_handle );
        
        txt_handle = ['set(handles.' ...
            'text_J_' num2str(k+index2) ', ''String'' ,' ...
            num2str( 0 ) ')'  ];
        
        eval ( txt_handle );
        
        
                
        
        
    end 
    
    index2 = index2 + 6;
end
    
    
    
% Update handles structure
guidata(hObject, handles);

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


% --- Outputs from this function are returned to the command line.
function varargout = Jacobian_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 pb_Jacobian.
function pb_Jacobian_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Jacobian (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
axes(handles.axes1);



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' );
Limitation = getappdata(0,'Limitation');   

% format short
Platform.Px = str2double( get( handles.edit_Px, 'String' ));
Platform.Py = str2double( get( handles.edit_Py, 'String' ));
Platform.Pz = str2double( get( handles.edit_Pz, 'String' ));
Platform.alpha = str2double( get( handles.edit_Alpha, 'String' ));
Platform.beta = str2double( get( handles.edit_Beta, 'String' ));
Platform.gamma = str2double( get( handles.edit_Gamma, 'String' ));


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 );
axes(handles.axes1);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;

%%%%    LegNumber = number of Legs in connection matrix.
%%%%    sizeNaN   = number of NaN in connection matrix.
[ sizeNaN , LegNumber ] = LegNumbers( ConMatrix );

%%%% identity matrix nxn square matrix where n is equal to number of Legs.
identity_matrix = eye(LegNumber,LegNumber);

%%%% depracated popupmenu.
jacobian_method = get(handles.popupmenu_Jacobian_Method, 'Value');

%%%% choose Screw-based Jacobian Method
jacobian_method = 2;        


index=1;index2=0;


for i = 1:1:6
  if ~isnan(ConMatrix(1,i)) && ~isnan(ConMatrix(2,i))     
      
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      
        %%% Vector Loop-Closure Equation Method ( TSAI ) 
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        if ( jacobian_method == 1 ) 
            Jx(index,:) = ...
             [ Leg.unit_Lvector(i,:) ...
               cross( ( Platform.Rotation * Platform.PTable(ConMatrix(2,i),:)' )' , Leg.unit_Lvector(i,:)' ) ];
            Jq =  identity_matrix;

           
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%            
        %%% Screw-based Jacobian Method    
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        elseif ( jacobian_method == 2 ) 
            Jx(index,:) = ...
                [ cross( ( Platform.Rotation * Platform.PTable(ConMatrix(2,i),:)' )' , Leg.L_vector(i,:)' ) ...
                  Leg.L_vector(i,:)  ];             
            Jq(index,:) = identity_matrix(index,:) .* Leg.L(i);          
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% Write all Jacobian matrix to textboxes.        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         format long
%         format short;
    for k = 1:1:LegNumber

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% Very small values must be zero
         if ( Jx(index, k) < 1e-14 && Jx(index, k) > -1e-14 ) Jx(index,k) = 0 ; end
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        txt_handle = ['set(handles.' ... 
            'text_JX_' num2str(i) num2str(k) ', ''String'' ,' ...
            num2str( Jx(index,k) ) ')'  ];
        
        eval ( txt_handle );

        txt_handle = ['set(handles.' ...
            'text_Q_' num2str(k+index2) ', ''String'' ,' ...
            num2str( Jq(index,k) ) ')'  ];
        
        eval ( txt_handle );
    end    
 
    index=index+1;  
    index2=index2+6;
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Overall Jacobian
if ( LegNumber < 6) 
        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');

        columns = [ 1 2 3 4 5 6] 
        Limits = [ Px Py Pz Alpha Beta Gamma ]

        index=1;
        selects = [];


        if ( Px == 1 ) 
            selects(index) = 1;

            index=index+1;
        end

        if ( Py == 1 ) 
            selects(index) = 2;
            index=index+1;
        end

        if ( Pz == 1 ) 
            selects(index) = 3;
            index=index+1;
        end

        if ( Alpha == 1 ) 
            selects(index) = 4;
            index=index+1;
        end

        if ( Beta == 1 ) 
            selects(index) = 5;
            index=index+1;
        end

        if ( Gamma == 1 ) 
            selects(index) = 6;
            index=index+1;
        end

        Jx = Jx( : , selects );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Overall Jacobian
Jacobian = inv(Jq)*Jx;

Jinverse = inv(Jacobian);

% if ( LegNumber < 6) 
%     Jinverse = pinv(Jacobian);
%     
% else
%     Jinverse = inv(Jacobian);
% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%     condition number 
W= identity_matrix * 1/LegNumber;

K = sqrt ( trace( Jacobian*W*Jacobian' ) ) * sqrt( trace( Jinverse*W*Jinverse' ) ) ;
%         K = sqrt( trace( Jinverse*W*Jinverse' ) ) * sqrt ( trace( J*W*J' ) );        
condition_numbers( i ) = 1/K;
    
index2=0;
for i = 1:1:LegNumber
    for k = 1:1:LegNumber

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% Very small values must be zero
        if ( Jacobian(i, k) < 1e-14 && Jacobian(i, k) > -1e-14 ) Jacobian(i,k) = 0 ; end
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        


        txt_handle = ['set(handles.' ...
            'text_J_' num2str(k+index2) ', ''String'' ,' ...
            num2str( Jacobian(i,k) ) ')'  ];
        
        eval ( txt_handle );
    end    
    index2=index2+6;
end

% Jacobian;
% Jacobian_sutunlar_degisen = [ Jacobian(:,4:6) , Jacobian(:,1:3) ];

% 
% sum1 = 0; 
% sum2 = 0;
% 
% for i = 1:1:LegNumber
%     for k = 1:1:3
% 
%         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         %%% Very small values must be zero
%         sum1 = sum1 + Jacobian(i, k)^2;
%         sum2 = sum2 + Jacobian(i, k+3)^2;
%         
%         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
% 
%     end    
% end
% 
% 
% L = sqrt( sum1 / sum2 ) 
% 
% 
% Jacobian_homogenized = [ 1/L.*Jacobian(:,1:3) Jacobian(:,4:6)  ]






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



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 on button press in pushbutton4.
function pushbutton4_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
axes(handles.axes1);

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' );
Limitation = getappdata(0,'Limitation');   

Platform.RotationMatrix = 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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% for index = 1:1:6
%   if ~isnan(ConMatrix(1,index)) && ~isnan(ConMatrix(2,index))     
%         Leg.L(index) = norm ( Platform.PBase(ConMatrix(2,index),:)...
%                                     - Base(ConMatrix(1,index),:) );
% 
%             Leg.Lvektor(index,:) = Platform.PBase(ConMatrix(2,index),:) - Base(ConMatrix(1,index),:);     
%             Leg.unit_Lvector(index,:) =  Leg.Lvektor(index,:) / norm( Leg.Lvektor(index,:) );         
%   end
% end

disp('unit vectors');
Leg.unit_Lvector

%P = [ px , py , pz ];
P =  Platform.Translate;
%w = [ wx , wy , wz ];
w = [  Platform.alpha ,  Platform.beta , Platform.gamma ];

%    Jx = zeros( 6 , 6 );
index=1;


               
for i = 1:1:6
  if ~isnan(ConMatrix(1,i)) && ~isnan(ConMatrix(2,i))     
        Jx(index,:) = [ Leg.unit_Lvector(i,:) ...
                        cross( Platform.PBase(ConMatrix(2,i),:) , Leg.unit_Lvector(i,:)' ) ];
        Jq = eye(6,6);
       
        index=index+1;
        
  end
end

    JFattahX = Jx
    %%inv(Jx)
    inv(Jx)*Jx;
    Jx*inv(Jx);

    
    
    
disp('JIA');
JIA = blkdiag ( Leg.unit_Lvector(1,:)' , Leg.unit_Lvector(2,:)' , Leg.unit_Lvector(3,:)' ,...
              Leg.unit_Lvector(4,:)' , Leg.unit_Lvector(5,:)' , Leg.unit_Lvector(6,:)' );

%JIIA       
%birinci sütunu birim matrislerden oluşuyor.

skew_symmetric_X = [ 0,0 ,0 ; 0,0,-1 ; 0,1,0 ];
skew_symmetric_Y = [ 0,0 ,1 ; 0,0, 0 ;-1,0,0 ];
skew_symmetric_Z = [ 0,-1,0 ; 1,0, 0 ; 0,0,0 ];


alpha = deg2rad(Platform.alpha);
beta = deg2rad(Platform.beta);
gamma = deg2rad(Platform.gamma);

RxAlpha =    [  1   0             0;...
                0   cos(alpha)   -sin(alpha);...
                0   sin(alpha)   cos(alpha)     ];

RyBeta =     [  cos(beta)   0    sin(beta);...
                0           1    0 ;...
                -sin(beta)   0    cos(beta)     ];

RzGamma =    [  cos(gamma)   -sin(gamma)    0;...
                sin(gamma)    cos(gamma)    0;...
                0             0             1   ];
            
J2_41 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(1,:)';
J2_42 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(2,:)';
J2_43 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(3,:)';
J2_44 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(4,:)';      
J2_45 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(5,:)';
J2_46 = RyBeta * skew_symmetric_X * RxAlpha * RzGamma * Platform.PBase(6,:)';

J2_51 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(1,:)';
J2_52 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(2,:)';
J2_53 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(3,:)';
J2_54 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(4,:)';
J2_55 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(5,:)';
J2_56 = skew_symmetric_Y * RyBeta * RxAlpha * RzGamma * Platform.PBase(6,:)';

J2_61 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(1,:)';
J2_62 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(2,:)';
J2_63 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(3,:)';
J2_64 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(4,:)';
J2_65 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(5,:)';
J2_66 = RyBeta * RxAlpha * skew_symmetric_Z * RzGamma * Platform.PBase(6,:)';

J2= [ eye(3)    J2_41   J2_51   J2_61
      eye(3)    J2_42   J2_52   J2_62
      eye(3)    J2_43   J2_53   J2_63
      eye(3)    J2_44   J2_54   J2_64
      eye(3)    J2_45   J2_55   J2_65
      eye(3)    J2_46   J2_56   J2_66 ]
  JIA' * J2
  
%aciklama


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


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


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


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


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


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


% --- Executes on button press in pb_Rotation.
function pb_Rotation_Callback(hObject, eventdata, handles)
% hObject    handle to pb_Rotation (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
RotationGUI;
set(handles.edit_Alpha,'Enable', 'on' );
set(handles.edit_Beta,'Enable', 'on' );
set(handles.edit_Gamma,'Enable', 'on' );

set(handles.uipanel8,'Visible','on');

set(handles.pushbutton17,'Visible','on');

% set(handles.pb_Jacobian,'Visible','on');



function edit17_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 edit17_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 edit18_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 edit18_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 edit19_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 edit19_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


% --- Executes on button press in pushbutton17.
function pushbutton17_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton17 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
Rotation_Matrix = getappdata(0,'Rotation_Matrix');

Alpha = str2double( get(handles.edit_Alpha, 'String') );
Beta = str2double(get(handles.edit_Beta, 'String'));
Gamma = str2double(get(handles.edit_Gamma, 'String'));



alpha=deg2rad(Alpha);beta=deg2rad(Beta);gamma=deg2rad(Gamma);
rot = eval(Rotation_Matrix)
set(handles.Rotation11, 'String', num2str( rot(1,1) ) , 'Visible', 'on' );
set(handles.Rotation12, 'String', num2str( rot(1,2) ), 'Visible', 'on' );
set(handles.Rotation13, 'String', num2str( rot(1,3) ), 'Visible', 'on' );

set(handles.Rotation21, 'String', num2str( rot(2,1) ), 'Visible', 'on' );
set(handles.Rotation22, 'String', num2str( rot(2,2) ), 'Visible', 'on' );
set(handles.Rotation23, 'String', num2str( rot(2,3) ), 'Visible', 'on' );

set(handles.Rotation31, 'String', num2str( rot(3,1) ), 'Visible', 'on' );
set(handles.Rotation32, 'String', num2str( rot(3,2) ), 'Visible', 'on' );
set(handles.Rotation33, 'String', num2str( rot(3,3) ), 'Visible', 'on' );

set( handles.left_line, 'Visible', 'on' );
set( handles.right_line, 'Visible', 'on' );
set( handles.text277, 'Visible', 'on' );
set( handles.text278, 'Visible', 'on' );
set( handles.text279, 'Visible', 'on' );
set( handles.text280, 'Visible', 'on' );


%%%% if Legnumber < 6 then we must choose the controllable parameters.
ConMatrix = getappdata( 0 , 'ConMatrix' );
[ sizeNaN , LegNumber ] = LegNumbers( ConMatrix );

if ( LegNumber < 6 ) 
    set(handles.uipanel_limitations,'visible','on');
else
    set( handles.pb_Jacobian, 'enable', 'on' , 'Visible', 'on' );
end


function edit14_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 edit14_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 edit15_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 edit15_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 edit16_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 edit16_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 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 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 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 pushbutton9.
function pushbutton9_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton9 (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_Jb_limit.
function pushbutton_Jb_limit_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton_Jb_limit (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
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' );
Limitation = getappdata(0,'Limitation');   

% format short
Platform.Px = str2double( get( handles.edit_Px, 'String' ));
Platform.Py = str2double( get( handles.edit_Py, 'String' ));
Platform.Pz = str2double( get( handles.edit_Pz, 'String' ));
Platform.alpha = str2double( get( handles.edit_Alpha, 'String' ));
Platform.beta = str2double( get( handles.edit_Beta, 'String' ));
Platform.gamma = str2double( get( handles.edit_Gamma, 'String' ));


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 );
axes(handles.axes1);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;


%%%%    LegNumber = number of Legs in connection matrix.
%%%%    sizeNaN   = number of NaN in connection matrix.
[ sizeNaN , LegNumber ] = LegNumbers( ConMatrix );

%%%% identity matrix nxn square matrix where n is equal to number of Legs.
identity_matrix = eye(LegNumber,LegNumber);
 Jq =  identity_matrix;
 Jx = eye(LegNumber, 6 );
            
%%%% depracated popupmenu.
jacobian_method = get(handles.popupmenu_Jacobian_Method, 'Value');

%%%% choose Screw-based Jacobian Method
jacobian_method = 2;        


index=1;index2=0;


for i = 1:1:6
  if ~isnan(ConMatrix(1,i)) && ~isnan(ConMatrix(2,i))     
      
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      
        %%% Vector Loop-Closure Equation Method ( TSAI ) 
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        if ( jacobian_method == 1 ) 
            Jx(index,:) = ...
             [ Leg.unit_Lvector(i,:) ...
               cross( ( Platform.Rotation * Platform.PTable(ConMatrix(2,i),:)' )' , Leg.unit_Lvector(i,:)' ) ];
            Jq =  identity_matrix;
           
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%            
        %%% Screw-based Jacobian Method    
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        elseif ( jacobian_method == 2 ) 
            Jx(index,:) = ...
                [ cross( ( Platform.Rotation * Platform.PTable(ConMatrix(2,i),:)' )' , Leg.L_vector(i,:)' ) ...
                  Leg.L_vector(i,:)  ];             
            Jq(index,:) = identity_matrix(index,:) .* Leg.L(i);          
        end
        index=index+1;
  end
end

index=1;
        
for i = 1:1:6        
    for k = 1:1:6

        try
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            %%% Very small values must be zero
             if ( Jx(index, k) < 1e-14 && Jx(index, k) > -1e-14 ) Jx(index,k) = 0 ; end

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
            txt_handle = ['set(handles.' ... 
                'text_JX_' num2str(i) num2str(k) ', ''String'' ,' ...
                num2str( Jx(index,k) ) ')'  ];
        catch
            txt_handle = ['set(handles.' ... 
                'text_JX_' num2str(i) num2str(k) ', ''String'' ,'' '')'  ];    
        end
        
        eval ( txt_handle );

    end    
    
     for k = 1:1:6
        try
        txt_handle = ['set(handles.' ...
            'text_Q_' num2str(k+index2) ', ''String'' ,' ...
            num2str( Jq(index,k) ) ')'  ];
        catch
        txt_handle = ['set(handles.' ...
            'text_Q_' num2str(k+index2) ', ''String'' ,'' '')'  ];
        end
        eval ( txt_handle );
    end  
    index=index+1;  
    index2=index2+6;   
    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Overall Jacobian
if ( LegNumber < 6) 
        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');

        columns = [ 1 2 3 4 5 6] 
        Limits = [ Px Py Pz Alpha Beta Gamma ]

        index=1;
        selects = [];


        if ( Px == 1 ) 
            selects(index) = 1;
            index=index+1;
        end

        if ( Py == 1 ) 
            selects(index) = 2;          
            index=index+1;
        end

        if ( Pz == 1 ) 
            selects(index) = 3;
             index=index+1;
        end

        if ( Alpha == 1 ) 
            selects(index) = 4;          
            index=index+1;
        end

        if ( Beta == 1 ) 
            selects(index) = 5;
            index=index+1;
        end

        if ( Gamma == 1 ) 
            selects(index) = 6;           
            index=index+1;
        end
        if length(selects) ~= LegNumber
            warndlg(['Please select one parameter for each leg = ' num2str(LegNumber)]);
            return;
        end
        Jx = Jx( : , selects );
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Overall Jacobian
Jacobian = inv(Jq)*Jx;

Jinverse = inv(Jacobian);

index=1;index2=0;
for i=1:1:6
        for k = 1:1:6
            try
            txt_handle = ['set(handles.' ...
                'text_J_' num2str(k+index2) ', ''String'' ,' ...
                num2str( Jacobian(index,k) ) ')'  ];
            catch
            txt_handle = ['set(handles.' ...
                'text_J_' num2str(k+index2) ', ''String'' ,'' '')'  ];
            end
            eval ( txt_handle );
        end
   index=index+1;     
   index2=index2+6;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%     condition number 
W= identity_matrix * 1/LegNumber;

K = sqrt ( trace( Jacobian*W*Jacobian' ) ) * sqrt( trace( Jinverse*W*Jinverse' ) ) ;
%         K = sqrt( trace( Jinverse*W*Jinverse' ) ) * sqrt ( trace( J*W*J' ) );        
condition_numbers( i ) = 1/K;

Jx
Jq
Jacobian

% Jacobian;
% Jacobian_sutunlar_degisen = [ Jacobian(:,4:6) , Jacobian(:,1:3) ];

% 
% sum1 = 0; 
% sum2 = 0;
% 
% for i = 1:1:LegNumber
%     for k = 1:1:3
% 
%         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         %%% Very small values must be zero
%         sum1 = sum1 + Jacobian(i, k)^2;
%         sum2 = sum2 + Jacobian(i, k+3)^2;
%         
%         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
% 
%     end    
% end
% 
% 
% L = sqrt( sum1 / sum2 ) 
% 
% 
% Jacobian_homogenized = [ 1/L.*Jacobian(:,1:3) Jacobian(:,4:6)  ]
