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];
% DVT1-2
motorConstant = [0.21,0.21,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:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
posDir = [1,1,1,1,1,1,1,-1,1];

% J9 traj
% hack
% Filter outliers using moving median filter
windowSize = 21; % Must be odd
data_filtered = zeros([length(data(:,(6*(2+1))+11*3)),robot.ndof]);
for i= robot.ndof:-1:2
data_filtered(:,i) = data(:,(6*(i+1))+11*3);
% Apply moving median filter
medianFiltered = movmedian(data_filtered(:,i), windowSize);
% Find outliers using median absolute deviation
medianVal = median(data_filtered(:,i));
madVal = median(abs(data_filtered(:,i) - medianVal));
threshold = 3; % Number of MAD deviations to consider as outlier
outlierIdx = abs(data_filtered(:,i) - medianVal) > threshold * madVal;
% Replace outliers with median filtered values
data_filtered(outlierIdx,i) = medianFiltered(outlierIdx);
end

for i=6
    fileData=eval(strcat('fileData', num2str(i)));
    data = fileData.data;
    dataLength = length(data);
    figure(i)
    d1Length = floor(length(data) / 2);
    % Hack: change J8 dir
    %data(:,8+1+11) = -data(:,8+1+11);
    if i==2
        plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2),'r');    hold on;
        plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2),'b');    hold on;
        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
    else
        plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2),'r');    hold on;
        plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2),'b');    hold on;
        plot3(data(:,i+1+11),data(:,i+11),data_filtered*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
    end
    hold off;
    title(['J' num2str(i) ' Gravity Model']);
    xlabel('X Pos/rad');ylabel('Y Pos/rad');zlabel('Current /A')
end
%%
% should run identifcation program firstly
resolution = 20;
tau_estimate=[];
for k=6
    if k ==2
        qx = idntfcnTrjctry(k).q(k,:);
        qy = idntfcnTrjctry(k).q(6,:);
    else
        qx = idntfcnTrjctry(k).q(k,:);
        qy = idntfcnTrjctry(k).q(k-1,:);
    end
[X, Y] = meshgrid(linspace(min(qx), max(qx), resolution), linspace(min(qy), max(qy), resolution));
q = mean(idntfcnTrjctry(k).q,2);qd=zeros(9,1);qdd=zeros(9,1);
for i = 1 : length(X)
    for j = 1 : length(Y)
        q(k) = X(i,j);q(k-1) = Y(i,j);
        base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
        Yb = feval(base_regressor_func,q,qd,qdd,robot.baseQR);
        torque=Yb*robot.sol.pib_MLS;
        current_estimate(i,j)=torque(k)/(gearRatio(k)*motorConstant(k));
    end
end
end
hold on;
mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0)
%%
% saveas(gcf, ['./figure/' 'J' num2str(k) ' Gravity Model' '.jpg'])