%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 == 3)
                    pos = input('Position [-90 ; 0]º: ');
                    while (pos ~= [-90:0])
                        pos = input('Position [-90 ; 0]º: ');
                    end
                else
                    pos = input('Position [-90 ; 90]º: ');
                    while (pos ~= [-90:90])
                    	pos = input('Position [-90 90]º: ');
                    end
                end
                setPosId(H,id,pos);

            case 4
                disp(' All Servo Positions');
                IDs = [1 2 3];
                speed = 25;
                pos = 0;
                for i=1:3
                    pos = setSpeedReadPos(H,IDs(i),speed);
                    if (pos ~= -150)
                        disp(['  Servo   : ', num2str(IDs(i))]);
                        disp([' Position : ', num2str(pos), 'º']);
                    end
                    i = i+1;
                end
                disp(' ');
                disp(' All Servos Speeds are at 25%');
                
            case 5
                release(H);
                
            case 6
                nServosMove(H);

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

                dirKin(H,q1,q2,q3);
                
                [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-200;
                yt = yt/10-200;
                zt = zt/10;
                disp('  RESULTS  ');
                disp(['Pos Xt: ', num2str(xt)]);
                disp(['Pos Yt: ', num2str(yt)]);
                disp(['Pos Zt: ', num2str(zt)]);
                
            case 8         % Inverse Kinematic
                posX = input('Pos Xt (mm): ');
                posY = input('Pos Yt (mm): ');
                posZ = input('Pos Zt (mm): ');     % limites entre:
                posX = (posX+200)*10;                % [-234.0 +234.0]
                posY = (posY+200)*10;                % [-374.0 +374.0]
                posZ = posZ*10;                      % [-84.0 +384.0]

                [isVal] = invKin(H,posX,posY,posZ);
                
                if (isVal == 1)
                    [command,return1,return2,return3] = readServo(H);
                    pos = (return1*256) + return2;
                    qf1 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                    temp_ret = return3;
                    [command,return1,return2,return3] = readServo(H);
                    pos = (temp_ret*256) + return1;
                    qf2 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                    pos = (return2*256) + return3;
                    qf3 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;

                    disp(['Ang Joint 1: ', num2str(qf1), 'º']);
                    disp(['Ang Joint 2: ', num2str(qf2), 'º']);
                    disp(['Ang Joint 3: ', num2str(qf3), 'º']);
                end
                
            case 9
                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 10
                ini_pos= [5 20 -90];
                for i=1:3
                    pos= ini_pos(i);
                    setPosId(H,i,pos);
                end
                
            case 11
                out_pos= [0 0 -70];
                for i=1:3
                    pos= out_pos(i);
                    setPosId(H,i,pos);
                    pause(0.5);
                end
                
            case 12     % Direct Kinematic
                sendServo(H,1,12,0,0,0,0,0,0);
                
            case 13     % Inverse Kinematic
                sendServo(H,1,13,0,0,0,0,0,0);
                
            case 14         % Inverse Kinematic
                PtPMov(H);

            case 15
                ContMov(H);

            case 16
                ContMov_v2(H);
                
            case 17
                    % [x y z Tx ord]
                seq1 = [109.4 0 311.2 1 3];
                seq2 = [102 0 314 1 3];
                seq3 = [60 0 325 1 3];
                Seq_ContMov(H, seq3);
                
                seq1 = [0 109.4 311.2 2 3;
                        -109.4 0 311.2 2 3;
                        0 -109.4 311.2 2 3;
                        0 0 311.2 2 3;
                        0 0 333 2 3];
                seq2 = [0 102 314 2 3;
                        -102 0 314 2 3;
                        0 -102 314 2 3;
                        102 0 314 2 3];
                seq3 = [0 60 325 2 3;
                        -60 0 325 2 3;
                        0 -60 325 2 3;
                        60 0 325 2 3];
                
                nlinhas = 4;
                tlast = 0;
                Ntotal = 0;
                lengPee = 0;
                for i= 1:nlinhas
                    disp(' ');
                    disp(['---------- Movement ', num2str(i), ' ----------']);
                    
                    [N,posArray,t,XYZi,XYZf] = Seq_ContMov(H, seq3(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
                        if (posArray(1,j)==-150 || posArray(2,j)==-150 || posArray(3,j)==-150)
                            XYZ(:,j) = XYZf-0.1;      % FILTRO
                        else
                            XYZ(:,j) = DirKin3dof(posArray(:,j)');
                        end
                    end

                    XYZtotal(:,nn) = XYZ(:,1:N);
                    
                    XYZinicial(:,i) = XYZi';
                    XYZfinal(:,i) = XYZf';
                    
        %------------------------------------------------------------------
                    % Obter a Traj teorica
                    Ts = 0.05;                      % Sampling time
                    t = 0:Ts:seq3(i,4);             % Executation time
                    DIM = length(t);                % Dimension of time vector
                    for tt = 2:DIM,
                        [Pee,dPee] = CartTrajPlanning( t(tt),t(end),XYZi,XYZf,seq3(i,5));
                        Pee_aux(:,tt) = Pee;                        
                    end

                    di = ((DIM-1)*(i-1))+1;
                    df = (DIM-1)*i;
                    Pee_total(:,di:df) = Pee_aux(:,2:DIM);
        %------------------------------------------------------------------
                end
                
                figure
                hold on
                plot(time',ang','.')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee')
                axis([0 max(time) -160 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')
                plot3(Pee_total(1,:),Pee_total(2,:),Pee_total(3,:),'-.g')
                xlabel('pos X')
                ylabel('pos Y')
                zlabel('pos Z')
                grid on
                %axis square
                hold off

%%%%%%   Sensores    %%%%%%
            case 18        % read all sensors - command 30
                sendServo(H,1,18,0,0,0,0,0,0);
                for i=1:4
                    [command,return1,return2,return3]=readServo(H);
                    valSensor(i,1)=return1*256+return2;
                    disp(['Sensor ' num2str(i) ': ' num2str(valSensor(i,1))]);
                end
                [command,return1,return2,return3]=readServo(H);
                totalSensor = return1*256+return2;
                disp(['Total Sensors (without calibration): ' num2str(totalSensor)]);
                
            case 19         % calibr all sensors - command 31
                sendServo(H,1,19,0,0,0,0,0,0);
                for i=1:4
                    [command,return1,return2,return3]=readServo(H);
                    valSensor(i,1)=return1*256+return2;
                    disp(['Calibration Sensor ' num2str(i) ': ' num2str(valSensor(i,1))]);
                end
                
            case 20
                % Leitura de todos os sensores e calculo do Centro de Massa
                x = 1:nSampPlot;
                COPxp=0;
                COPyp=0;

                subplot(211)
                hold on    

                xxl=-middleX;
                xxr=middleX;
                yyb=-middleY;
                yyt=middleY;
                axis([xxl-0.5 xxr+0.5 yyb-0.5 yyt+0.5]);
                plot(xxl,yyt,'r.',xxr,yyt,'g.',xxl,yyb,'c.',xxr,yyb,'m.','MarkerSize',30);
                hPlot(5)=plot(COPxp,COPyp,'.','XDataSource','COPxp','YDataSource','COPyp','MarkerSize',30);
                title('Centro de Pressão','FontWeight','bold','FontSize',12);
                text(xxl+0.1,yyt,'Sensor 1','FontWeight','bold','FontSize',12);
                text(xxl+0.1,yyb,'Sensor 2','FontWeight','bold','FontSize',12);
                text(xxr-1.1,yyb,'Sensor 3','FontWeight','bold','FontSize',12);
                text(xxr-1.1,yyt,'Sensor 4','FontWeight','bold','FontSize',12);
                xlabel('Distância (cm)','FontWeight','bold','FontSize',12);
                ylabel('Distância (cm)','FontWeight','bold','FontSize',12);
                h = vline(0,'k');
                xplot=xxl-0.5:1:xxr+0.5;
                yplot=0*xplot;
                plot(xplot,yplot,'k-')
                hold off

                subplot(212)
                hold on
                hPlot(1) = plot(x,y(1,:),'r','MarkerSize',20);
                hPlot(2) = plot(x,y(2,:),'c');
                hPlot(3) = plot(x,y(3,:),'m');
                hPlot(4) = plot(x,y(4,:),'g');
                title('Valor dos Sensores','FontWeight','bold','FontSize',12)
                ylabel('Valor (0...1023)','FontWeight','bold','FontSize',12)
                legend('Sensor 1','Sensor 2','Sensor 3','Sensor 4','FontWeight','bold','FontSize',11);
                
                StripChart('Initialize',gca)
                hold off

                while(ishandle(hPlot(1)))
                    sendServo(H,1,18,0,0,0,0,0,0);
                    for i=1:1:4
                        [command,return1,return2,return3]=readServo(H);
                        y(i,N)=return1*256+return2-calibSensor(i);
                       StripChart('Update',hPlot(i),y(i,N));
                    end
                    [command,return1,return2,return3]=readServo(H);
                    totalSensor = return1*256+return2;

                    COPxp=(-y(1,N)-y(2,N)+y(3,N)+y(4,N))*middleX/totalSensor;
                    COPyp=(y(1,N)-y(2,N)-y(3,N)+y(4,N))*middleY/totalSensor;

                    refreshdata(hPlot(5),'caller');
                    drawnow
                end
                
            case 21
                sendServo(H,1,17,0,0,0,0,0,0);
                [command,return1,return2,return3]=readServo(H);
                N = return1;
                for i=1:N
                    [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;
                    PeeArray(1,i) = xt/10-200;
                    PeeArray(2,i) = yt/10-200;
                    PeeArray(3,i) = zt/10;
                end
                
                PeeArray
                
            case 22
                ContMov_v3(H);
                
            case 23
                    % [x y z Tx ord]
                seq1 = [109.4 0 311.2 1 3];
                seq2 = [102 0 314 1 3];
                seq3 = [60 0 325 1 3];
                Seq_ContMov_2(H, seq3);
                
                seq1 = [0 109.4 311.2 2 3;
                        -109.4 0 311.2 2 3;
                        0 -109.4 311.2 2 3;
                        0 0 311.2 2 3;
                        0 0 333 2 3];
                seq2 = [0 102 314 2 3;
                        -102 0 314 2 3;
                        0 -102 314 2 3;
                        102 0 314 2 3];
                seq3 = [0 60 325 2 3;
                        -60 0 325 2 3;
                        0 -60 325 2 3;
                        60 0 325 2 3];
                
                nlinhas = 4;
                tlast = 0;
                Ntotal = 0;
                lengPee = 0;
                for i= 1:nlinhas
                    disp(' ');
                    disp(['---------- Movement ', num2str(i), ' ----------']);
                    
                    [N,posArray,t,XYZi,XYZf,PeeArray] = Seq_ContMov_2(H, seq3(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);
                    imag_ang(:,nn) = PeeArray(:,1:N);
                    
                    for j=1:N
                        if (posArray(1,j)==-150 || posArray(2,j)==-150 || posArray(3,j)==-150)
                            XYZ(:,j) = XYZf-0.1;      % FILTRO
                        else
                            XYZ(:,j) = DirKin3dof(posArray(:,j)');
                        end
                    end

                    XYZtotal(:,nn) = XYZ(:,1:N);
                    
                    XYZinicial(:,i) = XYZi';
                    XYZfinal(:,i) = XYZf';
                    
        %------------------------------------------------------------------
                    % Obter a Traj teorica
                    Ts = 0.05;                      % Sampling time
                    t = 0:Ts:seq3(i,4);             % Executation time
                    DIM = length(t);                % Dimension of time vector
                    for tt = 2:DIM,
                        [Pee,dPee] = CartTrajPlanning( t(tt),t(end),XYZi,XYZf,seq3(i,5));
                        Pee_aux(:,tt) = Pee;                        
                    end

                    di = ((DIM-1)*(i-1))+1;
                    df = (DIM-1)*i;
                    Pee_total(:,di:df) = Pee_aux(:,2:DIM);
        %------------------------------------------------------------------
                end
                
                figure
                hold on
                plot(time',ang','.')
                plot(time',imag_ang','-')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','Imag Servo1 - Ankle1','Imag Servo2 - Ankle2','Imag Servo3 - Knee')
                axis([0 max(time) -160 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')
                plot3(Pee_total(1,:),Pee_total(2,:),Pee_total(3,:),'-.g','LineWidth',2)
                xlabel('pos X')
                ylabel('pos Y')
                zlabel('pos Z')
                grid on
                %axis square
                hold off
                
            case 24
                ContMov_v4(H);

            case 25
                Leg_Sens(H);
                
            case 26
                Leg_Sens_TransJ(H);
                
            case 27
                sendServo(H,1,27,0,0,0,0,0,0);
                
                return1 = 0;        % apenas para limpar o registo
                i=0;
                while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
                    i = i+1;
                    [command,return1,return2,return3] = readServo(H);
                    Qvel(1,i) = return1*256 + return2;
                    [command,return1,return2,return3] = readServo(H);
                    t(1,i) = return1*256+return2;
                end
                N = i-1;
                Qvel1 = Qvel(1,1:N);
                t1 = t(1,1:N);
                
                return1 = 0;        % apenas para limpar o registo
                i=0;
                while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
                    i = i+1;
                    [command,return1,return2,return3] = readServo(H);
                    Qvel(1,i) = return1*256 + return2;
                    [command,return1,return2,return3] = readServo(H);
                    t(1,i) = return1*256+return2;
                end
                N = i-1;
                Qvel2 = Qvel(1,1:N);
                t2 = t(1,1:N);
                
                figure
                hold on
                plot(t1',Qvel1','.b')
                plot(t2',Qvel2','*g')
                xlabel('Tempo (ms)')
                ylabel('Qvel (%)')
                legend('Servo1 with constant Qvel','Servo1 with variable Qvel')
                axis([-100 (t2(1,N)+200) 0 20])
                grid on
                hold off
                
            case 28
                sendServo(H,1,28,0,0,0,0,0,0);
                
                return1 = 0;        % apenas para limpar o registo
                i=0;
                while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
                    i = i+1;                    
                    [command,return1,return2,return3] = readServo(H);
                    posArray(1,i) = return1*256 + return2;
                    temp_ret = return3;
                    [command,return1,return2,return3] = readServo(H);
                    posArray(2,i) = temp_ret*256 + return1;
                    posArray(3,i) = return2*256 + return3;
                    
                    [command,return1,return2,return3] = readServo(H);
                    t(1,i) = return1*256+return2;
                end
                N = i-1;
                
                for j=1:3;
                    for i=1:N
                        posArray(j,i) = (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
           % neste caso é melhor não corrigir para ver os valores reais
%                         % TENTAR diminuir o erro
%                         if (posArray(j,i)==-150)
%                             if (i-1 ~= 0)
%                                 posArray(j,i) = posArray(j,i-1);
%                             end
%                         end
                    end
                end
                posArray = posArray(:,1:N);
                t = t(1,1:N);
                
                figure
                hold on
                plot(t',posArray(1,:)','*r')
                plot(t',posArray(2,:)','*b')
                plot(t',posArray(3,:)','.g')
                xlabel('Tempo (ms)')
                ylabel('Position (º)')
                legend('Q real','Q teorical','Q final')
                axis([0 max(t) -100 100])
                grid on
                hold off
                
            case 29
                N = 50;
                for k= 1:N
                    sendServo(H,1,18,0,0,0,0,0,0);
                    for i=1:4
                        [command,return1,return2,return3]=readServo(H);
                        sensor(i,k) = return1*256+return2;
                    end
                    [command,return1,return2,return3]=readServo(H);
                    totalSensor(k) = return1*256+return2;
                end
                for i=1:4
                    media_sensor(i) = sum(sensor(i,:))/N;
                    disp(['Media do Sensor ' num2str(i) ': ' num2str(media_sensor(i))]);
                end
                disp(['Media do Total dos Sensores (sem calibração): ' num2str(sum(totalSensor)/N)]);
                
            case 30
                Leg_Sens_simpl(H)
    
            case 31
                sendServo(H,1,31,0,0,0,0,0,0);
                
            case 32
                Leg_Sens_simpl_2(H)

            case 33
                PtPvsCont(H)
                
            case 34
                test_sensor(H)
                
            case 35
%                 id = 1;
%                 speed = input('Speed [0 ; 100]%: ');
%                 pos = setSpeedReadPos(H,id,speed);
%                 disp(['Position: ', num2str(pos), 'º']);
%                 
%                 pos = -30;
%                 setPosId(H,id,pos);
%                 
%                 pos = -10;
%                 setPosId(H,id,pos);
%                 
%                 pos = 0;
%                 setPosId(H,id,pos);
                
                id = 2;
                speed = input('Speed [0 ; 100]%: ');
                pos = setSpeedReadPos(H,id,speed);
                disp(['Position: ', num2str(pos), 'º']);
                
                pos = -10;
                setPosId(H,id,pos);
                pause(3)
                
                pos = 10;
                setPosId(H,id,pos);
                pause(3)
                
                pos = 0;
                setPosId(H,id,pos);
                pause(3)
                
%                 XYZ = zeros(1,3);
%                 XYZ(1,1) = 2+100;
%                 XYZ(1,1) = -10+100;
%                 XYZ(1,1) = 10+100;
% 
%                 Tramas = 1;
%                 for i = 1:Tramas
%                     param1 = bitand(XYZ(i,1),MASK_HIGH) / 256;
%                     param2 = bitand(XYZ(i,1),MASK_LOW);
%                     param3 = bitand(XYZ(i,2),MASK_HIGH) / 256;
%                     param4 = bitand(XYZ(i,2),MASK_LOW);
%                     param5 = bitand(XYZ(i,3),MASK_HIGH) / 256;
%                     param6 = bitand(XYZ(i,3),MASK_LOW);
%                     sendServo(H,Tramas,35,param1,param2,param3,param4,param5,param6);
%                 end
                
            otherwise
                break;

        end
    end
    
killCom(H);