%% Read c3d file using function

clear all;close all;clc

warning('on','all');

file_path = '/home/data/essua/17 Jun 2014/vicon/jorge almeida/jorge almeida/jorge almeida/jorge almeida02.c3d';
% file_path = '/home/data/essua/17 Jun 2014/vicon/jorge almeida/jorge almeida/jorge almeida/test.c3d';

disp('Loading c3d binary file ...')
[header, groups, data] = c3dread(file_path);
disp('loaded')

%Create variables for parameters
for g=1:length(groups)    
    for p=1:length(groups{g}.parameters)
        eval(sprintf('%s.%s = groups{%d}.parameters{%d}.data;',groups{g}.name,groups{g}.parameters{p}.name,g,p))
    end
end

clear g p;

%% Copy data to usefull variables

n_labels = POINT.USED;
labels = POINT.LABELS;
n_analog_channels = ANALOG.USED;
n_analog_samples_per_frame = header.number_analog_samples_per_frame;
n_frames = size(data,1);

data_backup = data;
time = data(:,1);
data(:,1) = [];
points = data(:,1:n_labels*4);
data(:,1:n_labels*4) = [];

%Convert to meters
if strcmp(POINT.UNITS,'mm')
    x = points(:,1:4:end)/1000;
    y = points(:,2:4:end)/1000;
    z = points(:,3:4:end)/1000;
    fl = points(:,4:4:end);
    
    %fl with -1 marks an invalid measurement, the rest of the values correspond to
    %measuremnt residual and camera coverage, check the C3D doc for more
    %information
    
    x(fl==-1)=NaN;
    y(fl==-1)=NaN;
    z(fl==-1)=NaN;
else
    warning(['Unexpected measurement units: ' POINT.UNITS])
end

%Check if there are any analog signals and copy to variables
%In one of the trials the analog data is present and has the following
%structure: 28 channels, 10 samples per frame
if n_analog_channels
    analog = data;
    analog_channels = zeros(n_analog_channels,n_frames,n_analog_samples_per_frame);
    analog_time = linspace(time(1),time(end),n_frames*n_analog_samples_per_frame);

    for c=1:n_analog_channels
        readings = analog(:,c:n_analog_channels:end);
        analog_channels(c,:,:) = (readings - ANALOG.OFFSET(c))*ANALOG.GAIN(c)*ANALOG.GEN_SCALE;
    end
end

trial_length = size(x,1);

data = data_backup;
clear data_backup

%% Create the basic labels and final connections

tags = {'t.C7'};
tags{end+1} = 't.L1';
tags{end+1} = 't.L2';
tags{end+1} = 't.L3';
tags{end+1} = 't.L4';
tags{end+1} = 't.L5';
tags{end+1} = 't.L6';
tags{end+1} = 't.L7';
tags{end+1} = 't.L8';
tags{end+1} = 't.L9';
tags{end+1} = 't.L10';
tags{end+1} = 't.L11';
tags{end+1} = 't.L12';
tags{end+1} = 't.L13';
tags{end+1} = 't.L14';
tags{end+1} = 't.L15';
tags{end+1} = 't.L16';
tags{end+1} = 't.L17';
tags{end+1} = 't.L18';
tags{end+1} = 't.L19';
tags{end+1} = 't.L20';
tags{end+1} = 't.L21';
tags{end+1} = 't.L22';
tags{end+1} = 't.L23';
tags{end+1} = 't.L24';
tags{end+1} = 't.L25';
tags{end+1} = 't.L26';
tags{end+1} = 't.L27';
tags{end+1} = 't.L26';
tags{end+1} = 't.L29';
tags{end+1} = 't.L30';
tags{end+1} = 't.L31';
tags{end+1} = 't.L32';
tags{end+1} = 't.L33';
tags{end+1} = 't.L34';
tags{end+1} = 't.L35';

%Create variables with the tags names
    
for i=1:length(tags)
    eval(sprintf('%s=[x(:,%d) y(:,%d) z(:,%d)];',tags{i},i,i,i))
    eval(sprintf('%s=%s'';',tags{i},tags{i}))
end

%Create future connections, labels must be switch for these connections to
%work
connections = {'t.rfhd','t.lfhd'};
connections = [connections; {'t.rsho','t.relb'}];
connections = [connections; {'t.relb','t.rwra'}];
connections = [connections; {'t.rwra','t.rfin'}];
connections = [connections; {'t.rasi','t.lasi'}];
connections = [connections; {'t.lsho','t.lelb'}];
connections = [connections; {'t.lelb','t.lwra'}];
connections = [connections; {'t.lwra','t.lfin'}];
connections = [connections; {'t.rasi','t.rkne'}];
connections = [connections; {'t.rasi','t.rknei'}];
connections = [connections; {'t.rkne','t.rknei'}];
connections = [connections; {'t.lasi','t.lknei'}];
connections = [connections; {'t.lasi','t.lkne'}];
connections = [connections; {'t.lkne','t.lknei'}];
connections = [connections; {'t.lwrb','t.lfin'}];
connections = [connections; {'t.lwra','t.lwrb'}];
connections = [connections; {'t.lelb','t.lwrb'}];
connections = [connections; {'t.rfhd','t.rbhd'}];
connections = [connections; {'t.lfhd','t.lbhd'}];
connections = [connections; {'t.rbhd','t.lbhd'}];
connections = [connections; {'t.ltoe','t.lanki'}];
connections = [connections; {'t.ltoe','t.lank'}];
connections = [connections; {'t.lanki','t.lank'}];
connections = [connections; {'t.rtoe','t.rank'}];
connections = [connections; {'t.rtoe','t.ranki'}];
connections = [connections; {'t.rank','t.ranki'}];
connections = [connections; {'t.lpsi','t.lasi'}];
connections = [connections; {'t.rsho','t.C7'}];
connections = [connections; {'t.C7','t.lsho'}];
connections = [connections; {'t.rpsi','t.lpsi'}];
connections = [connections; {'t.rpsi','t.rasi'}];
connections = [connections; {'t.rsho','t.rasi'}];
connections = [connections; {'t.rsho','t.rpsi'}];
connections = [connections; {'t.lsho','t.lpsi'}];
connections = [connections; {'t.lsho','t.lasi'}];
connections = [connections; {'t.rwrb','t.rwra'}];
connections = [connections; {'t.rwrb','t.rfin'}];
connections = [connections; {'t.relb','t.rwrb'}];
connections = [connections; {'t.rank','t.rhee'}];
connections = [connections; {'t.rhee','t.ranki'}];
connections = [connections; {'t.lanki','t.lhee'}];
connections = [connections; {'t.lhee','t.lank'}];
connections = [connections; {'t.rkne','t.rank'}];
connections = [connections; {'t.rknei','t.ranki'}];
connections = [connections; {'t.lkne','t.lank'}];
connections = [connections; {'t.lknei','t.lanki'}];
connections = [connections; {'t.lpsi','t.lkne'}];
connections = [connections; {'t.lpsi','t.lknei'}];
connections = [connections; {'t.rpsi','t.rkne'}];
connections = [connections; {'t.rpsi','t.rknei'}];

%% Save data to be processed by the labeling function

data_file = 'original_data.mat';
fprintf('Saving data to: %s\n',data_file)
save(data_file,'tags','t','connections')
disp('saved')
