% jumpropeode.m  2/21/2006  Darryl Yong / Craig Weidert
function dydt = ballode(t, y, flag, l_dry, mu_oil, mu_dry, r_ball, m_ball, r_delta, I)
global Alpha;
global otherInfo;

g = 9.81;

R_con = [0 ; 0 ; -r_ball];

R_cm = [y(1) ; y(2) ; y(3)];
Velo = [y(4) ; y(5) ; y(6)];

R_delta = [y(7) ; y(8) ; y(9)];
W = [y(10) ; y(11) ; y(12)];

if (R_cm(1) < l_dry)
    mu = mu_oil;
else
    mu = mu_dry;
end

S = cross(R_con, W) - Velo;
Mu = Mu_funk(mu, S);

slippage = norm(S);
otherInfo = [otherInfo ; [R_cm(1) slippage]];

if (slippage > .01)
    A_alpha = cross(R_delta, Alpha);
    a_alpha = A_alpha(3);
    A_W = cross(cross(W, R_delta), W);
    a_W = A_W(3);

    F_con = m_ball*(g+a_alpha+a_W)*Mu;
    F_g = m_ball*[0 ; 0 ; -g];
    
    I_o = I_o_funk(I);
    I_dev = I_dev_funk(I);
    I_delta = I_delta_funk(r_ball, R_delta, Mu);
    I_deltadelta = I_deltadelta_funk(R_delta, Mu);
    I_tot = I_o + I_dev + I_delta + I_deltadelta;

    T_fric = g*cross(R_con, Mu);
    T_dev = cross((I_dev*W), W);
    T_delta = g*cross(R_delta, Mu) + a_W*cross(R_con, Mu);
    T_deltadelta = a_W*cross(R_delta, Mu);
    T_tot = T_fric + T_dev + T_delta + T_deltadelta;

    Accel = (F_con + F_g)/m_ball;
    Alpha = I_tot\T_tot;
    
    AngDeriv = cross(W, R_delta);
    
    dydt = [Velo(1) ; Velo(2) ; Velo(3) ; ...
        Accel(1) ; Accel(2) ; Accel(3) ; ...
        AngDeriv(1) ; AngDeriv(2) ; AngDeriv(3) ; ...
        Alpha(1) ; Alpha(2) ; Alpha(3)]; 
else
    A_alpha = cross(R_delta, Alpha);
    a_alpha = A_alpha(3);
    A_W = cross(cross(W, R_delta), W);
    a_W = A_W(3);

    w = norm(W);
    N = [0 ; 0 ; 1];
    
    F_con = m_ball*(g+a_alpha+a_W)*Mu;
    F_g = m_ball*[0 ; 0 ; -g];

    I_o = I_o_funk(I);
    I_dev = I_dev_funk(I);
    I_roll = [(r_ball*r_ball) 0 0 ; 0 (r_ball*r_ball) 0 ; 0 0 0];
    I_roll_delta = I_roll_delta_funk(r_ball, R_delta);
    I_tot = I_o + I_dev + I_roll + I_roll_delta;
    
    T_dev = cross((I_dev*W), W);
    T_roll_delta = g*cross(R_delta, N) + r_ball*(w*w*cross(R_delta, N) + dot(W, R_delta)*cross(N, W));
    T_roll_deltadelta = dot(W, R_delta)*cross(W, R_delta);
    T_tot = T_dev + T_roll_delta + T_roll_deltadelta;
    
    AngDeriv = cross(W, R_delta);

    Accel = (F_con + F_g)/m_ball;
    Alpha = I_tot\T_tot;

    dydt = [Velo(1) ; Velo(2) ; Velo(3) ; ...
        Accel(1) ; Accel(2) ; Accel(3) ; ...
        AngDeriv(1) ; AngDeriv(2) ; AngDeriv(3) ; ...
        Alpha(1) ; Alpha(2) ; Alpha(3)]; 
    
end 

return

function toRet = I_delta_funk(r_ball, R_delta, Mu)
    m = [R_delta(2)*Mu(2) -R_delta(1)*Mu(2) 0 ; ...
        R_delta(2)*Mu(1) R_delta(1)*Mu(1) 0 ; ...
        0 0 0];
    toRet = r_ball * m;
return 

function toRet = I_deltadelta_funk(r, m)
    toRet = [(r(2)*(r(2)-r(3)*m(2))) (-r(1)*(r(2)-r(3)*m(2))) 0 ; ...
        (r(2)*(r(3)*m(1)-r(1))) (-r(1)*(r(3)*m(1)-r(1))) 0 ; ...
        (r(2)*(r(1)*m(2)-r(2)*m(1))) (-r(1)*(r(1)*m(2)-r(2)*m(1))) 0];
return

function toRet = I_o_funk(I)
    ave = sum(diag(I)) / 3;
    toRet = ave*eye(3);
return

function toRet = I_dev_funk(I)
    ave = sum(diag(I)) / 3;
    toRet = I - ave*eye(3);
return 

function toRet = Mu_funk(mu, S)
    S(3) = 0;
    toRet = mu*S/norm(S);
    toRet(3) = 1;
return 

function toRet = I_roll_delta_funk(r_ball, R_delta)
    toRet = [(R_delta(2)*R_delta(2) + R_delta(3)*R_delta(3) - 2*R_delta(3)*r_ball) ...
              (-R_delta(1)*R_delta(2)) ...
               (-R_delta(1)*R_delta(3) + R_delta(1)*r_ball); ...
             (-R_delta(1)*R_delta(2)) ...
              (R_delta(1)*R_delta(1) + R_delta(3)*R_delta(3) - 2*R_delta(3)*r_ball) ...
               (R_delta(1)*R_delta(1) + R_delta(2)*R_delta(2)) ; ...
             (-R_delta(1)*R_delta(3) + R_delta(1)*r_ball) ...
              (-R_delta(2)*R_delta(3) + R_delta(2)*r_ball) ...
               (R_delta(1)*R_delta(1) + R_delta(2)*R_delta(2))];
return