clc;
clear all;
close all;

%%
%Script Configuration
%%
aniFreq = 100;
tsim = [0 10];

rootDir = pwd;

%%
% Set up simulation
%%
gen_vIC;
gen_vParam;
vInput = [0;0;0;0;0;0];


calcXdot = @(t,x) calc_xdot(t, x, vParam, vInput);
calcContactForce = @(t,x) calc_contactInfo(t, x, vParam);

disp(sprintf('x0: vx(%f),vy(%f),vz(%f)',vx,vy,vz));
disp(sprintf('    wx(%f),wy(%f),wz(%f)',wx,wy,wz));
disp(sprintf('     x(%f), y(%f), z(%f)',x,y,z));
disp(sprintf('     a(%f), b(%f),ze(%f)\n\n',zeta, eta, xi));

xdot = calcXdot(0,vIC);  

disp(sprintf('dx0:dvx(%f), dvy(%f), dvz(%f)',xdot(1),xdot(2),xdot(3)));
disp(sprintf('    dwx(%f), dwy(%f), dwz(%f)',xdot(4),xdot(5),xdot(6)));
disp(sprintf('     dx(%f),  dy(%f),  dz(%f)',xdot(7),xdot(8),xdot(9)));
disp(sprintf('     da(%f),  db(%f), dze(%f)\n\n',xdot(10),xdot(11),xdot(12)));

pos = calcPosition(vIC');
rm  = calcRotationMatrix(vIC');
disp(sprintf('r: x(%f), y(%f), z(%f)', pos(1),pos(2),pos(3)));
disp(sprintf('RM  %f , %f , %f ', rm(1),rm(2), rm(3)));
disp(sprintf('    %f , %f , %f ', rm(4),rm(5), rm(6)));
disp(sprintf('    %f , %f , %f ', rm(7),rm(8), rm(9)));
disp('Maple generates the rotation matrix R02');


%%
% Simulate
%%
options = odeset('RelTol',1e-8,'AbsTol',1e-8);
[t sol] = ode45(calcXdot,tsim,vIC,options);


    
%%
% Post process to generate files to animate results
%%
%resample
tspan = tsim(2)-tsim(1);
npts = length(t);
if(aniFreq > 0)
    npts = floor(tspan*aniFreq);
end
tani = [tsim(1):tspan/(npts-1):tsim(2)]';
solAni = zeros(npts,12);

for i=1:1:12
    solAni(:,i) = interp1(t,sol(:,i),tani);
end


%%
%Get contact point location, velocity, force, and extras
%%
contactForce = zeros(npts,9);
contactVel = zeros(npts,3);
contactExtra = zeros(npts,3);
for i=1:1:npts    
    cInfo = calcContactForce(tani(i),solAni(i,:)');
    contactForce(i,1:3) = cInfo.r';
    contactForce(i,4:6) = cInfo.f';
    contactVel(i,:) = cInfo.v';
    contactExtra(i,:) = cInfo.extra';
end

%%
%Export animation files
%%
rcom = calcPosition(solAni);
RMcom = calcRotationMatrix(solAni);
RMcomt = [RMcom(:,1) RMcom(:,4) RMcom(:,7) ...
          RMcom(:,2) RMcom(:,5) RMcom(:,8) ...
          RMcom(:,3) RMcom(:,6) RMcom(:,9)];
dlmwrite('animation/disk.dat',[tani,rcom,RMcomt],'\t');

dlmwrite('animation/forces.dat',[tani,contactForce],'\t');

v1 = ones(size(tani));
v0 = zeros(size(tani));
camera = [tani v0 v1.*(-5) v1.*(1)  v1 v0 v0 v0 v0 -v1 v0 v1 v0];
dlmwrite('animation/camera.dat',camera,'\t');


fig_basic = figure;
subplot(2,2,1);
    plot(t,sol(:,7),'r');
    hold on;
    plot(t,sol(:,8),'g');
    hold on;
    plot(t,sol(:,9),'b');
    hold on;
    xlabel('Time (s)');
    ylabel('Position (m)');
    title('X(r) Y(g) Z(b)');

subplot(2,2,2);
    plot(tani,contactForce(:,4),'r');
    hold on;
    plot(tani,contactForce(:,5),'g');
    hold on;
    plot(tani,contactForce(:,6),'b');
    hold on;
    xlabel('Time (s)');
    ylabel('Force (N)');
    title('Fx(r) Fy(g) Fz(b)');    
    
subplot(2,2,3)
    contactVelTan = [contactVel(:,1), contactVel(:,2)];
    plot(tani,sum(contactVelTan.^2,2).^0.5)   
    xlabel('Time (s)');
    ylabel('Velocity (m/s)');
    title('Tangential Contact Velocity');

subplot(2,2,4)    
    radiusV = rcom-contactForce(:,1:3);
    radiusM = sum(radiusV.^2,2).^0.5;
    radiusAnalytic = r.*(1-exp(-c.*sin(contactExtra(:,1))));
    
    plot(tani,contactExtra(:,2),'b');
    hold on;
    plot(tani,radiusAnalytic,'--r');
    hold on;
    plot(tani,radiusM,'--k');
    
    xlabel('Tilt Angle (rad)');
    ylabel('Radius (m)');
    


sim.t = tani;
sim.x = solAni;
sim.r = rcom;
sim.RM=RMcomt;
sim.cop = contactForce(:,1:3);
sim.copV= contactVel;
sim.f = contactForce(:,4:6);
save(['sim_rotateDisk_c',num2str(c),'.mat'],'sim');

cd animation;
dos('compileWRL');
cd ..