%clear all
global oc obs actpath;

% set the joint parameters body
sx = se3(vector3d(0,0,0),vector3d(1,0,0));
sy = se3(vector3d(0,0,0),vector3d(0,1,0));
rz = se3(vector3d(0,0,1),vector3d(0,0,0));

m = matrix4d;   %by default the identity matrix
len1 = 11.807*0.0254; % inches to meters
len2 = 12*0.0254;
rad_wheel = 0.150;
%l1 = link(m,sx);
%l1 = setlimits(l1,-1,1);
l2 = link(m,sy);
l2 = setlimits(l2,-30,30);

% rotate orientation of matrix by q1 + alpha
% so by -16 + 5.000 degrees
theta = (-16 + 5.000)*pi/180;  %puts total rotation angle in radians
 
m3 = matrix4d([cos(theta)  -sin(theta)  0; ...
               sin(theta)   cos(theta)  0; ...
                 0              0       1],vector3d(0,0,0));
l3 = link(m3,rz);
l3 = setlimits(l3,3*pi,3*pi);

%m1 = matrix4d(eye(3),vector3d(len1,0,0));
%m2 = matrix4d(eye(3),vector3d(-len1,0,0));
alpha = 5*pi/180;
trans_vector = vector3d((len1-rad_wheel)*cos(alpha), ...
                       -(len1-rad_wheel)*sin(alpha),0);

m4 = matrix4d([cos(-alpha)  -sin(-alpha)  0; ...
               sin(-alpha)   cos(-alpha)  0; ...
                 0              0       1],trans_vector);
l4 = link(m4,rz);
%l4 = setlimits(l4,-pi-0.1745,0.1745);
l4 = setlimits(l4,-pi + 0.1745,pi);

%-------------------add shapes----------------------------------
radius = .025;
%l3 = addtogeom(l3,rod(vector3d(-len1,0,0),vector3d(len1,0,0),0.01,0.1));
l3 = addtogeom(l3,csphere(vector3d(0,0,0),rad_wheel,.3));
l3 = addtogeom(l3,csphere(trans_vector,radius,.4));

%l4 = addtogeom(l4,rod(vector3d(0,0,0),vector3d(0,-len2,0),0.01,0.1));
l4 = addtogeom(l4,csphere(vector3d(len2,0,0),radius,.2));

%------------------add mass parameters-------------------------
% set up the mass parameters
mass = 3;  %in kg
I = [0 0 0; 0 (mass*len1*len1/12) 0; 0 0 (mass*len1*len1/12)];
cent_mass = trans_vector + vector3d(-len1/2,0,0);
l3 = setmassinfo(l3,I,mass,cent_mass);

mass = 3;
I = [0 0 0; 0 (mass*len2*len2/12) 0; 0 0 (mass*len2*len2/12)];
cent_mass = vector3d(len2/2,0,0);
l4 = setmassinfo(l4,I,mass,cent_mass);

%l = l1;
%l(2) = l2;
%l(3) = l3;
%l(4) = l4;
l = l2;
l(2) = l3;
l(3) = l4;

%l = parentmap(l,[0 1 2 3]);
%l = childmap(l, [2 3 4 0]);
%l = nextmap(l,  [0 0 0 0]);
l = parentmap(l,[0 1 2]);
l = childmap(l, [2 3 0]);
l = nextmap(l,  [0 0 0]);

l = apmap(l,[0 0 1]);

l = baseaccel(l,vector3d(0,9.81,0));
oc = l;

%--------------make obstacle---------------------
obs = geometry;
rrod=.02;
rod1 = rod(vector3d(-100,-.22,0),vector3d(100,-.22,0),rrod);
obs = addshape(obs,rod1);

%this is the for display only obstacle:
obs2 = geometry;
rrod2 = .22;
rod22 = rod(vector3d(-1,-.22,0),vector3d(1,-.22,0),rrod2);
obs2 = addshape(obs2,rod22);

%rod2 = rod(vector3d(-1,1,0),vector3d(1,1,0),rrod);
%obs = addshape(obs,rod2);
%rod2 = rod(vector3d(-1,-.2,0),vector3d(1,-.2,0),(rrod+.01));
%obs = addshape(obs,rod2);