%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');
                IDs = [3 1 5 4 2];
                speed = 25;
                pos = 0;
                for i=1:5
                    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         % Direct Kinematic
                q1 = input('Ang Joint 1 (º): ');
                q2 = input('Ang Joint 2 (º): ');
                q3 = input('Ang Joint 3 (º): ');
                q4 = input('Ang Joint 4 (º): ');
                q5 = input('Ang Joint 5 (º): ');
                q6 = input('Ang Joint 6 (º): ');

                dirKin(H,q1,q2,q3,q4,q5,q6);
                
                [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-200;
                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;                % [ ]
                posY = (posY+200)*10;                % [ ]
                posZ = (posZ+200)*10;                % [ ]

                [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;
                    [command,return1,return2,return3] = readServo(H);
                    pos = (return1*256) + return2;
                    qf4 = (((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;
                    qf5 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                    pos = (return2*256) + return3;
                    qf6 = (((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), 'º']);
                    disp(['Ang Joint 4: ', num2str(qf4), 'º']);
                    disp(['Ang Joint 5: ', num2str(qf5), 'º']);
                    disp(['Ang Joint 6: ', num2str(qf6), 'º']);
                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= [-10 0 0 -10 80 0];
                for i=1:6
                    pos= ini_pos(i);
                    setPosId(H,i,pos);
                end
                
            case 11
                out_pos= [10 45 20 45 45 0];
                for i=1:6
                    pos= out_pos(i);
                    setPosId(H,i,pos);
                    pause(0.5);
                end
                
            case 12     % Direct Kinematic
                sendServo(H,1,13,0,0,0,0,0,0);
                
            case 13     % Inverse Kinematic
                sendServo(H,1,14,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 = [0 0 -25.6 1 3];
                seq2 = [60 0 -16 1 3];
                Seq_ContMov(H, seq2);
                
                seq1 = [0 0 30 3 3;
                        100 0 0 3 3;
                        0 0 0 3 3;
                        0 0 -25.6 3 3];
                seq2 = [0 60 -16 2 3;
                        -60 0 -16 2 3;
                        0 -60 -16 2 3;
                        60 0 -16 2 3];
                
                nlinhas = 4;
                tlast = 0;
                Ntotal = 0;
                for i= 1:nlinhas
                    disp(' ');
                    disp(['---------- Movement ', num2str(i), ' ----------']);
                    
                    [isVal,N,posArray,t,XYZi,XYZf,PeeArray] = Seq_ContMov(H, seq2(i,:));
                    
                    if (isVal == 1)
                        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
                            P = DirKin_6dof(posArray(:,j)');
                            XYZ(:,j) = P(:,4);
                        end
%                         for j=1:N
%                             if (posArray(1,j)==-150 || posArray(2,j)==-150 || posArray(3,j)==-150 || posArray(4,j)==-150 || posArray(5,j)==-150 || posArray(6,j)==-150)
%                                 XYZ(:,j) = XYZf-0.1;      % FILTRO
%                             else
%                                 P = DirKin_6dof(posArray(:,j)');
%                                 XYZ(:,j) = P(:,4);
%                             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:seq2(i,4);             % Executation time
                        DIM = length(t);                % Dimension of time vector
                        for tt = 2:DIM,
                            [Pee,dPee] = CartTrajPlanning( t(tt),t(end),XYZi,XYZf,seq2(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
                end
                
                if (isVal == 1)
                    figure
                    hold on
                    plot(time',ang','.')
                    plot(time',imag_ang','-')
                    xlabel('Tempo (ms)')
                    ylabel('Posição (º)')
                    legend('Servo1 - Hip1','Servo2 - Hip2','Servo3 - NotUse','Servo4 - Knee','Servo5 - Ankle1','Servo6 - Ankle2','Imag Servo1 - Hip1','Imag Servo4 - Knee','Imag Servo5 - Ankle1')
%                     for i=1:nlinhas
%                         h=vline(seq2(i,4)*1000,'k');
%                     end
                    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
                    
                    ini_pos= [-10 0 0 -10 80 0];
                    for i=1:6
                        pos= ini_pos(i);
                        setPosId(H,i,pos);
                    end
                end

%%%%%%   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
                Impact(H);
                
            otherwise
                break;

        end
    end
    
killCom(H);