function robot = estimate_dyn(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
time = 0:0.01:2;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;-q_J];
qd=[qd_J; -qd_J];
qdd=[qdd_J; -qdd_J];
g = [0; 0; -9.8];
tau = zeros([2,101]);
% pi -> [m;mc;I] 10 element 
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi=[robot_pi1;robot_pi2];
for i = 1:length(q_J)
    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
    tau(:,i)=regressor*robot_pi;
end
idntfcnTrjctry.t = time;
idntfcnTrjctry.q = q;
idntfcnTrjctry.qd = qd;
idntfcnTrjctry.qdd = qdd;
idntfcnTrjctry.tau = tau;
% -------------------------------------------------------------------
% Generate Regressors based on data
% ------------------------------------------------------------------------
drvGains = [];
baseQR = robot.baseQR;
[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains);

% ---------------------------------------------------------------------
% Estimate parameters
% ---------------------------------------------------------------------
sol = struct;
method = 'OLS';
if strcmp(method, 'OLS')
    % Usual least squares
    [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb);
elseif strcmp(method, 'PC-OLS')
    % Physically consistent OLS using SDP optimization
    [sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
else
    error("Chosen method for dynamic parameter estimation does not exist");
end
robot.sol = sol;
% Local unctions
    function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
        % The function builds observation matrix for UR10E
        E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);

        Wb = []; Tau = [];
        for i = 1:1:length(idntfcnTrjctry.t)
            %             Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',...
            %                 idntfcnTrjctry.qd(i,:)',...
            %                 idntfcnTrjctry.q2d(i,:)');
            %             Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)');
            %             Ybi = [Yi*E1, Yfrctni];
            base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
            Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
                idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i));
            Wb = vertcat(Wb, Yb);
            %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
            Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
        end
    end


    function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb)
        % Function perfroms ordinary least squares estimation of parameters
%         pi_OLS = (Wb'*Wb)\(Wb'*Tau);
%         pib_OLS = pi_OLS(1:40); % variables for base paramters
%         pifrctn_OLS = pi_OLS(41:end);
          pib_OLS=pinv(Wb)*Tau;
          pifrctn_OLS = 0;
    end
    function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb)
        % Function perfroms Multi step ordinary least squares estimation of parameters
          
        pib_OLS=pinv(Wb)*Tau;
        pifrctn_OLS = 0;
    end
end