%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  simulation of human arm movements 
%  
% by Hiroaki Gomi 21/July/2000
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


if 0
% arm length
L1 = 0.282;
L2 = 0.303;
end

% dynamics parameters for inverse dynamics 
% Z1_sim = 0.327;
% Z2_sim = 0.109;
% Z3_sim = 0.106;

Simp = 0.001;		% sampling interval for data storage (sec)
			% should not change
%Cimp = 3.0;
Cimp = 2;		% data store period during experiment (sec)
			% can be change

mov_start_time = 0.5;	% (sec)
%mov_duration_time = 1.0;	% (sec)
mov_duration_time = 0.75;	% (sec)

freq = 1.0/Simp;
one_length = freq*Cimp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Minimum Jerk Trajectory Calc

x_mj = zeros(one_length,1);
y_mj = zeros(one_length,1);
xd_mj = zeros(one_length,1);
yd_mj = zeros(one_length,1);
xdd_mj = zeros(one_length,1);
ydd_mj = zeros(one_length,1);

th1_mj = zeros(one_length,1);
th2_mj = zeros(one_length,1);
th1d_mj = zeros(one_length,1);
th2d_mj = zeros(one_length,1);
th1dd_mj = zeros(one_length,1);
th2dd_mj = zeros(one_length,1);

des_start = mov_start_time * freq;
des_end = (mov_start_time + mov_duration_time)*freq;

diffX = TrialEndX - TrialStartX;
diffY = TrialEndY - TrialStartY;

t = [Simp:Simp:mov_duration_time];
t = (t')/mov_duration_time;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calc cursor data vector
%		(pos, vel, acc)

x_mj(1:des_start) = TrialStartX * (ones(des_start,1));
y_mj(1:des_start) = TrialStartY * (ones(des_start,1));
x_mj(des_end+1:one_length) = TrialEndX * (ones(one_length-des_end,1));
y_mj(des_end+1:one_length) = TrialEndY * (ones(one_length-des_end,1));

x_mj(des_start+1:des_end) =...
	 diffX*(10.0*(t.^3) - 15*(t.^4) + 6.0*(t.^5)) + TrialStartX;
y_mj(des_start+1:des_end) =...
	 diffY*(10.0*(t.^3) - 15*(t.^4) + 6.0*(t.^5)) + TrialStartY;
xd_mj(des_start+1:des_end) = diffX*...
	( (30.0/mov_duration_time)*(t.^2)...
	- (60.0/mov_duration_time)*(t.^3)...
	+ (30.0/mov_duration_time)*(t.^4));
yd_mj(des_start+1:des_end) = diffY*...
	( (30.0/mov_duration_time)*(t.^2)...
	- (60.0/mov_duration_time)*(t.^3)...
	+ (30.0/mov_duration_time)*(t.^4));
xdd_mj(des_start+1:des_end) = diffX*...
	( (60.0/(mov_duration_time^2))*t...
	- (180.0/(mov_duration_time^2))*(t.^2)...
	+ (120.0/(mov_duration_time^2))*(t.^3));
ydd_mj(des_start+1:des_end) = diffY*...
	( (60.0/(mov_duration_time^2))*t...
	- (180.0/(mov_duration_time^2))*(t.^2)...
	+ (120.0/(mov_duration_time^2))*(t.^3));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% angular variables
%		(pos, vel, acc)

Kappa = ((x_mj.^2 + y_mj.^2 + L1^2 + L2^2).^2)...
		- 2.0*((x_mj.^2 + y_mj.^2).^2 + L1^4 + L2^4);
Kappa = sqrt(Kappa);

th1_mj = atan2(y_mj, x_mj) - atan2(Kappa, x_mj.^2 + y_mj.^2 + L1^2 - L2^2);
th2_mj = atan2(Kappa, x_mj.^2 + y_mj.^2 - L1^2 - L2^2);

% jacobian matrix component temporal vector produced
jacob11v = (-L1 * sin(th1_mj)- L2 * sin(th1_mj+th2_mj));
jacob12v = (- L2 * sin(th1_mj+th2_mj));
jacob21v =   L1 * cos(th1_mj)+ L2 * cos(th1_mj+th2_mj);
jacob22v =  L2 * cos(th1_mj+th2_mj);

% inverse jacobian matrix
Det = (jacob11v).*(jacob22v) - (jacob12v).*(jacob21v);
jacob11v_inv = jacob22v./Det;
jacob12v_inv = -jacob12v./Det;
jacob21v_inv = -jacob21v./Det;
jacob22v_inv = jacob11v./Det;

th1d_mj = jacob11v_inv.*xd_mj + jacob12v_inv.*yd_mj;
th2d_mj = jacob21v_inv.*xd_mj + jacob22v_inv.*yd_mj;

jacob11vd =...
	-L1*cos(th1_mj).*th1d_mj - L2*cos(th1_mj+th2_mj).*(th1d_mj+th2d_mj);
jacob12vd = -L2*cos(th1_mj+th2_mj).*(th1d_mj+th2d_mj);
jacob21vd =...
	-L1*sin(th1_mj).*th1d_mj - L2*sin(th1_mj+th2_mj).*(th1d_mj+th2d_mj);
jacob22vd = -L2*sin(th1_mj+th2_mj).*(th1d_mj+th2d_mj);

temp1 = (xdd_mj - (jacob11vd.*th1d_mj + jacob12vd.*th2d_mj));
temp2 = (ydd_mj - (jacob21vd.*th1d_mj + jacob22vd.*th2d_mj));
th1dd_mj = jacob11v_inv.* temp1 + jacob12v_inv.* temp2;
th2dd_mj = jacob21v_inv.* temp1 + jacob22v_inv.* temp2;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calc Dynamic Torque 

p11 = th1dd_mj;
p12 = cos(th2_mj).*(2*th1dd_mj + th2dd_mj) ...
      - sin(th2_mj).*(th2d_mj .* th2d_mj + 2 * th1d_mj .* th2d_mj);
p13 = th2dd_mj;

p22 = cos(th2_mj).*th1dd_mj + sin(th2_mj) .* (th1d_mj .* th1d_mj);
p23 = th1dd_mj + th2dd_mj;


tau1_dyn = Z1_sim * p11 + Z2_sim * p12 + Z3_sim * p13;
tau2_dyn = Z2_sim * p22 + Z3_sim * p23;

tau_ff_h = [tau1_dyn, tau2_dyn]';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize valiables

th_r = [th1_mj,th2_mj,th1d_mj,th2d_mj]'; % reference input 
th_h = zeros(size(th_r));                % realized theta buffer 
th_s = zeros(size(th_r));                % predicted theta buffer 
tau_smith_h = zeros(2,size(th_r,2));     % torque generated by smith predictor
tau_h = zeros(2,size(th_r,2));           % torque buffer 

th0 = [th1_mj(1), th2_mj(1), 0, 0]';
th_h(:,1) = th0;
th_s(:,1) = th0;
th = th0;
thP = th0;

%FbGain = [8,0,3,0; 0,8,0,3];
%FbGain = [5,0,2,0; 0,5,0,2];
%delay_t = 0.040; % sec
%delayP_t = 0.040; % sec

delay = delay_t/Simp;
delayP = delayP_t/Simp;


mu1 = 1e-6; % gains for parameter adaptation
mu2 = mu1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=0:Cimp/Simp-1
  t = i*Simp;	

  %%%%%%%%%%%%%%%%%%%%%%%%%
  % feedback loop

  th_ref = th_r(:,round(t/Simp)+1);
  if round(t/Simp)+1-delay < 1
    th_obs = th_h(:,1);
  else
    th_obs = th_h(:,round(t/Simp)+1-delay);
  end

  tau_fb = FbGain*(th_ref - th_obs);

  %%%%%%%%%%%%%%%%%%%%%%%%%
  % smith predictor

  th_p = th_s(:,round(t/Simp)+1);
  if round(t/Simp)+1-delayP < 1
    th_pd = th_s(:,1);
  else
    th_pd = th_s(:,round(t/Simp)+1-delayP);
  end

  tau_smith = FbGain*(th_pd - th_p);
  tau_smith_h(:,round(t/Simp)+1) = tau_smith;
  if sw_smith == 0
    tau_smith = 0;
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%
  % feedforward loop

  if 0
    tau1_dy = Z1_sim * p11(round(t/Simp)+1) ...
	      + Z2_sim * p12(round(t/Simp)+1) + Z3_sim * p13(round(t/Simp)+1);
    tau2_dy = Z2_sim * p22(round(t/Simp)+1) + Z3_sim * p23(round(t/Simp)+1);

    tau_ff = [tau1_dy, tau2_dy]';

    Z1_sim = Z1_sim + mu1*p11(round(t/Simp)+1)*(tau_fb(1)+tau_smith(1));
    Z2_sim = Z2_sim + mu1*p12(round(t/Simp)+1)*(tau_fb(1)+tau_smith(1)) ...
	     + mu2*p22(round(t/Simp)+1)*(tau_fb(2)+tau_smith(2));
    Z3_sim = Z3_sim + mu1*p13(round(t/Simp)+1)*(tau_fb(1)+tau_smith(1)) ...
	     + mu2*p23(round(t/Simp)+1)*(tau_fb(2)+tau_smith(2));
    Z_h(:,round(t/Simp)+1) = [Z1_sim, Z2_sim, Z3_sim]'; 
  end

  %tau_ff = 0;
  tau_ff = tau_ff_h(:,round(t/Simp)+1);

  %%%%%%%%%%%%%%%%%%%%%%%%%
  % total input to dynamics

  %tau = tau_fb + tau_smith + tau_ff;
  tau = tau_fb + tau_smith; % For Smith Predictor
  %tau = tau_fb + tau_ff;
  %tau = tau_fb;
  
  
  %%%%%%%%%%%%%%%%%%%%%%%%%
  % Perturbation
  
  %fext = [0, 2]';
  %if t > (mov_start_time + 0.15)
  %   if t < (mov_start_time + 0.2)
  %      tau = tau + fext;
  %   end
  %end
  
  
  tau_h(:,round(t/Simp)+1) = tau;
  
  %%%%%%%%%%%%%%%%%%%%%%%%%
  % forward dynamics for smith predictor
  thPdot = d2P(t,thP);
  thP = thP + thPdot *Simp;
  th_s(:,round(t/Simp)+2) = thP;

  %%%%%%%%%%%%%%%%%%%%%%%%%
  thdot = d2(t,th);
  th = th + thdot * Simp;
  th_h(:,round(t/Simp)+2) = th;
end
%%%%%%%%%%%%%%%%%%%%%%%%%

figure(2)
plot(th_h')              
hold on; 
plot(th_r','--')

pos_x = L1*cos(th_h(1,:))+L2*cos(th_h(1,:)+th_h(2,:));
pos_y = L1*sin(th_h(1,:))+L2*sin(th_h(1,:)+th_h(2,:));

figure(1)
plot(pos_x,pos_y)
set(gca,'PlotBoxAspectRatio',[1,1,1])
set(gca,'ylim',[-0.4,0.6]);  set(gca,'xlim',[-0.4,0.6]); 

% figure(3)
% plot(Z_h')