function ydot = linksimgrad(t,y,flag,oc,pth,obs)

global start_const;
global end_const;

% this file is used to integrate the information needed to compute
%  the gradient

% this file should be completely general now...you should be able to
%   use this to integrate _any_ openchain...not just the diver

np = numpassive(oc);
na = numactive(oc);
nj = numjoints(oc);
Hap = getmap(oc);


% derivative of position = velocity
ydot(1:np) = y((np+1):(2*np));

% active joint pos, vel and acc
[pa,va,aa] = eval(pth,t);

pos = zeros(1,nj);
vel = zeros(1,nj);
acc = zeros(1,nj);
tau = zeros(1,nj);
ctau_p = zeros(nj,nj);
ctau_v = zeros(nj,nj);
ctau_a = zeros(nj,nj);
aidx = 1;
pidx = 1;


%Okay, so for the Jacobian, need to figure out what the force is on every link,
%put it here in tau, and then we're all set...rest can go exactly as it is.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%make vector of positions and velocities for all the joints
for i=1:nj
  if (Hap(i) == 0)  % joint is passive
    pos(i) = y(pidx);
    vel(i) = y(pidx+np);
    pidx = pidx + 1;
  else % joint is active
    pos(i) = pa(aidx);
    vel(i) = va(aidx);
    acc(i) = aa(aidx);
    aidx = aidx + 1;
  end
end

nobs = gettotshapes(obs);
vec_shapes = [0 0 3 2 2];
%vec_shapes = [0 0 2 1 1];
ds = linkdist6(oc,pos',obs,nobs,vec_shapes);

jointtorque=zeros(nj,1);
ctau_p=zeros(nj,nj);
ctau_v=zeros(nj,nj);

[m,n] = size(ds); %m rows, n cols

%for changing where the ground turns on
don = .2;
doff = .19;
c1 = 3*(doff^2 - don^2)/(2*(don - doff));
c3 = -2*don*c1 - 3*don^2;
c2 = -c3*don - c1*don^2 - don^3;
a3 = 1/(c2 + c3*doff + c1*doff^2 + doff^3);
a0 = c2*a3;
a1 = a3*c3;
a2 = a3*c1;

for i = 1:m
  dvec = ds(i,1:3);
  point = ds(i,4:6);
  linkn = ds(i,7);
  
  d = norm(dvec);

  if (d < don)
    lc1 = ds(i,8);
    lc2 = ds(i,9);
    %load up the other things into matrices, for easy passing, I think
    vert(1:3,1) = ds(i,10:12)';
    lam(1) = ds(i,13);
    vert(1:3,2) = ds(i,14:16)';
    lam(2) = ds(i,17);
    vert(1:3,3) = ds(i,18:20)';
    lam(3) = ds(i,21);
    vert(1:3,4) = ds(i,22:24)';
    lam(4) = ds(i,25);
    vert(1:3,5) = ds(i,26:28)';
    lam(5) = ds(i,29);
    vert(1:3,6) = ds(i,30:32)';
    lam(6) = ds(i,33);
    vert(1:3,7) = ds(i,34:36)';
    lam(7) = ds(i,37);
    vert(1:3,8) = ds(i,38:40)';
    lam(8) = ds(i,41);
    %beta, a smoothing function, zero at d=rho, one at d = 0
    %beta = a0 + a1*x + a2*x^2 + a3*x^3 
    %NB: d is the norm, so be careful!  It's always positive.

    [J,dJdq]=dqjacobian(oc,linkn,vector3d(point(1),point(2),point(3)),pos');

    V = J*vel';
    v = V(4:6);
    ud = dvec'/(d+eps);      %Unit vec in d direction.
    vnormal = v'*ud;        %Normal velocity scalar.
    vnormvec = vnormal*ud;  %Normal velocity vector.
    vt = v - vnormvec;      %Tangential velocity.                
    Jv = J(4:6,:);
    dJvdq = dJdq(4:6,:,:);
    vtnorm = norm(vt);
    
    %%%%%%%%%%%%%%%%%put the derivatives here...%%%%%%%%%%%%%%%%%%%%
    %%%new way that we're testing
    lcmax = lc2;
    if (lc1 > lc2)
      lcmax = lc1;
    end
    
    [dud_dq,ddvec_dq,dd_dq,dv_dq] = getudderiv(oc, vert, lam, ...
                  lcmax, lc1, lc2, linkn, pos, dvec, d, nj, dJvdq, vel)
    %%%%%%%%%%%%%%end new improved version

    dvnormal_dq = (ud' * dv_dq) + (v' * dud_dq); % row vec, length nj
    dvnormal_dqd = Jv'*ud;

    for j=1:nj
      dvnormvec_dq(:,j) = dvnormal_dq(j)*ud + vnormal*dud_dq(:,j);
      dvnormvec_dqd(:,j) = dvnormal_dqd(j)*ud; 
    end
    
    dvt_dq = dv_dq - dvnormvec_dq;
    dvt_dqd = Jv - dvnormvec_dqd;

    %dd_dq =  (1/(d+eps))*udterm1;
    dvtnorm_dq = (1/(vtnorm+eps))*(vt'*dvt_dq);
    dvtnorm_dqd = (1/(vtnorm+eps))*(vt'*dvt_dqd);

    %for j=1:nj
    %  term4(:,j) = dvtnorm_dq(j)*vt; 
    %  term44(:,j) = dvtnorm_dqd(j)*vt;
    %end
    %dvtvtnorm_dq = (1/(vtnorm+eps)^2)*(vtnorm*dvt_dq - term4); 
    %dvtvtnorm_dqd = (1/(vtnorm+eps)^2)*(vtnorm*dvt_dqd - term44);
  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if (d > doff)
      beta = a0 + a1*d + a2*(d^2) + a3*(d^3);
      dbeta = (a1 + a2*2*d + a3*3*d^2)*dd_dq;
    else
      beta = 1;
      dbeta = 0;
    end


    k = 1500;
    c = 80;
    rawN = k*(don-d) - c*vnormal;
    N = beta*rawN;   %normal force scalar

    dN_dq = dbeta*rawN + beta*(-k*dd_dq - c*dvnormal_dq);
    dN_dqd = -beta*c*dvnormal_dqd;

    vmax = .1;  %do NOT change this, unless it's to make it bigger.
    dftang = [0 0 0]';
    dftang_dqd = zeros(3,nj);
    if ( vtnorm < vmax )
      if (vtnorm == 0)
        gamma = 0;
        fric = 0;
        ut = [0 0 0]';
        %WTF are the derivs of the friction force at zero? Zero!
        dgamma = 0*dvtnorm_dq;
        dgamma_dqd = 0*dvtnorm_dqd;
      else
        % here need to put a smoothing function gamma  
        % gamma = 1.5*ratio - .5*ratio^3;
        % where the ratio I'm talking about is vtnorm/vmax
        ratio = vtnorm/vmax;

        gamma = 1.5*ratio - .5*(ratio^3);
        dgamma = ((1.5/vmax) - (.5/vmax^3)*3*vtnorm^2)*dvtnorm_dq;
        dgamma_dqd = ((1.5/vmax) - (.5/vmax^3)*3*vtnorm^2)*dvtnorm_dqd;
        ut = vt/vtnorm;
      end

      fric = -.3*N*gamma;

      for j=1:nj
        dfric_term1(:,j) = dN_dq(j)*gamma;
        dfric_term2(:,j) = N*dgamma(j);

        dfric_dqd_term1(:,j) = dN_dqd(j)*gamma;
        dfric_dqd_term2(:,j) = N*dgamma_dqd(j);
      end

      dfric_dq = -.3*(dfric_term1 + dfric_term2);
      dfric_dqd = -.3*(dfric_dqd_term1 + dfric_dqd_term2);
    else
      % gamma = 1, dgamma with everything is 0
      fric = -.3*N;
      dfric_dq = -.3*dN_dq;
      dfric_dqd = -.3*dN_dqd;
      ut = vt/vtnorm;
    end

    bigf = N*ud + fric*ut;
    jointtorque = Jv'*bigf;

    for j=1:nj
      dbigf_term1(:,j) = dN_dq(j)*ud;
      dbigf_dqd_term1(:,j) = dN_dqd(j)*ud;
        
      dbigf_term2(:,j) = dfric_dq(j)*ut;
      dbigf_dqd_term2(:,j) = dfric_dqd(j)*ut;
        
      dtau_term1(:,j) = dJvdq(:,:,j)'*bigf;
    end
    %dbigf_dq = dbigf_term1 + N*dud_dq + dbigf_term2 + fric*dvtvtnorm_dq;
    dbigf_dq = dbigf_term1 + dbigf_term2;
    %%dbigf_dqd = dbigf_dqd_term1 + dbigf_dqd_term2 + fric*dvtvtnorm_dqd;
    dbigf_dqd = dbigf_dqd_term1 + dbigf_dqd_term2;
    
    dtau_dq = dtau_term1 + Jv'*dbigf_dq;
    dtau_dqd = Jv'*dbigf_dqd;
    %tau = [0 N];
    %ctau_p = ctau_p + [0 0; dN_dq]; 
    %ctau_v = ctau_v + [0 0; dN_dqd];

     
    tau = tau + jointtorque';
    ctau_p = ctau_p + dtau_dq;
    ctau_v = ctau_v + dtau_dqd;
  end  %end if (d < rho)
end  %end for i=1:m

%tau = jointtorque;
%if (norm(tau) > 0)
%  tau
%  ctau_p
%  t
%  pos
%  pause
%end

%pidx = 1;
%for i=1:nj
%  if (Hap(i) == 0)  % joint is passive
%    tau(i) = jointtorque(i);
%    pidx = pidx + 1;
%  end
%end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end JVC mod%%%%%%%%%%%%%%%%%%%%%%%%
% Need to add in, getting the ctau matrices


% linkgrad4 calculates:
%  active joint torque (tq) and passive accelerations (qdd) and
%   derivatives of above wrt joint angles, velocities and 
%   accelerations(active only)
%
%  d_x = d(qdd, tq)/dq = derivative of qdd and tq with respect to 
%        joint positions
%  d_xd = d(qdd, tq)/dqd = derivative of qdd and tq with respect to 
%        joint velocities
%  d_x = d(qdd, tq)/dqdd = derivative of qdd and tq with respect to 
%        joint accelerations (which is non-zero for active joints only)
%
%oc = setjoints(oc,pos,vel,acc,tau);
%[d_x, d_xd, d_xdd, tq, qdd] = linkgrad4(oc,pos,vel,acc,tau);

% in order to use non-zero torques on the passive joints:
%
% [d_x, d_xd, d_xdd, d_tau, tq, qdd] = linkgrad5(oc, pos, vel, acc, ...
%       tau, ctau_p, ctau_v, ctau_a);
%
%
%  d_tau returns d(qdd, tq)/dtq_p = derivatives of qdd and tq with respect
%      to passive joint torques (tq_p)
%
%   The extra input arguments are:
%  tau: contains a vector of passive joint torques and
%  ctau_p is a matrix whose columns contain the derivative of the passive
%     joint torques with respect to the joint positions (i.e. first column
%     is dtau/dqi where qi is the first joint position. This is useful if
%     you want to model springs
%  ctau_v is a matrix whose columns contain the derivative of the passive
%     joint torques with respect to the joint velocities. This is useful
%     if you want to model dampers.
%  ctau_a is a matrix whose columns contain the derivative of the passive
%     joint torques with respect to the _active_ joint accelerations. I have
%     no idea what you would use this for, but i figured I'd let you do
%     it if you wanted to..;)
%     NOTE that we cannot vary passive joint torques with respect to passive
%     joint accelerations since they are unknown
%    
[d_x, d_xd, d_xdd, d_tau, tq, qdd] = linkgrad5(oc, pos, vel, acc, ...
       tau, ctau_p, ctau_v, ctau_a);

%Check the time derivative of applied torque here...
%tempqdd = [qdd'; aa(1); aa(2) ]
%dtemptau_dt = ctau_p*vel' + ctau_v*tempqdd
%tau
%t
%pause
   
% derivative of velocity = acceleration
ydot((np+1):(2*np)) = qdd;

% keep track of the 0.5*torque^2
ydot(2*np+1) = 0.5*tq*tq';

coeff = getmults(pth);
[mp np_pj] = size(coeff);
order = getorder(pth);

% num_param is the number of variable spline parameters
num_param = mp*(np_pj-start_const-end_const);

%  these are derivatives of the pos, vel, and accelerations of all the
%    joints wrt the parameters
pd = zeros(nj,num_param);
vd = zeros(nj,num_param);
ad = zeros(nj,num_param);

% fill in active joint derivatives: derivative of active joint pos, vel
%   and acel wrt spline parameters
for i=start_const:(np_pj-end_const-1)
  aidx = 1;
  [temp1,temp2,temp3] = pathpt2Dn(pth,t,i);
  for j = 1:nj
    if (Hap(j) == 1) 
      pd(j,aidx + (i-start_const)*na) = temp1(aidx);
      vd(j,aidx + (i-start_const)*na) = temp2(aidx);
      ad(j,aidx + (i-start_const)*na) = temp3(aidx);
      aidx = aidx+1;
    end
  end 
end

% fill in passive joint derivatives: derivative of passive joint pos and
%    velocity wrt spline parameters
% NOTE: these values are present in the state vector 'y' and are being
%    integrated along with everything else
pidx = 1;
for i = 1:nj
  if (Hap(i) == 0)
    for j = 1:num_param
      pd(i,j) = y(2*np + 1 + j + (pidx-1)*2*num_param);
      vd(i,j) = y(2*np + 1 + j + num_param + (pidx-1)*2*num_param);
    end
    pidx = pidx+1;
  end
end

% derivatives of qdd and tq with respect to the spline parameters = 
%
% let x = (qdd,tq) and v be the spline parameters, then:
%   (dx/dv) = (dx/d(pos)) * (d(pos)/dv) + (dx/d(vel)) * (d(vel)/dv) + 
%                       (dx/d(acc)) * (d(acc)/dv) 
%
% assembling all this inforation into a matrix yields:
%dT = [d_x'*pd+d_xd'*vd+d_xdd'*ad];

%
% or if you have non-zero torques:
%    (dx/dv) = dx/d(pos) * d(pos)/dv + dx/d(vel) * d(vel)/dv + 
%       dx/d(acc) * d(acc)/dv + dx/d(tau) * 
% [d(tau)/d(pos)*d(pos)/dv + d(tau)/d(vel)*d(vel)/dv + d(tau)/d(acc)*d(acc)/dv]
%
% dT = [d_x'*pd + d_xd'*vd + d_xdd'*ad + ...
%         d_tau'*(ctau_p*pd + ctau_v*vd + ctau_a*ad)];
%
% is the above correct for dT? I'm only 90% sure...
%
%dT = [d_x'*pd + d_xd'*vd + d_xdd'*ad + ...
%         d_tau'*(ctau_p*pd + ctau_v*vd + ctau_a*ad)];
dT = [d_x'*pd+d_xd'*vd+d_xdd'*ad];




% use the information in dT to fill out the remaining states

% passive joint partials:
pidx = 1;
for i=1:nj
  if (Hap(i) == 0)
    i1 = 2+2*np+(pidx-1)*2*num_param;
    ydot(i1:(i1+num_param-1)) = y((i1+num_param):(i1+2*num_param-1));
    ydot((i1+num_param):(i1+2*num_param-1)) = dT(i,:);
    pidx = pidx + 1;
  end
end

% for the torque partials we need the portions of dT associated with
%   the active joints
aidx = 1;
dTa = zeros(na,num_param);
for i=1:nj
  if (Hap(i) == 1)
    dTa(aidx,:) = dT(i,:);
    aidx = aidx+1;
  end
end

% fill out the partial of the torque wrt parameters
ydot((2+(2*np)*(1+num_param)):(2+(2*np)*(1+num_param)+num_param-1)) = tq*dTa;

ydot = ydot(:);