close all
clear all
clc

globalVar;

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

speedmax=100;

N=6;

realpos=zeros(3,N*8);
speed=zeros(3,N*8);
difpos=zeros(1,3);
time=zeros(1,N*8);

H=initCom;

pos1 = setSpeedReadPos(H,id1,speedmax);
pos2 = setSpeedReadPos(H,id2,speedmax);
pos3 = setSpeedReadPos(H,id3,speedmax);
setPosId(H,id1,0);
setPosId(H,id2,0);
setPosId(H,id3,-90);
setPosId(H,id1,0);
setPosId(H,id2,0);
setPosId(H,id3,-90);
position=[250 335 280 300 335 230 280 335];
tic
for i=1:1:8
    pos=position(i);

    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=3.5*q(2);
    q3=-q(3)-90;
    qnew=[q1 q2 q3];

    for j=1:1:3   
        if(qnew(j) > ANGLE_MAX)
            qnew(j)=ANGLE_MAX;
        else if(qnew(j) < ANGLE_MIN)
                qnew(j)=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 l=1:1:3
        for j=1:1:3
            if(difpos(l)>=difpos(j))
                aux=difpos(j);
                difpos(j)=difpos(l);
                difpos(l)=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!');
%     realpos(1,1)=pos1;
%     realpos(2,1)=pos2;
%     realpos(3,1)=pos3;
   
    setPosId(H,id1,q1);
    setPosId(H,id2,q2);
    setPosId(H,id3,q3);
  %  time(1)=toc;

    for j=1:1:N
        realpos(1,j+(i-1)*N)=setSpeedReadPos(H,id1,speed1);
        realpos(2,j+(i-1)*N)=setSpeedReadPos(H,id2,speed2);
        realpos(3,j+(i-1)*N)=setSpeedReadPos(H,id3,speed3); 
        time(1,j+(i-1)*N)=toc;
        speed(1,j+(i-1)*N)=speed1;
        speed(2,j+(i-1)*N)=speed2;
        speed(3,j+(i-1)*N)=speed3;
    end

end

    time(1,:)=time(1,:)-time(1,1);
    joints_alternate.realpos=realpos;
    joints_alternate.speed=speed;
    joints_alternate.time=time;
    save .\results\joints_alternate joints_alternate

    S=load('.\results\joints_alternate');
    time=S.joints_alternate.time;
    speed=S.joints_alternate.speed;
    realpos=[S.joints_alternate.realpos(1,:); S.joints_alternate.realpos(2,:)/3.5; -(S.joints_alternate.realpos(3,:)+90)];
    N=6;
    t=1:1:8*N;
    subplot(211);
    hold on     
%     axis([0 ceil(time(end)*1e3) -200 90])
    plot(time*1e3,realpos(1,:),'*-',time*1e3,realpos(2,:),'*-',time*1e3,realpos(3,:),'*-')
    title('Inverse Kinematic Movement')
    ylabel('Position (º)')
    xlabel('Time (ms)')
    legend('Joint Q0','Joint Q1','Joint Q2')
    hold off
    subplot(212);
    hold on
    axis([0 ceil(time(end)*1e3) -10 110])
    plot(time*1e3,speed(1,:),time*1e3,speed(2,:),time*1e3,speed(3,:))
    ylabel('Speed (%)')
    xlabel('Time (ms)')
    legend('Joint Q0','Joint Q1','Joint Q2')
    hold off
    
killCom(H);