function varargout = infLoop(varargin)
%INFLOOP M-file for infLoop.fig
%      INFLOOP, by itself, creates a new INFLOOP or raises the existing
%      singleton*.
%
%      H = INFLOOP returns the handle to a new INFLOOP or the handle to
%      the existing singleton*.
%
%      INFLOOP('Property','Value',...) creates a new INFLOOP using the
%      given property value pairs. Unrecognized properties are passed via
%      varargin to infLoop_OpeningFcn.  This calling syntax produces a
%      warning when there is an existing singleton*.
%
%      INFLOOP('CALLBACK') and INFLOOP('CALLBACK',hObject,...) call the
%      local function named CALLBACK in INFLOOP.M with the given input
%      arguments.
%
%      *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 infLoop

% Last Modified by GUIDE v2.5 23-Jul-2007 22:13:10

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @infLoop_OpeningFcn, ...
                   'gui_OutputFcn',  @infLoop_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 infLoop is made visible.
function infLoop_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   unrecognized PropertyName/PropertyValue pairs from the
%            command line (see VARARGIN)

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

handles.DM = varargin{1};
handles.H = varargin{2};

% Update handles structure
guidata(hObject, handles);

% init(handles)
% UIWAIT makes infLoop wait for user response (see UIRESUME)
uiwait(handles.infLoop);



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



function pushbutton_stop_Callback(hObject, eventdata, handles)
delete(handles.infLoop);


function init(handles)

    persistent fase;
    
    H = handles.H;
    
    % define uma velocidade igual para todos os servos de 4 segundos por
    % movimento
        defineVel(H, 200, 1, 2, 3, 4);
    
    % ciclo de actuação infinito
    while(strcmp(get(handles.pushbutton_pause, 'String'), 'PAUSE'))
        
        if(isempty(fase) || fase == size(handles.DM.q,1))
            fase=1;
        end
        pause(1);
                
        % executa todas as fases do movimento
        for fase=fase:size(handles.DM.q,1) % percorre todas as fases
                
                if(strcmp(get(handles.pushbutton_pause, 'String'), 'RUN'))
                    return
                end
                set(handles.text_number, 'String', num2str(fase));
                pause(1/5);

                % adaptação das posições ao humanoide real
                    slave = transform(handles.DM.q(fase,:));
                    pos = adapt([], slave(1:4,:), 1);
                    
                % aplicação das posições
                    for scu=1:4,
                        applyjoint(H, scu, 0, pos(scu,:));
                    end
                    waitFinalization(H, [1 2 3 4]); % espera que todos os servos finalizem o movimento
        end

    end

    

function pushbutton_pause_Callback(hObject, eventdata, handles)
    if(strcmp(get(hObject, 'String'), 'PAUSE'))        
        set(hObject, 'String', 'RUN', 'ForegroundColor', 'g')                
    else        
        set(hObject, 'String', 'PAUSE', 'ForegroundColor', 'k')
        init(handles);        
    end
    


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
%   Lixo                                                                  %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%         for fase=fase:4,
% 
%             if(strcmp(get(handles.pushbutton_pause, 'String'), 'RUN'))
%                 return
%             end
% 
%             set(handles.text_number, 'String', num2str(fase));
%             pause(1/2);
%         end