function robot = estimate_dyn_form_data(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
posData = robot.posData;
currentData = robot.currentData;
[nRow,nCol] = size(posData);
index_p = find(posData(:,nCol-1)==7);
jointPos_p = posData(index_p,1:9);
jointPos_p = jointPos_p(1:100:end,:);
index_n = find(posData(:,nCol-1)==-7);
jointPos_n = posData(index_n,1:9);
jointPos_n = jointPos_n(1:100:end,:);
q = [jointPos_p]';
qd = q; qdd = q;
motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
gearRatio = [101,101,100,100,100,100,100,100,100];
% pi -> [m;mc;I] 10 element 
index_p = find(currentData(:,nCol-1)==7);
jointCur_p = currentData(index_p,1:9);
jointCur_p = jointCur_p(1:100:end,:);
index_n = find(currentData(:,nCol-1)==-7);
jointCur_n = currentData(index_n,1:9);
jointCur_n = jointCur_n(1:100:end,:);
current = (jointCur_p+jointCur_n)/2;
torque_cur = gearRatio.*motorConst.*current;
tau = torque_cur(:,7)';

idntfcnTrjctry.t = 1:length(q);
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 = 'PC-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] = physicallyConsistentEstimation(Tau, Wb);
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);
                %hack                
                joint_idex = robot.ndof-2;
                Yb = Yb((joint_idex-1)+1:(joint_idex),:);
                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
    function pib = physicallyConsistentEstimation(Tau, Wb)
        % 求解线性规划问题
        options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter');
        pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options)
    end
end