function robot = estimate_dyn(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
time = 0:0.01:1;
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;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
g = [0; 0; -9.8];
% pi -> [m;mc;I] 10 element 
[nLnkPrms, nLnks] = size(robot.pi);
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
if opt.isJointTorqueSensor
    tau = zeros([robot.ndof,length(q_J)]);
    for i = 1:length(q_J)
        %     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
        regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
        tau(:,i)=regressor*robot_pi;
    end
elseif opt.isSixAxisFTSensor
    tau = zeros([6,length(q_J)]);
    for i = 1:length(q_J)
        regressor_func = sprintf('regressor_%s',opt.robotName);
        regressor = feval(regressor_func,q(:,i),qd(:,i),qdd(:,i));
        FT = regressor*robot_pi;
        joint_idex = robot.ndof-2;
        tau(:,i) = FT(6*(joint_idex-1)+1:6*(joint_idex));
    end
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, opt);

% ---------------------------------------------------------------------
% 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,opt)
        % 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];
            if opt.isJointTorqueSensor
                base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
                Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
                    idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
                Wb = vertcat(Wb, Yb);
                %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
                Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
            elseif opt.isSixAxisFTSensor
                base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName);
                Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ...
                    idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
                joint_idex = robot.ndof-2;
                Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:);
                Wb = vertcat(Wb, Yb);
                %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
                Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));                
            end
        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