% Copyright (c) 2017, Domenico L. Gatti
% All rights reserved.
% 
% Redistribution and use in source and binary forms, with or without 
% modification, are permitted provided that the following conditions are 
% met:
% 
%     * Redistributions of source code must retain the above copyright 
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright 
%       notice, this list of conditions and the following disclaimer in 
%       the documentation and/or other materials provided with the 
%       distribution
%       
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%% General dependencies
% We always start from the PRACTICE directory and we add to the path
% subdirectories containing various tools described in the book chapters.
addpath(genpath('../GENERAL_SCRIPTS_FUNCTIONS'));
% addpath(genpath('../DATABASE'));
% addpath(genpath('../TOOLBOXES'));

%% CHAPTER 2. Change of basis.

% 1. Use Gaussian elimination (function gauss_elim_step_by_step.m) to solve
% the matrix equation Ax = b, where:

A = [3 7 8;1 4 7;2 2 4];
b = [59 34 22]';

% Make a copy of the function and add comments to show you understand what
% is happening at each step.

% 2. Find the rotation matrix that will rotate clockwise the vector a =
% [3 5 1] by 13 degrees around x, 18 degrees around y and -30 around z.
% Write a function that can carry out this operation, then, compare your
% own function with the function 'R_from_Euler'. Be ready to discuss the
% code of this function and/or your own code.

% 3. Derive back the Euler angles from the R matrix found in the previous
% problem. Use the function 'Euler_from_R'. Be ready to discuss the code of
% this function.
 
% 4. Write a function that calculates the transformation matrix Q and the
% rotation matrix R for three consecutive rotations around z, x, and y.

% 5.Consider the linear transformation in R3 represented by the matrix:

A = [1 2 -2;-0.5 4 -2;-0.5 1 1]; 

% such that A*x = b for:

x = [3 6 9]';
 
% a. Explain why this linear transformation has a special meaning in the
% space defined by the basis:
% 
U = [2 0 2;3 1 1;1 1 1];
% 
% b. Show the corresponding U space product A_u*x_u  = b_u
% 
% c. Carry out the similarity transformation x ==> b in U space.
% 
% d. Do you notice anything special about the relationship between A and U?
% For example, what happens if we multiply A by U (AU = C); do you
% recognize the columns in the C matrix? 

% 6. Consider the following transformation matrix:

Q = [0 1 0;
     0 0 1;
     1 0 0];

% Any transformation matrix can be represented as a rotation around a
% single axis p by an angle .  Use the function 'p_and_theta_from_Q.m' to
% identify this axis and angle, and then the function 'Q_from_p_and_theta'
% to calculate back the original transformation matrix. You can also try
% the original algorithm developed by Euler to carry out these operations:
% they are provided as the functions 'p_and_theta_from_Q_alt.m' and
% 'Q_from_p_and_theta_alt'.

% 7. Calculate the product:
% 
%  {A^20}*x = {A*A*A*A ...}*x  = b . 
% 
% Do you see a way of simplifying this operation using similarity
% transformation?

% 8. We have generated the following basis U:
u1 = [1 2 3]'
u2 = [4 5 6]'
u3 = cross(u1,u2)
u2 = cross(u1,u3)
u1 = u1/norm(u1)
u2 = u2/norm(u2)
u3 = u3/norm(u3)
U = [u1 u2 u3]
U'*U
U*U'
V = eye(3)

% What are the angles in degrees between each basis vector of the U basis
% and each basis vector of the standard Cartesian basis V?

angles_U_V = acosd(U')

% 9. We have a third basis W:
w1 = [2 3 0]'
w2 = [1 5 5]'
w3 = [3 2 1]'
W = [w1 w2 w3]
rank(W)

% and a vector a_u in U basis:
a_u = [4 5 6]'

% what is a_w, vector a in W basis?
a_v = U*a_u
a_w = W\U*a_u
a_u = U\W*a_w
a_v = W*a_w

% What are the angles in degrees between each basis vector of the W basis
% and each basis vector of the U basis?

w1n = w1/norm(w1)
w2n = w2/norm(w2)
w3n = w3/norm(w3)
Wn = [w1n w2n w3n]

angles_W_U = acosd(Wn'*U)

% check:
acosd(W(:,1)'/norm(W(:,1))*U(:,3))

%% SPECIAL TOPICS: Covariant and contravariant bases

% 1. Calculate the contravariant basis cvG knowing the covariant basis vG:
g1 = [1 0]';
g2 = [1/sqrt(2) 1/sqrt(2)]';

vG = [g1 g2]

cvG = inv(vG')

% 2.
a = [45 0 0]'
gamma = 49
Q3 = [cosd(gamma) sind(gamma) 0;-sind(gamma) cosd(gamma) 0;0 0 1]
R3 = Q3'
b = [60 0 0]'
b = R3*b
b_on_e2 = b'*[0 1 0]'
b_on_e1 = b'*[1 0 0]'

a = 45 ; b = 60 ; c = 71 ; alpha = 60 ; beta = 71 ; gamma = 49 ;
alpha = 60 ; beta = 71 ; gamma = 49 
a_basis_vec = [a 0 0]'

Q3 = [cosd(gamma) sind(gamma) 0;-sind(gamma) cosd(gamma) 0;0 0 1]
R3 = Q3'
b_vec = [b 0 0]
b_basis_vec= R3*b_vec'
b_basis_vec= [b*cosd(gamma) b*sind(gamma) 0]'
c1 = c*cosd(beta)
c2 = c*(cosd(alpha) - cosd(beta)*cosd(gamma))/sind(gamma)
c3 = c*sqrt(1-(cosd(beta))^2 -(cosd(alpha) -cosd(beta)*cosd(gamma))^2/(sind(gamma))^2)
c3 = sqrt(c*c - c1*c1 - c2*c2)
c_basis_vec = [c1 c2 c3]'
P = [a_basis_vec b_basis_vec c_basis_vec]
acosd(a_basis_vec'*b_basis_vec/(norm(a_basis_vec)*norm(b_basis_vec)))
acosd(a_basis_vec'*c_basis_vec/(norm(a_basis_vec)*norm(c_basis_vec)))
acosd(b_basis_vec'*c_basis_vec/(norm(b_basis_vec)*norm(c_basis_vec)))

% 2a.
show_frame('new',eye(3))
show_frame('add',P,'--r','--g','--b','g1','g2','g3')
test = [12 31 44.3]
plot3([0 test(1)]',[0 test(2)]',[0 test(3)]','-c','LineWidth',3)
xlim([-20 60])
ylim([-20 60])
zlim([-20 60])
hold on

xyz_test = test'
abc_test = P\xyz_test
a_test = [abc_test(1) 0 0]'
b_test = [0 abc_test(2) 0]'
c_test = [0 0 abc_test(3)]'
xyz_a_test = P*a_test
xyz_b_test = P*b_test
xyz_c_test = P*c_test
xyz_test = xyz_a_test + xyz_b_test + xyz_c_test

plot3([0 xyz_a_test(1)]',[0 xyz_a_test(2)]',[0 xyz_a_test(3)]',...
      ':r','LineWidth',3)
plot3([xyz_a_test(1) xyz_a_test(1)+xyz_b_test(1)]',...
      [xyz_a_test(2) xyz_a_test(2)+xyz_b_test(2)]',...
      [xyz_a_test(3) xyz_a_test(3)+xyz_b_test(3)]',...
      ':g','LineWidth',3)
plot3([xyz_a_test(1)+xyz_b_test(1) xyz_a_test(1)+xyz_b_test(1)+xyz_c_test(1)]',...
      [xyz_a_test(2)+xyz_b_test(2) xyz_a_test(2)+xyz_b_test(2)+xyz_c_test(2)]',...
      [xyz_a_test(3)+xyz_b_test(3) xyz_a_test(3)+xyz_b_test(3)+xyz_c_test(3)]',...
      ':b','LineWidth',3)

%show_frame('add',[xyz_a_test xyz_b_test xyz_c_test],':r',':g',':b','t1','t2','t3')

% 2b.
rs_P = inv(P')


% 3. 
g = [1 1/sqrt(2);0 1/sqrt(2)]
g_cv = inv(g')
g*g_cv

% 3a.
g = [1 1/sqrt(3) 0;0 1/sqrt(3) 0;0 1/sqrt(3) 1]
g_cv = inv(g')

% 3b
tau = [100 60 30;60 200 20;30 20 100]
eps= [0.001 0.002 0.001;0.002 0.003 0.004;0.001 0.004 0.002]

% 3c
tau_g =inv(g)*tau*g
eps_g_cv = inv(g_cv)*eps*g_cv
eps_g_cv = g'*eps*g_cv

E_sd_v = 0.5*sum(sum(tau.*eps))
E_sd_g = 0.5*sum(sum(tau_g.*eps_g_cv))


% Details on 3c.
t = [100 60 30;...
     60 200 20;...
     30 20 100];
e = [0.001 0.002 0.001;...
     0.002 0.003 0.004;...
     0.001 0.004 0.002];
 
% If the two matrices are symmetric, the trace of the product is the same
% as the double inner product (Einstein summation convention) of the two
% tensors.

Ecart = 0.5*trace(t*e)
Ecart = 0.5*sum(sum(t.*e))

% Covariant basis
g1 = [1 0 0]';
g2 = [1/sqrt(3) 1/sqrt(3) 1/sqrt(3)]';
g3 = [0 0 1]'
G = [g1 g2 g3]

% The contravariant basis is derived by solving a system of linear
% equations:
cvG = G'\eye(3)

% Similarity transformation to derive 't' in the contravariant basis and
% 'e' in the covariant basis

v_t = inv(cvG)*t*cvG
cv_t = inv(G)*t*G

v_e = inv(cvG)*e*cvG
cv_e = inv(G)*e*G

% Since t and e are now referred to two different frames the trace does not
% correspond any longer to the double inner product.

Ecv_v = 0.5*trace(cv_t*v_e)
Ev_cv = 0.5*trace(v_t*cv_e)

Ecv_v = 0.5*sum(sum(cv_t.*v_e))
Ev_cv = 0.5*sum(sum(v_t.*cv_e))

% Notice that if both t and e are referred to the same frame (covariant or
% contravariant), then the double inner product does not provide any longer
% the correct result.

Ecv_cv = 0.5*sum(sum(cv_t.*cv_e))
Ev_v = 0.5*sum(sum(v_t.*v_e))

% however in this case the trace of the regular matrix product does provide
% the correct result.

Ecv_cv = 0.5*trace(cv_t*cv_e)
Ev_v = 0.5*trace(v_t*v_e)

