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 15-Jun-2007 18:36:32

% 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


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

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


            
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
                    
                    guidata(handles.TwoLegs_22dof, handles);

                    setDMcontrols(handles)
                    setPaineiscontrols(handles)
                    setMEcontrols(handles)
                    setCtrlcontrols(handles)
                    setPMcontrols(handles)
                    setGPcontrols(handles)
                    setJIcontrols(handles)


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

            % Função de Inicialização
                function JI = initJI()
                        JI.comm.COM = 3;
                        JI.comm.BR = 115200;
                        JI.comm.H = 0;
                        JI.humanoid = data_struct();
                        
                function setJIcontrols(handles)
                        
                        
        %-----------------------------------------------------------------%
            
 
        
            % 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)

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

                % 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'))
                            H = initcom(handles.JI.comm.COM, handles.JI.comm.BR);
                            handles.JI.comm.H = H;
                            if(~handles.JI.comm.H)
                                return
                            end
                            set(hObject, 'String', 'Desligar', 'ForegroundColor', 'r')
                            
                                for i=1:4,
                                    applyjoint(H, i, 3, [1 0 0]);
                                    applyjoint(H, i, 1, [100 100 100]);
                                end
                            
                            handles = actualizaHumanoid(handles);
                            actualizaPainelHumanoid(handles);
                            
                            disp('ligado com sucesso');
                        else
                            handles.JI.comm.H = killcom(handles.JI.comm.H);
                            set(hObject, 'String', 'Ligar', 'ForegroundColor', 'g')
                        end
                        guidata(hObject, handles)
                        
                % 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.JI.humanoid.slave),       % condicionado ao numero de slaves disponiveis
                            pos = readjoint(H, slave, 0);
                            for servo=1:length(handles.JI.humanoid.slave(slave).servo),
                                handles.JI.humanoid.slave(slave).servo(servo).pos = pos(servo);
                            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.JI.humanoid.slave),
                            for servo=1:length(handles.JI.humanoid.slave(slave).servo),
                                text_box = getfield(handles, ['text_Humanoid_pos_slave' num2str(slave) '_servo' num2str(servo)]);
                                set(text_box, 'String', num2str(handles.JI.humanoid.slave(slave).servo(servo).pos));
                            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'         };
                
                
                function setPaineiscontrols(handles)
                            
                                        
            % 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


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


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



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


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

                function inserir_fase_Callback(hObject, eventdata, handles)
                        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);
                        
                function actualiza_menu_juntas_inserir(fase,handles)
%                     keyboard
                    ultima = get(get(handles.inserir, 'Children'),'Label');
                    if (iscellstr(ultima))
                        ultima = ultima{1,:};
                    end
                    ultima = str2num(ultima(6:end));
                    if(ultima < size(handles.DM.q,1)),
                        for fase=ultima:size(handles.DM.q,1),
                            uimenu(handles.inserir, 'Label', ['Fase ' num2str(fase)], 'Callback', ['TwoLegs_22dof(''inserir_fase_Callback'',gcbo,' fase ',guidata(gcbo))']);
                        end
                    elseif(ultima > size(handles.DM.q,1))
                        fases = get(handles.inserir, 'Children');
                        for fase=size(handles.DM.q,1):ultima,
                            delete(fases(ultima-fase));
                        end
                    end
                        
                        
                function fase_num_Callback(hObject, eventdata, handles)
                        
                        
        %-----------------------------------------------------------------%
            % Espaço 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);

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


                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)
            
            
        %-----------------------------------------------------------------%
            % 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)
            
            
        %-----------------------------------------------------------------%
            % Controlo de Fase
            
                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);

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

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

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


                function edit_duracao_Callback(hObject, eventdata, handles)
                        handles.DM.duracao(handles.DM.fase) = str2num(get(hObject, 'String'));
                        guidata(hObject, handles);        

                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];
                        selection = get(handles.uipanel_pe,'SelectedObject');
                        switch get(selection,'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);


                

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


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

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

%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%-------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%       Movimentos Especiais

            % Função de Inicialização
                function ME = initME()
                        ME.andar = initAndar();
                        ME.rodar = initRodar();
                        ME.chutar = initChutar();
                    
                function setMEcontrols(handles)
                        andar_val_def(handles);
                        rodar_val_def(handles);
                        chutar_val_def(handles);
        %-----------------------------------------------------------------%
            % Outros
            
                function slider_baixar_Callback(hObject, eventdata, handles)
%                     [q9,q10] = CinInv2R( [0.2 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;

         %-----------------------------------------------------------------%
            % Chutar
            
                % Função de Inicio
                function chutar = initChutar()
                        chutar.pe = 'dir';
                        chutar.forca = 1;
                        chutar.x = 0;
                        chutar.y = 0;
                        chutar.z = 0;

                function chutar_val_def(handles);
                        handles.ME.chutar = initChutar();
%                         set(handles.edit_chutar_graus, 'String', num2str(handles.ME.chutar.graus));
                        guidata(handles.TwoLegs_22dof, handles);
                        
                function pushbutton_Valores_Defeito_Chutar_Callback(hObject, eventdata, handles)
                        chutar_val_def(handles);
                        
                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);
                        
        %-----------------------------------------------------------------%
            % Rodar
            
                % Função de Inicio
                function rodar = initRodar()
                        rodar.graus = 22.5;

                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);
                        
                
                function pushbutton_Valores_Defeito_Rodar_Callback(hObject, eventdata, handles)
                        rodar_val_def(handles);
                        
                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);
                        
                        
                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);        
                        
                        
        %-----------------------------------------------------------------%
            % Andar
            
                % Função de Inicio
                function andar = initAndar()
                        andar.nPassos = 1;
                        andar.Sl = 0.10;     %-- Comprimento do passo
                        andar.Hh = 0.39;     %-- Altura da anca e Elevaçao do Pe
                        andar.Yg = 0.0;      %-- Posiçao lateral do centro da anca (valor constante)
                        andar.Fc = 0.02;     %-- Maxima Elevaçao do Pe            
                        
                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);


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


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


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

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

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





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

            % Função de Inicialização
                function PM = initPM()
                        PM.fase = 1;
                        PM.L1 = 1;
                        PM.L2 = 2;
                        PM.L3 = 3;
                        PM.L4 = 4;
                      
                function setPMcontrols(handles)
                        axes(handles.axes_PM);
                        text(   0.5,0,'Sem dados para mostrar',...
                                'FontSize', 20,...
                                'HorizontalAlignment','center',... 
                                'BackgroundColor','r');
                        axis([0 1 -90 90]);
                        axis off
                        
                        
        %-----------------------------------------------------------------%



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

                % Definições Tapete
                function tapete = initTapete()
                        tapete.comp = 1.6;
                        tapete.larg = 0.8;
                        tapete.x    = 0.0;
                        tapete.y    = 0.0;

                % 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;
                        
                % Definições Bola
                function  bola = initBola()
                        bola.raio = 0.08;
                        bola.x    = -0.5;
                        bola.y    = 0;
                        
                function setGPcontrols(handles)
                        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 );
                        guidata(handles.TwoLegs_22dof, handles);
                    
                    
        %-----------------------------------------------------------------%
        % Plot        
                function def = actualiza_cart_coord(handles)
                        DM = handles.DM;
                        def = handles.GP.def;
                        A = CinDir20dof( DM ); 
                        def = CoordJuntas( A,DM,def );
                        
        %-----------------------------------------------------------------%
        % 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);
                        
                    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];
                        
                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);
                        
                        
        %-----------------------------------------------------------------%
        % 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);
                        
                function GP = actualiza_escadas(GP) 
                        GP.def.S = stairs(GP);

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

                        
                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);
                    
        %-----------------------------------------------------------------%
        % Barra de ferramentas do plot
        
            function pushbutton_Rodar_Callback(hObject, eventdata, handles)
                    rotate3d

            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)

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

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

%set(gcf,'pointer','fullcrosshair');

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



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

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



function pushbutton_send_Callback(hObject, eventdata, handles)
    keyboard
    H = handles.JI.comm.H;
    fase = handles.DM.fase;
    for scuid=1:4,
        switch (scuid),
            case 1,
                pos = round(rad2deg([handles.DM.q(fase, (scuid-1)*3+1) handles.DM.q(fase, (scuid-1)*3+2) handles.DM.q(fase, (scuid-1)*3+3)]));
            case 2,
                pos = round(rad2deg([handles.DM.q(fase, 12) handles.DM.q(fase, 11) handles.DM.q(fase, 10)]));
            case 3,
                pos = round(rad2deg([handles.DM.q(fase, 4) handles.DM.q(fase, 5) handles.DM.q(fase, 6)]));
            case 4,
                pos = round(rad2deg([handles.DM.q(fase, 9) handles.DM.q(fase, 8) handles.DM.q(fase, 7)]));            
        end
        for servo=1:3,
            NRel(servo) = handles.JI.humanoid.slave(scuid).servo(servo).transm;
        end
        
        pos = adapt(scuid, pos, NRel, 1);
        
        if(max(abs(pos)) > 90), % condição de segurança
            disp('posição não enviada')
        else
            applyjoint(H, scuid, 0, pos);
        end
    end
    
    


