%Registration of MRT data for the use in AnyBody
clear all
close all

%loading MRT healthy and pathological data
mrt_healthy = dlmread('ModelInfo_Healthy_2.txt');
mrt_patho = dlmread('ModelInfo_Pathological_2.txt');
mrt_healthy_x = mrt_healthy(1,2:10)-mrt_healthy(1,2);
mrt_healthy_y = (mrt_healthy(2,2:10)-mrt_healthy(2,2))*-1;
mrt_healthy_data = [mrt_healthy_x(1,:);mrt_healthy_y(1,:)];
mrt_patho_x = mrt_patho(1,2:10)-mrt_patho(1,2);
mrt_patho_y = (mrt_patho(2,2:10)-mrt_patho(2,2))*-1;
mrt_patho_data = [mrt_patho_x(1,:);mrt_patho_y(1,:)];

figure(1)
subplot(2,2,1)
plot(mrt_healthy_x,mrt_healthy_y,'-+')
HOLD 
plot(mrt_patho_x,mrt_patho_y,'r-+')
    
%offset angle for patho data
lhscapsub = mrt_healthy_data(:,4)-mrt_healthy_data(:,1);
lpscapsub = mrt_patho_data(:,4)-mrt_patho_data(:,1);
angle_off = (acos(dot(mrt_healthy_data(:,4),mrt_patho_data(:,4))/(norm(lhscapsub)*norm(lpscapsub))))*180/pi

%Rotation around z-axis
rotmat = [cos(angle_off),-sin(angle_off);sin(angle_off),cos(angle_off)]

A = zeros(2,9);

for i = 1:9
        A(:,i) = mrt_patho_data(:,i)
        patho_data_new = (A')*rotmat
        A= [patho_data_new']
        
    
end

subplot(2,2,2)
plot(mrt_healthy_x,mrt_healthy_y,'-+')
HOLD 
plot(A(1,:),A(2,:),'r-+')

% dh4p4 = mrt_healthy_data(:,4)-mrt_patho_data(:,4)
% 
% mrt_patho_data_dis_x = mrt_patho_data(1,:)+dh4p4(1,1);
% mrt_patho_data_dis_y = mrt_patho_data(2,:)+dh4p4(2,1);
% mrt_patho_data_dis = [mrt_patho_data_dis_x;mrt_patho_data_dis_y];
% 
% subplot(2,2,2)
% plot(mrt_healthy_x,mrt_healthy_y,'-+')
% HOLD 
% plot(mrt_patho_data_dis_y,mrt_patho_data_dis_x,'r-+')