%constructing chassis and axes frame
%% draw AUV
% AUV configuration

p1 = [ 0.2; -0.15; 0.15];
p2 = [ 0.2;  0.15; 0.15];
p3 = [-0.2;  0.15; 0.15];
p4 = [-0.2; -0.15; 0.15];

p5 = [ 0.2; -0.15; -0.15];
p6 = [ 0.2;  0.15; -0.15];
p7 = [-0.2;  0.15; -0.15];
p8 = [-0.2; -0.15; -0.15];

% Body frame
origin = [0; 0; 0];
x_axis = [0.1; 0; 0];
y_axis = [0; 0.1; 0];
z_axis = [0; 0; 0.1];

%% loop samples
last_sample = length(x_fpid.signals.values);

time_at_sample_sec = 0.1;

sample_time = 0.001;
sample_step = time_at_sample_sec / sample_time;

% current_sample = last_sample;
i = 1;

figure;

%ovewrite the sample step
sample_step = 200;

for current_sample = 1: sample_step: last_sample
    plot3(x_ref.signals.values(current_sample), y_ref.signals.values(current_sample), z_ref.signals.values(current_sample),'x:b');
end

for current_sample = 1: sample_step: last_sample
    
    dx = x_fpid.signals.values(current_sample);
    dy = y_fpid.signals.values(current_sample);
    dz = z_fpid.signals.values(current_sample);

    t = [dx; dy; dz];

    phi   = phi_fpid.signals.values(current_sample)   * (180/pi);
    theta = theta_fpid.signals.values(current_sample) * (180/pi);
    psi   = psi_fpid.signals.values(current_sample)   * (180/pi);

    R = rotz(psi)*roty(theta)*rotx(phi);

    p1_new = R*p1;
    p2_new = R*p2;
    p3_new = R*p3;
    p4_new = R*p4;
    p5_new = R*p5;
    p6_new = R*p6;
    p7_new = R*p7;
    p8_new = R*p8;

    origin_new = R*origin;
    x_axis_new = R*x_axis;
    y_axis_new = R*y_axis;
    z_axis_new = R*z_axis;

    p1_new = p1_new + t;
    p2_new = p2_new + t;
    p3_new = p3_new + t;
    p4_new = p4_new + t;
    p5_new = p5_new + t;
    p6_new = p6_new + t;
    p7_new = p7_new + t;
    p8_new = p8_new + t;

    origin_new = origin_new + t;
    x_axis_new = x_axis_new + t;
    y_axis_new = y_axis_new + t;
    z_axis_new = z_axis_new + t;


    %% plotting Body chassis

    p1_vect = [p1_new(1) p2_new(1) p3_new(1) p4_new(1) p1_new(1)];
    p2_vect = [p1_new(2) p2_new(2) p3_new(2) p4_new(2) p1_new(2)];
    p3_vect = [p1_new(3) p2_new(3) p3_new(3) p4_new(3) p1_new(3)];
    j1=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j1=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;

    p1_vect = [p5_new(1) p6_new(1) p7_new(1) p8_new(1) p5_new(1)];
    p2_vect = [p5_new(2) p6_new(2) p7_new(2) p8_new(2) p5_new(2)];
    p3_vect = [p5_new(3) p6_new(3) p7_new(3) p8_new(3) p5_new(3)];
    j2=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j2=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;

    p1_vect = [p1_new(1) p5_new(1)];
    p2_vect = [p1_new(2) p5_new(2)];
    p3_vect = [p1_new(3) p5_new(3)];
    j3=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j3=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;

    p1_vect = [p2_new(1) p6_new(1)];
    p2_vect = [p2_new(2) p6_new(2)];
    p3_vect = [p2_new(3) p6_new(3)];
    j4=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j4=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;

    p1_vect = [p3_new(1) p7_new(1)];
    p2_vect = [p3_new(2) p7_new(2)];
    p3_vect = [p3_new(3) p7_new(3)];
    j5=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j5=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;

    p1_vect = [p4_new(1) p8_new(1)];
    p2_vect = [p4_new(2) p8_new(2)];
    p3_vect = [p4_new(3) p8_new(3)];
    j6=plot3(p1_vect,p2_vect,p3_vect,'black','LineWidth',3);
%     j6=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;
    
    
    p1_vect = [p1_new(1) p3_new(1)];
    p2_vect = [p1_new(2) p3_new(2)];
    p3_vect = [p1_new(3) p3_new(3)];
    j10=plot3(p1_vect,p2_vect,p3_vect,'green','LineWidth',3);
%     j6=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;
    
    p1_vect = [p2_new(1) p4_new(1)];
    p2_vect = [p2_new(2) p4_new(2)];
    p3_vect = [p2_new(3) p4_new(3)];
    j11=plot3(p1_vect,p2_vect,p3_vect,'green','LineWidth',3);
%     j6=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;
    
    p1_vect = [p5_new(1) p7_new(1)];
    p2_vect = [p5_new(2) p7_new(2)];
    p3_vect = [p5_new(3) p7_new(3)];
    j12=plot3(p1_vect,p2_vect,p3_vect,'yellow','LineWidth',3);
%     j6=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;
    
    p1_vect = [p6_new(1) p8_new(1)];
    p2_vect = [p6_new(2) p8_new(2)];
    p3_vect = [p6_new(3) p8_new(3)];
    j13=plot3(p1_vect,p2_vect,p3_vect,'yellow','LineWidth',3);
%     j6=plot(p1_vect,p2_vect,'black','LineWidth',3);
    hold on;
    

    %% Plotting Body axes

    axis([-5 12 -5 12 -5 12]);
    xlabel('x axis');
    ylabel('y axis');
    zlabel('z axis');
    
    plot3(dx,dy,dz,'x:r');
    hold on;
    
%     xref = [0 x_ref.signals.values(current_sample)];
%     yref = [0 y_ref.signals.values(current_sample)];
%     zref = [0 z_ref.signals.values(current_sample)];
    xref = x_ref.signals.values(current_sample);
    yref = y_ref.signals.values(current_sample);
    zref = z_ref.signals.values(current_sample);
    plot3(xref,yref,zref,'o:b');
    hold on;

    
    % Body frame x axes
    x_vect_pt_x = [origin_new(1) x_axis_new(1)];
    x_vect_pt_y = [origin_new(2) x_axis_new(2)];
    x_vect_pt_z = [origin_new(3) x_axis_new(3)];
    j7=plot3(x_vect_pt_x,x_vect_pt_y,x_vect_pt_z,'black','LineWidth',5);
%     j7=plot(x_vect_pt_x,x_vect_pt_y,'black','LineWidth',5);
    hold on;

    % Body frame y axes
    y_vect_pt_x = [origin_new(1) y_axis_new(1)];
    y_vect_pt_y = [origin_new(2) y_axis_new(2)];
    y_vect_pt_z = [origin_new(3) y_axis_new(3)];
    j8=plot3(y_vect_pt_x,y_vect_pt_y,y_vect_pt_z,'blue','LineWidth',5);
%     j8=plot(y_vect_pt_x,y_vect_pt_y,'blue','LineWidth',5);
    hold on;

    % Body frame z axes
    z_vect_pt_x = [origin_new(1) z_axis_new(1)];
    z_vect_pt_y = [origin_new(2) z_axis_new(2)];
    z_vect_pt_z = [origin_new(3) z_axis_new(3)];
    j9=plot3(z_vect_pt_x,z_vect_pt_y,z_vect_pt_z,'red','LineWidth',5);
    hold on;
   
    M(i) = getframe(gcf);
    i = i + 1;

    delete(j1);
    delete(j2);
    delete(j3);
    delete(j4);
    delete(j5);
    delete(j6);
    delete(j7);
    delete(j8);
    delete(j9);
    delete(j10);
    delete(j11);
    delete(j12);
    delete(j13);
end

% movie2avi(M,'test1.avi');

clear mex