%% Wektory i macierze - powtórzenie
% Celem tej części laboratorium wprowadzającego z przedmiotu Teoria Sterowania jest powtórzenie/uzupełnienie wiadomości dotyczących wektorów.
% Imiona i nazwiska studentów:
% Imie i nazwisko
% Imię i nazwisko

clear all;
clc;

% Zadanie 1
% By wykonać kolejne zadania, należy odczytać dane z akcelerometru korzystając z aplikacji mobilnej Matlaba.
%
%          An accelerometer reading in the MATLAB Mobile app
% Odczytaj cztery pomiary przyśpieszenia i zapis do wektorów \vec{a} ,\vec{b} ,\vec{c} ,\vec{d}.

% Dane z akcelerometru
a = [0 0 0]; % Default: [0 0 0]
b = [0 0 0]; % Default: [0 0 0]
c = [0 0 0]; % Default: [0 0 0]
d = [0 0 0]; % Default: [0 0 0]

% Wizualizacja wektorów przyśpieszenia
axisLims = [-10 10];
figure("position",[1 1 400 400])
hold on
plotVector3DAxis([axisLims axisLims axisLims])
plotVector3D([0 0 0],a,"\mathbf{a}",-2)
plotVector3D([0 0 0],b,"\mathbf{b}",-2)
plotVector3D([0 0 0],c,"\mathbf{c}",-2)
plotVector3D([0 0 0],d,"\mathbf{d}",-2)
hold off
view([110 25])

%  Pytanie kontrolne
% Jak działa funkcja plotVector3D?
% Odpowiedz:
% ..........................................
%
% Zadanie 2
% Oblicz długość każdego z wektorów \vec{a} ,\vec{b} ,\vec{c} ,\vec{d}.
% Pamiętaj: LINK

% Wpisz odpowiedz poniżej z dokładnością do 4. miejsca po przecinku
mag_a = 0; % Default: 0
mag_b = 0; % Default: 0
mag_c = 0; % Default: 0
mag_d = 0; % Default: 0

checkMagnitudes(a,b,c,d,mag_a,mag_b,mag_c,mag_d);

% Pytanie kontrolne
% Jak działa funkcja checkMagnitudes?
% Odpowiedz:
% ..........................................
%
% Zadanie 3
% W tym zadaniu określimy orientacje wektorów. W tym celu, tj. by prawidłowo zwizualizować orientacje, wektory można znormalizować, konwertując je na wektory jednostkowe wiedząc, że: wektor jednostkowy w kierunku wektora  to \mathbf{v} to \hat{\mathbf{v}} = \mathbf{v}/\Vert \mathbf{v} \Vert, przy czym \hat{\mathbf{v}}  ma długość 1.
% Należy pamiętać, że dzielenie wektora przez skalar działa tak samo jak mnożenie przez skalar: podziel każdy element przez skalar.
% Przekształć wektory przyspieszenia na wektory jednostkowe i zapisz wynik.

an = [0 0 0]; % Default: [0 0 0]
bn = [0 0 0]; % Default: [0 0 0]
cn = [0 0 0]; % Default: [0 0 0]
dn = [0 0 0]; % Default: [0 0 0]

checkNormalization(a,an)
checkNormalization(b,bn)
checkNormalization(c,cn)
checkNormalization(d,dn)

% Graph the accelerometer vector
axisLims = [-1.5 1.5];
figure("position",[1 1 400 400])
hold on
plotVector3DAxis([axisLims axisLims axisLims])
plotVector3D([0 0 0],an,"\hat{\mathbf{a}}",-0.3)
plotVector3D([0 0 0],bn,"\hat{\mathbf{b}}",-0.3)
plotVector3D([0 0 0],cn,"\hat{\mathbf{c}}",-0.3)
plotVector3D([0 0 0],dn,"\hat{\mathbf{d}}",-0.3)
hold off
view([110 25])

%
% Zadanie 4.
% Oblicz: \vec{a} -\vec{c} ,\vec{b} +\vec{d} ,\vec{a} \times \vec{b}. Wynik każdej operacji pokaż na wykresie (wykorzystają funkcje plotVector3D).
% Pamiętaj: Link

%Miejsce na obliczenia i wizualizacje

%

%% Helper functions
%% These functions are for plotting 2D vectors with their labels.

%

function setup2DVectorPlot(axisLims) 
% Sets up axes for visualizing 2D vectors
    fs = 14; % font size for graph
    ax = gca;
    ax.XAxisLocation = "origin";
    ax.YAxisLocation = "origin";    
    ax.Position = [0.05 0.05 0.9 0.9];
    axis equal tight
    axis(axisLims)
    box on
    xlabel("$x$","Interpreter","latex","fontsize",fs)
    ylabel("$y$","Interpreter","latex","fontsize",fs)
    ax.XTick = round(axisLims(1)):round(axisLims(2));
    ax.XTickLabel = [];
    ax.YTick = round(axisLims(1)):round(axisLims(2));
    ax.YTickLabel = [];
end

% Visualizes a 2D vector with the given name
function plotVector2D(u0,u,name,nameOffset) 
    fs = 14; % font size for the graph
    quiver(u0(1),u0(2),u(1),u(2),"autoscale","off","maxheadsize",1/sqrt(dot(u,u)),"linewidth",1.5)
    offset = null(u)/3*nameOffset;
    text(u0(1) + u(1)/2 + offset(1), u0(2) + u(2)/2 + offset(2),strcat("$",name,"$"), ...
        "Interpreter","latex","fontsize",fs,"HorizontalAlignment","center")
end

 % Visualizes a 2D vector and display the angle it makes with the x-axis
function plotVectorWithAngle(u0,u)
    fs = 14; % font size for graph
    theta = atan2d(u(2),u(1));
    name = strcat("\theta =", num2str(round(theta),3),"^\circ");
    t = linspace(0,theta);
    x = 1*cosd(t);
    y = 1*sind(t);
    plot(x,y,"k")
    quiver(u0(1),u0(2),u(1),u(2),"autoscale","off","maxheadsize",1/sqrt(dot(u,u)),"linewidth",1.5)
    text(x(end/2)*2, y(end/2)*2, strcat("$",name,"$"), ...
        "Interpreter","latex","fontsize",fs,"HorizontalAlignment","center")
end

% Visualizes a 2D vector with its components
function plotVector2DWithComponents(v,name,nameOffset) 
    plotVector2D([0 0],v,strcat("\mathbf{",name,"}"),nameOffset)
    plotVector2D([0 0],[v(1) 0],strcat(name,"_x"),nameOffset)
    plotVector2D([0 0],[0 v(2)],strcat(name,"_y"),nameOffset)  
    plot([0 v(1)],[v(2) v(2)],":","color",[0.9290, 0.6940, 0.1250])
    plot([v(1) v(1)],[0 v(2)],":","color",[0.8500, 0.3250, 0.0980])
end

% Sets up nice axes for visualizing 3D vectors
function plotVector3DAxis(axlims) 
    cla
    fs = 14; % fontsize for graph
    lb = axlims(1);
    ub = axlims(2);
    a = lb;
    b = ub-a;
    buff = b*0.1;
    c = ub+buff/2;
    ca = [0.3 0.3 0.3 0.6];
    axis([a-buff,c+buff,a-buff,c+buff,a-buff,c+buff])
    
    % Plot the axes and labels
    quiver3(a,0,0,b,0,0,"autoscale","off","maxheadsize",1/10,"linewidth",1,"color",ca)
    quiver3(0,a,0,0,b,0,"autoscale","off","maxheadsize",1/10,"linewidth",1,"color",ca)
    quiver3(0,0,a,0,0,b,"autoscale","off","maxheadsize",1/10,"linewidth",1,"color",ca)
    text(c,0,0,"$x$", "Interpreter","latex","fontsize",fs)
    text(0,c,0,"$y$", "Interpreter","latex","fontsize",fs)
    text(0,0,c,"$z$", "Interpreter","latex","fontsize",fs)
    axis equal tight
    set(gca,"Position",[0.05 0.05 0.9 0.9]);
    set(gca,"CameraViewAngleMode","Manual")    
    set(gca,"Color","none","XColor","none","YColor","none","ZColor","none");
    set(gca,"ColorOrderIndex",1)
end


function plotVector3D(u0,u,name,nameOffset)
% Visualize a 3D vector with the given name
    xlims = 0.5*get(gca,"xlim");
    scaleFactor = 0.25*(xlims(2) - xlims(1));
    fs = 14; % font size for graph
    quiver3(u0(1),u0(2),u0(3),u(1),u(2),u(3),"autoscale","off","maxheadsize",scaleFactor/norm(u),"linewidth",1.5)
    offset = null(u)/3*nameOffset;
    text(u0(1) + u(1)/2 + offset(1), u0(2) + u(2)/2 + offset(2), u0(3) + u(3)/2 + offset(3), ...
        strcat("$",name,"$"),"Interpreter","latex","fontsize",fs,"HorizontalAlignment","center")
end

function plotVector3DWithComponents(v,name,nameOffset) 
% Visualizes a 3D vector and its components
    plotVector3D([0 0 0],v,strcat("\mathbf{",name,"}"),nameOffset)
    plotVector3D([0 0 0],[v(1) 0 0],strcat(name,"_x"),nameOffset)
    plotVector3D([0 0 0],[0 v(2) 0],strcat(name,"_y"),nameOffset)
    plotVector3D([0 0 0],[0 0 v(3)],strcat(name,"_z"),nameOffset)
    plot3([0 v(1)],[v(2) v(2)], [0 0],":","color",[0.8500 0.3250 0.0980])
    plot3([v(1) v(1)],[0 v(2)], [0 0],":","color",[0.9290 0.6940 0.1250])
    plot3([v(1) v(1)],[v(2) v(2)], [0 v(3)],":","color",[0.4940 0.1840 0.5560])
end

%% These functions check the example answers
function checkAns(val,userVal) 
% Checks the answer of a variable
    if(all( abs( userVal - val ) < 1e-3 ))
        disp("Correct!")
    else
        warning("Incorrect :(")
    end
end

function gradeVectorQuantities(vectorQuantities)
    checkAns(sort(vectorQuantities),[2 3])
end

function gradeAxisAngle(axisAngle)
    checkAns(axisAngle,45)
end

function checkAngle2D(userValue)
    checkAns(abs(userValue),56.30993247)
end

function checkMagnitudes(aLean1,aLean2,aFlat1,aFlat2,magLean1,magLean2,magFlat1,magFlat2)
    checkAns(norm(aLean1),magLean1);
    checkAns(norm(aLean2),magLean2);
    checkAns(norm(aFlat1),magFlat1);
    checkAns(norm(aFlat2),magFlat2);
end

function checkAngles(a,b,c,d,angle_a,angle_b,angle_c,angle_d)
    theta = @(v) atand( sqrt(v(1).^2 + v(2).^2)./v(3));
    checkAns(theta(a),angle_a);
    checkAns(theta(b),angle_b);
    checkAns(theta(c),angle_c);
    checkAns(theta(d),angle_d);
end

function checkMag(userVal) 
    val = 5;
    checkAns(val,userVal);
end

function check3DMag(userVal)
    v = [3 5 -3];
    checkAns(norm(v),userVal)
end

function checkSymbolicMag(userVal)
    syms v_x v_y v_z
    ansValue = sqrt(v_x^2 + v_y^2 + v_z^2);
    if(isequal(userVal,ansValue))
        disp("Correct!")
    else
        warning("Incorrect :(")
    end
end
