function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal)
% Reproduction with a linear quadratic regulator of finite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
%          http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, we would be grateful if you cite  
% the following reference in any academic publication that uses this code or part of it: 
%
% @inproceedings{Calinon14ICRA,
%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
%   title="A task-parameterized probabilistic model with minimal intervention control",
%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
%   year="2014",
%   month="May-June",
%   address="Hong Kong, China",
%   pages="3339--3344"
% }

nbData = size(DataIn,2);
nbVarOut = model.nbVar - size(DataIn,1);

%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Definition of a double integrator system (DX = A X + B u with X = [x; dx])
A = kron([0 1; 0 0], eye(nbVarOut));
B = kron([0; 1], eye(nbVarOut));
%Initialize Q and R weighting matrices
Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
for t=1:nbData
	Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t)); 
end
R = eye(nbVarOut) * rFactor;

if nargin<6
	%Pfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
	Pfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions)
end

%Auxiliary variables to minimize the cost function
P = zeros(nbVarOut*2, nbVarOut*2, nbData);
d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation

%Feedback term
L = zeros(nbVarOut, nbVarOut*2, nbData);
%Compute P_T from the desired final feedback gains L_T,  
P(:,:,nbData) = Pfinal; 

%Compute derivative of target path
%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation

%Variables for feedforward term computation (optional for movements with low dynamics)
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;

%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;

%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],1,2)/model.dt;


%Backward integration of the Riccati equation and additional equation
for t=nbData-1:-1:1
	P(:,:,t) = P(:,:,t+1) + model.dt * (A'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B')*P(:,:,t+1) + Q(:,:,t+1));  
	%Optional feedforward term computation
	d(:,t) = d(:,t+1) + model.dt * ((A'-P(:,:,t+1)*B*(R\B'))*d(:,t+1) +  P(:,:,t+1)*dtar(:,t+1) - P(:,:,t+1)*A*tar(:,t+1)); 
end
%Computation of the feedback term L and feedforward term M in u=-LX+M
for t=1:nbData
	L(:,:,t) = R\B' * P(:,:,t); 
	M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
end

%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
	%Compute acceleration (with only feedback term)
	%ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx];

	%Compute acceleration (with both feedback and feedforward terms)
	ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t);
	%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
	%r.FF(:,t) = M(:,t);

	%Update velocity and position
	dx = dx + ddx * model.dt;
	x = x + dx * model.dt;
	%Log data
	r.Data(:,t) = [DataIn(:,t); x]; 
	r.ddxNorm(t) = norm(ddx);
	r.kpDet(t) = det(L(:,1:nbVarOut,t));
	r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
end