% RW    :  position
% TLW   :  Tangent, Longitudinal (not normalized)
% TTW,  :  Tangent, Transversal  (not normalized)

function [RW, T1, T2, DDRs1, DDRs2, DDRs1s2, KD] = GeomNeumaticoTrasero(q,sp,cod) 

% cod = CODIGO DE OUTPUTS
% cod = 1   Posicion, tangentes y derivadas segundas
% cod = 2   Posicion, tangentes, derivadas segundas, curvaturas principales
%           y direcciones principales de curvatura

global MBody
Param=ParametrosMotocicleta;

% s1 = sp(1);
% s2 = sp(2);
s1=0;
s2=0;

a = Param.a_n;
b = Param.b_n;
R = Param.Rt;

r = R-b+b*sqrt(1-(s1/a)^2);
rp = (-b/(a^2))*s1*(1-(s1/a)^2)^(-0.5);
rpp = (-b/(a^2))*(1-(s1/a)^2)^(-0.5)-(b/(a^4))*s1^2*(1-(s1/a)^2)^(-1.5);

% Position of the point (wheel coordinate system)
rW    = ([r*cos(s2), s1, r*sin(s2)])';  % Izquierda

% Longitudinal tangent (wheel coordinate system)
t2   = ([-r*sin(s2), 0, r*cos(s2)])';

% Transverse tangent (wheel coordinate system)
t1   = ([rp*cos(s2), 1.0, rp*sin(s2)])';

% Wheel  translation and rotation matrix   
Rw = RG2(q);
Aw = A2(q);


% Express in global coordinate system:
RW    = Rw + Aw*rW;
T2   = Aw*t2;
T1   = Aw*t1;

dt = [rpp*cos(s2), 0.0, rpp*sin(s2)]';
DDRs1=Aw*dt;

dt = [-r*cos(s2), 0, -r*sin(s2)]';
DDRs2=Aw*dt;

dt = [-rp*sin(s2), 0, rp*cos(s2)]';
DDRs1s2=Aw*dt;

KD = [];

N =-cross(T1,T2);
N = N/norm(N);



if cod == 2,
    nn = N;
    EE = T1'*T1;
    FF = T1'*T2;
    GG = T2'*T2;
    LL = DDRs1'*nn;
    MM = DDRs1s2'*nn;
    NN = DDRs2'*nn;
    KD = curvature(EE,FF,GG,LL,MM,NN);
end