%function phua(flag)

    clear all, close all
    delete(instrfind);
    H=initCom;

    flag = 1;
    
    globalVar;
    N=200;
    Nlength=0;
    Nsensors=4;
    Nsamples=1000;
    hPlot=zeros(Nsensors+1);
    nSampPlot=50;
    y=zeros(Nsensors,nSampPlot);

    middleX=7;
    totalX=14;
    middleY=4;
    totalY=8;

    val=zeros(Nsensors,1);
    calibSensor=zeros(Nsensors,1);
    weight=zeros(1,5);
    
    nMoves=0;
    Nmoves=0;
    tlast=0;
    posArray=zeros(3,N);
    posArraySaved=zeros(3,Nsamples);
    t=zeros(1,N);
    tSaved=zeros(nMoves,Nsamples);
    timeAdjust=zeros(1,Nsamples);
    valSensor=zeros(N,4);
    calibSensor=zeros(4,1);
    
    if(flag)
        initMsg = [hex2dec('FF') hex2dec('00') hex2dec('00') hex2dec('FF') hex2dec('00') hex2dec('00') hex2dec('FF')];
        fwrite(H,initMsg,'uint8');
    end
        
        displayOptions;         %%%% DISPLAY OPTIONS %%%%
        
    while (1)
        disp(' ');
        disp('                          Display Options Again - 0');
        opt=input('Option: ');
    
        switch opt
            case 0
                displayOptions;
                
            case 1
                initialization(H);
            
            case 2
                id = input('ID: ');
                speed = input('Speed [0 ; 100]%: ');
                pos = setSpeedReadPos(H,id,speed);
                disp(['Position: ', num2str(pos), 'º']);
                
            case 3
                id = input('ID: ');
                if (id == 6)
                    pos = input('Position [-30 ; 90]º: ');
                    while (pos ~= [-30:90])
                        pos = input('Position [-30 ; 90]º: ');
                    end
                else if (id == 9)
                        pos = input('Position [-90 ; 30]º: ');
                    	while (pos ~= [-90:30])
                            pos = input('Position [-90 ; 30]º: ');
                        end
                    else
                        pos = input('Position [-90 ; 90]º: ');
                    end
                end
                setPosId(H,id,pos);

            case 4
                disp(' All Servo Positions');
                id = 6;
                speed = 100;
                pos = 0;
                while (pos ~= -150)
                    pos = setSpeedReadPos(H,id,speed);
                    if (pos ~= -150)
                        disp(['  Servo   : ', num2str(id)]);
                        disp([' Position : ', num2str(pos), 'º']);
                    end
                    id = id+1;
                end
                disp(' ');
                disp(' All Servos Speeds are at 100%');
                
            case 5
                release(H);
                
            case 6
                nServosMove(H);
                
                pause(2);          % temporario
                setSpeedReadPos(H,6,100);
                setPosId(H,6,85);

            case 7
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,1,23,maxSpeed,0,0,0,0,0);
                
            case 8         % Direct Kinematic
                q1 = input('Ang Joint 1 (º): ');
                q2 = input('Ang Joint 2 (º): ');
                disp('Ang Joint 3 (º): 0');
                q4 = input('Ang Joint 4 (º): ');

                dirKin(H,q1,q2,q4);
                
                [command,return1,return2,return3] = readServo(H);
                xt = (return1*256) + return2;
                temp_ret = return3;
                [command,return1,return2,return3] = readServo(H);
                yt = (temp_ret*256) + return1;
                zt = (return2*256) + return3;
                xt = xt/10-400;
                yt = yt/10-400;
                zt = zt/10;
                disp('  RESULTS  ');
                disp(['Pos Xt: ', num2str(xt), ' mm']);
                disp(['Pos Yt: ', num2str(yt), ' mm']);
                disp(['Pos Zt: ', num2str(zt), ' mm']);
                
            case 9         % Inverse Kinematic
                posX = input('Pos Xt (mm): ');
                posY = input('Pos Yt (mm): ');
                posZ = input('Pos Zt (mm): ');     % limites entre:
                posX = (posX+400);                % [-234.0 +234.0]
                posY = (posY+400);                % [-374.0 +374.0]
                posZ = (posZ+400);                % [-84.0 +384.0]
                invKin(H,posX,posY,posZ);

            case 10
                sendServo(H,1,N_VALUES,0,0,0,0,0,0);
                [command,return1,return2] = readServo(H);
                N = (return1*256) + return2;
                if N==0;
                    disp(['No movement was executed.']);
                end
                disp(['Movement with ', num2str(N), ' values.']);
                
            case 11
                ini_pos= [85 -85 -45 -85 85 45];
                for i=1:6
                    id= i+5;
                    pos= ini_pos(i);
                    setPosId(H,id,pos);
                end
                
            case 12
                out_pos= [45 -45 0 -45 45 0];
                for i=1:6
                    id= i+5;
                    pos= out_pos(i);
                    setPosId(H,id,pos);
                    pause(0.5);
                end
                
            case 13     % Direct Kinematic
                sendServo(H,1,13,0,0,0,0,0,0);
                
            case 14     % Inverse Kinematic
                sendServo(H,1,14,0,0,0,0,0,0);
                
            case 15         % Inverse Kinematic
                PtPMov_2Arms(H);

            case 16
                ContMov_2Arms(H);

            case 17
                ContMov_2Arms_v2(H);
                
            case 18
                JacobMov_2Arms(H);
                
            case 19
                draw_Qvel(H);
                
            case 20
                    % [x y z L x y z R TxL TxR ord]
                seq1 = [91 140 31 1 0 0 0 0 0.5 0 3];
                seq2 = [91 218 -47 1 0 0 0 0 0.5 0 3];
                Seq_ContMov(H, seq1);
                
                seq1 = [91 140 -47 1 0 0 0 0 2 0 3;
                        91 218 -47 1 0 0 0 0 2 0 3;
                        91 218 31 1 0 0 0 0 2 0 3;
                        91 140 31 1 0 0 0 0 2 0 3;
                        13 140 -47 1 0 0 0 0 2 0 3;      % fora dos limites [13 140 31 1 0 0 0 0 1 0 3]
                        91 140 -47 1 0 0 0 0 2 0 3];
%                         91 140 31 1 0 0 0 0 2 0 3];

                seq2 = [91 218 31 1 0 0 0 0 1 0 3;
                        91 140 31 1 0 0 0 0 1 0 3;
                        91 140 -47 1 0 0 0 0 1 0 3;
                        91 218 -47 1 0 0 0 0 1 0 3;
                        13 218 -47 1 0 0 0 0 1 0 3;
                        13 218 31 1 0 0 0 0 1 0 3;      % <- este ta fora dos limites
                        91 218 31 1 0 0 0 0 1 0 3];
                
                nlinhas = 6;
                tlast = 0;
                Ntotal = 0;
                for i= 1:nlinhas
                    disp(['---------- Movement ', num2str(i), ' ----------']);
                    
                    [N,posArray,t,XYZiL,XYZfL] = Seq_ContMov(H, seq1(i,:));
                    
                    Ntotal = Ntotal + N;
                    t = t + tlast;
                    tlast = t(N);
                    
                    nn = (Ntotal-N+1):Ntotal;       % array de indices
                    ang(:,nn) = posArray(:,1:N);                  
                    time(1,nn) = t(1,1:N);
                    
                    for j=1:N
                        P = DirKin_4dof(posArray(:,j),1);
                        XYZL(:,j) = P(:,3);
                    end

                    XYZtotal(:,nn) = XYZL(:,1:N);
                    
                    XYZinicial(:,i) = XYZiL';
                    XYZfinal(:,i) = XYZfL';
                end
                
                save('saveVideo1','time','ang','XYZtotal')
                
                figure
                hold on
                plot(time',ang','*')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo 8','Servo 7','Servo 6')
                axis([0 max(time) -100 100])
                grid on
                hold off

                figure
                hold on
                plot3(XYZinicial(1,:),XYZinicial(2,:),XYZinicial(3,:),'*r')
                plot3(XYZtotal(1,:),XYZtotal(2,:),XYZtotal(3,:),'.b')
                plot3(XYZfinal(1,:),XYZfinal(2,:),XYZfinal(3,:),'*g')
                xlabel('pos X')
                ylabel('pos Y')
                zlabel('pos Z')
                grid on
                %axis square
                hold off
                
                
%             case x        % leitura de uma sequencia de movimentos
%                 disp('Maximum Speed: 50');
%                 sendServo(H,23,50,0,0,0,0,0);
% 
%                 S = load('.\graf_will\sequencia');
%                 nValores=10;
%                 for i= 1:nValores
%                     invKin(H, 400+S.testar(i,1), 400+S.testar(i,2), 331-S.testar(i,3));
%                 end
%                               
%             case x         % Inverse Kinematic Point-to-Point
%                 disp(' Inverse Kinematic Point-to-Point');
%                 posX = input(' X position: ');
%                 posY = input(' Y position: ');
%                 posZ = input(' Z position: ');
%                 [posArray,t] = invKinPtP(H,posX,posY,posZ);
%                 
%                 figure
%                 hold on
%                 plot(t',posArray','*')
%                 xlabel('Tempo (ms)')
%                 ylabel('Posição (º)')
%                 legend('Servo 0','Servo 1','Servo 2')
%                 axis([0 max(t) -160 100])
%                 grid on
%                 hold off
%                 
%                 [posArray,t] = invKinPtP(H,0,0,0);
%                 
% %%%%%%%%%%%% Move Arm %%%%%%%%%%%%%            
%             case x
%                 %maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,30,0,0,0,0,0);
%                 move(H,0,85,-90);
%                 pause(2);
%                 move(H,0,90,-35);
%                 pause(2);
%                 move(H,0,-90,-35);
%                 pause(2);
%                 move(H,0,85,45);    % Posição final
%                 pause(2);
%                 for i=0:5
%                     move(H,75,90,-35);
%                     pause(0.3);                
%                     move(H,75,60,-35);
%                     pause(0.3);
%                 end
%                 move(H,0,85,45);    % Posição final
%                 pause(2);
%                 

            case 21
                ContMov_using_2Arms(H)

            otherwise
                break;

        end
    end
    
killCom(H);