close all
clear all
clc

globalVar;

id1=42;
id2=21;
id3=60;

time=1;
speedmax=100;
N=10;
realpos=zeros(3,N);
difpos=zeros(1,3);

H=initCom;

while(1)
    opt=input('\nOption: ');
    
    switch opt
        case 0
            id=input('ID: ');
            pos=input('Position: ');
            setPosId(H,id,pos);
        case 0
            id = input('ID: ');
            pos = setSpeedReadPos(H,id,50);
            disp(['Position: ' num2str(pos)]);
        case 1
            pos = input('Desired Position: ');
            setPosAll(H,pos);
        case 2
            pos=input('Position: ');
          %  time=input('Time: ');
            
            pos1 = setSpeedReadPos(H,id1,speedmax);
            pos2 = setSpeedReadPos(H,id2,speedmax);
            pos3 = setSpeedReadPos(H,id3,speedmax);
            
            q = InvKin_3dof(0,0,pos,50,140,145);
            q1=q(1);
            q2=2*q(2);
            q3=-q(3)-90;
            qnew=[q1 q2 q3];
            
            for i=1:1:3   
                if(qnew(i) > ANGLE_MAX)
                    qnew(i)=ANGLE_MAX;
                else if(qnew(i) < ANGLE_MIN)
                        qnew(i)=ANGLE_MIN;
                    end
                end
            end
            q1=qnew(1);
            q2=qnew(2);
            q3=qnew(3);
            
            difpos1=abs(pos1-q1);
            difpos2=abs(pos2-q2);
            difpos3=abs(pos3-q3);

            difpos=[difpos1 difpos2 difpos3];
            
            for i=1:1:3
                for j=1:1:3
                    if(difpos(i)>=difpos(j))
                        aux=difpos(j);
                        difpos(j)=difpos(i);
                        difpos(i)=aux;
                    end
                end
            end
            difpos_max=difpos(1);
            
            speed1=difpos1/difpos_max*speedmax;
            pos1 = setSpeedReadPos(H,id1,speed1); 
            disp(['Initial Position 1: ' num2str(pos1)]);
            disp(['Final Position 1 [-90º;90º]: ' num2str(q1)]);
            disp(['Position Differencial 1: ' num2str(abs(pos1-q1))]);
            disp(['Speed 1: ' num2str(speed1)]);
 
            speed2=difpos2/difpos_max*speedmax;
            pos2 = setSpeedReadPos(H,id2,speed2);
            disp(['Initial Position 2: ' num2str(pos2)]);
            disp(['Final Position 2 [-90º;90º]: ' num2str(q2)]);
            disp(['Position Differencial 2: ' num2str(abs(pos2-q2))]);
            disp(['Speed 2: ' num2str(speed2)]);
            
            speed3=difpos3/difpos_max*speedmax;
            pos3 = setSpeedReadPos(H,id3,speed3);
            disp(['Initial Position 3: ' num2str(pos3)]);
            disp(['Final Position 3 [-90º;90º]: ' num2str(q3)]);
            disp(['Position Differencial 3: ' num2str(abs(pos3-q3))]);
            disp(['Speed 3: ' num2str(speed3)]);
            
            input('ENTER!');
            setPosId(H,id1,q1);
            setPosId(H,id2,q2);
            setPosId(H,id3,q3);
            
            for i=1:1:N
                realpos(1,i)=setSpeedReadPos(H,id1,speed1);
                realpos(2,i)=setSpeedReadPos(H,id2,speed2);
                realpos(3,i)=setSpeedReadPos(H,id3,speed3);  
            end
            t=1:1:N;
            subplot(211);
            plot(t,realpos(1,t),t,realpos(2,t),t,realpos(3,t))
            subplot(212);
            plot(t,speed1*ones(1,N),t,speed2*ones(1,N),t,speed3*ones(1,N))
        case 3
            pos1 = setSpeedReadPos(H,id1,speed);
            disp(['Position 1: ' num2str(pos1)]);
            pos2 = setSpeedReadPos(H,id2,speed);
            disp(['Position 2: ' num2str(pos2)]);
            pos3 = setSpeedReadPos(H,id3,speed);
            disp(['Position 3: ' num2str(pos3)]);
        case 4
            addr=input('Address: ');
            data=readEeprom(H,addr);
            disp(['Data: ' num2str(data)]); 
        case 5
            id=input('ID: ');
            writeEeprom(H,hex2dec('29'),id); 
        case 6
            pos=readPos(H);
            disp(['Position: ' num2str(pos)]);

        case 8
            aux=input('nº adc: ');
            sendMessageADC(H,aux);
            [r1,r2]=readMessageADC(H);
            val=(r1*256)+r2;
            disp(['valor ADC: ' num2str(val)]);
        case 9
            break;
        otherwise
            break;     
    end
end

killCom(H);