%% EXERCISE 6
% Visual servoing loop
clear all;
%% CUBE OBJECT DEFINITION
%Cube size; origin at a vertex.
sizex=.8;
sizey=.8;
sizez=.5;
% Definition of 3D points w.r.t. cube frame
P1 = [sizex,  sizey, 0]'; 
P2 = [sizex,  0, 0]'; 
P3 = [0,  sizey, 0]'; 
P4 = [0,  0, 0]'; 
P = [P1 P2 P3 P4]

% Setting object frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
Tobject = transl(0, -1, sizez)*rpy2tr(0,0,-10*pi/180); 

%% SETUP
% Definition of the camera
cam = CentralCamera('default');
cam.f=0.008; %default is 0.008
% Tc0 = Initial transform of the camera w.r.t world
Tc0 = transl(0, 0, 3)*rpy2tr(0, 160*pi/180, 10*pi/180);
%Projection of the points P into the image plane
p0 = cam.project(P, 'objpose', Tobject, 'pose', Tc0) %with object translated
%Desired transform of the object w.r.t. camera
TcStar_t = troty(pi)*transl(-sizex/2, -sizey/2, -1.5)


%% VISUALIZE 2D IMAGE
% plots the points as seen by the camera when located at Tc0
cam.plot(p0) 
cam.hold;
% plots the points as seen by the camera when located at the desired pose
% of the camera
Tc_desired = Tobject*inv(TcStar_t)
%plots the points as seen by the camera when located at Tc_desired
pStar = cam.plot(P, 'objpose', Tobject, 'pose', Tc_desired, '*r')

%% VISUAL SERVO LOOP
%
% *_TODO_* : Uncomment and complete.
%
% lambda is the percentage of advance towards the desired pose of the cam
lambda0 = 0.1;
% initialize the pose of the camera
Tc = Tc0
T(:,:,1) = Tc;
ph(:,:,1)=p0;
p=p0;
% do the approach in maxn steps
maxn=50
for n = 2:maxn
    % Compute the new pose of the camera
    % First, estimate the pose of the object, w.r.t. camera, from P and p
    
    %Tc_t_est = 
    
    % Compute the error between desired and current camera transform
    
    %Tdelta = 

    % Move a fraction lambda in the direction to reduce the error
    % (lambda can be set greater as we approach the goal)
    % (when multiplying transforms, use trnorm to guarantee that the 
    %  rotation submatrix of the resultant transform is a proper 
    %  orthogonal matrix)
    
    %lambda = 
    %Tdelta =   
    %Tc =  
    
    %plots the points as seen by the camera when located at Tc
    p = cam.plot(P,  'objpose', Tobject,'pose', Tc, '.b');
    ph(:,:,n)=p;
    % store it in a vector for animation purposes
    T(:,:,n) = Tc;
end
Tc
%% ANIMATE in 3D
figure
%plot the object
W = transl(0,0,0);
trplot(W, 'frame', 'W',  'axis', [-3 3 -3 3 -2 4], 'view', [-60 10])
hold on
cube_plot(Tobject,sizex,sizey,sizez,'r');

% Animate motion of the camera frame.
trplot(Tc0, 'color', 'r')%plot the current pose of the camera
trplot(Tc_desired, 'color', 'k')%plot the desired pose of the camera
Pw=Tobject*cat(1,P,ones(1,size(P,2)));
plot3(Pw(1,:),Pw(2,:),Pw(3,:),'*r')%plot the points on the object
title('Flying camera')
grid on 
xlabel('X')
ylabel('Y')
zlabel('Z')

cam.T = Tc0;
cam.plot_camera
hold on
tranimate(T,'view', [-60 10], 'nsteps', 100)
for i=1:50
    trplot(T(:,:,i),  'color', 'c') 
end
cam.T = Tc_desired;
cam.plot_camera

%% PLOT CAMERA COORDINATES
%
figure
X=squeeze(T(1,4,:));
Y=squeeze(T(2,4,:));
Z=squeeze(T(3,4,:));
subplot(2,1,1);
plot(X)
hold on
plot(Y,'g')
plot(Z,'r')
title('Camera position');
xlabel('Steps');
legend('X','Y', 'Z');
subplot(2,1,2);
for i=1:size(T,3)
    rpy(i,:)=tr2rpy(T(1:3,1:3,i));
end
plot(rpy(:,1))
hold on
plot(rpy(:,2),'g')
plot(rpy(:,3),'r')
title('Camera orientation');
xlabel('Steps');
legend('r','p', 'y');
%% PLOT PROJECTED POINTS
%
figure
axis([0 1024 0 1024]);
axis ij;
hold on
plot(ph(1,:,1),ph(2,:,1),'ro')
for i=2:size(ph,3)
    plot(ph(1,:,i),ph(2,:,i),'r*')
end
for i=1:size(ph,3)
    plot( squeeze(ph(1,1,:)), squeeze(ph(2,1,:)))
    plot( squeeze(ph(1,2,:)), squeeze(ph(2,2,:)))
    plot( squeeze(ph(1,3,:)), squeeze(ph(2,3,:)))
    plot( squeeze(ph(1,4,:)), squeeze(ph(2,4,:)))
end