%______________________________________________________________
% File: Sim_ArrowTracking_PTU
% Date: 20-Oct-2014
% Purpose: Simulate the tracking of the "arrow" cue with 
%                   the pan-tilt unit (PTU) mounted on the FANUC
%______________________________________________________________
clear all
close all
clc
%______________________________________________________________
% Parameters and initializations
%______________________________________________________________
%L = [36.4 35.5 60.0 1]*1e-3;                         % Link lengths
L  = [60 35.5 30 1]*1e-3;                             % Link lengths
FANUC_flag = 1;
if FANUC_flag == 1,
    % Read joint angles and FANUC's end-effector coordinates
    JOINT_data = csvread('data/servos_position.txt');    
    FANUC_data = csvread('data/positions.txt');
    % Synchronized time vector
    if JOINT_data(1,1) > FANUC_data(1,1),  dtime  = JOINT_data(1,1);
    else dtime  = FANUC_data(1,1); end
    % PTU joint angles
    tr = JOINT_data(:,1)-dtime; 
    Qr = JOINT_data(:,2:3)';
    ILog = Qr == 0;
    Qr(ILog) = NaN;
    Qr = Qr([2 1],:);
    % FANUC's end-effector coordinates
    FANUC_data = FANUC_data(1:2:end,:);   
    fPptu = 1e-3*FANUC_data(:,2:4)';
    
    %Calculate the experiment max velocity
    maxVel = calculateSpeed(FANUC_data(:,[1 3]))
    
    fPptu(2,:) = fPptu(2,:)-260*1e-3;           % Gripper's offset (YY-axis)
    t = FANUC_data(:,1)-dtime;
    Texe = t(end);
else
       Ts = 5e-3;                                   % Sampling rate
       Texe = 2;                                    % Simulation time
       t = 0 :Ts:Texe;
end
DIM = length(t);
%______________________________________________________________
% Static arrow / Moving PTU
%______________________________________________________________
%posi�l�o da seta em rela��o � ref (1-> 1metro)
% 1000+614 = distacia fanuc seta (Eixo X)
% tem de estar alinhado com a seta
% descobrir onde esta a origem do fanuc e conseguitr realacionar 
rP_arrow = [1 0 0 1]';        % Arrow position in Sr


if FANUC_flag == 1,
    fPptu = [fPptu ; ones(1,DIM)];
    %fanuc para a seta 
    %origem do fanuc no sistema de referencia
    rTf = [ 1  0  0  -885.086*1e-3%X
                 0  1  0  -5.95e-3%Y com val Positivo é pior! 
                 0  0  1   5.07e-3%ZZ
                 0  0  0    1 ];
    rPo = rTf*fPptu;  %Posição do end-efector ao longo do movimento.     
    rPo = rPo(1:3,:);
else %Simula um movimento do fanuc com planeamento de trajetoria
       Pi = [0 2 0.5]'; Pf = [0 -2 0.5]'; % Initial/final camera position S0
       rPo = zeros(3,DIM);
      [ rPo(1,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(1),Pf(1),5 );
      [ rPo(2,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(2),Pf(2),5 );
      [ rPo(3,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(3),Pf(3),5 );
end
%______________________________________________________________
% Desired PTU's tracking: reference joint angles
%___________________________________________rTo___________________
Qd = zeros(2,DIM);
for ii = 1:DIM, % Inverse kinematics _________________________
      rTo = [eye(3) rPo(:,ii); 0 0 0 1];% assume que não ha rotacao do gripper
      SoP_arrow = rTo\rP_arrow;
      Qd(:,ii) = InverseKinematics( SoP_arrow,L );      
end
%______________________________________________________________
% Animate motion using a PTU's proportional controller
%______________________________________________________________
H=figure(1); set(H,'Color',[1 1 1]), EL = 0; AZ = 90;
figure(2)
Q = zeros(2,DIM);
ERR_img = zeros(2,DIM);
P_ptu = zeros(3,4); P_cam = zeros(3,4);
for ii = 1:DIM,
    
      if ii == 1, % Initial alignment of the camera with the arrow
          % Inverse kinematics _____________________________________
          rTo = [eye(3) rPo(:,1); 0 0 0 1];
          SoP_arrow = rTo\rP_arrow;
          Q(:,ii) = InverseKinematics( SoP_arrow,L );      
      else
             % Proportional control based on the image error
             La = P_ptu(:,4); Lb = rP_arrow(1:3);
             P0 = P_cam(:,1); P1 = P_cam(:,2); P2 = P_cam(:,3);
             A = [La-Lb P1-P0 P2-P0];
             b = La-P0;
             S = A\b;
             P_img = La+(Lb-La)*S(1);
             q1 = Q(1,ii-1); q2 = Q(2,ii-1);
             s1 = sin(q1); c1 = cos(q1); s2 = sin(q2); c2 = cos(q2);
             l1 = L(1); l2 = L(2); l3 = L(3)-0.01*L(3);
             T = [ c1*c2 -c1*s2 s1 (l3*c1*c2-l2*c1*s2)+rPo(1,ii-1)%Usa a posição do fanuc?
                      s1*c2 -s1*s2 -c1 (l3*s1*c2-l2*s1*s2)+rPo(2,ii-1)
                      s2        c2         0   (l3*s2+l2*c2+l1)+rPo(3,ii-1)    
                      0          0           0  1                                ];
             P_img = T\[P_img; 1];
             ERR_img(:,ii) = [P_img(3) P_img(2)]'; 
             Q(1,ii) = 400*P_img(3)+Q(1,ii-1); %O que representa o 400?
             Q(2,ii) = -400*P_img(2)+Q(2,ii-1); 
             Q_ptu = Q(:,1:ii)*180/pi;
             %pause
      end
      % Direct kinematics ______________________________________
      [P_ptu P_cam] = DirectKinematics( Q(:,ii),L ); 
      P_ptu = P_ptu + repmat(rPo(:,ii),1,4);
      P_cam = P_cam + repmat(rPo(:,ii),1,4);
%       % Animate motion
%       FontSize = 16;
%       figure(1), clf
%       get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]);
%       hold on
%       H = plot3(P_ptu(1,:),P_ptu(2,:),P_ptu(3,:),'gp-', rP_arrow(1),rP_arrow(2),rP_arrow(3),'bx',...
%                 [P_ptu(1,end) rP_arrow(1)],[P_ptu(2,end) rP_arrow(2)],[P_ptu(3,end) rP_arrow(3)],'b-.',...
%                 rPo(1,1:ii),rPo(2,1:ii),rPo(3,1:ii),'r.','markers',FontSize ); 
%             
%       fill3([P_cam(1,:) P_cam(1,1)],[P_cam(2,:) P_cam(2,1)],[P_cam(3,:) P_cam(3,1)],'k')
%       xlabel('XX','FontSize', FontSize ), ylabel('YY','FontSize', FontSize ), zlabel('ZZ','FontSize', FontSize )
%       set(gca,'FontSize',FontSize ),
% %       axis equal, axis([-0.5 1.5 -0.5 0.5 -1 2]), view(EL,AZ), %grid on
%       axis equal, axis([-0.5 1.5 -0.5 2.5 -1 2]), view(EL,AZ), %grid on
%       % Pan-tilt joint angles
%       figure(2);
%       get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]);
%       
%       subplot(2,1,1), plot(t(1:ii),Q(1,1:ii)*180/pi,'r.'), xlim([0 Texe])
%                                   xlabel('Time (s)','FontSize', FontSize ), ylabel('PAN: Q1 (deg)','FontSize', FontSize )
%                                   set(gca,'FontSize',FontSize );
%       subplot(2,1,2), plot(t(1:ii),Q(2,1:ii)*180/pi,'r.'), xlim([0 Texe])
%                                   xlabel('Time (s)','FontSize', FontSize ), ylabel('TILT: Q2 (deg)','FontSize', FontSize )
%                                   set(gca,'FontSize',FontSize );
      if ii == 1, pause, [EL,AZ] = view;  end
      pause(0.0001)
      if ii== 210, pause;end
end
%______________________________________________________________
% Performance measure
%______________________________________________________________
if FANUC_flag == 1,
%     figure('Name', 'Q','Color',[1 1 1])
%     subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r:', t,Q(1,:)*180/pi,'k', tr,(-Qr(1,:)+1454)/10,'b')
%                                 xlabel('Time (s)','FontSize', 12), ylabel('PAN: Q1 (deg)','FontSize', 12), legend('q1d','q1','q1r')
%                                 xlim([17 35]),ylim([-30 30])%xlim([13 21])%xlim([11 24])
%                                 set(gca,'FontSize',12);
%     subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r:', t,Q(2,:)*180/pi,'k', tr,(-Qr(2,:)+1862)/10,'b')%1896
%                                 xlabel('Time (s)','FontSize', 12), ylabel('TILT: Q2 (deg)','FontSize', 12), legend('q2d','q2','q2r')                                                                                
%                                 xlim([17 35]),ylim([-30 30])%xlim([13 21])%xlim([11 24])
%                                 set(gca,'FontSize',12);
    
     %XLim = [14 26];
     %YLim = [-45 45];%xlim([13 21])%
     XLim = [11 24];
     YLim = [-30 30];
%     t = t - XLim(1);
%     tr = tr - XLim(1);
%     XLim = XLim - XLim(1);
    
    FontSize = 16;
    figure('Name', 'Q','Color',[1 1 1])
    subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r', tr,(-Qr(1,:)+1454)/10,'b','LineWidth',2,'MarkerSize',4)
                                xlabel('Time (s)','FontSize', FontSize), ylabel('PAN: Q1 (deg)','FontSize', FontSize ), legend('q1d','q1r')
                                xlim(XLim),ylim(YLim)
                                set(gca,'FontSize',FontSize );
    subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r',tr,(-Qr(2,:)+1862)/10,'b','LineWidth',2,'MarkerSize',2)%1896
                                xlabel('Time (s)','FontSize', FontSize ), ylabel('TILT: Q2 (deg)','FontSize', FontSize ), legend('q2d','q2r')                                                                                
                                xlim(XLim),ylim(YLim)
                                set(gca,'FontSize',FontSize );
end



figure('Name', 'XX/YY - ERROR','Color',[1 1 1])%,'FontSize', 12)
subplot(2,1,1), plot(t,ERR_img(1,:),'r')
                            xlabel('Time (s)','FontSize', 12), ylabel('XX-err (m)','FontSize', 12), title('Arrow Centroid Projected on the Image Plane','FontSize', 12)
                            xlim([17 35]),set(gca,'FontSize',12);
                            
subplot(2,1,2), plot(t,ERR_img(2,:),'r')
                            xlabel('Time (s)','FontSize', 12), ylabel('YY-err (m)','FontSize', 12)
                            xlim([17 35]),set(gca,'FontSize',12);
                            
                            
figure('Name', 'PAN/TILT - ERROR','Color',[1 1 1])
subplot(2,1,1), plot(t,(Qd(1,:)-Q(1,:))*180/pi,'r')
                            xlabel('Time (s)','FontSize', 12), ylabel('PAN err (deg)','FontSize', 12)
                            set(gca,'FontSize',12),xlim([17 35]);
subplot(2,1,2), plot(t,(Qd(2,:)-Q(2,:))*180/pi,'r')
                            xlabel('Time (s)','FontSize', 12), ylabel('TILT err (deg)','FontSize', 12)
                            set(gca,'FontSize',12),xlim([17 35]);
%______________________________________________________________
% MSE calculos

%Time 
valTime = [11.64 21.19];% => 
timeExe = t(414)-t(28); % Time of execution

% Error real trajectory
    % Pan
panErrorRealTrajA = (Q(1,1:1154)*180/pi);% Setpoint
panErrorRealTrajB = ((-Qr(1,:)+1454)/10);% Real traj
panErrorRealTrajBinterpol = interp1(tr,panErrorRealTrajB,t(1:1154))';

panErrorRealTraj = panErrorRealTrajA - panErrorRealTrajBinterpol;
    % Tilt
tiltErrorRealTrajA = (Q(2,1:1154)*180/pi);% Setpoint
tiltErrorRealTrajB = ((-Qr(2,:)+1862)/10);% Real traj
tiltErrorRealTrajBinterpol = interp1(tr,tiltErrorRealTrajB,t(1:1154))';
tiltErrorRealTraj = tiltErrorRealTrajA - tiltErrorRealTrajBinterpol;

panErrorRealTrajMse = sum((panErrorRealTraj(28:414)').^2)/timeExe
tiltErrorRealTrajMse = sum((tiltErrorRealTraj(28:414)').^2)/timeExe

% Arrow Centroid Projected on the Image Plane
xxError = ERR_img(1,28:414);% Get valid data
yyError = ERR_img(2,28:414);

xxErrorMse = sum(xxError.^2)/timeExe 
yyErrorMse = sum(yyError.^2)/timeExe 


% angle error of PTU
panError = Qd(1,:)*180/pi - Q(1,:)*180/pi;% Qd(1,:)-Q(1,:);
panError = panError(1,28:414); % Get valid data
tiltError = Qd(2,:)*180/pi - Q(2,:)*180/pi;
tiltError = tiltError(1,28:414); % Get valid data


panMseError = sum(panError.^2)/timeExe
tiltMseError = sum(tiltError.^2)/timeExe

%______________________________________________________________
% Plot the difference between ideal, real setpoint, and real trajectory

figure;
get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]);%Para que é que isto serve?!! 
      
subplot(2,1,1),  plot(t(1:1154),panErrorRealTraj,[0 t(28:414)' 30],[0 panError 0]), xlim([11 24])
                xlabel('Time (s)','FontSize', 12), ylabel('PAN (deg)','FontSize', 12),
                title('Difference between Ideal Set Point, Calculated Set Point and Real Trajectory','FontSize', 12),
                legend('Diff Calc Set Point vs Real Trajectory','Diff Idial Set Point vs Calc Set Point'),
            set(gca,'FontSize',12),xlim([17 35]);
subplot(2,1,2),  plot(t(1:1154),tiltErrorRealTraj,[0 t(28:414)' 30],[0 tiltError 0]), xlim([11 24]),ylim([-10 10])
                xlabel('Time (s)','FontSize', 12), ylabel('TILT (deg)','FontSize', 12),
                legend('Diff Calc Set Point vs Real Trajectory','Diff Idial Set Point vs Calc Set Point'),
            set(gca,'FontSize',12),xlim([17 35]);
            
% Plot Pand and Tilt diference between real trajectory and calculated with
% kinematics

figure;
plot(t(1:1154),panErrorRealTraj,t(1:1154),tiltErrorRealTraj),xlim([11 24])
           