close all

H = 1;
        
% Calibração Inicial
    
    factorLon = 1.6812;
    factorLat = -1.6812;
    
    offset_1 = [15 30 10];
    offset_3 = [0 0 0];
    if(~(exist('factorLat') & exist('factorLon')))
        [factorLat, factorLon] = calibra(H,3)
    end
    pos_init_slave1 = ler_pos(H,1,offset_1);
    pos_init_slave3 = ler_pos(H,3,offset_3);

% definições da tábua

    c = 50;   % comprimento %  
    l = 30;   % largura     %% centimetros
    d = l/2;
    [xc, yc, zc] = cylinder(d, 1000);
    zc = [zc(1,:)-d; zc(2,:)*d/4];
    color = [zeros(1,length(xc)); ones(1,length(xc))];
    
    Rx = makehgtform('xrotate', pi/2);
    Ry = makehgtform('yrotate', pi/2);

% inicialização de variaveis e constantes

    defs = defineRob;
    XYZRef = [-c/2 0 0];
    perna = deg2rad([pos_init_slave1 pos_init_slave3(1:2)]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   

    
% Criação da zona de construcção dos plots

    [AZ, EL] = view(3);
    f_sim = figure(1);
    set(f_sim,  'Units', 'normalized',...
                'Position', [0.53 0.04 0.47 0.38],...
                'Name', 'Teste Inclinómetros',...
                'DoubleBuffer', 'on',...
                'Renderer', 'OpenGl');
    
% criação e animação do robo

    % robo virtual
       hg = hgtransform;        % corpo de painel móvel e perna
       hc = hgtransform;        % corpo do cilindro representativo do angulo
       
       
       a = patch([0 -c -c 0], [-l/2 -l/2 l/2 l/2], [0 0 0 0] , 'c', 'Parent', hg);
       b = patch(2*[0 -c -c 0], 2*[-l/2 -l/2 l/2 l/2], 2*[0 0 0 0] , 'r', 'Parent', get(hg, 'Parent'));
       
       Ai = CinDir4dof(perna, defs.L, XYZRef);
       [P, C, F, E] = CoordJuntas(Ai, XYZRef, defs.L, defs.O);
       [Hh, Eh, Jh, Fh, hr] = DesenharRoboPZ(P, C, F, E);
       set(hr, 'Parent', hg);
       
       sx = surface([], [], [], [], 'Parent', hc);

       set(hc, 'Matrix', Rx);
       
       tx = text('Position', [-c/2, 0, 2*l], 'Parent', hc);

       axis('auto'), axis('off'), axis('equal')
       

% inicio
        
while(1)

    servo(1:3) = ler_pos(H,1,offset_1);
    servo(4:6) = ler_pos(H,3,offset_3);
    perna = deg2rad(servo(1:4));
    
    Ai = CinDir4dof(perna, defs.L, XYZRef);
    [P, C, F, E] = CoordJuntas(Ai, XYZRef, defs.L, defs.O);
    ActualizaRobo(Hh, Eh, Jh, Fh, hr, P, C, F, E)
    
    
    teta = deg2rad(ler_inclinometro(H,3,factorLon));
    beta = teta - (pi/2-((pi/2-perna(2))+perna(3)+perna(4)));
    
%     teta = deg2rad(ler_inclinometro(H,4,factorLon));
%     zeta = teta - (pi/2-((pi/2-perna(2))+perna(3)+perna(4)));
    

%     Rzeta = makehgtform('xrotate', zeta);
    
    set(tx, 'string', num2str(rad2deg(mod(beta+pi,pi))));
%     set(ty, 'string', num2str(rad2deg(mod(zeta+pi,pi))));
    
    switch sign(beta)
        case 1,
            trans = 0;
            gama_x(1) = cos(pi-beta)*d;
            j = ((xc(1,:)<=gama_x(1)) & (yc(1,:)>=0));
            zrot = eye(4);
        case -1,
            trans = c*sin(-beta);
            gama_x(1) = cos(pi+beta)*d;
            j = ((xc(1,:)<=gama_x(1)) & (yc(1,:)>=0));
            zrot = makehgtform('yrotate', pi, 'translate', c-d, 0, 0);
            
    end


        x = xc(:,j);
        y = yc(:,j);
        z = zc(:,j);
        col = color(:,j);
    
    if(~isempty(x))
        set(sx, 'xdata', x, 'ydata', y, 'zData', z, 'cData', col)
    else
        set(sx, 'xdata', [], 'ydata', [], 'zData', [], 'cData', [])
    end
    
    Rbeta = makehgtform('translate', 0, 0, trans, 'yrotate', beta);
    set(hg, 'Matrix', Rbeta);
    set(hc, 'Matrix', Rx*zrot)
    
%     switch sign(zeta)
%         case 1,
%             gama_x(1) = cos(pi-zeta)*d;
%             j = ((xc(1,:)<=gama_x(1)) & (yc(1,:)>=0));
%         case -1,
%             gama_x(1) = cos(pi+zeta)*d;
%             j = (yc(1,:)>=0) | (xc(1,:)>=gama_x(1));
%     end
% 
% 
%         x = xc(:,j);
%         y = yc(:,j);
%         z = zc(:,j);
%         col = color(:,j);
%     
%     if(~isempty(x))
%         set(sy, 'xdata', x, 'ydata', y, 'zData', z, 'cData', col)
%     else
%         set(sy, 'xdata', [], 'ydata', [], 'zData', [], 'cData', [])
%     end
    
    if(get(f_sim, 'currentCharacter') == 's')
        break;
    end
    
    pause(1)
    drawnow;
end
        
        

                                            
% FIM