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

% Last Modified by GUIDE v2.5 25-Jan-2012 10:13:24

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

%save handle to this FIGURE to hide 
hinverseGSP = gcf;
setappdata(0,'hinverseGSP',hinverseGSP);

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

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes inverseGSP wait for user response (see UIRESUME)
% uiwait(handles.figure1);
initialize_gui(hObject, handles, false);
handles=guidata(hObject);

%%pb_Draw_Callback(hObject, eventdata, handles);

guidata(hObject, handles);

% --- Outputs from this function are returned to the command line.
function varargout = inverseGSP_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 initialize_gui(fig_handle, handles, isreset)
% If the metricdata field is present and the reset flag is false, it means
% we are we are just re-initializing a GUI by calling it from the cmd line
% while it is up. So, bail out as we dont want to reset the data.
if isfield(handles, 'metricdata') && ~isreset
    return;
end

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);  
else load(selected);
end

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


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

setappdata( 0 , 'Platform' ,Platform );
setappdata( 0 , 'Leg' , Leg );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           PLOT Mechanism on GUI
handles = Plot_Mechanism( handles );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% set( handles.platform_OK, 'enable', 'off' );
% set( handles.Jacobian, 'enable', 'on');
set( handles.pb_Draw, 'enable', 'off' );

%translation (Translate) == current platform points 
Platform.currentPx = Platform.Translate(1);
Platform.currentPy = Platform.Translate(2);
Platform.currentPz = Platform.Translate(3);
%orientation (Rotation) of mobile platform
Platform.currentAlpha = Platform.alpha;
Platform.currentBeta = Platform.beta;
Platform.currentGamma = Platform.gamma;

set(handles.currentPx, 'String', Platform.currentPx );
set(handles.currentPy, 'String', Platform.currentPy );
set(handles.currentPz, 'String', Platform.currentPz );
set(handles.currentAlpha, 'String', Platform.currentAlpha );
set(handles.currentBeta, 'String', Platform.currentBeta );
set(handles.currentGamma, 'String', Platform.currentGamma );

%destination of the mobile platform coordinate axes and rotation
%P should be on base coordinate system
Platform.targetPx = 30; 
Platform.targetPy = 40;
Platform.targetPz = Platform.currentPz;
Platform.targetAlpha = 0;
Platform.targetBeta = 0;
Platform.targetGamma = 0;

set(handles.editPx, 'String', Platform.targetPx );
set(handles.editPy, 'String', Platform.targetPy );
set(handles.editPz, 'String', Platform.targetPz );
set(handles.editAlpha, 'String', Platform.targetAlpha );
set(handles.editBeta, 'String', Platform.targetBeta );
set(handles.editGamma, 'String', Platform.targetGamma );


% set(handles.PTable,'Data',Platform.PBase);

set( handles.textL1 , 'String' , Leg.L(1) ) ; 
set( handles.textL2 , 'String' , Leg.L(2) ) ;
set( handles.textL3 , 'String' , Leg.L(3) ) ;
set( handles.textL4 , 'String' , Leg.L(4) ) ;
set( handles.textL5 , 'String' , Leg.L(5) ) ;
set( handles.textL6 , 'String' , Leg.L(6) ) ;

%Maximum Leg Length and Minimum Leg Length
% set(handles.editLmax, 'String', LegLimits(1,2) );
% set(handles.editLmin, 'String', LegLimits(1,1) );
% 
% set(handles.editJointMax, 'String', JointLimits(1,2) );
% set(handles.editJointMin, 'String', JointLimits(1,1) );

%Display on GUI Platform Data with respect to Base frame as Table
% set(handles.PTable,'Data', Platform.PBase);


setappdata(0, 'Platform', Platform );
setappdata(0, 'Leg', Leg );


grid on;rotate3d on;zlabel('Z');ylabel('Y');xlabel('X');

maxBase = max( Base.BTable ); minBase = min( Base.BTable );
maxPlatform = max( Platform.PBase ); minPlatform = min( Platform.PBase );
campos( [minBase(1,1) maxBase(1,2) maxPlatform(1,3)*1.5 ] ) ;
% axis([-50,50,-50,50,0,50,0,50]);
% campos([ -30 , -60 , 60 ]); 

haxesAnalysis = get(handles.axesAnalysis);
default_axis = axis ;
set(handles.axesAnalysis, 'XLim' , [-Base.Rb*1.7 , Base.Rb*1.7] );
set(handles.axesAnalysis, 'YLim' , [-Base.Rb*1.7 , Base.Rb*1.7] );
set(handles.axesAnalysis, 'ZLim' , [0, Platform.Translate(3)*2.5] );




% Update handles structure
guidata(fig_handle, handles);


% --- Executes on button press in pb_GO_directly.
function pb_GO_directly_Callback(hObject, eventdata, handles)
% hObject    handle to pb_GO_directly (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' );
% campos( [ -Base.Rb*1.5, -Base.Rb*1.5 , Base.Rb  ] );
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' );


%%Set target parameters to current paramaters.
Platform.Px = Platform.targetPx;
Platform.Py = Platform.targetPy;
Platform.Pz = Platform.targetPz;
Platform.alpha = Platform.targetAlpha;
Platform.beta = Platform.targetBeta;
Platform.gamma = Platform.targetGamma;


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

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

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.PTable,'Data',Platform.PBase);

set( handles.textL1 , 'String' , Leg.L(1) ) ; 
set( handles.textL2 , 'String' , Leg.L(2) ) ;
set( handles.textL3 , 'String' , Leg.L(3) ) ;
set( handles.textL4 , 'String' , Leg.L(4) ) ;
set( handles.textL5 , 'String' , Leg.L(5) ) ;
set( handles.textL6 , 'String' , Leg.L(6) ) ;

set(handles.currentPx, 'String', Platform.currentPx );
set(handles.currentPy, 'String', Platform.currentPy );
set(handles.currentPz, 'String', Platform.currentPz );
set(handles.currentAlpha, 'String', Platform.currentAlpha );
set(handles.currentBeta, 'String', Platform.currentBeta );
set(handles.currentGamma, 'String', Platform.currentGamma );
drawnow;

setappdata( 0 , 'Platform' ,Platform );

guidata(hObject, handles);





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

currentFigure = gcf;

% figure(currentFigure);

% handles = Plot_Mechanism( handles );

setappdata(0,'Resolution', str2num(get(handles.Resolution,'String')) );
setappdata(0,'Time', str2num(get(handles.Time,'String')) );
            
trajectory_GSP;

guidata(hObject,handles)



function editPx_Callback(hObject, eventdata, handles)
% hObject    handle to editPx (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 editPx as text
%        str2double(get(hObject,'String')) returns contents of editPx as a double
Px = str2double(get(hObject, 'String'));
if isnan(Px)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetPx = Px;
setappdata( 0, 'Platform', Platform ) ;
guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editPx_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editPx (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 editPy_Callback(hObject, eventdata, handles)
% hObject    handle to editPy (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 editPy as text
%        str2double(get(hObject,'String')) returns contents of editPy as a double
Py = str2double(get(hObject, 'String'));
if isnan(Py)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetPy = Py;
setappdata( 0, 'Platform', Platform ) ;
guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editPy_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editPy (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 editPz_Callback(hObject, eventdata, handles)
% hObject    handle to editPz (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 editPz as text
%        str2double(get(hObject,'String')) returns contents of editPz as a double
Pz = str2double(get(hObject, 'String'));
if isnan(Pz)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetPz = Pz;
setappdata( 0, 'Platform', Platform ) ;
guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editPz_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editPz (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 editAlpha_Callback(hObject, eventdata, handles)
% hObject    handle to editAlpha (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 editAlpha as text
%        str2double(get(hObject,'String')) returns contents of editAlpha as a double
Alpha = str2double(get(hObject, 'String'));
if isnan(Alpha)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetAlpha = Alpha;
setappdata( 0, 'Platform', Platform ) ;

Rotation_Matrix = getappdata(0,'Rotation_Matrix');


Beta = str2double(get(handles.editBeta, 'String'));
Gamma = str2double(get(handles.editGamma, 'String'));

alpha=Alpha;beta=Beta;gamma=Gamma;
eval(Rotation_Matrix)



guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editAlpha_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editAlpha (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 during object creation, after setting all properties.
function currentPx_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentPx (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 currentPy_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentPy (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 currentPz_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentPz (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 currentAlpha_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentAlpha (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 currentBeta_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentBeta (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 currentGamma_CreateFcn(hObject, eventdata, handles)
% hObject    handle to currentGamma (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called


function editBeta_Callback(hObject, eventdata, handles)
% hObject    handle to editBeta (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 editBeta as text
%        str2double(get(hObject,'String')) returns contents of editBeta as a double
Beta = str2double(get(hObject, 'String'));
if isnan(Beta)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetBeta = Beta;
setappdata( 0, 'Platform', Platform ) ;
guidata(hObject,handles)


% --- Executes during object creation, after setting all properties.
function editBeta_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editBeta (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 editGamma_Callback(hObject, eventdata, handles)
% hObject    handle to editGamma (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 editGamma as text
%        str2double(get(hObject,'String')) returns contents of editGamma as a double
Gamma = str2double(get(hObject, 'String'));
if isnan(Gamma)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
Platform = getappdata( 0, 'Platform');
Platform.targetGamma = Gamma;
setappdata( 0, 'Platform', Platform ) ;
guidata(hObject,handles)


% --- Executes during object creation, after setting all properties.
function editGamma_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editGamma (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 editTx_Callback(hObject, eventdata, handles)
% hObject    handle to editTx (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 editTx as text
%        str2double(get(hObject,'String')) returns contents of editTx as a double


% --- Executes during object creation, after setting all properties.
function editTx_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editTx (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 editTy_Callback(hObject, eventdata, handles)
% hObject    handle to editTy (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 editTy as text
%        str2double(get(hObject,'String')) returns contents of editTy as a double


% --- Executes during object creation, after setting all properties.
function editTy_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editTy (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 editTz_Callback(hObject, eventdata, handles)
% hObject    handle to editTz (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 editTz as text
%        str2double(get(hObject,'String')) returns contents of editTz as a double


% --- Executes during object creation, after setting all properties.
function editTz_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editTz (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 editLmin_Callback(hObject, eventdata, handles)
% hObject    handle to editLmin (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 editLmin as text
%        str2double(get(hObject,'String')) returns contents of editLmin as a double
Lmin = str2double(get(hObject, 'String'));
if isnan(Lmin)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
%handles.Leg.LMin = Lmin;
guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editLmin_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editLmin (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 editLmax_Callback(hObject, eventdata, handles)
% hObject    handle to editLmax (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 editLmax as text
%        str2double(get(hObject,'String')) returns contents of editLmax as a double
Lmax = str2double(get(hObject, 'String'));
if isnan(Lmax)
    set(hObject, 'String', 0);
    errordlg('Input must be a number','Error');
end

% Save the new volume value
%handles.Leg.LMax = Lmax;
guidata(hObject,handles)

% --- Executes during object creation, after setting all properties.
function editLmax_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editLmax (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 editJointMin_Callback(hObject, eventdata, handles)
% hObject    handle to editJointMin (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 editJointMin as text
%        str2double(get(hObject,'String')) returns contents of editJointMin as a double


% --- Executes during object creation, after setting all properties.
function editJointMin_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editJointMin (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 editJointMax_Callback(hObject, eventdata, handles)
% hObject    handle to editJointMax (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 editJointMax as text
%        str2double(get(hObject,'String')) returns contents of editJointMax as a double


% --- Executes during object creation, after setting all properties.
function editJointMax_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editJointMax (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 pbSetLimit.
function pbSetLimit_Callback(hObject, eventdata, handles)
% hObject    handle to pbSetLimit (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_RESET.
function pb_RESET_Callback(hObject, eventdata, handles)
% hObject    handle to pb_RESET (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
initialize_gui(gcbf, handles, true);


% --- Executes on button press in pb_pointwithMouse.
function pb_pointwithMouse_Callback(hObject, eventdata, handles)
% hObject    handle to pb_pointwithMouse (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
rotate3d off
%pause
%p = select3d(2)
target = ginput(1)
set(handles.editPx,'String',mat2str(target(1)));
set(handles.editPy,'String',mat2str(target(2)));
Platform.targetPx = target(1);
Platform.targetPy = target(2);

guidata(handles.figure1, handles);
%pb_Draw_Callback(hObject, eventdata, handles);
ters_kinematik_gui_anaprog('platform_OK_Callback',hObject,eventdata,guidata(hObject))
rotate3d on;
guidata(handles.figure1, handles);


function pb_Forward_Kinematics_Callback(hObject, eventdata, handles)
%Ters kinematik çözümü 3-3 veya 6-3 için analitik olarak yap?labilmektedir.
%Buradaki kodlar 6-3 çözümünü analitik olarak yapacakt?r. 
%6-6 çözümü için newton-rapson yönetimi ayr?ca tan?mlanacakt?r.

%bacak uzunluklar? veriliyor L1,..,L6 --> SCALER values.
% L1 = handles.Leg.L1;L2 = handles.Leg.L2;L3 = handles.Leg.L3;
% L4 = handles.Leg.L4;L5 = handles.Leg.L5;L6 = handles.Leg.L6;

% %Said N. Yurt Say?sal Örne?ine göre veriler : 
% L1 =7;L2 =7;L3 =8;L4 =8;L5 =8;L6 =8;
% %NANUA 
L1 =5.0;    L2 =4.5;    L6 =5.0;
L3 =5.5;    L5 =5.5;    L4 =5.7;

% platformun kenar uzunluklar? verilmelidir. P1,..,P6 noktalar?n?n verildi?i
% dü?ünüldü?ünden bu uzunluklar hesaplanabilmektedir. SCALER VALUES.
% platform_length_1 = norm( Platform.P1 - Platform.P3 ) 
% platform_length_2 = norm( Platform.P4 - Platform.P6 ) 
% platform_length_3 = norm( Platform.P5 - Platform.P2 ) 

% %NANUA
platform_length_1 = 3;
platform_length_2 = 2;
platform_length_3 = 2;


%sabit platformun kö?e noktalar?n?n pozisyon vektörleri --> ROW VECTOR
% B1 = Base1; B2 = Base2; B3 = Base3; 
% B4 = Base4; B5 = Base5; B6 = Base6; 
% %N.yurt
% B1 = [ 5.147 , 0.971 , 0];B2 = [ -2.365 , 4.612 , 0];B3 = [ 0.88 , -2.16 , 0];
% B4 = [ 2.929 , -2.356 , 0];B5 = [ 1.379 , 6.017 , 0];B6 = [ -2.32 , 0.24 , 0];
% %NANUA
B1 = [ -2.9 , -0.9 , 0];    B2 = [ -1.2 , 3.0 , 0];
B3 = [  1.3 , -2.3 , 0];    B4 = [ -1.2 , -3.7 , 0];
B5 = [  2.5 ,  4.1 , 0];    B6 = [ 3.2 , 1.0 , 0];


%sabit platformun kenar uzunluklar? bulunur. --> SCALER VALUES
B1B2 = norm( B1 - B2 );
B3B4 = norm( B3 - B4 );
B5B6 = norm( B5 - B6 );
% O1, O2, O3 daire merkezinin konumu hesaplan?r. --> SCALER VALUES
LB1O1 = ( L1^2 - L2^2 + B1B2^2 ) / ( 2 * B1B2 );    %denklem 2.5
%LO1B2 = B1B2 - LB1O1;                               %denklem 2.5

LB3O2 = ( L4^2 - L3^2 + B3B4^2 ) / ( 2 * B3B4 );    %denklem 2.6
%LO2B4 = B3B4 - LB3O2;                               %denklem 2.6

LB5O3 = ( L6^2 - L5^2 + B5B6^2 ) / ( 2 * B5B6 );    %denklem 2.7
%LO3B6 = B5B6 - LB5O3;                               %denklem 2.7

%P101 aras?ndaki uzunlu?a q1 denilmi?tir.
q1 = sqrt( L1^2 - LB1O1^2 );                        %denklem 2.8
%P302 aras?ndaki uzunlu?a q2 denilmi?tir.
q2 = sqrt( L6^2 - LB3O2^2 );                        %denklem 2.9
%P503 aras?ndaki uzunlu?a q3 denilmi?tir.
q3 = sqrt( L4^2 - LB5O3^2 );                        %denklem 2.10

%i = unit vector x  -- j = unit vector y -- z = unit vector z
unit_x = [ 1 0 0 ];             %i
unit_y = [ 0 1 0 ];             %j
unit_z = [ 0 0 1 ];             %k

%beta1x = u1 birim vektörünün xy düzlemindeki izdü?ümünün x koordinat
%ekseni ile yapt??? aç?d?r
%beta1x = acos( dot (   cross ( (B2-B1) , unit_z )/ norm( cross( (B2-B1) , unit_z ) ) , unit_x )); %fark? yoktur.
%beta1x = acos( dot (   cross ( (B2-B1) , unit_z ) , unit_x ) / norm( cross( (B2-B1) , unit_z ) ))
beta1x = asin( dot( cross( (B2-B1) , unit_z ) , unit_y ) / norm( cross( (B2-B1) , unit_z ) ) );
radtodeg(beta1x)
%denklem 2.14
% beta2x = acos( dot( cross( (B4-B3) , unit_z ) , unit_x ) / norm( cross( (B4-B3) , unit_z ) ) );
beta2x = -asin( dot( cross( (B4-B3) , unit_z ) , unit_y ) / norm( cross( (B4-B3) , unit_z ) ) );
radtodeg(beta2x)
%denklem 2.15
%beta3x = acos( dot( cross( (B6-B5) , unit_z ) , unit_x ) / norm( cross( (B6-B5) , unit_z ) ) );
beta3x = -asin( dot( cross( (B6-B5) , unit_z ) , unit_y ) / norm( cross( (B6-B5) , unit_z ) ) );
radtodeg(beta3x)
%denklem 2.16

% % O1 = cross ( (B2-B1) , unit_z );
% O1 = (B2-B1);
% line('XData',[ 0 O1(1)],'YData',[ 0 O1(2)],'ZData',[ 5 5 ],'Color','c','LineWidth',3,'Marker','< ','MarkerFaceColor','b','MarkerSize',5);
% O1 = cross ( (B2-B1) , unit_z )%/ norm( cross( (B2-B1) , unit_z ) )
% line('XData',[ 0 O1(1)],'YData',[ 0 O1(2)],'ZData',[ 4 4 ],'Color','m','LineWidth',3,'Marker','< ','MarkerFaceColor','b','MarkerSize',5);
% 
% % O2 = cross( (B4-B3) , unit_z )
% O2 = (B4-B3)
% line('XData',[ 0 O2(1)],'YData',[ 0 O2(2)],'ZData',[ 5 5],'Color','c','LineWidth',3,'Marker','<','MarkerFaceColor','b','MarkerSize',5);
% O2 = cross( (B4-B3) , unit_z )%/norm( cross( (B4-B3) , unit_z ) )
% line('XData',[ 0 O2(1)],'YData',[ 0 O2(2)],'ZData',[ 4 4],'Color','m','LineWidth',3,'Marker','<','MarkerFaceColor','b','MarkerSize',5);
% 
% % O3 = cross( (B6-B5) , unit_z )
% O3 = (B6-B5)
% line('XData',[ 0 O3(1)],'YData',[ 0 O3(2)],'ZData',[ 5 5],'Color','c','LineWidth',3,'Marker','<','MarkerFaceColor','b','MarkerSize',5);
% O3 = cross( (B6-B5) , unit_z ) %/ norm( cross( (B6-B5) , unit_z ) ) 
% line('XData',[ 0 O3(1)],'YData',[ 0 O3(2)],'ZData',[ 4 4],'Color','m','LineWidth',3,'Marker','<','MarkerFaceColor','b','MarkerSize',5);


%beta1y = u1 birim vektörünün xy düzlemindeki izdü?ümünün y koordinat
%ekseni ile yapt??? aç?d?r. Bu aç?n?n kullan?lmas?na gerek yoktur çünkü
%cos(90-beta1x)= sin ( beta1x ) ile beta1x 'in sinüsü ile hesap
%yap?lmaktad?r.
%beta1y = asin( dot (   cross ( (B2-B1) , unit_z ) , unit_x ) / norm( cross( (B2-B1) , unit_z ) ) )
%beta2y = asin( dot (   cross ( (B4-B3) , unit_z ) , unit_x ) / norm( cross( (B4-B3) , unit_z ) ) )
%beta3y = asin( dot (   cross ( (B6-B5) , unit_z ) , unit_x ) / norm( cross( (B6-B5) , unit_z ) ) )

% O1 , O2 , O3 pozisyon vektörleri --> ROW VECTOR
O1 = B1 +  LB1O1 * ( (B2-B1) / norm(B2-B1)   );     %denklem 2.17
O2 = B3 +  LB3O2 * ( (B4-B3) / norm(B4-B3)   );     %denklem 2.18
O3 = B5 +  LB5O3 * ( (B6-B5) / norm(B6-B5)   );     %denklem 1.19
%O1(1) = O1x , O1(2)=O1y , O1(3)=O1z=0

%O1 = [ 0 0 0 ];%O2 = [ 0 0 0 ];%O3 = [ 0 0 0 ];
% handles.Base.O1line = line('XData',[ 0 O1(1)],'YData',[ 0 O1(2)],'ZData',[ 0 O1(3)],'Color','yellow','LineWidth',3,'Marker','>','MarkerFaceColor','b','MarkerSize',5);
% handles.Base.O2line = line('XData',[ 0 O2(1)],'YData',[ 0 O2(2)],'ZData',[ 0 O2(3)],'Color','yellow','LineWidth',3,'Marker','>','MarkerFaceColor','b','MarkerSize',5);
% handles.Base.O3line = line('XData',[ 0 O3(1)],'YData',[ 0 O3(2)],'ZData',[ 0 O3(3)],'Color','yellow','LineWidth',3,'Marker','>','MarkerFaceColor','b','MarkerSize',5);

% set( handles.Base.O1line, 'XData',[ 0 O1(1)],'YData',[ 0 O1(2)],'ZData',[ 2 2] );
% set( handles.Base.O2line, 'XData',[ 0 O2(1)],'YData',[ 0 O2(2)],'ZData',[ 2 2] );
% set( handles.Base.O3line, 'XData',[ 0 O3(1)],'YData',[ 0 O3(2)],'ZData',[ 2 2] );

text( O1(1)-5,O1(2),0,'\bf\fontname{Tahoma}\fontsize{12}\color{black}O_1');
text( O2(1)+3,O2(2)-5,0,'\bf\fontname{Tahoma}\fontsize{12}\color{black}O_2');
text( O3(1)+2,O3(2)+2,0,'\bf\fontname{Tahoma}\fontsize{12}\color{black}O_3');


% syms O1x O1y O2x O2y O3x O3y
% syms O1 O2 O3
% O1 = [ O1x , O1y , 0 ];O2 = [ O2x , O2y , 0 ];O3 = [ O3x , O3y , 0 ];
% %phi1, u1 vektörünün xy düzlemi ile yapt??? aç?d?r. Bu bilinmeyen
% %de?i?kendir. Denklem çözme i?lemlerinin sonucunda bu elde edilmeye
% %çal???lmaktad?r.
% syms beta1x beta2x beta3x
syms phi1 phi2 phi3
%syms cos_phi1 cos_phi2 cos_phi3
%syms sin_phi1 sin_phi2 sin_phi3
syms u1 u2 u3
% syms beta1x beta2x beta3x
% syms platform_length_1 platform_length_2 platform_length_3
% syms q1 q2 q3
% 
% % tan(phi1/2) dönü?ümü yap?lmadan denklemler MATLAB sym engine solve komutu 
% % ile çözülemedi.dolay?s?yla BEZOUT metoduna göre yap?lmaktad?r.
% syms x1 x2 x3
% %x1 = tan( phi1 / 2 ) 
% % cos_phi1 = -1 * ( x1^2 - 1 / x1^2 + 1 )
% % cos_phi2 = -1 * ( x2^2 - 1 / x2^2 + 1 )
% % cos_phi3 = -1 * ( x3^2 - 1 / x3^2 + 1 )
% syms sin_phi1 sin_phi2 sin_phi3
% % sin_phi1 = 2 * x1 / ( x1^2 + 1 )
% % sin_phi2 = 2 * x2 / ( x2^2 + 1 )
% % sin_phi3 = 2 * x3 / ( x3^2 + 1 )
% cos_phi1 = -1 * ( x1^2 - 1 / x1^2 + 1 )
% cos_phi2 = -1 * ( x2^2 - 1 / x2^2 + 1 )
% cos_phi3 = -1 * ( x3^2 - 1 / x3^2 + 1 )
% sin_phi1 = 2 * x1 / ( x1^2 + 1 )
% sin_phi2 = 2 * x2 / ( x2^2 + 1 )
% sin_phi3 = 2 * x3 / ( x3^2 + 1 )
% 
% %u1 = P1 vektörünün do?rultusundaki birim vektördür. 
% % [x y z ] ?eklinde üç bile?eni olmal?d?r ve bu üç bile?ende sembolik
% % olarak denklemler içermektedir. 
 u1 =  cos(beta1x)*cos(phi1)*unit_x +...
       sin(beta1x)*cos(phi1)*unit_y +...
       sin(phi1)*unit_z
%  u1 =  cos(beta1x)*cos_phi1*unit_x +...
%        sin(beta1x)*cos_phi1*unit_y +...
%        sin_phi1*unit_z
% 
% 
 u2 =  cos(beta2x)*cos(phi2)*unit_x +...
       sin(beta2x)*cos(phi2)*unit_y +...
       sin(phi2)*unit_z
%  u2 =  cos(beta2x)*cos_phi2*unit_x +...
%        sin(beta2x)*cos_phi2*unit_y +...
%        sin_phi2*unit_z
% 
 u3 =  cos(beta3x)*cos(phi3)*unit_x +...
       sin(beta3x)*cos(phi3)*unit_y +...
       sin(phi3)*unit_z
%  u3 =  cos(beta3x)*cos_phi3*unit_x +...
%        sin(beta3x)*cos_phi3*unit_y +...
%        sin_phi3*unit_z
%    
%    

D1 =  2*q1*cos(beta1x) * ( O1(1)-O2(1) ) + 2*q1*sin(beta1x) * ( O1(2)-O2(2) ); 
D2 = -2*q2*cos(beta2x) * ( O1(1)-O2(1) ) - 2*q2*sin(beta2x) * ( O1(2)-O2(2) );
D3 = -2*q1*q2*cos(beta1x-beta2x); 
D4 = -2*q1*q2; 
D5 = ( O1(1)-O2(1) )^2 + ( O1(2)-O2(2) )^2 +q1^2 + q2^2 - platform_length_1^2; 

%denklem 2.38 
E1 =  2*q2*cos(beta2x) * ( O2(1)-O3(1) ) + 2*q2*sin(beta2x) * ( O2(2)-O3(2) ); 
E2 = -2*q3*cos(beta3x) * ( O2(1)-O3(1) ) - 2*q3*sin(beta3x) * ( O2(2)-O3(2) );
E3 = -2*q2*q3*cos(beta2x-beta3x); 
E4 = -2*q2*q3; 
E5 = ( O2(1)-O3(1) )^2 + ( O2(2)-O3(2) )^2 +q2^2 + q3^2 - platform_length_2^2; 
%denklem 2.39
F1 =  2*q3*cos(beta3x) * ( O3(1)-O1(1) ) + 2*q3*sin(beta3x) * ( O3(2)-O1(2) ); 
F2 = -2*q1*cos(beta1x) * ( O3(1)-O1(1) ) - 2*q1*sin(beta1x) * ( O3(2)-O1(2) );
F3 = -2*q3*q1*cos(beta3x-beta1x); 
F4 = -2*q3*q1; 
F5 = ( O3(1)-O1(1) )^2 + ( O3(2)-O1(2) )^2 +q1^2 + q3^2 - platform_length_3^2; 
%denklem 2.36
G1 = -D1 - D2 + D3 + D5; 
G2 =  D1 - D2 - D3 + D5; 
G3 = 4*D4; 
G4 = -D1 + D2 - D3 + D5; 
G5 =  D1 + D2 + D3 + D5; 
%denklem 2.42
H1 = -E1 - E2 + E3 + E5; 
H2 =  E1 - E2 - E3 + E5; 
H3 =  4*E4; 
H4 = -E1 + E2 - E3 + E5; 
H5 =  E1 + E2 + E3 + E5; 
%denklem 2.42
J1 = -F1 - F2 + F3 + F5; 
J2 =  F1 - F2 - F3 + F5; 
J3 =  4*F4; 
J4 = -F1 + F2 - F3 + F5; 
J5 =  F1 + F2 + F3 + F5; 

%buraya kadar MAPLE ile ayn? ç?kt?. bundan sonraki k?s?mlar?n MAPLE
%kar??l??? yok çünkü orada bezout metodu var.
%denklem 2.48
M1=(H2*G1-G4*H1)^2; 
M2=(2*(H2*G1-G4*H1)*(G2*H2-G5*H1))-(H1*H2*(G3^2)); 
M3=(G2*H2-G5*H1)^2; 
M4=-G1*G3*H2*H3-H1*H3*G3*G4; 
M5=-G2*G3*H2*H3-H1*H3*G3*G5; 
M6=(2*(H2*G1-H4*G4)*(H5*G1-H4*G4))+(G1*G4*(H3^2)); 
M7=2*(H5*G1-H4*G4)*(G2*H2-G5*H1)+2*(H2*G1-G4*H1)*(G2*H5*G5*H4)+G1*G5*(H3^2)+G2*G4*(H3^2)+H1*H5*(G3^2)+H2*H4*(G3^2); 
M8=2*(G2*H2-G5*H1)*(G2*H5*G5*H4)+G2*G5*(H3^2); 
M9=-H3*H4*G3*G4-H3*H5*G1*G5; 
M10=-H3*H4*G3*G5-H3*H5*G2*G3; 
M11=(H5*G1-H4*G4)^2; 
M12=2*(G1*H5-H4*G4)*(G2*H5-G5*H4)+H4*H5*G3^2; 
M13=(G2*H5-G5*H4)^2;


syms x1 x2 x3

%denklem 2.47
N1=(M1*x1^4)+(M2*x1^2)+M3; 
N2=(M4*x1^3)+M5*x1; 
N3=(M6*x1^4)+(M7*x1^2)+M8; 
N4=(M9*x1^3)+M10*x1; 
N5=(M11*x1^4)+(M12*x1^2)+M13; 

%denklem 2.46
% N1*x3^4 + N2*x3^3 + N3*x3^2 + N4*x3 + N5 = 0



%denklem X.X ?? 
R1=(J1*x1^2)+J4; 
R2=J3*x1; 
R3=(J2*x1^2)+J5; 

SS=[N2*R1-N1*R2    N3*R1-N1*R3                N4*R1      N5*R1; 
    N3*R1-N1*R3    N3*R2-N2*R3+N4*R1        N4*R2+N5*R1  N5*R2; 
         R1             R2                      R3        0; 
          0             R1                      R2        R3    ]; 

      
      
SSp=sym2poly(det(SS))

sols=roots(SSp)

bb=0;
sols2=[]; 

for aa=1:16  
    if sols(aa,1)>0  
        if imag(sols(aa,1))==0 
            bb=bb+1;
            sols2(bb,1)=sols(aa,1); 
        end 
    end 
end 

cc=0; 
for aa=1:bb 
    if roots([(G1*sols2(aa,1)^2+G2) G3*sols2(aa,1) G4*sols2(aa,1)^2+G5])>0  
        if imag(roots([(G1*sols2(aa,1)^2+G2) G3*sols2(aa,1) G4*sols2(aa,1)^2+G5]))==0 
       cc=cc+1;x2(cc,:)=roots([(G1*sols2(aa,1)^2+G2) G3*sols2(aa,1) G4*sols2(aa,1)^2+G5]); 
        end 
    end 
end 
dd=0; 
for aa=1:bb 
    if roots([(J1*sols2(aa,1)^2+J4) J3*sols2(aa,1) J2*sols2(aa,1)^2+J5])>0  
        if imag(roots([(J1*sols2(aa,1)^2+J4) J3*sols2(aa,1) J2*sols2(aa,1)^2+J5]))==0 
        dd=dd+1;x3(dd,:)=roots([(J1*sols2(aa,1)^2+J4) J3*sols2(aa,1) J2*sols2(aa,1)^2+J5]); 
        end 
    end 
end 

%h=0; h2=0; 
ee=0;ff=1; 
for aa=1:cc 
    for ee=1:dd 
    
        h(ff,1)=(H1*x3(ee,1)^2+H4)*x2(aa,1)^2+H3*x3(ee,1)*x2(aa,1)+H2*x3(ee,1)^2+H5;
        h2(ff,1)=x2(aa,1);h3(ff,1)=x3(ee,1); 

        h(ff,2)=(H1*x3(ee,1)^2+H4)*x2(aa,2)^2+H3*x3(ee,1)*x2(aa,2)+H2*x3(ee,1)^2+H5;
        h2(ff,2)=x2(aa,2);h3(ff,2)=x3(ee,1); 

        h(ff,3)=(H1*x3(ee,2)^2+H4)*x2(aa,1)^2+H3*x3(ee,2)*x2(aa,1)+H2*x3(ee,2)^2+H5;
        h2(ff,3)=x2(aa,1);h3(ff,3)=x3(ee,2); 

        h(ff,4)=(H1*x3(ee,2)^2+H4)*x2(aa,2)^2+H3*x3(ee,2)*x2(aa,2)+H2*x3(ee,2)^2+H5;
        h2(ff,4)=x2(aa,2);h3(ff,4)=x3(ee,2); 
        ff=ff+1;  
    end 
end

 hm=[h(:,1);h(:,2);h(:,3);h(:,4)];
 hm2=[h2(:,1);h2(:,2);h2(:,3);h2(:,4)]; 
 
 hm3=[h3(:,1);h3(:,2);h3(:,3);h3(:,4)]; 
 for aa=1:size(hm) 
     if min(abs(hm))==hm(aa,1) 
         x_2=hm2(aa,1); 
         x_3=hm3(aa,1); 
     end 
 end 
g1=[(x_2^2)*G1+G4 G3*x_2 (G2*x_2^2)+G5]; 
x_111=roots(g1); 
x_11=x_111(1,1); 
x_12=x_111(2,1); 
digits(3) 
for aa=1:bb 
    if vpa(x_11)==vpa(sols2(aa,1)) 
        x_1=sols2(aa,1) 
    elseif vpa(x_12)==vpa(sols2(aa,1)) 
        x_1=sols2(aa,1) 
    end 
end 
phi_1=2*atan(x_1);phi_2=2*atan(x_2);phi_3=2*atan(x_3)
omega_1=[cos(beta1x)*cos(phi_1);sin(beta1x)*cos(phi_1);sin(phi_1)]; 
omega_2=[cos(beta2x)*cos(phi_2);sin(beta2x)*cos(phi_2);sin(phi_2)]; 
omega_3=[cos(beta3x)*cos(phi_3);sin(beta3x)*cos(phi_3);sin(phi_3)]; 
P_1=PO1+q1*omega_1;P_2=PO2+m2*omega_2;P_3=PO3+m3*omega_3;

P_1
P_2
P_3






function Jacobian_Callback(hObject, eventdata, handles)

% Jacobian_GSP;

% 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');   
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %       TERS KINEMATIK
% for index = 1:1:6  
%         Platform.PBase(index,:) = ...
%             Platform.Rotation * Platform.P(index,:)' + Platform.Translate;
% end
%    
% Leg.unit_Lvektor = ones(6,3)*NaN;
% Leg.Lvektor = ones(6,3)*NaN;
%  
% 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_Lvektor(index,:) =  Leg.Lvektor(index,:) / norm( Leg.Lvektor(index,:) );         
%   end
% end
% 
% disp('unit vectors');
% Leg.unit_Lvektor
% 
% %P = [ px , py , pz ];
% P =  Platform.Translate;
% %w = [ wx , wy , wz ];
% w = [  Platform.alpha ,  Platform.beta , Platform.gamma ];
% 
% %    Jx = zeros( 6 , 6 );
% index=1;
% 
% 
% %Limitation içinde alpha beta gamma px py pz var.
% %limitation içindekilere göre sütunları silelim.
% LimitationMatrix = [ Limitation.Px , Limitation.Py , Limitation.Pz ,...
%                      Limitation.Alpha , Limitation.Pz , Limitation.Gamma ];
%                             
% Lim2 = find( xor(LimitationMatrix,[1 1 1 1 1 1]) );
%                  
% for i = 1:1:6
%   if ~isnan(ConMatrix(1,i)) && ~isnan(ConMatrix(2,i))     
%         Jx(index,:) = [ Leg.unit_Lvektor(i,:) cross( Platform.PBase(ConMatrix(2,i),:) , Leg.unit_Lvektor(i,:)' ) ];
%         
%         %JJx(index,:) = [ Leg.unit_Lvektor(i,:) cross( Platform.PBase(ConMatrix(2,i),:) , Leg.unit_Lvektor(i,:)' ) ];
% 
%         %dot( s_i , p )
%         
%         index=index+1;
%         
%   end
% end
% 
%     JJx = Jx;
%     
%     %JJx = Jx[LimitationMatrix];
%     
%     Jx = JJx(:,Lim2)
% 
%     disp('Jx');
%     Jx
%     %%inv(Jx)
%     inv(Jx)*Jx;
%     Jx*inv(Jx);
%     
%     [sat,sut] = size(pinv(Jx))
%     
%     inv(Jx)*eye(sat,sut)
% 
%     handles.AnalysisFigure = gcf;
%     handles.JacobianFigure = figure('Position',[20 60 1024 768]);
%     %%%%%%%%%%%%%%%%  Jacobian Matrix Jx
%     uicontrol('Style','text',...
%         'Position',[100 155 220 20],...
%         'String','Jacobian Matrix Jx')
%     ut1=uitable('Position',[0 10 605 140]);
%     set(ut1,'Data',Jx, ...
%             'ColumnWidth',{95})
% 
%     %%%%%%%%%%%%%%%%  Jacobian Matrix Jx
%     uicontrol('Style','text',...
%         'Position',[100 495 220 20],...
%         'String','Jacobian Matrix Jq')
%     ut1=uitable('Position',[0 350 305 140]);
%     set(ut1,'Data',eye(size(Jx)), ...
%             'ColumnWidth',{45})
%         
%         
%      %%%%%%%%%%%%%%%%  Unit vectors of each Legs
%     uicontrol('Style','text',...
%         'Position',[100 315 220 20],...
%         'String','Unit vectors of the Legs')
%     ut1=uitable('Position',[0 180 325 140],'ColumnName',{'x','y','z'},...
%                 'RowName',{'L1','L2','L3','L4','L5','L6',});
%     set(ut1,'Data',Leg.unit_Lvektor, ...
%             'ColumnWidth',{95})
%       
        
    %pause(5);
    
   %figure( handles.AnalysisFigure );   
     
%     disp('JIA');
%     JIA = blkdiag ( Leg.unit_Lvektor(1,:)' , Leg.unit_Lvektor(2,:)' , Leg.unit_Lvektor(3,:)' ,...
%                   Leg.unit_Lvektor(4,:)' , Leg.unit_Lvektor(5,:)' , Leg.unit_Lvektor(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 = Platform.currentAlpha;
% beta = Platform.currentBeta;
% gamma = Platform.currentGamma;
% 
% 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

  

function checkbox_showNames_Callback(hObject, eventdata, handles)
checked = get(hObject, 'Value');


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 ~checked
    for index = 1:1:length(Basetext)
       set(Basetext(index),'String','');
    end   
    for index = 1:1:length(Platform.Ptext)
       set(Platform.Ptext(index), 'Position', [-5,-5,0]);        
       set(Platform.Ptext(index),'String','');
    end
    for index = 1:1:length(handles.Leg.Ltext)
       set(handles.Leg.Ltext(index), 'Position', [-5,-5,0]);        
        set(handles.Leg.Ltext(index),'String',' ');      
    end
    
else
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
    % Noktalar?n isimlerinin ekranda gösterilmesi için bir checkbox
    % kullan?labilir. önce checkbox de?eri kontrol edilip daha sonra ekrana
    % yaz?d?r?labilir. Önce B ve P noktalar?n?n isimleri yazd?r?l?r. Daha
    % sonrada L bacaklar?n?n isimleri bacak üzerinde bir yerlere yazd?r?l?r.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
    for index = 1:1:6
        if ~isnan(ConMatrix(1,index)) && ~isnan(ConMatrix(2,index))

       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         
       %B nokta isimlerinin yazd?r?lmas?
       B = Base.BTable(ConMatrix(1,index),:);
       txt = ['\bf\fontname{Tahoma}\fontsize{12}\color{black}B_' int2str(ConMatrix(1,index))];
       Basetext(index)=text( B(1)+3,B(2)-3,B(3),txt );

       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
       %P nokta isimlerinin yazd?r?lmas?
       P = Platform.PBase(ConMatrix(2,index),:);
       txt = ['\bf\fontname{Tahoma}\fontsize{12}\color{black}P_' int2str(ConMatrix(2,index))];
       Platform.Ptext(index)=text( P(1)-5,P(2)+5,P(3),txt );

       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
       %L bacak isimlerinin yazd?r?lmas?

       %önce bacak vektörünün do?rultusundaki birim vektör hesaplan?r
       L_vector = Platform.PBase(ConMatrix(2,index),:) - Base(ConMatrix(1,index),:);

       unit_Lvector =  L_vector / norm ( L_vector ) ;
        %bacak birim vektörün yar? uzunlu?undaki noktaya L ismi yazd?r?l?r.
        nokta = unit_Lvector * handles.Leg.L(index)/2 + ...
                Base(ConMatrix(1,index),:); 

        txt = ['\bf\fontname{Tahoma}\fontsize{12}\color{black}L_' int2str(index)];
        handles.Leg.Ltext(index)=text( nokta(1),nokta(2)+5,nokta(3), txt);

        end

    end
end
guidata(hObject, handles);


function checkbox_isBroken_Callback(hObject, eventdata, handles)



function Workspace_Callback(hObject, eventdata, handles)

workspaceSTEWSIM;

disp('Workspace fonksiyonu dıoşındayız');
return
%WORKSPACE ANALYSIS OF THE STEWART PLATFORM
px_start = -20;      px_end = 20;
py_start = -20;      py_end = 20;
pz_start = 25;      pz_end = 25;

step_size = 0.5;
UNreachable = zeros(1,3); %zeros( ( px_end - px_start ) / step_size  , 3 );
reachable = zeros(1,3); %zeros( ( px_end - px_start ) / step_size  , 3 );

for x=px_start : step_size : px_end
    for y=py_start : step_size : py_end
        for z=pz_start : step_size : pz_end
            
            Platform.currentPx = x;
            Platform.currentPy = y;
            Platform.currentPz = z;       
        
            T_step =[Platform.currentPx; Platform.currentPy;Platform.currentPz; ];
            Platform.Translate = T_step;
           
            %Fix orientation
            alpha = Platform.currentAlpha;  
            beta  = Platform.currentBeta; 
            gamma = Platform.currentGamma;             
          
            
%             Platform.Rotation =[ cos(alpha)*cos(beta) , cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma)...
%      , cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma) ;
%        sin(alpha)*cos(beta) , sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma) ,  sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma) ;
%        -sin(beta)           ,        cos(beta)*sin(gamma)                         ,  cos(beta)*cos(gamma)     ;
%        ];   

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

            
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %       TERS KINEMATIK
        %  Ustteki harakettli platformun her bir kö?esinin konumu alttaki
        %  koordinat eksenine göre yaz?lmaktad?r. 
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %platform coordinates according to the base coordinate frame   
        %NaN as a number doesnt cause a problem. But NaN as an index is a fatal
        %error.
        for index = 1:1:6

                Platform.PBase(index,:) = ...
                    Platform.Rotation * Platform.P(index,:)' + Platform.Translate;
        end

        %yap? sorunlumu diye kontrol etmek için
        yapi_sorunlumu = false;
        %hangi bacaklarda sorun oldu?unu tutan sat?r vektörü, (index) eri?imi için
        %sat?r vektörü olarak seçilmi?tir.
        yapi_sorunlari = ones(6,1)*NaN;

        for index = 1:1:6
            if ~isnan(ConMatrix(1,index)) && ~isnan(ConMatrix(2,index))  
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%                   
        %Leg length        
                handles.Leg.L(index) = norm ( Platform.PBase(ConMatrix(2,index),:)...
                                            - Base(ConMatrix(1,index),:) );
    
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        %   BACAK limit kontrollerinin yapılması
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
            if handles.Leg.L(index) >= handles.Leg.LMax
                yapi_sorunlumu = true;
                yapi_sorunlari(index) = index;
                
                

                %yapi sorunlu oldu?unda bacaklar için birim vektörler hesaplanmal?d?r.
                %Lvektor = Platform.PBase(ConMatrix(2,index),:) - Base(ConMatrix(1,index),:);     
                %unit_Lvektor =  Lvektor / norm( Lvektor );

                %birim vektör ile LMAX çarp?l?nca baca??n olabilecek en büyük
                %de?eri vektörel olarak hesaplanm?? olacakt?r.
                %Lvektor = unit_Lvektor * handles.Leg.LMax+Base(ConMatrix(1,index),:);

                %set ( handles.Leg.Leg_handle(index) , ...
                %    'XData',[Base(ConMatrix(1,index),1); Lvektor( 1 ) ],...
                %    'YData',[Base(ConMatrix(1,index),2); Lvektor( 2 ) ],...
                %    'ZData',[Base(ConMatrix(1,index),3); Lvektor( 3 ) ] );

            elseif handles.Leg.L(index)<= handles.Leg.LMin 
                yapi_sorunlumu = true;
                yapi_sorunlari(index) = index;

                %ulaşılamayan noktalara arasına x,y,z değerlerini ekle.
                %yönelimde ilave edilebilir.                 
                UNreachable = [ UNreachable ; x , y , z ];
                

                %yapi sorunlu oldu?unda bacaklar için birim vektörler hesaplanmal?d?r.
                %Lvektor = Platform.PBase(ConMatrix(2,index),:) - Base(ConMatrix(1,index),:);     
                %unit_Lvektor =  Lvektor / norm( Lvektor );

                %birim vektör ile LMAX çarp?l?nca baca??n olabilecek en büyük
                %de?eri vektörel olarak hesaplanm?? olacakt?r.
                %Lvektor = unit_Lvektor * handles.Leg.LMin+Base(ConMatrix(1,index),:);

                %set ( handles.Leg.Leg_handle(index) , ...
                %    'XData',[Base(ConMatrix(1,index),1); Lvektor( 1 ) ],...
                %    'YData',[Base(ConMatrix(1,index),2); Lvektor( 2 ) ],...
                %    'ZData',[Base(ConMatrix(1,index),3); Lvektor( 3 ) ] );

       
            end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%                       
        %set editbox with Leg lengths
                %eval( sprintf( 'set(handles.textL%d, ''String'', %f)',index, handles.Leg.L(index) ) );

            end
        end
        
        if ( yapi_sorunlumu == true ) 
            %ulaşılamayan noktalara arasına x,y,z değerlerini ekle.
                %yönelimde ilave edilebilir. 
                UNreachable = [ UNreachable ; x , y , z ];
        else
             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%                   
        % Erişilebilir Çalışma Uzayına eklenebilir
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%                           
                %x y z değerleri erişilebilir çalışma uzayına eklenir.
                reachable = [ reachable ; x , y , z ];
        end
        

        
   
        end %z
    end%y
end%x

disp('Workspace hesaplandı');
%UNreachable
%plot3(UNreachable(:,1),UNreachable(:,2),UNreachable(:,3));
%reachable
%plot3(reachable(:,1),reachable(:,2),reachable(:,3));


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

if ( ishandle(hStewSIM)==1 )
    set(hStewSIM,'visible','on');
else
   %% STewSIM_1
   AnalysisGSP;
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 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.editAlpha,'Enable', 'on' );
set(handles.editBeta,'Enable', 'on' );
set(handles.editGamma,'Enable', 'on' );

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

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



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


% --- Executes during object creation, after setting all properties.
function Time_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Time (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 Resolution_Callback(hObject, eventdata, handles)
% hObject    handle to Resolution (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 Resolution as text
%        str2double(get(hObject,'String')) returns contents of Resolution as a double


% --- Executes during object creation, after setting all properties.
function Resolution_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Resolution (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 popupmenu1.
function popupmenu1_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (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 popupmenu1 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu1


% --- Executes during object creation, after setting all properties.
function popupmenu1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (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 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)
% Save the new volume value
Rotation_Matrix = getappdata(0,'Rotation_Matrix');

Alpha = str2double( get(handles.editAlpha, 'String') );
Beta = str2double(get(handles.editBeta, 'String'));
Gamma = str2double(get(handles.editGamma, '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.text_Time, 'Visible', 'on' );
set( handles.text82, 'Visible', 'on' );
set(handles.Time, 'Enable', 'on', 'Visible', 'on' );
set(handles.Resolution, 'Enable', 'on', 'Visible', 'on' );

set( handles.left_line, 'Visible', 'on' );
set( handles.right_line, 'Visible', 'on' );

set( handles.pb_runmanipulator, 'enable', 'on' , 'Visible', 'on' );



% --- Executes on button press in pb_runmanipulator.
function pb_runmanipulator_Callback(hObject, eventdata, handles)
% hObject    handle to pb_runmanipulator (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set( handles.platform_OK, 'enable', 'on' , 'Visible', 'on' );
set( handles.pushbutton19, 'enable', 'on' , 'Visible', 'on' );




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

%check current and target parameter 
pt = [ Platform.targetPx , Platform.targetPy, Platform.targetPz , ...
    Platform.targetAlpha, Platform.targetBeta, Platform.targetGamma ];

pc = [ Platform.currentPx , Platform.currentPy, Platform.currentPz , ...
    Platform.currentAlpha, Platform.currentBeta, Platform.currentGamma ];

%%check for moving . if NO CHANGE require then return..
if ( pt == pc ) 
    return ;
end


%PATH PLANNING = LINEAR PATH PLANNING
step_size = 100;
step_size = str2double ( get(handles.Resolution, 'String') );


time = 2;
time = str2double ( get(handles.Time, 'String') );


total_step_size = time * step_size;
setappdata( 0, 'step_size' , step_size );
setappdata( 0, 'total_step_size' , total_step_size );
setappdata( 0, 'time' , time );


% step_size_Tx = (Platform.targetPx - Platform.currentPx) / step_size;
% step_size_Ty = (Platform.targetPy - Platform.currentPy) / step_size;
% step_size_Tz = (Platform.targetPz - Platform.currentPz) / step_size;
% step_size_Alpha = ( Platform.targetAlpha - Platform.currentAlpha ) / step_size;
% step_size_Beta = ( Platform.targetBeta - Platform.currentBeta ) / step_size;
% step_size_Gamma = ( Platform.targetGamma - Platform.currentGamma ) / step_size;


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


setappdata( 0 , 'trajectory_path_velocity', [ step_size_Tx_dt , step_size_Ty_dt, step_size_Tz_dt ]);
setappdata( 0 , 'trajectory_angle_velocity', [ step_size_Alpha_dt , step_size_Beta_dt, step_size_Gamma_dt ]);
setappdata( 0 , 'trajectory_path_acceleration', [ step_size_Tx_ddt , step_size_Ty_ddt, step_size_Tz_ddt ]);
setappdata( 0 , 'trajectory_angle_acceleration', [ step_size_Alpha_ddt , step_size_Beta_ddt, step_size_Gamma_ddt ]);

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

condition_numbers = zeros( total_step_size,1 );

for i=1:1:total_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);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %       ROTATION MATRIX , Roll-Pitch-Yaw
        %  Ustteki harakettli platformun her bir kö?esinin konumu alttaki
        Rotation_Matrix = getappdata(0,'Rotation_Matrix');
        alpha=deg2rad(Platform.currentAlpha);
        beta=deg2rad(Platform.currentBeta);
        gamma=deg2rad(Platform.currentGamma);
        Platform.Rotation = eval(Rotation_Matrix);
%         Platform.Rotation = Rotation( Platform.currentAlpha, Platform.currentBeta, Platform.currentGamma , 'RPY' ) ;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %           Inverse Kinematic of this position
        [ Leg, Platform.PBase, ErrorMsg ]= inverse_kinematic(Base, Platform, ConMatrix, LegLimits, JointLimits );
        %if ( strcmp( ErrorMsg, ' 'No ERROR' ) ~= 0 ) return end;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        index=1;
        [ sizeNaN , LegNumber ] = LegNumbers( ConMatrix );
        birim_matris66 = eye(LegNumber,LegNumber);
        identity_matrix = eye(LegNumber,LegNumber);
        
        for j = 1:1:6
          if ~isnan(ConMatrix(1,j)) && ~isnan(ConMatrix(2,j))     
%               Jx(index,:) = [ Leg.unit_Lvector(j,:) cross( Platform.PBase(ConMatrix(2,j),:) , Leg.unit_Lvector(j,:)' ) ];
%             Jx(index,:) = [ cross( Platform.Rotation*Platform.PTable(ConMatrix(2,j),:) , Leg.L_vector(j,:)' ) Leg.L_vector(j,:)  ];        
             Jq(index,:) = [ birim_matris66( index,:) .* Leg.L(j,:) ];
             Jx(index,:) = ...
             [ Leg.unit_Lvector(j,:) ...
               cross( ( Platform.Rotation * Platform.PTable(ConMatrix(2,j),:)' )' , Leg.unit_Lvector(j,:)' ) ];
            Jq =  identity_matrix;

%             step_size_Ty_dt
%             q_velocity(index) = Jx(index * 
            
            index=index+1;
          end
        end
        
%         phim = 80;
%         phib = 80;
%         ksi = Platform.Rm - Base.Rb * cos((phim-phib)/2); 
%         
%         L = sqrt( Platform.Rm^2 * ( Platform.currentPx^2 + Platform.currentPy^2 + 2*Platform.currentPz^2 ...
%                               +2*Base.Rb^2*sin( (phim-phib)/2)^2 ) / ...
%                   2*( Platform.currentPx^2 + Platform.currentPy^2 + Platform.currentPz^2 + ksi^2 ) );
%               
%         JX_Normalized = [ Jx(:,1)/L Jx(:,2)/L Jx(:,3)/L Jx(:,4) Jx(:,5) Jx(:,6) ]
%         J_Normalized =inv(Jq)*JX_Normalized;
%         J_Normalized_inverse = pinv( J_Normalized ) ;
% 
% %         J = inv(Jx) *  Jq;
% %         J = inv(Jq) * Jx ;
%           J = Jx ;
%         
%         Jinverse = pinv(J);
% 
%         W= eye(LegNumber,LegNumber) * 1/LegNumber;
%         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         %%%%%%%     condition number 
%         K = sqrt ( trace( J*W*J' ) ) * sqrt( trace( Jinverse*W*Jinverse' ) ) ;
% %         K = sqrt( trace( Jinverse*W*Jinverse' ) ) * sqrt ( trace( J*W*J' ) );        
%         condition_numbers( i ) = 1/K;
%         
%         K2 = sqrt ( trace( J_Normalized*W*J_Normalized' ) ) * sqrt( trace( J_Normalized_inverse*W*J_Normalized_inverse' ) ) ;
%         condition_numbers2( i ) = 1/K2;
%         
        
%         Xdot = [ step_size_Tx_dt(i) , step_size_Ty_dt(i) , step_size_Tz_dt(i) , ...
%           step_size_Alpha_dt(i) , step_size_Beta_dt(i) , step_size_Gamma_dt(i) ]';
%          q_velocity(i,:) = inv( Jq ) * Jx * Xdot;
        

        setappdata( 0 , 'Platform' ,Platform );
        setappdata( 0 , 'Leg' , Leg );
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %           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;
            
            handles = Plot_Mechanism( handles );
            drawnow;
            pause(0.000001);
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        yapi_sorunlumu = false;
        yapi_sorunlari = ones(6,1)*NaN;

        trajectory_path(i,:) = [ Platform.currentPx , Platform.currentPy, Platform.currentPz ];
        trajectory_angles(i,:) = [ Platform.currentAlpha , Platform.currentBeta, Platform.currentGamma ];
        trajectory_leg_length(i,:)  = Leg.L;        
        
%         set(handles.currentPx, 'String', Platform.currentPx );
%         set(handles.currentPy, 'String', Platform.currentPy );
%         set(handles.currentPz, 'String', Platform.currentPz );
%         set(handles.currentAlpha, 'String', Platform.currentAlpha );
%         set(handles.currentBeta, 'String', Platform.currentBeta );
%         set(handles.currentGamma, 'String', Platform.currentGamma );
%         
%         set( handles.textL1 , 'String' , Leg.L(1) ) ; 
%         set( handles.textL2 , 'String' , Leg.L(2) ) ;
%         set( handles.textL3 , 'String' , Leg.L(3) ) ;
%         set( handles.textL4 , 'String' , Leg.L(4) ) ;
%         set( handles.textL5 , 'String' , Leg.L(5) ) ;
%         set( handles.textL6 , 'String' , Leg.L(6) ) ;
% 
%         set(handles.PTable,'Data',Platform.PBase);

%          pause(0.000001);
        
end
% 
% currentFigure = gcf;
% figure(1);
% plot(condition_numbers);
% title('Condition Numbers');
% figure(2);
% plot(condition_numbers2);
% title('Normalized JAcobian');
% condition_numbers
% condition_numbers2



        set(handles.currentPx, 'String', Platform.currentPx );
        set(handles.currentPy, 'String', Platform.currentPy );
        set(handles.currentPz, 'String', Platform.currentPz );
        set(handles.currentAlpha, 'String', Platform.currentAlpha );
        set(handles.currentBeta, 'String', Platform.currentBeta );
        set(handles.currentGamma, 'String', Platform.currentGamma );
        
        
        set( handles.textL1 , 'String' , Leg.L(1) ) ; 
        set( handles.textL2 , 'String' , Leg.L(2) ) ;
        set( handles.textL3 , 'String' , Leg.L(3) ) ;
        set( handles.textL4 , 'String' , Leg.L(4) ) ;
        set( handles.textL5 , 'String' , Leg.L(5) ) ;
        set( handles.textL6 , 'String' , Leg.L(6) ) ;

%         set(handles.PTable,'Data',Platform.PBase);

        

setappdata(0, 'trajectory_path', trajectory_path );
setappdata(0, 'trajectory_angles', trajectory_angles );
setappdata(0, 'trajectory_leg_length', trajectory_leg_length );

setappdata(0, 'q_velocity', q_velocity );
setappdata(0, 'q_acceleration', q_acceleration );

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

setappdata( 0 , 'Platform' ,Platform );

% figure(currentFigure);
% handles = Plot_Mechanism( handles );

            
% trajectory_GSP;

guidata(hObject,handles)




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


% --- Executes on button press in pushbutton19.
function pushbutton19_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton19 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
%%% Motion history
set( handles.text99, 'enable', 'on' , 'Visible', 'on' );
set( handles.frame4, 'enable', 'on' , 'Visible', 'on' );
set( handles.popupmenu1, 'enable', 'on' , 'Visible', 'on' );
