%temp get DVT traj
gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
load("C:\GitLab\gtools\development_files\SLX_Development\R1000\SA System\large_range_lab1.mat");
posDir = [1,1,1,1,1,1,1,-1,1];
isCurrentSensor = true;
% J9 traj
i = 9;
fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data;
dataLength = length(data);
d1Length = floor(length(data) / 2);
% idntfcnTrjctry(i).tau = (data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i)+...
%     data(d1Length+1:end,i+1+11*2)*gearRatio(i)*motorConstant(i))/2;
idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';

q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
for j=1:robot.ndof
    if(j==i||j==i-1)
        q(j,:)=data(:,j+1+11);
        continue;
    end
    q(j,:) = mean(data(:,j+1+11),1);
end
idntfcnTrjctry(i).q = q;
idntfcnTrjctry(i).qd = qd;
idntfcnTrjctry(i).qdd = qdd;

% J8 to J2 traj
for i=8:-1:2
    fileData=eval(strcat('fileData', num2str(i)));
    data = fileData.data;
    dataLength = length(data);
    d1Length = floor(length(data) / 2);
    if(isCurrentSensor)
        idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
    else
        idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i);
    end
    idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
    q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
    for j=1:robot.ndof
        if((j==i||j==i-1)&&i~=2) %hack i=2
            q(j,:)=data(:,j+1+11);
            continue;
        end
        if((i==2&&j==6)||(i==2&&j==2))
            q(j,:)=data(:,j+1+11);
            continue;
        end
        q(j,:) = mean(data(:,j+1+11),1);
    end
    idntfcnTrjctry(i).q = q;
    idntfcnTrjctry(i).qd = qd;
    idntfcnTrjctry(i).qdd = qdd;
end

% Reduce data size
for i=robot.ndof:-1:2
    if i == 9 || i == 8 || i==2
        dataLength = length(idntfcnTrjctry(i).tau);
        idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 400) == 0);
        idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 400) == 0);
        idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 400) == 0);
        idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0);
        continue;
    end
    dataLength = length(idntfcnTrjctry(i).tau);
    idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 400) == 0);
    idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 400) == 0);
    idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 400) == 0);
    idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0);
end

%hack
% for i=robot.ndof:-1:2
%     idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:); 
% end

% pi -> [m;mc;I] 10 element 
% [nLnkPrms, nLnks] = size(robot.pi);
% robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
% for i=2:robot.ndof
%     q = idntfcnTrjctry(i).q;
%     qd = idntfcnTrjctry(i).qd;
%     qdd = idntfcnTrjctry(i).qdd;
%     [nRow,nCol] = size(idntfcnTrjctry(i).qd);
%     for j = 1:nRow/robot.ndof
%         for k = 1:nCol
%             standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
%             regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
%                 qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k));
%             tau_Full=regressor*robot_pi;
%             tau(j,k) = tau_Full(i);
%         end
%     end
%     Trjctry(i).tau = tau;
% end