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

% Last Modified by GUIDE v2.5 31-Jul-2007 12:43:14

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @TwoLegs_22dof_OpeningFcn, ...
                   'gui_OutputFcn',  @TwoLegs_22dof_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
end

% --- Executes just before TwoLegs_22dof is made visible.
function TwoLegs_22dof_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 TwoLegs_22dof
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);
% UIWAIT makes TwoLegs_22dof wait for user response (see UIRESUME)
% uiwait(handles.TwoLegs_22dof);
init(handles)
end


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

            
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fim das definições automáticas do GUI-----------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Inicio

            % passa por todos os sectores de actuação para incializar
            % valores e iniciar controlos
            function init(handles)
                    handles.DM = initDM();              % Definição de Movimento
                    handles.ME = initME();              % Movimentos Especiais
                    handles.PM = initPM();              % Plot do Movimento
                    handles.Ctrl = initCtrl();          % Controlo
                    handles.paineis = initPaineis();    % Gestão de Paineis
                    handles.GP = initGP();              % Gestão do Plot
                    handles.JI = initJI();              % Janela inicial
                    handles.Humanoid = initHumanoid();
                    
                    handles.start=0;

                    handles = setDMcontrols(handles);
                    handles = setPaineiscontrols(handles);
                    handles = setMEcontrols(handles);
                    handles = setCtrlcontrols(handles);
                    handles = setPMcontrols(handles);
                    handles = setGPcontrols(handles);
                    handles = setJIcontrols(handles);
                    handles = setHumanoidcontrols(handles);

                    guidata(handles.TwoLegs_22dof, handles);
            end

%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%                
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Janela Inicial

            % Função de Inicialização
                function JI = initJI()
                        JI.comm.COM = 3;
                        JI.comm.BR = 115200;
                        JI.comm.H = 0;
                end
                        
                function handles = setJIcontrols(handles)
                end
                        
                        
                        
        %-----------------------------------------------------------------%
            
 
        
            % Comunicações
                
                % alteração da porta serie para comunicação RS232
                function popupmenu_portasCom_Callback(hObject, eventdata, handles)
                        lista = get(hObject, 'String');
                        index = get(hObject, 'Value');
                        porta_name = lista{index};
                        porta = str2num(porta_name(4:end));
                        handles.JI.comm.COM = porta;
                        guidata(hObject, handles)
                end

                % alteração do BaudRate para comunicação RS232
                function popupmenu_BaudRate_Callback(hObject, eventdata, handles)
                        lista = get(hObject, 'String');
                        index = get(hObject, 'Value');
                        porta_BR = lista{index};
                        BR = str2num(porta_BR);
                        handles.JI.comm.BR = BR;
                        guidata(hObject, handles)
                end

                % Estado da porta série para comunicação RS232 (ligar e
                % desligar)
                function text_ligar_ButtonDownFcn(hObject, eventdata, handles)
                        keyboard
                        if(strcmp(get(hObject, 'String'), 'Ligar'))
                            disp('A ligar...')
                            H = initcom(handles.JI.comm.COM, handles.JI.comm.BR)
                            handles.JI.comm.H = H;
                            guidata(hObject, handles)
                            if(~handles.JI.comm.H)
                                return
                            end
                            set(hObject, 'String', 'Desligar', 'ForegroundColor', 'r')
                            
                            activaPWM(H, 1, 2, 3, 4);
                            
                            handles = actualizaHumanoid(handles);
                            disp('Humanoid actualizado');
                            actualizaPainelHumanoid(handles);
                            start(handles.timer);
                            set(handles.slider_foot_sensors_fresh_rate, 'Enable', 'On');
                            
                            disp('ligado com sucesso');
                        else
                            H = handles.JI.comm.H;
                                for i=1:4,
                                    applyjoint(H, i, 3, [0 0 0]);
                                end
                            handles.JI.comm.H = killcom(handles.JI.comm.H);
                            set(hObject, 'String', 'Ligar', 'ForegroundColor', 'g')
                        end
                        guidata(hObject, handles)
                end
                        
                % enviar a fase actual para o humanoide
                function pushbutton_enviar_Callback(hObject, eventdata, handles)
                        H = handles.JI.comm.H;
                        fase = handles.DM.fase;
                        defineVel(H, 200, 1, 2, 3, 4);

                        slave = transform(handles.DM.q(handles.DM.fase,:));
                        % limitado às primeiras 4 slaves
                        pos = adapt([], slave(1:4,:), 1);

                        for i=1:4,
                            applyjoint(H, i, 0, pos(i,:));
                        end

                end
            
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Paineis
    
            % Função de inicialização
            
                % guarda na estrutura paineis todos os paineis que são para
                % funcionar como TabFolder
                function paineis = initPaineis()
                        paineis.principal = {   'uipanel_DefMov'    ,   'text_DefMov'       ;
                                                'uipanel_Inicio'    ,   'text_Inicio'       ; 
                                                'uipanel_controlo'  ,   'text_controlo'     ;
                                                'uipanel_PlotsMov'  ,   'text_PlotsMov'     ;
                                                'uipanel_Humanoid'  ,   'text_Humanoid'     };

                        paineis.DM =        {   'uipanel_juntas'    ,   'text_juntas'       ;
                                                'uipanel_cartesianas',  'text_cartesianas'  };

                        paineis.ME =        {   'uipanel_andar'     ,   'text_andar'        ;
                                                'uipanel_rodar'     ,   'text_rodar'        ; 
                                                'uipanel_escadas'   ,   'text_escadas'      ;
                                                'uipanel_chutar'    ,   'text_chutar'       ;
                                                'uipanel_outros'    ,   'text_outros'       };

                        paineis.GP =        {   'uipanel_tapete'    ,   'text_tapete'       ;
                                                'uipanel_gp_escadas',   'text_gp_escadas'   ; 
                                                'uipanel_bola'      ,   'text_bola'         };
                                            
                        paineis.Humanoid =  {   'uipanel_servos'    ,   'text_servos'       ;
                                                'uipanel_CoP'       ,   'text_CoP'          };
                
                end
                
                function handles = setPaineiscontrols(handles)
                end
                            
                                        
            % Função de Callback
            
                % função generica de alteração do painel visivel (aplica-se
                % em todos os casos)
                function mudar_painel(hObject, handles, nome_painel)
                    %   nome_painel - nome do campo da estrutura handles com a
                    %   lista dos paineis pertencentes a essa categoria
                    
                        CLA = [1.0000 0.5020 0]; CFA = [0.9255 0.9137 0.8471];  % CLA - Cor das Letras (do painel) Activo
                        CLD = [0 0 1]; CFD = [0.7529 0.7529 0.7529];            % CFA - Cor do Fundo (do painel) Activo
                        lista = getfield(handles.paineis, nome_painel);
                        painel = get(hObject, 'Tag');
                        painel = strcmp(lista, painel);
                        [painel, y] = find(painel, 1);
                        for i=1:length(lista),
                            aux = lista(i,:);
                            h_painel = getfield(handles, aux{1});
                            h_text = getfield(handles, aux{2});
                            if(i~=painel)
                                set(h_painel, 'Visible', 'Off')
                                set(h_text, 'ForegroundColor', CLD)
                                set(h_text, 'BackgroundColor', CFD)
                            else
                                set(h_painel, 'Visible', 'On')
                                set(h_text, 'ForegroundColor', CLA)
                                set(h_text, 'BackgroundColor', CFA)
                            end
                        end

                end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Definição de Movimento


            % Função de Inicialização
                function DM = initDM()
                        DM = DefineRob;
                        DM.q = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
                        DM.XYZRef = [0 0 0];
                        DM.fase = 1;
                        DM.pe = 'dir';
                        DM.duracao = 1;
                        DM.tetaPe = 0;
                        DM.dependencia = zeros(20,20);
                        
                end
                
                
                function handles = setDMcontrols(handles)
                        max = 90;       % 
                        min = -90;      %% é para ser usado com as limitações reais do humanoide
                        passo_x = 1/(max-min);
                        passo_y = 10*passo_x;
                        for i=1:22,
                            h = getfield(handles, ['slider_junta' num2str(i)]);
                            set(h,  'Max', max, ...
                                    'Min', min, ...
                                    'SliderStep', [passo_x passo_y], ...
                                    'Value', 0);

                            h = getfield(handles, ['text_junta' num2str(i)]);
                            set(h, 'string', num2str(0));

                            h = getfield(handles, ['edit_junta' num2str(i) '_lock']);
                            set(h, 'string', num2str(0));
                        end
                        set(handles.pushbutton_Fase_Anterior, 'Enable', 'Off');
                        set(handles.pushbutton_inicio, 'Enable', 'Off');
                end

        %-----------------------------------------------------------------%
            % Menus de Contexto
                
                % guarda valor da slave actual para futura cópia
                function slave_actual(hObject, handles, slave)
                        switch slave,
                            case (1),
                                juntas = [1 2 3];
                            case (2),
                                juntas = [12 11 10];
                            case (3),
                                juntas = [4 5 6];        
                            case (4),
                                juntas = [9 8 7];
                            case (5),
                                juntas = [13 14];
                            case (6),
                                juntas = [17 18 21];
                            case (7),
                                juntas = [19 20 22];
                            case (8),
                                juntas = [15 16];
                        end
                        handles.menu.copiar.juntas = juntas;
                        guidata(hObject, handles);
                end 
                        
                % permite inverter ou trocar as posições das juntas, está
                % associada ao menu das juntas
                function menu_juntas_Callback(hObject, eventdata, handles)
                        juntas = handles.menu.copiar.juntas;
                        valor = handles.DM.q(handles.DM.fase, juntas);
                        switch eventdata
                            case ('inv'),
                                valor = -1*valor;
                            otherwise
                                valor = feval(eventdata, valor);
                        end
                        handles.DM.q(handles.DM.fase, juntas) = valor;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles)
                        guidata(hObject, handles);

                end
                
                % faz a cópia do valor das juntas para uma variável
                % especial
                function copiar_Callback(hObject, eventdata, handles)
                        handles.menu.copiar.valor = handles.DM.q(handles.DM.fase,handles.menu.copiar.juntas);
                        set(handles.colar, 'Enable', 'On');
                        guidata(hObject, handles);
                end
                
                        
                % permite colar os valores copiados, normalmente,
                % invertidos ou trocados
                function colar_Callback(hObject, eventdata, handles)
                        juntas = handles.menu.copiar.juntas;
                        switch eventdata
                            case ('normal'),
                                valor = handles.menu.copiar.valor;
                            case ('inv'),
                                valor = -1*handles.menu.copiar.valor;
                            otherwise
                                valor = feval(eventdata, handles.menu.copiar.valor);
                        end
                        for i=1:min(length(valor), length(juntas)),
                            handles.DM.q(handles.DM.fase, juntas(i)) = valor(i);
                        end
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles)
                        guidata(hObject, handles);
                end
                
                % associado ao menu das juntas, função de copiar os valores
                % de outra fase para a actual
                function inserir_fase_Callback(hObject, eventdata, handles)
%                     keyboard
                        fase = get(hObject, 'Label');
                        fase = str2num(fase(5:end));
                        juntas = handles.menu.copiar.juntas;
                        handles.DM.q(handles.DM.fase, juntas) = handles.DM.q(fase, juntas);
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles)
                        guidata(hObject, handles);
                end
                
                % acrescenta as novas fases, ou retira as fases apagadas no
                % menu inserir das juntas
                function actualiza_menu_juntas_inserir(fase,handles)
                    fases = get(handles.inserir, 'Children');
                    ultima = length(fases);
                    fase = ultima+1;
                    while(fase <= size(handles.DM.q,1)),
                        uimenu(handles.inserir, 'Label', ['Fase ' num2str(fase)], 'Callback', ['TwoLegs_22dof(''inserir_fase_Callback'',gcbo,' num2str(fase) ',guidata(gcbo))']);
                        fase = fase+1;
                    end                    
                    while(ultima > size(handles.DM.q,1))
                        delete(fases(ultima));
                        ultima = ultima-1;
                    end
                end        
                        
                function Fase_num_Callback(hObject, eventdata, handles)
                end
                        
                        
                        
        %-----------------------------------------------------------------%
            % Espaço das Juntas
                % função de callback dos sliders das juntas
                function slider_junta_Callback(hObject, junta, handles)
                        valor = deg2rad(get(hObject,'Value'));
                    if(junta > 20)
                        return
                    end
                        handles.DM.q(handles.DM.fase,junta) = valor;
                        handles.DM.q(handles.DM.fase,handles.DM.dependencia(junta,:)==1) = valor;
                        handles.DM.q(handles.DM.fase,handles.DM.dependencia(junta,:)==-1) = -valor;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles);
                end
                
                % função de callback das caixas de dependencia das juntas
                function edit_junta_Lock_Callback(hObject, junta, handles)
                        juntaTutora = str2num(get(hObject,'String'));
                        if(juntaTutora==0)
                            handles.DM.dependencia(:, junta) = 0;
                        else
                            handles.DM.dependencia(abs(juntaTutora), junta) = 1*sign(juntaTutora);
                        end
                        guidata(hObject, handles);
                end
                
                % faz a actualização dos sliders, em função da fase em
                % questão
                function correct_sliders(fase, handles)
                        for i=1:22,
                            valor = round(rad2deg(handles.DM.q(fase,i)));
                            slider_handle = eval(['handles.slider_junta' num2str(i)]);
                            set(slider_handle, 'Value', valor);
                            text_box = eval(['handles.text_junta' num2str(i)]);
                            set(text_box, 'String', num2str(valor));
                        end
                        set(handles.edit_duracao, 'String', num2str(handles.DM.duracao(fase)));
                        h_pe = eval(['handles.radiobutton_' handles.DM.pe(fase,:)]);
                        set(handles.uipanel_pe, 'SelectedObject', h_pe);
                        ActualizaRobo(handles.GP)
%                         Show_real(handles.DM);
                end
                
                % relacionada com o botão "0" existente por cima dos
                % sliders das juntas
                function pushbutton_zeros_Callback(hObject, juntas, handles)
                        handles.DM.q(handles.DM.fase, juntas) = 0;
                        handles.GP.def = actualiza_cart_coord(handles);
                        guidata(hObject, handles);
                        correct_sliders(handles.DM.fase, handles)
                end
            
        %-----------------------------------------------------------------%
            % Espaço Cartesiano
            
                % não está a ser utilizado
                function uipanel_pe_suporte_ButtonDownFcn(hObject, eventdata, handles)
                        axesH(1) = handles.axes_DM_frente;
                        axesH(2) = handles.axes_DM_lado;
                        axesH(3) = handles.axes_DM_cima;
                        desenha(handles.GP, axesH)
                end
            
        %-----------------------------------------------------------------%
            % Controlo de Fase
            
                % adiciona uma nova fase ao movimento
                function handles = adicionar_Nova_Fase(handles)
                        handles.DM.q(end+1,:)       = handles.DM.q(end,:);
                        handles.DM.pe(end+1,:)      = handles.DM.pe(end,:);
                        handles.DM.XYZRef(end+1,:)  = handles.DM.XYZRef(end,:);
                        handles.DM.duracao(end+1)   = handles.DM.duracao(end);
                        handles.DM.tetaPe(end+1)    = handles.DM.tetaPe(end);
                        actualiza_menu_juntas_inserir(handles.DM.fase, handles);
                end
                
                % callback do botão de avanço de fase (">")
                function pushbutton_Fase_Seguinte_Callback(hObject, eventdata, handles)
                        fase = handles.DM.fase+1;
                        handles.DM.fase = fase;
                        if( size(handles.DM.q,1) < fase )
                            handles = adicionar_Nova_Fase(handles);
                        else
                            handles.GP.def = actualiza_cart_coord(handles);
                            correct_sliders(fase, handles);
                        end
                        if(fase == 2)
                            set(handles.pushbutton_Fase_Anterior, 'Enable', 'On');
                            set(handles.pushbutton_inicio, 'Enable', 'On');
                        end
                        set(handles.text_fase, 'String', num2str(fase));
                        guidata(hObject, handles);
                end
                
                % callback do botão de avanço até ao fim de fase (">>")
                function pushbutton_fim_Callback(hObject, eventdata, handles)
                        handles.DM.fase = size(handles.DM.q, 1);
                        if(handles.DM.fase > 1)
                            set(handles.pushbutton_Fase_Anterior, 'Enable', 'On');
                            set(handles.pushbutton_inicio, 'Enable', 'On');
                        end
                        set(handles.text_fase, 'String', num2str(handles.DM.fase));
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles);
                        
                end
                
                % callback do botão de recuo de fase ("<")
                function pushbutton_Fase_Anterior_Callback(hObject, eventdata, handles)
                        if(handles.DM.fase>1)
                            handles.DM.fase = handles.DM.fase-1;
                            set(handles.text_fase, 'String', num2str(handles.DM.fase));
                            handles.GP.def = actualiza_cart_coord(handles);
                            correct_sliders(handles.DM.fase, handles);
                            guidata(hObject, handles);
                        end
                        if(handles.DM.fase == 1)
                            set(handles.pushbutton_Fase_Anterior, 'Enable', 'Off');
                            set(handles.pushbutton_inicio, 'Enable', 'Off');
                        end
                end

                % callback do botão de recuo até ao inicio de fase ("<<")
                function pushbutton_inicio_Callback(hObject, eventdata, handles)
                        if(handles.DM.fase>1)
                            handles.DM.fase = 1;
                        end
                        set(handles.pushbutton_Fase_Anterior, 'Enable', 'Off');
                        set(handles.pushbutton_inicio, 'Enable', 'Off');
                        set(handles.text_fase, 'String', num2str(handles.DM.fase));
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles);
                end
                
                % callback da caixa de texto de duração da fase
                function edit_duracao_Callback(hObject, eventdata, handles)
                        handles.DM.duracao(handles.DM.fase) = str2num(get(hObject, 'String'));
                        guidata(hObject, handles);        
                end
                
                % alteração do pé de suporte
                function uipanel_pe_SelectionChangeFcn(hObject, eventdata, handles)
                        DM = handles.DM;
                        pe = DM.pe(DM.fase, :);
                        F = handles.GP.def.F;
                        XYZRef = handles.GP.def.P(14,:) - [0 0 0];
                        
                        switch get(hObject,'Tag')
                            case 'radiobutton_dir'
                                if(pe=='esq')
                                    ref = 3;
                                else
                                    return
                                end
                                handles.DM.pe(DM.fase,:) = 'dir';
                            case 'radiobutton_esq'
                                if(pe=='dir')
                                    ref = 1;
                                else
                                    return
                                end        
                                handles.DM.pe(DM.fase,:) = 'esq';
                        end
                        handles.DM.XYZRef(DM.fase,:) = XYZRef;
                        if(handles.DM.pe(DM.fase,:)=='dir')
                            handles.DM.tetaPe(DM.fase) = -atan2((F(2,2,ref)-F(2,3,ref)),(F(1,4,ref)-F(1,1,ref)));
                        else
                            handles.DM.tetaPe(DM.fase) = -atan2((F(2,4,ref)-F(2,1,ref)),(F(1,2,ref)-F(1,3,ref)));
                        end
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles);
                end

                
                % botão de remover a fase actual
                function pushbutton_remover_fase_Callback(hObject, eventdata, handles)
                        if(handles.DM.fase ~= 1)
                            if (handles.DM.fase == size(handles.DM.q,1))
                                handles.DM.fase = handles.DM.fase-1;
                                handles.DM.q = handles.DM.q(1:handles.DM.fase,:);                                   
                                handles.DM.XYZRef = handles.DM.XYZRef(1:handles.DM.fase,:);                                         
                                handles.DM.pe = handles.DM.pe(1:handles.DM.fase,:);                                     
                                handles.DM.duracao = handles.DM.duracao(1:handles.DM.fase);
                                handles.DM.tetaPe = handles.DM.tetaPe(1:handles.DM.fase);
                            else
                                handles.DM.fase = handles.DM.fase-1;
                                handles.DM.q = [handles.DM.q(1:handles.DM.fase,:);
                                                handles.DM.q(handles.DM.fase+2:end,:)];
                                handles.DM.XYZRef = [handles.DM.XYZRef(1:handles.DM.fase,:);
                                                     handles.DM.XYZRef(handles.DM.fase+2:end,:)];
                                handles.DM.pe = [handles.DM.pe(1:handles.DM.fase,:);
                                                 handles.DM.pe(handles.DM.fase+2:end,:)];
                                handles.DM.duracao = [handles.DM.duracao(1:handles.DM.fase), handles.DM.duracao(handles.DM.fase+2:end)];
                                handles.DM.tetaPe = [handles.DM.tetaPe(1:handles.DM.fase), handles.DM.tetaPe(handles.DM.fase+2:end)];
                            end
                        else
                            return;
                        end
                        if(handles.DM.fase == 1)
                            set(handles.pushbutton_Fase_Anterior, 'Enable', 'Off');
                            set(handles.pushbutton_inicio, 'Enable', 'Off');
                        end
                        set(handles.text_fase, 'String', num2str(handles.DM.fase));
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles);
                end     
                        
                % botão de inserir uma nova fase
                function pushbutton_inserir_fase_Callback(hObject, eventdata, handles)
                        if(handles.DM.fase == 1)
                            set(handles.pushbutton_Fase_Anterior, 'Enable', 'On');
                            set(handles.pushbutton_inicio, 'Enable', 'On');
                        end
                        handles.DM.q = [handles.DM.q(1:handles.DM.fase,:);
                                        handles.DM.q(handles.DM.fase:end,:)];
                        handles.DM.XYZRef = [handles.DM.XYZRef(1:handles.DM.fase,:);
                                             handles.DM.XYZRef(handles.DM.fase:end,:)];
                        handles.DM.pe = [handles.DM.pe(1:handles.DM.fase,:);
                                         handles.DM.pe(handles.DM.fase:end,:)];
                        handles.DM.duracao = [handles.DM.duracao(1:handles.DM.fase), handles.DM.duracao(handles.DM.fase:end)];
                        handles.DM.fase = handles.DM.fase+1;
                        handles.DM.tetaPe = [handles.DM.tetaPe(1:handles.DM.fase), handles.DM.tetaPe(handles.DM.fase:end)];
                        set(handles.text_fase, 'String', num2str(handles.DM.fase));
                        guidata(hObject, handles);
                end
                
                % Faz a cópia integral das definições desta fase
                function pushbutton_copiar_fase_Callback(hObject, eventdata, handles)
                        handles.menu.copiar.q = handles.DM.q(handles.DM.fase, :);
                        handles.menu.copiar.pe = handles.DM.pe(handles.DM.fase, :);
                        handles.menu.copiar.duracao = handles.DM.duracao(handles.DM.fase);
                        guidata(hObject, handles);
                end
                
                % Cola os dados copiados na função anterior
                function pushbutton_colar_fase_Callback(hObject, eventdata, handles)
                        handles.DM.q(handles.DM.fase, :) = handles.menu.copiar.q;
                        handles.DM.pe(handles.DM.fase, :) = handles.menu.copiar.pe;
                        handles.DM.duracao(handles.DM.fase) = handles.menu.copiar.duracao;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles)
                        guidata(hObject, handles);
                end
                
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Movimentos Especiais

            % Função de Inicialização
                function ME = initME()
                        ME.andar = initAndar();
                        ME.rodar = initRodar();
                        ME.chutar = initChutar();
                end
                
                function handles = setMEcontrols(handles)
                        andar_val_def(handles);
                        rodar_val_def(handles);
                        chutar_val_def(handles);
                        set_outros(handles)
                end
        %-----------------------------------------------------------------%
            % Outros
            
                function set_outros(handles)
                        max = 90/2;       % 
                        min = 0;      %% é para ser usado com as limitações reais do humanoide
                        passo_x = 1/(max-min);
                        passo_y = 10*passo_x;

                        h = handles.slider_baixar;
                        set(h,  'Max', max, ...
                                'Min', min, ...
                                'SliderStep', [passo_x passo_y], ...
                                'Value', 0);
                            
                        max = 90;       % 
                        min = -90;      %% é para ser usado com as limitações reais do humanoide
                        passo_x = 1/(max-min);
                        passo_y = 10*passo_x;

                        h = handles.slider_lateral;
                        set(h,  'Max', max, ...
                                'Min', min, ...
                                'SliderStep', [passo_x passo_y], ...
                                'Value', 0);
                            
                        max = 90/2;       % 
                        min = 0;      %% é para ser usado com as limitações reais do humanoide
                        passo_x = 1/(max-min);
                        passo_y = 10*passo_x;

                        h = handles.slider_levantar;
                        set(h,  'Max', max, ...
                                'Min', min, ...
                                'SliderStep', [passo_x passo_y], ...
                                'Value', 0);
                end         
            
                function slider_baixar_Callback(hObject, eventdata, handles)
%                     [q9,q10] = CinInv2R( [0.2 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
                        valor = round(get(hObject,'Value'));
                        
                        q = handles.DM.q(handles.DM.fase,:);
                        grau = deg2rad(valor);
%                         q(2) = q(2)+grau;
%                         q(3) = q(3)-2*grau;
%                         q(4) = q(4)+grau;
%                         
%                         q(11) = q(11)+grau;
%                         q(10) = q(10)-2*grau;
%                         q(9) = q(9)+grau;
                        
                        handles.DM.q(handles.DM.fase,[2 4 9 11]) = grau;
                        handles.DM.q(handles.DM.fase,[3 10]) = -2*grau;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles)
                        
                end
                
                function slider_lateral_Callback(hObject, eventdata, handles)
                        valor = round(get(hObject,'Value'));
                        
                        grau = deg2rad(valor);
                        
                        handles.DM.q(handles.DM.fase,[1 8]) = grau;
                        handles.DM.q(handles.DM.fase,[5 12]) = -grau;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles)
                end
                        
                function slider_levantar_Callback(hObject, eventdata, handles)
                        valor = round(get(hObject,'Value'));
                        
                        grau = deg2rad(valor);
%                         keyboard
                        if(strcmp(handles.DM.pe(handles.DM.fase,:), 'dir'))
                            junta1 = [9  11];
                            junta2 = 10;
                        else
                            junta1 = [2  4];
                            junta2 = 3;
                        end
                        
                        handles.DM.q(handles.DM.fase,junta1) = grau;
                        handles.DM.q(handles.DM.fase,junta2) = -2*grau;
                        handles.GP.def = actualiza_cart_coord(handles);
                        correct_sliders(handles.DM.fase, handles);
                        guidata(hObject, handles)
                end     
         %-----------------------------------------------------------------%
            % Chutar
            
                % Função de Inicio
                function chutar = initChutar()
                        chutar.pe = 'dir';
                        chutar.forca = 1;
                        chutar.x = 0;
                        chutar.y = 0;
                        chutar.z = 0;
                end
                
                % definições do movimento chutar
                function chutar_val_def(handles);
                        handles.ME.chutar = initChutar();
                        % ...
                        guidata(handles.TwoLegs_22dof, handles);
                end
                
                % coloca os valores por defeito no painel chutar
                function pushbutton_Valores_Defeito_Chutar_Callback(hObject, eventdata, handles)
                        chutar_val_def(handles);
                end
                
                % adiciona o movimento do pontapé
                function pushbutton_adicionar_chutar_Callback(hObject, eventdata, handles)
                        chutar = Sim_Kick(handles.DM, handles.ME.chutar);
                        handles.DM.q = [handles.DM.q(1:handles.DM.fase,:); chutar.q];
                        handles.DM.XYZRef = [handles.DM.XYZRef(1:handles.DM.fase,:); chutar.XYZRef];
                        handles.DM.pe = [handles.DM.pe(1:handles.DM.fase,:); chutar.pe];
                        handles.DM.tetaPe = [handles.DM.tetaPe(1:handles.DM.fase), chutar.tetaPe];
                        while(size(handles.DM.q,1)>handles.DM.fase)
                            handles.DM.duracao(handles.DM.fase+1)=handles.DM.duracao(handles.DM.fase);
                            pushbutton_Fase_Seguinte_Callback(handles.pushbutton_Fase_Seguinte, eventdata, handles)
                            handles = guidata(hObject);
                        end
                        guidata(hObject, handles);
                end        
        %-----------------------------------------------------------------%
            % Rodar
            
                % Função de Inicio
                function rodar = initRodar()
                        rodar.graus = 22.5;
                end
                
                function rodar_val_def(handles);
                        handles.ME.rodar = initRodar();
                        set(handles.edit_rodar_graus, 'String', num2str(handles.ME.rodar.graus));
                        guidata(handles.TwoLegs_22dof, handles);
                end
                
                function pushbutton_Valores_Defeito_Rodar_Callback(hObject, eventdata, handles)
                        rodar_val_def(handles);
                end
                
                function pushbutton_adicionar_rodar_Callback(hObject, eventdata, handles)
                        rodar = Sim_Rotate(handles.DM, handles.ME.rodar);
                        handles.DM.q = [handles.DM.q(1:handles.DM.fase,:); rodar.q];
                        handles.DM.XYZRef = [handles.DM.XYZRef(1:handles.DM.fase,:); rodar.XYZRef];
                        handles.DM.pe = [handles.DM.pe(1:handles.DM.fase,:); rodar.pe];
                        handles.DM.tetaPe = [handles.DM.tetaPe(1:handles.DM.fase), rodar.tetaPe];
                        while(size(handles.DM.q,1)>handles.DM.fase)
                            handles.DM.duracao(handles.DM.fase+1)=handles.DM.duracao(handles.DM.fase);
                            pushbutton_Fase_Seguinte_Callback(handles.pushbutton_Fase_Seguinte, eventdata, handles)
                            handles = guidata(hObject);
                        end
                        guidata(hObject, handles);
                end        
                        
                function edit_rodar_Callback(hObject, param, handles)
                        valor = str2num(get(hObject, 'String'));
                        switch param
                            case 'graus', 
                                min = -180;
                                max = 180;
                            case 'dir_x',
                                
                            case 'dir_y',
                                
                        end     
                        if(isnumeric(valor) & (valor>=min & valor<=max))
                            handles.ME.rodar = setfield(handles.ME.rodar, param, valor);
                        else
                            set(hObject, 'String', num2str(getfield(handles.ME.rodar, param)));
                        end
                        guidata(hObject, handles);        
                end     
                        
        %-----------------------------------------------------------------%
            % Andar
            
                % Função de Inicio
                function andar = initAndar()
                        andar.nPassos = 1;
                        andar.Sl = 0.10;     %-- Comprimento do passo
                        andar.Hh = 0.34;     %-- Altura da anca e Elevaçao do Pe
                        andar.Yg = -0.02;    %-- Posiçao lateral do centro da anca (valor constante)
                        andar.Fc = 0.02;     %-- Maxima Elevaçao do Pe            
                end
                
                function andar_val_def(handles);
                        handles.ME.andar = initAndar();
                        set(handles.edit_Sl, 'String', num2str(handles.ME.andar.Sl));
                        set(handles.edit_Hh, 'String', num2str(handles.ME.andar.Hh));
                        set(handles.edit_Yg, 'String', num2str(handles.ME.andar.Yg));
                        set(handles.edit_Fc, 'String', num2str(handles.ME.andar.Fc));
                        guidata(handles.TwoLegs_22dof, handles);

                end
                
                function pushbutton_Valores_por_Defeito_Callback(hObject, eventdata, handles)
                        andar_val_def(handles);
                end

                function pushbutton_Adicionar_Callback(hObject, eventdata, handles)
                        andar = Sim_Walking(handles.DM, handles.ME.andar);
                        handles.DM.q = [handles.DM.q(1:handles.DM.fase,:); andar.q];
                        handles.DM.XYZRef = [handles.DM.XYZRef(1:handles.DM.fase,:); andar.XYZRef];
                        handles.DM.pe = [handles.DM.pe(1:handles.DM.fase,:); andar.pe];
                        handles.DM.tetaPe = [handles.DM.tetaPe(1:handles.DM.fase), andar.tetaPe];
                        while(size(handles.DM.q,1)>handles.DM.fase)
                            handles.DM.duracao(handles.DM.fase+1)=handles.DM.duracao(handles.DM.fase);
                            pushbutton_Fase_Seguinte_Callback(handles.pushbutton_Fase_Seguinte, eventdata, handles)
                            handles = guidata(hObject);
                        end
                        guidata(hObject, handles);
                end

                function edit_andar_Callback(hObject, param, handles)
                        valor = str2num(get(hObject, 'String'));
                        switch param
                            case 'Sl', 
                                min = 0;
                                max = 0.5;
                                
                            case 'Hh',
                                min = 0.3;
                                max = 0.5;
                                
                            case 'Fc',
                                min = 0;
                                max = 0.05;
                            
                            case 'Yg',
                                min = -0.5;
                                max = 0.5;

                            case 'nPassos',
                                min = 0;
                                max = inf;
                                if(isinteger(int8(valor)))
                                    valor = int8(valor);
                                else
                                    set(hObject, 'String', num2str(getfield(handles.ME.andar, param)));
                                    return
                                end
                        end     
                        if(isnumeric(valor) & (valor>min & valor<max))
                            handles.ME.andar = setfield(handles.ME.andar, param, valor);
                        else
                            set(hObject, 'String', num2str(getfield(handles.ME.andar, param)));
                        end
                        guidata(hObject, handles);        
                end     

%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Controlo

            % Função de Inicialização
                function Ctrl = initCtrl()
                        Ctrl = struct([]);
                end
                
                function handles = setCtrlcontrols(handles)
                end        
        %-----------------------------------------------------------------%





%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Plots do Movimento

            % Função de Inicialização
                function PM = initPM()
                        PM.fase = [1 1];
                        PM.L1 = 1;
                        PM.L2 = 2;
                        PM.L3 = 3;
                        PM.L4 = 4;
                        PM.axes = {'Juntas', 'CoG', 'Binario'};  % axes apresentados no painel PM
                end
                
                function handles = setPMcontrols(handles)
                        lista = handles.PM.axes;
                        
                        for i=1:length(lista),
                            aux = lista{i};
                            h_axes = getfield(handles, ['axes_PM_' aux]);
                            
                            axes(h_axes);
                            
                            text(   0.5,0,'Sem dados para mostrar',...
                                    'FontSize', 20,...
                                    'HorizontalAlignment','center',... 
                                    'BackgroundColor','r');
                            axis([0 1 -90 90]);
                            axis off
                        end
                end        
                        
        %-----------------------------------------------------------------%

                function popupmenu_PM_linha_Callback(hObject, linha, handles)
                        lista = get(hObject, 'String');
                        index = get(hObject, 'Value');
                        nome_junta = lista{index};
                        junta = str2num(nome_junta(7:end));
                        handles.PM = setfield(handles.PM, ['L' num2str(linha)], junta);
                        guidata(hObject, handles);
                        PM_Actualiza(handles)
                end
                
                function edit_PM_fase_inicio_Callback(hObject, eventdata, handles)
                        value = str2num(get(hObject, 'String'));
                        if((value < handles.PM.fase(2)) & (value < size(handles.DM.q, 1)))
                            handles.PM.fase(1) = value;
                        else
                            set(hObject, 'String', num2str(handles.PM.fase(1)));
                        end
                        guidata(hObject, handles);
                        PM_Actualiza(handles)
                end        
                        
                function edit_PM_fase_fim_Callback(hObject, eventdata, handles)
                        value = str2num(get(hObject, 'String'));
                        if((value > handles.PM.fase(1)) & (value <= size(handles.DM.q, 1)))
                            handles.PM.fase(2) = value;
                        else
                            set(hObject, 'String', num2str(handles.PM.fase(2)));
                        end
                        guidata(hObject, handles);
                        PM_Actualiza(handles)
                end
                
                function pushbutton_PM_Actualiza_Callback(hObject, eventdata, handles)
                        PM_Actualiza(handles)
                end
                
                function PM_Actualiza(handles)
                        PlotJuntas(handles.DM, handles.PM, handles.axes_PM_Juntas);
                end
                
                
                
                function uipanel_PM_SelectionChangeFcn(hObject, eventdata, handles)
                        lista = handles.PM.axes;

                        nome = get(hObject, 'Tag');
                        nome = nome(13:end);          
                        
                        h_axes = strcmp(lista, nome);
                        pos = find(h_axes, 1);
                        for i=1:length(lista),
                            aux = lista{i};
                            h_axes = getfield(handles, ['uipanel_axes_PM_' aux]);
                            if(i~=pos)
                                set(h_axes, 'Visible', 'Off');
                            else
                                set(h_axes, 'Visible', 'On');
                            end
                        end                        
                end
                
                
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Gestão do Plot


        % Função de Inicialização
                function GP = initGP()
                        GP.def.P = zeros(28,3);
                        GP.def.C = zeros([7 101 20]);
                        GP.def.F = zeros([3 4 4]);
                        GP.def.H = zeros([3 6 4]);
                        GP.def.W = zeros([3 4 4]);
                        GP.def.E = zeros([3 4 12]);
                        GP.def.T = zeros(3,4);
                        GP.def.S = [];
                        GP.def.Sp = [];
                        
                        GP.tapete  = initTapete;
                        GP.escadas = initEscadas;
                        GP.bola = initBola;
                        
                end
                
                % Definições Tapete
                function tapete = initTapete()
                        tapete.comp = 1.6;
                        tapete.larg = 0.8;
                        tapete.x    = 0.0;
                        tapete.y    = 0.0;
                end
                
                % Definições Escadas
                function  escadas = initEscadas()
                        escadas.nDegraus = 3;
                        escadas.comp     = 0.12;
                        escadas.altu     = 0.07;
                        escadas.x        = 0.5;
                        escadas.y        = 0.08;
                end
                
                % Definições Bola
                function  bola = initBola()
                        bola.raio = 0.08;
                        bola.x    = -0.5;
                        bola.y    = 0;
                end
                
                function handles = setGPcontrols(handles)
                        handles.GP.COG_0 = ZeroLocalCOG(handles.DM);
                        handles.GP.def = actualiza_cart_coord(handles);
                        handles.GP = actualiza_tapete(handles.GP);
                        handles.GP = actualiza_escadas(handles.GP);
                        handles.GP = actualiza_bola(handles.GP);
                        handles.GP.obj = DesenharRoboPZ( handles.GP, handles.robo_visual );
                end
                    
        %-----------------------------------------------------------------%
        % Plot        
                function def = actualiza_cart_coord(handles)
                        DM = handles.DM;
                        COG_0 = handles.GP.COG_0;
                        def = handles.GP.def;
                        A = CinDir20dof( DM ); 
                        def = CoordJuntas( A,DM,def );
                        def = RobCOG(A, DM, COG_0, def);
                end
                
                function checkbox_CoG_Callback(hObject, eventdata, handles)
                        if(get(hObject, 'Value'))
                            state = 'On';
                        else
                            state = 'Off';
                        end
                        set(handles.GP.obj.CG, 'Visible', state);
                        set(handles.GP.obj.CP, 'Visible', state);
                        set(handles.GP.obj.Cg, 'Visible', state);
                end        
        %-----------------------------------------------------------------%
        % Painel Tapete
        
                function checkbox_tapete_Callback(hObject, eventdata, handles)
                        if(get(hObject, 'Value'))
                            state = 'On';
                        else
                            state = 'Off';
                        end
                        set(handles.GP.obj.Th, 'Visible', state);
                end     
                
                function GP = actualiza_tapete(GP)
                        T_x1 = GP.tapete.x - GP.tapete.comp/2;
                        T_x2 = GP.tapete.x + GP.tapete.comp/2;
                        T_y1 = GP.tapete.y - GP.tapete.larg/2;
                        T_y2 = GP.tapete.y + GP.tapete.larg/2;

                        GP.def.T = [T_x1 T_x2 T_x2 T_x1;
                                    T_y1 T_y1 T_y2 T_y2;
                                    0.0 0.0 0.0 0.0];
                end
                        
                function edit_tapete_Callback(hObject, param, handles)
                        valor = str2num(get(hObject, 'String'));
                        switch param
                            case 'comp', 
                                min = 0;                                
                            case 'larg',
                                min = 0;                                
                            case 'x',
                                min = -inf;                            
                            case 'y',
                                min = -inf;
                        end     
                        if(isnumeric(valor) & (valor>min))
                            handles.GP.tapete = setfield(handles.GP.tapete, param, valor);
                        else
                            set(hObject, 'String', num2str(getfield(handles.GP.tapete, param)));
                        end
                        handles.GP = actualiza_tapete(handles.GP);
                        ActualizaRobo(handles.GP);
                        guidata(hObject, handles);
                end     
                        
        %-----------------------------------------------------------------%
        % Painel Escadas
        
                function checkbox_escadas_Callback(hObject, eventdata, handles)
                        if(get(hObject, 'Value'))
                            state = 'On';
                        else
                            state = 'Off';
                        end
                        set(handles.GP.obj.Esh, 'Visible', state);
                end
                
                function GP = actualiza_escadas(GP) 
                        GP.def.S = stairs(GP);
                end
                        
                function edit_escadas_Callback(hObject, param, handles)
                        valor = str2num(get(hObject, 'String'));
                        switch param
                            case 'nDegraus', 
                                min = 0;                              
                            case 'comp', 
                                min = 0;
                            case 'altu',
                                min = 0;                                
                            case 'x',
                                min = -inf;                            
                            case 'y',
                                min = -inf;
                        end     
                        if(isnumeric(valor) & (valor>min))
                            handles.GP.escadas = setfield(handles.GP.escadas, param, valor);
                        else
                            set(hObject, 'String', num2str(getfield(handles.GP.escadas, param)));
                        end
                        handles.GP = actualiza_escadas(handles.GP);
                        ActualizaRobo(handles.GP);
                        guidata(hObject, handles);
                end
                
        %-----------------------------------------------------------------%
        % Painel Bola
        
                function checkbox_bola_Callback(hObject, eventdata, handles)
                        if(get(hObject, 'Value'))
                            state = 'On';
                        else
                            state = 'Off';
                        end
                        set(handles.GP.obj.Sph, 'Visible', state);
                end
                
                function GP = actualiza_bola(GP) 
                        [X,Y,Z] = sphere(30);     %-- Esfera de raio unitario
                        R = GP.bola.raio; Sp(:,:,1) = R*X+GP.bola.x; Sp(:,:,2) = R*Y+GP.bola.y; Sp(:,:,3) = R*Z+R;
                        GP.def.Sp = Sp;
                end
                        
                function edit_bola_Callback(hObject, param, handles)
                        valor = str2num(get(hObject, 'String'));
                        switch param
                            case 'raio', 
                                min = 0;                                
                            case 'x',
                                min = -inf;                            
                            case 'y',
                                min = -inf;
                        end     
                        if(isnumeric(valor) & (valor>min))
                            handles.GP.bola = setfield(handles.GP.bola, param, valor);
                        else
                            set(hObject, 'String', num2str(getfield(handles.GP.bola, param)));
                        end
                        handles.GP = actualiza_bola(handles.GP);
                        ActualizaRobo(handles.GP);
                        guidata(hObject, handles);
                end 
        %-----------------------------------------------------------------%
        % Barra de ferramentas do plot
        
                function pushbutton_Rodar_Callback(hObject, eventdata, handles)
                        rotate3d
                end
                
                function pushbutton_Simular_Callback(hObject, eventdata, handles)
                        mousefrm(handles.TwoLegs_22dof, 'watch')
                        simulate(handles.DM, handles.GP);
                        mousefrm(handles.TwoLegs_22dof, 'arrow')
                        correct_sliders(handles.DM.fase, handles)
                end
        %-----------------------------------------------------------------%
        % Menus de Contexto
        
                function pushbutton_view_Callback(hObject, eventdata, handles)
                        str = get(hObject, 'Label');
                        switch str
                            case 'Frente',
                                AZ = 90; EL =0;
                            case 'Lado',
                                AZ = 0; EL =0;                    
                            case 'Cima'
                                AZ = 90; EL =90;   
                            case 'Vista por defeito'
                                [AZ, EL] = view(3);  
                        end
                        set(handles.robo_visual, 'View', [AZ, EL]);
                end
                    
                        
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
% Humanoid

        % Função de Inicialização
                function Humanoid = initHumanoid()
                        Humanoid = data_struct();
                        Humanoid.CoP.dir = [0 0];   % CoP no pé direito
                        Humanoid.CoP.esq = [0 0];   % CoP no pé esquerdo
                        
                end
                
                function handles = setHumanoidcontrols(handles)
                        handles.Humanoid.CoPH = drawfeet(handles.Humanoid, handles.axes_CoP_dir, handles.axes_CoP_esq);
                        
                        funcao = @exp2;
                        handles.timer = timer('TimerFcn', funcao, 'ExecutionMode', 'fixedRate', 'Period', 5);
                        
                        function exp2(src, h, a)
                            Humanoid.CoP.dir = [randn, randn];
                            Humanoid.CoP.esq = [randn, randn];
                            refreshdata(handles.Humanoid.CoPH, 'caller');
                        end
                end        
                        
        %-----------------------------------------------------------------%
        % Funções de leitura                     

                % devolve a estrutura do humanoide preenchida e actualizada
                % (actualmente só a posição é devolvida)
                function handles = actualizaHumanoid(handles)
                        H = handles.JI.comm.H;
                        for slave=1:length(handles.Humanoid.slave),       % condicionado ao numero de slaves disponiveis
                            pos = readjoint(H, slave, 0);
                            for servo=1:length(handles.Humanoid.slave(slave).servo),
                                handles.Humanoid.slave(slave).servo(servo).pos = pos(servo);
                            end
                        end
                        
                        for slave=1:2,      % só sensores de força
                            val = readspecial(H, slave);
                            for i=1:4,      % 4 sensores por pé
                                handles.Humanoid.slave(slave).sensor(i) = val(i);
                            end
                        end
                end
                
                % faz a actualização dos campos de posição do humanoide de
                % acordo com os valores presentes na estrutura humanoid
                function actualizaPainelHumanoid(handles)
                        for slave=1:length(handles.Humanoid.slave),
                            for servo=1:length(handles.Humanoid.slave(slave).servo),
                                text_box = getfield(handles, ['text_Humanoid_pos_slave' num2str(slave) '_servo' num2str(servo)]);
                                set(text_box, 'String', num2str(handles.Humanoid.slave(slave).servo(servo).pos));
                            end
                        end
                        
                        Humanoid = calcop(Humanoid);
                        
                        refreshdata(Humanoid.CoPH, 'caller');  %-- Pernas
                end   
                
        %-----------------------------------------------------------------%
        % CoP        
                function slider_foot_sensors_fresh_rate_Callback(hObject, eventdata, handles)
                        stop(handles.timer);
                        set(handles.timer, 'Period', 1/get(hObject, 'Value'));
                        start(handles.timer);
                end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
% Outros

function pushbutton_debug_Callback(hObject, eventdata, handles)
    keyboard
    guidata(hObject, handles)
end


function pushbutton_GestMovs_Callback(hObject, eventdata, handles)
        handles.DM = GestMovs(handles.DM);
        if(isempty(handles.DM))
            handles.DM = initDM();
        end
        actualiza_menu_juntas_inserir(handles.DM.fase, handles);
        pushbutton_inicio_Callback(handles.pushbutton_inicio, [], handles)
end


function TwoLegs_22dof_CloseRequestFcn(hObject, eventdata, handles)
    if(handles.JI.comm.H)
        killcom(handles.JI.comm.H);
    end
    stop(handles.timer);
    delete(handles.timer);
    delete(hObject);
end

%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-------------------------------------------------------------------------%



function pushbutton_adapt_Callback(hObject, eventdata, handles)
    q = transform(handles.DM.q);
    adapt(q);
    handles.DM.q(handles.DM.fase,:) = 0;
    handles.GP.def = actualiza_cart_coord(handles);
    correct_sliders(handles.DM.fase, handles)
    guidata(hObject, handles);
end


function pushbutton_start_Callback(hObject, eventdata, handles)
    infLoop(handles.DM, handles.JI.comm.H);
end

    


% function TwoLegs_22dof_WindowButtonMotionFcn(hObject, eventdata, handles)
%     %is mouse over a juntas painel?
%     mousePos = get(gco, 'CurrentPoint')
% %     switch(mousePos)
%         pos1 = get(handles.slider_junta1, 'Position');
%         xSize = pos1(3);
%         ySize = pos1(4);
%         
%         colum1Init = pos1(1);
%         row1Init = pos1(2);
%         
%         pos2 = get(handles.slider_junta2, 'Position');
%         separateSize = pos2(2) - (row1Init+ySize);
%         
%         
%         pos12 = get(handles.slider_junta12, 'Position');
%         separateVertSize = pos12(2) - (row1Init + 2*separateSize + 3*ySize);
%         
%         pos4 = get(handles.slider_junta4, 'Position');
%         separateHorzSize = pos4(1) - colum1Init + xSize;
%                 
%         if (mousePos(2)>row1Init && mousePos(2)<row1Init+ySize)
%             disp('yes');
%         end
                







