clear all;
close all;

%% Loading dat
filename='/Volumes/EUSTACHE/Dropbox/BIG/Pavan/experiments/Substack-WingGrooming-1_1';
extension='.csv';
fid = fopen(strcat(filename, extension));
out = textscan(fid,'%s%s%s%s','delimiter',',');
fclose(fid);

frame = out{1};
frame=str2double(frame(2:end));
id = [out{2}];
id=id(2:end);
x = out{3};
x=str2double(x(2:end));
y = out{4};
y=str2double(y(2:end));

frames=unique(frame);
nframes=size(frames,1);
frame0=frames(1);
colors=jet(nframes);

coordinatesArray=[];
headers={'Frame', 'Id', 'x', 'y'};
coordinatesArray=[coordinatesArray; headers];

%% Aligning
disp(['Processing frame ',num2str(frame0),'...'])

newX=zeros(size(x,1),1);
newY=zeros(size(y,1),1);

tip0X=zeros(nframes,1);
tip0Y=zeros(nframes,1);
tip1X=zeros(nframes,1);
tip1Y=zeros(nframes,1);
tip2X=zeros(nframes,1);
tip2Y=zeros(nframes,1);
tip3X=zeros(nframes,1);
tip3Y=zeros(nframes,1);
tip4X=zeros(nframes,1);
tip4Y=zeros(nframes,1);
tip5X=zeros(nframes,1);
tip5Y=zeros(nframes,1);

J10X=zeros(nframes,1);
J10Y=zeros(nframes,1);
J11X=zeros(nframes,1);
J11Y=zeros(nframes,1);
J12X=zeros(nframes,1);
J12Y=zeros(nframes,1);
J13X=zeros(nframes,1);
J13Y=zeros(nframes,1);
J14X=zeros(nframes,1);
J14Y=zeros(nframes,1);
J15X=zeros(nframes,1);
J15Y=zeros(nframes,1);

J20X=zeros(nframes,1);
J20Y=zeros(nframes,1);
J21X=zeros(nframes,1);
J21Y=zeros(nframes,1);
J22X=zeros(nframes,1);
J22Y=zeros(nframes,1);
J23X=zeros(nframes,1);
J23Y=zeros(nframes,1);
J24X=zeros(nframes,1);
J24Y=zeros(nframes,1);
J25X=zeros(nframes,1);
J25Y=zeros(nframes,1);

% Align frame 1 vertically
L0inds=intersect(find(~cellfun(@isempty,regexp(id,'L0\w*'))),find(frame==frame0));
L1inds=intersect(find(~cellfun(@isempty,regexp(id,'L1\w*'))),find(frame==frame0));
L2inds=intersect(find(~cellfun(@isempty,regexp(id,'L2\w*'))),find(frame==frame0));
L3inds=intersect(find(~cellfun(@isempty,regexp(id,'L3\w*'))),find(frame==frame0));
L4inds=intersect(find(~cellfun(@isempty,regexp(id,'L4\w*'))),find(frame==frame0));
L5inds=intersect(find(~cellfun(@isempty,regexp(id,'L5\w*'))),find(frame==frame0));

Cind=intersect(find(cellfun(@(x)strcmp(x,'C'),id)),find(frame==frame0));
Tind=intersect(find(cellfun(@(x)strcmp(x,'T'),id)),find(frame==frame0));
Hind=intersect(find(cellfun(@(x)strcmp(x,'H'),id)),find(frame==frame0));

%figure; 
%scatter(x(L0inds(1)), y(L0inds(1)));
%hold on; scatter([x(L1inds(1)), x(L2inds(1)), x(L3inds(1)), x(L4inds(1)), x(L5inds(1))], [y(L1inds(1)), y(L2inds(1)), y(L3inds(1)), y(L4inds(1)), y(L5inds(1))])

gravityX=mean(x([L0inds(1),L1inds(1),L2inds(1),L3inds(1),L4inds(1),L5inds(1)]));
gravityY=mean(y([L0inds(1),L1inds(1),L2inds(1),L3inds(1),L4inds(1),L5inds(1)]));

alpha=(((x(L4inds(1))-x(L5inds(1)))*(gravityX-x(L5inds(1))))+((y(L4inds(1))-y(L5inds(1)))*(gravityY-y(L5inds(1)))))/sqrt((x(L5inds(1))-x(L4inds(1))).^2+(y(L5inds(1))-y(L4inds(1))).^2);
xp=x(L5inds(1))+alpha*((x(L4inds(1))-x(L5inds(1)))/sqrt((x(L5inds(1))-x(L4inds(1))).^2+(y(L5inds(1))-y(L4inds(1))).^2));
yp=y(L5inds(1))+alpha*((y(L4inds(1))-y(L5inds(1)))/sqrt((x(L5inds(1))-x(L4inds(1))).^2+(y(L5inds(1))-y(L4inds(1))).^2));
d=sqrt((xp-gravityX).^2+(yp-gravityY).^2);
sinTheta0=-(xp-gravityX)/d;
cosTheta0=-(yp-gravityY)/d;

newX(L0inds)=gravityX+(cosTheta0*(x(L0inds)-gravityX))+(-1.0*sinTheta0*(y(L0inds)-gravityY));
newY(L0inds)=gravityY+(sinTheta0*(x(L0inds)-gravityX))+(cosTheta0*(y(L0inds)-gravityY));
newX(L1inds)=gravityX+(cosTheta0*(x(L1inds)-gravityX))+(-1.0*sinTheta0*(y(L1inds)-gravityY));
newY(L1inds)=gravityY+(sinTheta0*(x(L1inds)-gravityX))+(cosTheta0*(y(L1inds)-gravityY));
newX(L2inds)=gravityX+(cosTheta0*(x(L2inds)-gravityX))+(-1.0*sinTheta0*(y(L2inds)-gravityY));
newY(L2inds)=gravityY+(sinTheta0*(x(L2inds)-gravityX))+(cosTheta0*(y(L2inds)-gravityY));
newX(L3inds)=gravityX+(cosTheta0*(x(L3inds)-gravityX))+(-1.0*sinTheta0*(y(L3inds)-gravityY));
newY(L3inds)=gravityY+(sinTheta0*(x(L3inds)-gravityX))+(cosTheta0*(y(L3inds)-gravityY));
newX(L4inds)=gravityX+(cosTheta0*(x(L4inds)-gravityX))+(-1.0*sinTheta0*(y(L4inds)-gravityY));
newY(L4inds)=gravityY+(sinTheta0*(x(L4inds)-gravityX))+(cosTheta0*(y(L4inds)-gravityY));
newX(L5inds)=gravityX+(cosTheta0*(x(L5inds)-gravityX))+(-1.0*sinTheta0*(y(L5inds)-gravityY));
newY(L5inds)=gravityY+(sinTheta0*(x(L5inds)-gravityX))+(cosTheta0*(y(L5inds)-gravityY));

newX(Cind)=gravityX+(cosTheta0*(x(Cind)-gravityX))+(-1.0*sinTheta0*(y(Cind)-gravityY));
newY(Cind)=gravityY+(sinTheta0*(x(Cind)-gravityX))+(cosTheta0*(y(Cind)-gravityY));
newX(Tind)=gravityX+(cosTheta0*(x(Tind)-gravityX))+(-1.0*sinTheta0*(y(Tind)-gravityY));
newY(Tind)=gravityY+(sinTheta0*(x(Tind)-gravityX))+(cosTheta0*(y(Tind)-gravityY));
newX(Hind)=gravityX+(cosTheta0*(x(Hind)-gravityX))+(-1.0*sinTheta0*(y(Hind)-gravityY));
newY(Hind)=gravityY+(sinTheta0*(x(Hind)-gravityX))+(cosTheta0*(y(Hind)-gravityY));

centerX=newX(L5inds(1));
centerY=newY(L5inds(1));
L3X=newX(L3inds(1));
L3Y=newY(L3inds(1));

%figure; scatter(newX(L0inds(1)), newY(L0inds(1)));
%hold on; scatter([newX(L1inds(1)), newX(L2inds(1)), newX(L3inds(1)), newX(L4inds(1)), newX(L5inds(1))],[newY(L1inds(1)), newY(L2inds(1)), newY(L3inds(1)), newY(L4inds(1)), newY(L5inds(1))])

tip0X(1)=newX(L0inds(end));
tip0Y(1)=newY(L0inds(end));
tip1X(1)=newX(L1inds(end));
tip1Y(1)=newY(L1inds(end));
tip2X(1)=newX(L2inds(end));
tip2Y(1)=newY(L2inds(end));
tip3X(1)=newX(L3inds(end));
tip3Y(1)=newY(L3inds(end));
tip4X(1)=newX(L4inds(end));
tip4Y(1)=newY(L4inds(end));
tip5X(1)=newX(L5inds(end));
tip5Y(1)=newY(L5inds(end));

J10X(1)=newX(L0inds(1));
J10Y(1)=newY(L0inds(1));
J11X(1)=newX(L1inds(1));
J11Y(1)=newY(L1inds(1));
J12X(1)=newX(L2inds(1));
J12Y(1)=newY(L2inds(1));
J13X(1)=newX(L3inds(1));
J13Y(1)=newY(L3inds(1));
J14X(1)=newX(L4inds(1));
J14Y(1)=newY(L4inds(1));
J15X(1)=newX(L5inds(1));
J15Y(1)=newY(L5inds(1));

J20X(1)=newX(L0inds(2));
J20Y(1)=newY(L0inds(2));
J21X(1)=newX(L1inds(2));
J21Y(1)=newY(L1inds(2));
J22X(1)=newX(L2inds(2));
J22Y(1)=newY(L2inds(2));
J23X(1)=newX(L3inds(2));
J23Y(1)=newY(L3inds(2));
J24X(1)=newX(L4inds(2));
J24Y(1)=newY(L4inds(2));
J25X(1)=newX(L5inds(2));
J25Y(1)=newY(L5inds(2));

figure; line(newX(L0inds),newY(L0inds),'Color',colors(1,:));
hold on; 
line(newX(L1inds),newY(L1inds),'Color',colors(1,:));
line(newX(L2inds),newY(L2inds),'Color',colors(1,:));
line(newX(L3inds),newY(L3inds),'Color',colors(1,:));
line(newX(L4inds),newY(L4inds),'Color',colors(1,:));
line(newX(L5inds),newY(L5inds),'Color',colors(1,:));
%scatter([newX(Cind), newX(Tind), newX(Hind)],[newY(Cind), newY(Tind), newY(Hind)]);

Da53_42=sqrt((centerX-newX(L3inds(1))).^2+(centerY-newY(L3inds(1))).^2);
Da51_40=sqrt((centerX-newX(L1inds(1))).^2+(centerY-newY(L1inds(1))).^2);
Da10_32_54=sqrt((newX(L1inds(1))-newX(L0inds(1))).^2+(newY(L1inds(1))-newY(L0inds(1))).^2);
Da5C=sqrt((centerX-newX(Cind)).^2+(centerY-newY(Cind)).^2);
Da5T=sqrt((centerX-newX(Tind)).^2+(centerY-newY(Tind)).^2);
Da5H=sqrt((centerX-newX(Hind)).^2+(centerY-newY(Hind)).^2);

C={frame0, 'C', newX(Cind), newY(Cind)};
T={frame0, 'T', newX(Tind), newY(Tind)};
H={frame0, 'H', newX(Hind), newY(Hind)};

L0B={frame0, 'L0B', newX(L0inds(1)), newY(L0inds(1))};
L0I1={frame0, 'L0I1', newX(L0inds(2)), newY(L0inds(2))};
L0I2={frame0, 'L0I2', newX(L0inds(3)), newY(L0inds(3))};
L0T={frame0, 'L0T', newX(L0inds(4)), newY(L0inds(4))};

L1B={frame0, 'L1B', newX(L1inds(1)), newY(L1inds(1))};
L1I1={frame0, 'L1I1', newX(L1inds(2)), newY(L1inds(2))};
L1I2={frame0, 'L1I2', newX(L1inds(3)), newY(L1inds(3))};
L1T={frame0, 'L1T', newX(L1inds(4)), newY(L1inds(4))};

L2B={frame0, 'L2B', newX(L2inds(1)), newY(L2inds(1))};
L2I1={frame0, 'L2I1', newX(L2inds(2)), newY(L2inds(2))};
L2I2={frame0, 'L2I2', newX(L2inds(3)), newY(L2inds(3))};
L2T={frame0, 'L2T', newX(L2inds(4)), newY(L2inds(4))};

L3B={frame0, 'L3B', newX(L3inds(1)), newY(L3inds(1))};
L3I1={frame0, 'L3I1', newX(L3inds(2)), newY(L3inds(2))};
L3I2={frame0, 'L3I2', newX(L3inds(3)), newY(L3inds(3))};
L3T={frame0, 'L3T', newX(L3inds(4)), newY(L3inds(4))};

L4B={frame0, 'L4B', newX(L4inds(1)), newY(L4inds(1))};
L4I1={frame0, 'L4I1', newX(L4inds(2)), newY(L4inds(2))};
L4I2={frame0, 'L4I2', newX(L4inds(3)), newY(L4inds(3))};
L4T={frame0, 'L4T', newX(L4inds(4)), newY(L4inds(4))};

L5B={frame0, 'L5B', newX(L5inds(1)), newY(L5inds(1))};
L5I1={frame0, 'L5I1', newX(L5inds(2)), newY(L5inds(2))};
L5I2={frame0, 'L5I2', newX(L5inds(3)), newY(L5inds(3))};
L5T={frame0, 'L5T', newX(L5inds(4)), newY(L5inds(4))};

coordinatesArray=[coordinatesArray; C];
coordinatesArray=[coordinatesArray; T];
coordinatesArray=[coordinatesArray; H];
coordinatesArray=[coordinatesArray; L0B];
coordinatesArray=[coordinatesArray; L0I1];
coordinatesArray=[coordinatesArray; L0I2];
coordinatesArray=[coordinatesArray; L0T];
coordinatesArray=[coordinatesArray; L1B];
coordinatesArray=[coordinatesArray; L1I1];
coordinatesArray=[coordinatesArray; L1I2];
coordinatesArray=[coordinatesArray; L1T];
coordinatesArray=[coordinatesArray; L2B];
coordinatesArray=[coordinatesArray; L2I1];
coordinatesArray=[coordinatesArray; L2I2];
coordinatesArray=[coordinatesArray; L2T];
coordinatesArray=[coordinatesArray; L3B];
coordinatesArray=[coordinatesArray; L3I1];
coordinatesArray=[coordinatesArray; L3I2];
coordinatesArray=[coordinatesArray; L3T];
coordinatesArray=[coordinatesArray; L4B];
coordinatesArray=[coordinatesArray; L4I1];
coordinatesArray=[coordinatesArray; L4I2];
coordinatesArray=[coordinatesArray; L4T];
coordinatesArray=[coordinatesArray; L5B];
coordinatesArray=[coordinatesArray; L5I1];
coordinatesArray=[coordinatesArray; L5I2];
coordinatesArray=[coordinatesArray; L5T];

%%
for t=frame0+1:frame0+nframes-1
    disp(['Processing frame ',num2str(t),'...'])
    L0inds=intersect(find(~cellfun(@isempty,regexp(id,'L0\w*'))),find(frame==t));
    L1inds=intersect(find(~cellfun(@isempty,regexp(id,'L1\w*'))),find(frame==t));
    L2inds=intersect(find(~cellfun(@isempty,regexp(id,'L2\w*'))),find(frame==t));
    L3inds=intersect(find(~cellfun(@isempty,regexp(id,'L3\w*'))),find(frame==t));
    L4inds=intersect(find(~cellfun(@isempty,regexp(id,'L4\w*'))),find(frame==t));
    L5inds=intersect(find(~cellfun(@isempty,regexp(id,'L5\w*'))),find(frame==t));

    Cind=intersect(find(cellfun(@(x)strcmp(x,'C'),id)),find(frame==frame0));
    Tind=intersect(find(cellfun(@(x)strcmp(x,'T'),id)),find(frame==frame0));
    Hind=intersect(find(cellfun(@(x)strcmp(x,'H'),id)),find(frame==frame0));
    
    % Translation
    Tx=x(L5inds(1))-centerX;
    Ty=y(L5inds(1))-centerY;

	L5x=x(L5inds)-Tx;
    L5y=y(L5inds)-Ty;
    
    transCx=x(Cind)-Tx;
    transCy=y(Cind)-Ty;
    transTx=x(Tind)-Tx;
    transTy=y(Tind)-Ty;
    transHx=x(Hind)-Tx;
    transHy=y(Hind)-Ty;
    
    transL0x=x(L0inds)-Tx;
    transL0y=y(L0inds)-Ty;
    transL1x=x(L1inds)-Tx;
    transL1y=y(L1inds)-Ty;
    transL2x=x(L2inds)-Tx;
    transL2y=y(L2inds)-Ty;
    transL3x=x(L3inds)-Tx;
	transL3y=y(L3inds)-Ty;
    transL4x=x(L4inds)-Tx;
    transL4y=y(L4inds)-Ty;
    
    % Homothetie 1
    a53_42=Da53_42/sqrt((x(L5inds(1))-x(L3inds(1))).^2+(y(L5inds(1))-y(L3inds(1))).^2);
    
    h1L3x=centerX+a53_42*(transL3x-centerX);
    h1L3y=centerY+a53_42*(transL3y-centerY);
    
    h1L2x=transL4x(1)+a53_42*(transL2x-transL4x(1));
    h1L2y=transL4y(1)+a53_42*(transL2y-transL4y(1));
    
    a51_40=Da51_40/sqrt((x(L5inds(1))-x(L1inds(1))).^2+(y(L5inds(1))-y(L1inds(1))).^2);
    
    h1L1x=centerX+a51_40*(transL1x-centerX);
    h1L1y=centerY+a51_40*(transL1y-centerY);
    
    h1L0x=transL4x(1)+a51_40*(transL0x-transL4x(1));
    h1L0y=transL4y(1)+a51_40*(transL0y-transL4y(1));
    
    a5C=Da5C/sqrt((x(L5inds(1))-x(Cind)).^2+(y(L5inds(1))-y(Cind)).^2);
    h1Cx=centerX+a5C*(transCx-centerX);
    h1Cy=centerY+a5C*(transCy-centerY);
    
    a5T=Da5T/sqrt((x(L5inds(1))-x(Tind)).^2+(y(L5inds(1))-y(Tind)).^2);
    h1Tx=centerX+a5T*(transTx-centerX);
    h1Ty=centerY+a5T*(transTy-centerY);
    
    a5H=Da5H/sqrt((x(L5inds(1))-x(Hind)).^2+(y(L5inds(1))-y(Hind)).^2);
    h1Hx=centerX+a5H*(transHx-centerX);
    h1Hy=centerY+a5H*(transHy-centerY);
    
    % Homothetie 2
    a10_32_54=Da10_32_54/sqrt((x(L1inds(1))-x(L0inds(1))).^2+(y(L1inds(1))-y(L0inds(1))).^2);
    
    h2L0x=h1L1x(1)+a10_32_54*(h1L0x-h1L1x(1));
    h2L0y=h1L1y(1)+a10_32_54*(h1L0y-h1L1y(1));
    
    h2L2x=h1L3x(1)+a10_32_54*(h1L2x-h1L3x(1));
    h2L2y=h1L3y(1)+a10_32_54*(h1L2y-h1L3y(1));
    
    h2L4x=centerX+a10_32_54*(transL4x-centerX);
    h2L4y=centerY+a10_32_54*(transL4y-centerY);
    
    % Rotation
    xp=h2L4x(1)-centerX;
    yp=h2L4y(1)-centerY;
    d=sqrt(xp.^2+yp.^2);
    sinTheta=yp/d;
    cosTheta=-xp/d;
    
	newX(L0inds)=centerX+(cosTheta*(h2L0x-centerX))+(-1.0*sinTheta*(h2L0y-centerY));
    newY(L0inds)=centerY+(sinTheta*(h2L0x-centerX))+(cosTheta*(h2L0y-centerY));
	newX(L1inds)=centerX+(cosTheta*(h1L1x-centerX))+(-1.0*sinTheta*(h1L1y-centerY));
    newY(L1inds)=centerY+(sinTheta*(h1L1x-centerX))+(cosTheta*(h1L1y-centerY));
	newX(L2inds)=centerX+(cosTheta*(h2L2x-centerX))+(-1.0*sinTheta*(h2L2y-centerY));
    newY(L2inds)=centerY+(sinTheta*(h2L2x-centerX))+(cosTheta*(h2L2y-centerY));
	newX(L3inds)=centerX+(cosTheta*(h1L3x-centerX))+(-1.0*sinTheta*(h1L3y-centerY));
    newY(L3inds)=centerY+(sinTheta*(h1L3x-centerX))+(cosTheta*(h1L3y-centerY));
    newX(L4inds)=centerX+(cosTheta*(h2L4x-centerX))+(-1.0*sinTheta*(h2L4y-centerY));
    newY(L4inds)=centerY+(sinTheta*(h2L4x-centerX))+(cosTheta*(h2L4y-centerY));
    newX(L5inds)=centerX+(cosTheta*(L5x-centerX))+(-1.0*sinTheta*(L5y-centerY));
    newY(L5inds)=centerY+(sinTheta*(L5x-centerX))+(cosTheta*(L5y-centerY));
    
    newX(Cind)=centerX+(cosTheta*(h1Cx-centerX))+(-1.0*sinTheta*(h1Cy-centerY));
    newY(Cind)=centerY+(sinTheta*(h1Cx-centerX))+(cosTheta*(h1Cy-centerY));
    
    newX(Tind)=centerX+(cosTheta*(h1Tx-centerX))+(-1.0*sinTheta*(h1Ty-centerY));
    newY(Tind)=centerY+(sinTheta*(h1Tx-centerX))+(cosTheta*(h1Ty-centerY));
    
    newX(Hind)=centerX+(cosTheta*(h1Hx-centerX))+(-1.0*sinTheta*(h1Hy-centerY));
    newY(Hind)=centerY+(sinTheta*(h1Hx-centerX))+(cosTheta*(h1Hy-centerY));
    
    %hold on; scatter([newX(L0inds); newX(L1inds); newX(L2inds); newX(L3inds); newX(L4inds); newX(L5inds)],[newY(L0inds); newY(L1inds); newY(L2inds); newY(L3inds); newY(L4inds); newY(L5inds)])
    hold on; line(newX(L0inds),newY(L0inds),'Color',colors(t-frame0+1,:));
    hold on; line(newX(L1inds),newY(L1inds),'Color',colors(t-frame0+1,:));
    hold on; line(newX(L2inds),newY(L2inds),'Color',colors(t-frame0+1,:));
    hold on; line(newX(L3inds),newY(L3inds),'Color',colors(t-frame0+1,:));
    hold on; line(newX(L4inds),newY(L4inds),'Color',colors(t-frame0+1,:));
    hold on; line(newX(L5inds),newY(L5inds),'Color',colors(t-frame0+1,:));
    %hold on; scatter([newX(Cind), newX(Tind), newX(Hind)],[newY(Cind), newY(Tind), newY(Hind)]);
    
    tip0X(t-frame0+1)=newX(L0inds(end));
    tip0Y(t-frame0+1)=newY(L0inds(end));
    tip1X(t-frame0+1)=newX(L1inds(end));
    tip1Y(t-frame0+1)=newY(L1inds(end));
    tip2X(t-frame0+1)=newX(L2inds(end));
    tip2Y(t-frame0+1)=newY(L2inds(end));
    tip3X(t-frame0+1)=newX(L3inds(end));
    tip3Y(t-frame0+1)=newY(L3inds(end));
    tip4X(t-frame0+1)=newX(L4inds(end));
    tip4Y(t-frame0+1)=newY(L4inds(end));
    tip5X(t-frame0+1)=newX(L5inds(end));
    tip5Y(t-frame0+1)=newY(L5inds(end));
    
    J10X(t-frame0+1)=newX(L0inds(1));
    J10Y(t-frame0+1)=newY(L0inds(1));
    J11X(t-frame0+1)=newX(L1inds(1));
    J11Y(t-frame0+1)=newY(L1inds(1));
    J12X(t-frame0+1)=newX(L2inds(1));
    J12Y(t-frame0+1)=newY(L2inds(1));
    J13X(t-frame0+1)=newX(L3inds(1));
    J13Y(t-frame0+1)=newY(L3inds(1));
    J14X(t-frame0+1)=newX(L4inds(1));
    J14Y(t-frame0+1)=newY(L4inds(1));
    J15X(t-frame0+1)=newX(L5inds(1));
    J15Y(t-frame0+1)=newY(L5inds(1));
    
    J20X(t-frame0+1)=newX(L0inds(2));
    J20Y(t-frame0+1)=newY(L0inds(2));
    J21X(t-frame0+1)=newX(L1inds(2));
    J21Y(t-frame0+1)=newY(L1inds(2));
    J22X(t-frame0+1)=newX(L2inds(2));
    J22Y(t-frame0+1)=newY(L2inds(2));
    J23X(t-frame0+1)=newX(L3inds(2));
    J23Y(t-frame0+1)=newY(L3inds(2));
    J24X(t-frame0+1)=newX(L4inds(2));
    J24Y(t-frame0+1)=newY(L4inds(2));
    J25X(t-frame0+1)=newX(L5inds(2));
    J25Y(t-frame0+1)=newY(L5inds(2));
    
    C={t, 'C', newX(Cind), newY(Cind)};
    T={t, 'T', newX(Tind), newY(Tind)};
    H={t, 'H', newX(Hind), newY(Hind)};
    
    L0B={t, 'L0B', newX(L0inds(1)), newY(L0inds(1))};
    L0I1={t, 'L0I1', newX(L0inds(2)), newY(L0inds(2))};
    L0I2={t, 'L0I2', newX(L0inds(3)), newY(L0inds(3))};
    L0T={t, 'L0T', newX(L0inds(4)), newY(L0inds(4))};

    L1B={t, 'L1B', newX(L1inds(1)), newY(L1inds(1))};
    L1I1={t, 'L1I1', newX(L1inds(2)), newY(L1inds(2))};
    L1I2={t, 'L1I2', newX(L1inds(3)), newY(L1inds(3))};
    L1T={t, 'L1T', newX(L1inds(4)), newY(L1inds(4))};
    
    L2B={t, 'L2B', newX(L2inds(1)), newY(L2inds(1))};
    L2I1={t, 'L2I1', newX(L2inds(2)), newY(L2inds(2))};
    L2I2={t, 'L2I2', newX(L2inds(3)), newY(L2inds(3))};
    L2T={t, 'L2T', newX(L2inds(4)), newY(L2inds(4))};
    
    L3B={t, 'L3B', newX(L3inds(1)), newY(L3inds(1))};
    L3I1={t, 'L3I1', newX(L3inds(2)), newY(L3inds(2))};
    L3I2={t, 'L3I2', newX(L3inds(3)), newY(L3inds(3))};
    L3T={t, 'L3T', newX(L3inds(4)), newY(L3inds(4))};
    
    L4B={t, 'L4B', newX(L4inds(1)), newY(L4inds(1))};
    L4I1={t, 'L4I1', newX(L4inds(2)), newY(L4inds(2))};
    L4I2={t, 'L4I2', newX(L4inds(3)), newY(L4inds(3))};
    L4T={t, 'L4T', newX(L4inds(4)), newY(L4inds(4))};
    
    L5B={t, 'L5B', newX(L5inds(1)), newY(L5inds(1))};
    L5I1={t, 'L5I1', newX(L5inds(2)), newY(L5inds(2))};
    L5I2={t, 'L5I2', newX(L5inds(3)), newY(L5inds(3))};
    L5T={t, 'L5T', newX(L5inds(4)), newY(L5inds(4))};

    coordinatesArray=[coordinatesArray; C];
    coordinatesArray=[coordinatesArray; T];
    coordinatesArray=[coordinatesArray; H];
    coordinatesArray=[coordinatesArray; L0B];
    coordinatesArray=[coordinatesArray; L0I1];
    coordinatesArray=[coordinatesArray; L0I2];
    coordinatesArray=[coordinatesArray; L0T];
    coordinatesArray=[coordinatesArray; L1B];
    coordinatesArray=[coordinatesArray; L1I1];
    coordinatesArray=[coordinatesArray; L1I2];
    coordinatesArray=[coordinatesArray; L1T];
	coordinatesArray=[coordinatesArray; L2B];
    coordinatesArray=[coordinatesArray; L2I1];
    coordinatesArray=[coordinatesArray; L2I2];
    coordinatesArray=[coordinatesArray; L2T];
    coordinatesArray=[coordinatesArray; L3B];
    coordinatesArray=[coordinatesArray; L3I1];
    coordinatesArray=[coordinatesArray; L3I2];
    coordinatesArray=[coordinatesArray; L3T];
    coordinatesArray=[coordinatesArray; L4B];
    coordinatesArray=[coordinatesArray; L4I1];
    coordinatesArray=[coordinatesArray; L4I2];
    coordinatesArray=[coordinatesArray; L4T];
    coordinatesArray=[coordinatesArray; L5B];
    coordinatesArray=[coordinatesArray; L5I1];
    coordinatesArray=[coordinatesArray; L5I2];
    coordinatesArray=[coordinatesArray; L5T];
end

fid = fopen([filename,'_registeredToBody.csv'],'w');
fprintf(fid,'%s, %s, %s, %s\n',coordinatesArray{1,:});
for i=2:length(coordinatesArray)
    fprintf(fid,'%f, %s, %f, %f\n',coordinatesArray{i,:});
end
fclose(fid);

%% All tips (raw)
z = zeros(size(tip0X));
col = tip0X;  % This is the color, vary with x in this case.
figure; surface([tip0X,tip0X],[tip0Y,tip0Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([tip1X,tip1X],[tip1Y,tip1Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([tip2X,tip2X],[tip2Y,tip2Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([tip3X,tip3X],[tip3Y,tip3Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([tip4X,tip4X],[tip4Y,tip4Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([tip5X,tip5X],[tip5Y,tip5Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
    
hold on; surface([J10X,J10X],[J10Y,J10Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J11X,J11X],[J11Y,J11Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J12X,J12X],[J12Y,J12Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J13X,J13X],[J13Y,J13Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J14X,J14X],[J14Y,J14Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J15X,J15X],[J15Y,J15Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
    
hold on; surface([J20X,J20X],[J20Y,J20Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J21X,J21X],[J21Y,J21Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J22X,J22X],[J22Y,J22Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J23X,J23X],[J23Y,J23Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J24X,J24X],[J24Y,J24Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
hold on; surface([J25X,J25X],[J25Y,J25Y],[z,z],[col,col],...
        'facecol','no',...
        'edgecol','interp');
    
% %% Speeds
% speeds=zeros(size(tip0X,1)-1,1);
% for t=1:size(tip0X,1)-1
%     speeds(t)=sqrt((tip0X(t+1)-tip0X(t)).^2+(tip0Y(t+1)-tip0Y(t)).^2);
% end
% figure; plot(1:size(speeds,1),speeds);

% %% Positions
% figure; plot(1:nframes,tip0X);
% figure; plot(1:nframes,tip0Y);
% 
% figure; plot(1:nframes,tip1X);
% figure; plot(1:nframes,tip1Y);
% 
% figure; plot(1:nframes,tip2X);
% figure; plot(1:nframes,tip2Y);
% 
% figure; plot(1:nframes,tip3X);
% figure; plot(1:nframes,tip3Y);
% 
% figure; plot(1:nframes,tip4X);
% figure; plot(1:nframes,tip4Y);
% 
% figure; plot(1:nframes,tip5X);
% figure; plot(1:nframes,tip5Y);

%% Video
vw = VideoWriter(strcat(filename,'_walking.avi'));
vw.Quality = 100;
vw.FrameRate = 8;
open(vw);

h=figure;
set(gcf, 'Color','white');
axis tight;
set(gca, 'Visible','off');
for t=1:nframes
    L0inds=intersect(find(~cellfun(@isempty,regexp(id,'L0\w*'))),find(frame==t+frame0-1));
    L1inds=intersect(find(~cellfun(@isempty,regexp(id,'L1\w*'))),find(frame==t+frame0-1));
    L2inds=intersect(find(~cellfun(@isempty,regexp(id,'L2\w*'))),find(frame==t+frame0-1));
    L3inds=intersect(find(~cellfun(@isempty,regexp(id,'L3\w*'))),find(frame==t+frame0-1));
    L4inds=intersect(find(~cellfun(@isempty,regexp(id,'L4\w*'))),find(frame==t+frame0-1));
    L5inds=intersect(find(~cellfun(@isempty,regexp(id,'L5\w*'))),find(frame==t+frame0-1));
   
    h0=line(newX(L0inds),newY(L0inds));
    h1=line(newX(L1inds),newY(L1inds));
    h2=line(newX(L2inds),newY(L2inds));
    h3=line(newX(L3inds),newY(L3inds));
    h4=line(newX(L4inds),newY(L4inds));
    h5=line(newX(L5inds),newY(L5inds));
    
    if t==1
        axis manual;
    end
    
    writeVideo(vw, getframe(gca));

    delete(h0);
    delete(h1);
    delete(h2);
    delete(h3);
    delete(h4);
    delete(h5);
end
close(gcf);
close(vw);
