%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright 2014 National Renewable Energy Laboratory and National
% Technology & Engineering Solutions of Sandia, LLC (NTESS).
% Under the terms of Contract DE-NA0003525 with NTESS,
% the U.S. Government retains certain rights in this software.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
% http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

classdef cableClass<handle
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % The ``cableClass`` creates a ``cable`` object saved to the MATLAB
    % workspace. The ``cableClass`` includes properties and methods used
    % to define cable connections relative to other bodies.
    % It is suggested that the ``cableClass`` be used for connections between
    % joints or ptos.
    %
    %.. autoattribute:: objects.cableClass.cableClass
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (SetAccess = 'public', GetAccess = 'public') %input file
        damping                 = 0                                         % (`float`) Cable damping coefficient (N/(m/s)). Default = `0`.
        inertia                 = [1 1 1];                                  % (`1 x 3 float vector`) body inertia kg-m^2, default [1 1 1]
        initial                 = struct(...                                % (`structure`) Defines the initial displacement of the body.
            'displacement',         [0 0 0],...                             %
            'axis',                 [0 1 0], ...                            %
            'angle',                0)                                      % (`structure`) Defines the initial displacement of the body. ``displacement`` (`3x1 float vector`) is defined as the initial displacement of the body center of gravity (COG) [m] in the following format [x y z], Default = [``0 0 0``]. ``axis`` (`3x1 float vector`) is defined as the axis of rotation in the following format [x y z], Default = [``0 1 0``]. ``angle`` (`float`) is defined as the initial angular displacement of the body COG [rad], Default = ``0``.
        length                  = 0                                         % (`float`) Cable equilibrium length (m), calculated from rotloc and preTension. Default =`0`.
        linearDamping           = [0 0 0 0 0 0];                            % (`1 x 6 float vector`)linear damping aplied to body motions
        mass                    = 1;                                        % (`float`) mass in kg, default 1
        name                    = 'NOT DEFINED'                             % (`string`) Defines the Cable name. Default = ``NOT DEFINED``.
        orientation             = struct(...                                % (`structure`) Defines the orientation axis of the pto.
            'z',                    [0, 0, 1], ...                          %
            'y',                    [0, 1, 0], ...                          %
            'x',                    [], ...                                 %
            'rotationMatrix',       [])                                     % (`structure`) Defines the orientation axis of the pto. ``z`` (`3x1 float vector`) defines the direciton of the Z-coordinate of the pto, Default = [``0 0 1``]. ``y`` (`3x1 float vector`) defines the direciton of the Y-coordinate of the pto, Default = [``0 1 0``]. ``x`` (`3x1 float vector`) internally calculated vector defining the direction of the X-coordinate for the pto, Default = ``[]``. ``rotationMatrix`` (`3x3 float matrix`) internally calculated rotation matrix to go from standard coordinate orientation to the pto coordinate orientation, Default = ``[]``.
        paraview                = 1;                                        % (`integer`) Flag for visualisation in Paraview either 0 (no) or 1 (yes). Default = ``1`` since only called in paraview.
        preTension              = 0                                         % (`float`) Cable pretension (N).
        quadDrag                = struct(...                                % (`structure`) Defines the viscous quadratic drag forces.
            'area',                 [0 0 0 0 0 0], ...                      %
            'drag',                 zeros(6), ...                           %
            'cd',               [0 0 0 0 0 0]);                             % (`structure`) Defines the viscous quadratic drag forces. First option define ``drag``, (`6x6 float matrix`), Default = ``zeros(6)``. Second option define ``cd``, (`6x1 float vector`), Default = ``zeros(6,1)``, and ``area``, (`6x1 float vector`), Default = ``zeros(6,1)``.
        stiffness               = 0                                         % (`float`) Cable stiffness (N/m). Default = `0`.
        viz                     = struct(...                                % (`structure`) Defines visualization properties in either SimScape or Paraview.
            'color',                [1 0 0.5], ...                          %
            'opacity',              1)                                      % (`structure`) Defines visualization properties in either SimScape or Paraview. ``color`` (`3x1 float vector`) is defined as the body visualization color, Default = [``1 1 0``]. ``opacity`` (`integer`) is defined as the body opacity, Default = ``1``.
    end

    properties (SetAccess = 'public', GetAccess = 'public')%internal
        base                    = struct(...                                % (`structure`) Defines the base connection. `centerBuoyancy` (`1 x 3 float vector`) center of buoyancy location of the base drag body, Default = `[0 0 0]`. `centerGravity` (`1 x 3 float vector`) center of gravity location of the base drag body, Default = `[0 0 0]`. `location` (`3x1 float vector`) base location [m], Defined in the following format [x y z], Default = ``[]``. `name` (`string`) name of the base constraint or PTO, Default = `'NOT DEFINED'`;
            'centerBuoyancy',   	[0 0 0], ...                            %
            'centerGravity',        [0 0 0], ...                            %
            'location',             [], ...                                 %
            'name',                 'NOT DEFINED')                          % (`structure`) Defines the base connection. `centerBuoyancy` (`1 x 3 float vector`) center of buoyancy location of the base drag body, Default = `[0 0 0]`. `centerGravity` (`1 x 3 float vector`) center of gravity location of the base drag body, Default = `[0 0 0]`. `location` (`3x1 float vector`) base location [m], Defined in the following format [x y z], Default = ``[]``. `name` (`string`) name of the base constraint or PTO, Default = `'NOT DEFINED'`;
        follower                  = struct(...                              % (`structure`) Defines the follower connection. `centerBuoyancy` (`1 x 3 float vector`) center of buoyancy location of the base drag body, Default = `[0 0 0]`. `centerGravity` (`1 x 3 float vector`) center of gravity location of the base drag body, Default = `[0 0 0]`. `location` (`3x1 float vector`) base location [m], Defined in the following format [x y z], Default = ``[]``. `name` (`string`) name of the base constraint or PTO, Default = `'NOT DEFINED'`;
            'centerBuoyancy',   	[0 0 0], ...                            %
            'centerGravity',        [0 0 0], ...                            %
            'location',             [], ...                                 %
            'name',                 'NOT DEFINED')                          % (`structure`) Defines the follower connection. `centerBuoyancy` (`1 x 3 float vector`) center of buoyancy location of the base drag body, Default = `[0 0 0]`. `centerGravity` (`1 x 3 float vector`) center of gravity location of the base drag body, Default = `[0 0 0]`. `location` (`3x1 float vector`) base location [m], Defined in the following format [x y z], Default = ``[]``. `name` (`string`) name of the base constraint or PTO, Default = `'NOT DEFINED'`;
        location                = [999 999 999]                             % (`3x1 float vector`) pto location [m]. Defined in the following format [x y z]. Default = ``[999 999 999]``.
        volume                  = [];                                       % (`float`) displacement volume, defaults to neutral buoyancy
    end

    properties (SetAccess = 'private', GetAccess = 'public') %internal
        number                  = []                                        % Cable number.
    end

    %%
    methods

        function obj = cableClass(name,baseName,followerName)
            % This method initilizes the ``cableClass`` and creates a
            % ``cable`` object.
            %
            % Parameters
            % ------------
            %     name : string
            %         String specifying the name of the cable
            %
            %     baseConnection : string
            %         Variable for the base constraint/pto as a string
            %
            %     followerConnection : string
            %         Variable for the follower constraint/pto as a string
            %
            % Returns
            % ------------
            %     cable : obj
            %         cableClass object
            %
            if exist('name','var')
                obj.name = name;
                obj.base.name = baseName;
                obj.follower.name= followerName;
                obj.base.location = evalin('caller',[baseName '.location']);
                obj.follower.location = evalin('caller',[followerName '.location']);
            else
                error('The cable class number(s) in the wecSimInputFile must be specified in ascending order starting from 1. The cableClass() function should be called first to initialize each cable with a name.')
            end
        end

        function setTransPTOLoc(obj)
            % This method specifies the translational PTO location as half-
            % way between the fixed ends of the cable if not previously
            % set.
            if any(obj.location == [999 999 999])
                rotDiff = obj.base.location - obj.follower.location;
                obj.location = obj.follower.location + rotDiff/2;
                fprintf('\n\t location undefined, set halfway between follower.location and base.location \n')
            end
        end

        function obj = checkLoc(obj,action)
            % This method checks WEC-Sim user inputs and generate an error
            % message if the constraint location is not defined in constraintClass.
            % Checks if location is set and outputs a warning or error. Used in mask Initialization.
            switch action
                case 'W'
                    if obj.location == 999 % Because "Allow library block to modify its content" is selected in block's mask initialization, this command runs twice, but warnings cannot be displayed during the first initialization.
                        obj.location = [888 888 888];
                    elseif obj.location == 888
                        obj.location = [0 0 0];
                    end
                case 'E'
                    try
                        if obj.location == 999
                            s1 = ['For ' obj.name ': pto(#).location needs to be specified in the WEC-Sim input file.'...
                                ' pto.location is the [x y z] location, in meters, for the rotational PTO.'];
                            error(s1)
                        end
                    catch exception
                        throwAsCaller(exception)
                    end
            end
        end

        function obj = setOrientation(obj)
            % This method calculates the constraint ``x`` vector and
            % ``rotationMatrix`` matrix in the ``orientation`` structure
            % based on user input.
            obj.orientation.z = obj.orientation.z / norm(obj.orientation.z);
            obj.orientation.y = obj.orientation.y / norm(obj.orientation.y);
            z = obj.orientation.z;
            y = obj.orientation.y;
            if abs(dot(y,z))>0.001
                error('The Y and Z vectors defining the constraint''s orientation must be orthogonal.')
            end
            x = cross(y,z)/norm(cross(y,z));
            x = x(:)';
            obj.orientation.x = x;
            obj.orientation.rotationMatrix  = [x',y',z'];

        end

        function setInitDisp(obj, relCoord, axisAngleList, addLinDisp)
            % Function to set a body's initial displacement
            %
            % This function assumes that all rotations are about the same relative coordinate.
            % If not, the user should input a relative coordinate of 0,0,0 and
            % use the additional linear displacement parameter to set the cg or location
            % correctly.
            %
            % Parameters
            % ------------
            %    relCoord : [1 3] float vector
            %        Distance from x_rot to the body center of gravity or the constraint
            %        or pto location as defined by: relCoord = cg - x_rot. [m]
            %
            %    axisAngleList : [nAngle 4] float vector
            %        List of axes and angles of the rotations with the
            %        format: [n_x n_y n_z angle] (angle in rad)
            %        Rotations applied consecutively in order of dimension 1
            %
            %    addLinDisp : [1 3] float vector
            %        Initial linear displacement (in addition to the
            %        displacement caused by rotation) [m]
            %

            % initialize quantities before for loop
            axisList = axisAngleList(:,1:3);
            angleList = axisAngleList(:,4);
            nAngle = size(axisList,1);
            rotMat = eye(3);

            % Loop through all axes and angles.
            for i=1:nAngle
                rotMat = axisAngle2RotMat(axisList(i,:),angleList(i))*rotMat;
            end
            % calculate net axis-angle rotation
            [netAxis, netAngle] = rotMat2AxisAngle(rotMat);
            % calculate net displacement due to rotation
            rotatedRelCoord = relCoord*(rotMat');
            linDisp = rotatedRelCoord - relCoord;
            % apply rotation and displacement to object
            obj.initial.displacement = linDisp + addLinDisp;
            obj.initial.axis = netAxis;
            obj.initial.angle = netAngle;
        end

        function setVolume(obj, rho)
            % This method mades the mass of the cable drag bodies neutrally bouyant
            obj.volume = obj.mass/rho;
        end

        function dragForcePre(obj,rho)
            % This method performs Drag Body pre-processing calculations,
            % similar to hydroForcePre, but only loads in the necessary
            % values to calculate linear damping and viscous drag. Note
            % that body DOF is inherited from the length of the drag
            % coefficients.
            if  any(any(obj.quadDrag.drag)) ~= 1  %check if obj.quadDrag.drag is not defined
                obj.quadDrag.drag = diag(0.5*rho.*obj.quadDrag.cd.*obj.quadDrag.area);
            end

        end

        function linearDampingMatrix(obj)
            % This method makes the linear damping vector (as specified)
            % into a 6 x 6 matrix with this damping along the diagonal (as
            % required for calculation). Operates on the drag bodies
            % representing the cable dynamics.
            obj.linearDamping = diag(obj.linearDamping);
        end

        function setCb(obj)
            % This method sets the buoyancy center to equal the center of
            % gravity, if the center of buoyancy is not defined
            obj.base.centerBuoyancy = obj.base.centerGravity;
            obj.follower.centerBuoyancy = obj.follower.centerGravity;
        end

        function setCg(obj)
            % This method specifies the Cg of the drag bodies as coincident
            % with the fixed ends of the cable, if not otherwise specied.
            obj.base.centerGravity = obj.base.location;
            obj.follower.centerGravity = obj.follower.location;
        end

        function setLength(obj)
            % This method specifies length as the distance between cable fixed
            % ends (i.e. pretension = 0), if not otherwise specified.
            if ~any(obj.length) && ~any(obj.preTension)
                obj.length = sqrt((obj.base.location(1)-obj.follower.location(1)).^2 ...
                + (obj.base.location(2)-obj.follower.location(2)).^2 ...
                + (obj.base.location(3)-obj.follower.location(3)).^2);
                fprintf('\n\t cable(i).length undefined and cable(i).preTension undefined. \n \r',...
                    'cable(i).length set equal to distance between follower.location and base.location \n and cable(i).preTension set equal to zero \n');
            elseif ~any(obj.length) && any(obj.preTension)
                obj.length = sqrt((obj.base.location(1)-obj.follower.location(1)).^2 ...
                + (obj.base.location(2)-obj.follower.location(2)).^2 ...
                + (obj.base.location(3)-obj.follower.location(3)).^2) + obj.preTension/obj.stiffness;
            elseif ~any(obj.preTension) && any(obj.length)
                obj.preTension = obj.stiffness * (sqrt((obj.base.location(1)-obj.follower.location(1)).^2 ...
                + (obj.base.location(2)-obj.follower.location(2)).^2 ...
                + (obj.base.location(3)-obj.follower.location(3)).^2) - obj.length);
            elseif any(obj.preTension) && any(obj.length)
                error('System overdefined. Please define cable(i).preTension OR cable(i).length, not both.')
            end
        end

        function listInfo(obj)
            % This method prints cable information to the MATLAB Command Window.
            fprintf('\n\t***** Cable Name: %s *****\n',obj.name)
            fprintf('\tCable Stiffness           (N/m;Nm/rad) = %G\n',obj.stiffness)
            fprintf('\tCable Damping           (Ns/m;Nsm/rad) = %G\n',obj.damping)
        end

        function setNumber(obj,number)
            % Method to set the private number property
            obj.number = number;
        end
    end

end


function nlp = ZMPCost(nlp, varargin)

    w = varargin{1};

    plant = nlp.Plant;
    force = plant.Inputs.ConstraintWrench.ffoot;

    ws = SymVariable('w');

    zmp = -force(3)./force(2);
    constr = ws*(zmp+0.1).^2;
    m_fun = SymFunction(['zmp_cost_' plant.Name],tovector(constr),{force},{ws});
    addRunningCost(nlp,m_fun,{'ffoot'},{w});
end


function [hCBodySurf, hCBodySurfXForm] = ma_initOrbPlot(hFig, orbitDispAxes, bodyInfo)
    %ma_initOrbPlot Summary of this function goes here
    %   Detailed explanation goes here

    hold(orbitDispAxes,'on');
    set(orbitDispAxes,'XTickLabel',[]);
    set(orbitDispAxes,'YTickLabel',[]);
    set(orbitDispAxes,'ZTickLabel',[]);
    grid(orbitDispAxes,'on');

    axis(orbitDispAxes, 'equal');

    axis(orbitDispAxes, 'tight');

    if(~isempty(bodyInfo))
        dRad = bodyInfo.radius;
        [X,Y,Z] = sphere(50);

        if(not(isempty(bodyInfo.surftexturefile)))
            try
                I = bodyInfo.getSurfaceTexture();
                if(not(isempty(I)) && not(any(any(any(isnan(I))))))
                    hCBodySurf = surf(orbitDispAxes, dRad*X,dRad*Y,dRad*Z, 'CData',I, 'FaceColor','texturemap', 'BackFaceLighting','lit', 'FaceLighting','gouraud', 'EdgeLighting','gouraud', 'LineStyle','none');
                else
                    hCBodySurf = createUntexturedSphere(bodyInfo, orbitDispAxes, dRad, X, Y, Z);
                end

            catch ME
                if(exist('hCBodySurf','var'))
                    delete(hCBodySurf); %#ok<NODEF>
                    warning('Could not create textured celestial body "%s".  Error message: \n\n%s', bodyInfo.name, ME.message);
                end

                hCBodySurf = createUntexturedSphere(bodyInfo, orbitDispAxes, dRad, X, Y, Z);
            end

        else
            hCBodySurf = createUntexturedSphere(bodyInfo, orbitDispAxes, dRad, X, Y, Z);
        end

        hCBodySurfXForm = hgtransform('Parent', orbitDispAxes);
        set(hCBodySurf,'Parent',hCBodySurfXForm);
        material(hCBodySurf,'dull');
    else
        hCBodySurf = [];
    end

    mColor = colorFromColorMap(bodyInfo.bodycolor);
    plot3(orbitDispAxes, 0, 0, 0,'Marker','o','MarkerEdgeColor',mColor,'MarkerFaceColor',mColor,'MarkerSize',3);

    hold(orbitDispAxes, 'off');
end


%% AI Clinician Identifiying MIMIC-III sepsis cohort

% (c) Matthieu Komorowski, Imperial College London 2015-2019
% as seen in publication: https://www.nature.com/articles/s41591-018-0213-5

% version 16 Feb 19
% IDENTIFIES THE COHORT OF PATIENTS WITH SEPSIS in MIMIC-III

% PURPOSE:
% ------------------------------
% This creates a list of icustayIDs of patients who develop sepsis at some point
% in the ICU. records charttime for onset of sepsis. Uses sepsis3 criteria

% STEPS:
% -------------------------------
% IMPORT DATA FROM CSV FILES
% FLAG PRESUMED INFECTION
% PREPROCESSING
% REFORMAT in 4h time slots
% COMPUTE SOFA at each time step
% FLAG SEPSIS

% note: the process generates the same features as the final MDP dataset, most of which are not used to compute SOFA
% External files required: Reflabs, Refvitals, sample_and_hold (all saved in reference_matrices.mat file)

% This code is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE

%% ########################################################################
% IMPORT ALL DATA

tic
abx=table2array(readtable('D:/exportdir/abx.csv'));
culture=table2array(readtable('D:/exportdir/culture.csv'));
microbio=table2array(readtable('D:/exportdir/microbio.csv'));
demog=(readtable('D:/exportdir/demog.csv'));
ce010=table2array(readtable('D:/exportdir/ce010000.csv'));
ce1020=table2array(readtable('D:/exportdir/ce1000020000.csv'));
ce2030=table2array(readtable('D:/exportdir/ce2000030000.csv'));
ce3040=table2array(readtable('D:/exportdir/ce3000040000.csv'));
ce4050=table2array(readtable('D:/exportdir/ce4000050000.csv'));
ce5060=table2array(readtable('D:/exportdir/ce5000060000.csv'));
ce6070=table2array(readtable('D:/exportdir/ce6000070000.csv'));
ce7080=table2array(readtable('D:/exportdir/ce7000080000.csv'));
ce8090=table2array(readtable('D:/exportdir/ce8000090000.csv'));
ce90100=table2array(readtable('D:/exportdir/ce90000100000.csv'));
labU=[ table2array(readtable('D:/exportdir/labs_ce.csv')) ; table2array(readtable('D:/exportdir/labs_le.csv'))  ];
MV=table2array(readtable('D:/exportdir/mechvent.csv'));
inputpreadm=table2array(readtable('D:/exportdir/preadm_fluid.csv'));
inputMV=table2array(readtable('D:/exportdir/fluid_mv.csv'));
inputCV=table2array(readtable('D:/exportdir/fluid_cv.csv'));
vasoMV=table2array(readtable('D:/exportdir/vaso_mv.csv'));
vasoCV=table2array(readtable('D:/exportdir/vaso_cv.csv'));
UOpreadm=table2array(readtable('D:/exportdir/preadm_uo.csv'));
UO=table2array(readtable('D:/exportdir/uo.csv'));
toc

%% ########################################################################
%                       INITIAL DATA MANIPULATIONS
% #########################################################################

ii=isnan(microbio(:,3));  %if charttime is empty but chartdate isn't
microbio(ii,3)=microbio(ii,4);   %copy time
microbio( :,4)=[];    %delete chardate
% Add empty col in microbio (# 3 and #5)
microbio(:,4)=microbio(:,3);
microbio(:,[3 5])=0;
% Combine both tables for micro events
bacterio = [microbio ; culture];
% correct NaNs in DEMOG
demog.morta_90(isnan(demog.morta_90))=0;
demog.morta_hosp(isnan(demog.morta_hosp))=0;
demog.elixhauser(isnan(demog.elixhauser))=0;

% compute normalized rate of infusion
% if we give 100 ml of hypertonic fluid (600 mosm/l) at 100 ml/h (given in 1h) it is 200 ml of NS equivalent
% so the normalized rate of infusion is 200 ml/h (different volume in same duration)
inputMV(:,8)=inputMV(:,7).*inputMV(:,6)./inputMV(:,5);

% fill-in missing ICUSTAY IDs in bacterio
for i=1:size(bacterio,1)
if bacterio(i,3)==0   %if missing icustayid
    o=bacterio(i,4);  %charttime
    subjectid=bacterio(i,1);
    hadmid=bacterio(i,2);
   ii=find(demog.subject_id==subjectid);
   jj=find(demog.subject_id==subjectid & demog.hadm_id==hadmid);
    for j=1:numel(ii)
        if o>=demog.intime(ii(j))-48*3600 & o<=demog.outtime(ii(j))+48*3600
            bacterio(i,3)=demog.icustay_id(ii(j));
        elseif numel(ii)==1   %if we cant confirm from admission and discharge time but there is only 1 admission: it's the one!!
            bacterio(i,3)=demog.icustay_id(ii(j));
        end
    end
end
end
toc

for i=1:size(bacterio,1)
if bacterio(i,3)==0   %if missing icustayid
    subjectid=bacterio(i,1);
    hadmid=bacterio(i,2);
    jj=find(demog.subject_id==subjectid & demog.hadm_id==hadmid);
    if numel(jj)==1
        bacterio(i,3)=demog.icustay_id(jj);
    end
end
end

% fill-in missing ICUSTAY IDs in ABx

for i=1:size(abx,1)
if isnan(abx(i,2))
    o=abx(i,3);  %time of event
    hadmid=abx(i,1);
    ii=find(demog.hadm_id==hadmid);   %row in table demographics
    for j=1:numel(ii)
        if o>=demog.intime(ii(j))-48*3600 & o<=demog.outtime(ii(j))+48*3600
            abx(i,2)=demog.icustay_id(ii(j));
        elseif numel(ii)==1   %if we cant confirm from admission and discharge time but there is only 1 admission: it's the one!!
            abx(i,2)=demog.icustay_id(ii(j));
        end
    end
end
end

%% ########################################################################
%    find presumed onset of infection according to sepsis3 guidelines
% ########################################################################

% METHOD:
% I loop through all the ABx given, and as soon as there is a sample present
% within the required time criteria I pick this flag and break the loop.

onset=zeros(100000,3);

for icustayid=1:100000

    ab=abx(abx(:,2)==icustayid+200000,3);   %start time of abx for this icustayid
    bact=bacterio(bacterio(:,3)==icustayid+200000,4);  %time of sample
    subj_bact=bacterio(bacterio(:,3)==icustayid+200000,1);  %subjectid

    if ~isempty(ab) & ~isempty(bact)   %if we have data for both: proceed

      D = pdist2(ab, bact)/3600;  %pairwise distances btw antibio and cultures, in hours

      for i=1:size(D,1)  % looping through all rows of AB given, from early to late
        [M,I] = min(D(i,:));   %minimum distance in this row
        ab1=ab(i);       %timestamp of this value in list of antibio
        bact1=bact(I);      %timestamp in list of cultures

        if M<=24 & ab1<=bact1      %if ab was first and delay < 24h
            onset(icustayid,1)=subj_bact(1);   %subject_id
            onset(icustayid,2)=icustayid;       % icustay_id
            onset(icustayid,3)=ab1;     %onset of infection = abx time
              icustayid
            break
        elseif M<=72 & ab1>=bact1    %elseif sample was first and delay < 72h
            onset(icustayid,1)=subj_bact(1);
            onset(icustayid,2)=icustayid;
            onset(icustayid,3)=bact1;       %onset of infection = sample time
            break
        end
      end
    end
end
toc

%sum of records found
sum(onset(:,3)>0)

%% Replacing item_ids with column numbers from reference tables

% replace itemid in labs with column number
% this will accelerate process later

tic
for i=1:size(labU,1)
[~,locb]=ismember(Reflabs,labU(i,3));
labU(i,3)=find(max(locb')');
end
toc

% replace itemid in vitals with col number

for i=1:size(ce010,1)
[~,locb]=ismember(Refvitals,ce010(i,3));ce010(i,3)=find(max(locb')');
end
for i=1:size(ce1020,1)
[~,locb]=ismember(Refvitals,ce1020(i,3));ce1020(i,3)=find(max(locb')');
end
for i=1:size(ce2030,1)
[~,locb]=ismember(Refvitals,ce2030(i,3));ce2030(i,3)=find(max(locb')');
end
for i=1:size(ce3040,1)
[~,locb]=ismember(Refvitals,ce3040(i,3));ce3040(i,3)=find(max(locb')');
end
for i=1:size(ce4050,1)
[~,locb]=ismember(Refvitals,ce4050(i,3));ce4050(i,3)=find(max(locb')');
end
for i=1:size(ce5060,1)
[~,locb]=ismember(Refvitals,ce5060(i,3));ce5060(i,3)=find(max(locb')');
end
for i=1:size(ce6070,1)
[~,locb]=ismember(Refvitals,ce6070(i,3));ce6070(i,3)=find(max(locb')');
end
for i=1:size(ce7080,1)
[~,locb]=ismember(Refvitals,ce7080(i,3));ce7080(i,3)=find(max(locb')');
end
for i=1:size(ce8090,1)
[~,locb]=ismember(Refvitals,ce8090(i,3));ce8090(i,3)=find(max(locb')');
end
for i=1:size(ce90100,1)
[~,locb]=ismember(Refvitals,ce90100(i,3));ce90100(i,3)=find(max(locb')');
end

%% ########################################################################
%           INITIAL REFORMAT WITH CHARTEVENTS, LABS AND MECHVENT
% ########################################################################

% gives an array with all unique charttime (1 per row) and all items in columns.
% ################## IMPORTANT !!!!!!!!!!!!!!!!!!
% Here i use -48 -> +24 because that's for sepsis3 cohort defintion!!
% I need different time period for the MDP (-24 -> +48)

reformat=NaN(2000000,68);  %final table
qstime=zeros(100000,4);
winb4=49;   %lower limit for inclusion of data (48h before time flag)
winaft=25;  % upper limit (24h after)
irow=1;  %recording row for summary table
h = waitbar(0,'Initializing waitbar...');

tic
for icustayid=1:100000
qst=onset(icustayid,3); %flag for presumed infection
if qst>0  % if we have a flag
d1=table2array(demog(demog.icustay_id==icustayid+200000,[11 5])); %age of patient + discharge time

if d1(1)>6574  % if older than 18 years old

    waitbar(icustayid/100000,h,icustayid/1000) %moved here to save some time
% CHARTEVENTS
    if icustayid<10000
    temp=ce010(ce010(:,1)==icustayid+200000,:);
    elseif icustayid>=10000 & icustayid<20000
    temp=ce1020(ce1020(:,1)==icustayid+200000,:);
    elseif icustayid>=20000 & icustayid<30000
    temp=ce2030(ce2030(:,1)==icustayid+200000,:);
    elseif icustayid>=30000 && icustayid<40000
    temp=ce3040(ce3040(:,1)==icustayid+200000,:);
    elseif icustayid>=40000 & icustayid<50000
    temp=ce4050(ce4050(:,1)==icustayid+200000,:);
    elseif icustayid>=50000 & icustayid<60000
    temp=ce5060(ce5060(:,1)==icustayid+200000,:);
    elseif icustayid>=60000 & icustayid<70000
    temp=ce6070(ce6070(:,1)==icustayid+200000,:);
    elseif icustayid>=70000 & icustayid<80000
    temp=ce7080(ce7080(:,1)==icustayid+200000,:);
    elseif icustayid>=80000 & icustayid<90000
    temp=ce8090(ce8090(:,1)==icustayid+200000,:);
    elseif icustayid>=90000
    temp=ce90100(ce90100(:,1)==icustayid+200000,:);
    end

ii=temp(:,2)>= qst-(winb4+4)*3600 & temp(:,2)<=qst+(winaft+4)*3600; %time period of interest -4h and +4h
temp=temp(ii,:);   %only time period of interest

%LABEVENTS
ii=labU(:,1)==icustayid+200000;
temp2=labU(ii,:);
ii=temp2(:,2)>= qst-(winb4+4)*3600 & temp2(:,2)<=qst+(winaft+4)*3600; %time period of interest -4h and +4h
temp2=temp2(ii,:);   %only time period of interest

%Mech Vent + ?extubated
ii=MV(:,1)==icustayid+200000;
temp3=MV(ii,:);
ii=temp3(:,2)>= qst-(winb4+4)*3600 & temp3(:,2)<=qst+(winaft+4)*3600; %time period of interest -4h and +4h
temp3=temp3(ii,:);   %only time period of interest

t=unique([temp(:,2);temp2(:,2); temp3(:,2)]);   %list of unique timestamps from all 3 sources / sorted in ascending order

if t
for i=1:numel(t)

    %CHARTEVENTS
    ii=temp(:,2)==t(i);
    col=temp(ii,3);
    value=temp(ii,4);
    reformat(irow,1)=i; %timestep
    reformat(irow,2)=icustayid;
    reformat(irow,3)=t(i); %charttime
    reformat(irow,3+col)=value;%(locb(:,1)); %store available values

    %LAB VALUES
    ii=temp2(:,2)==t(i);
    col=temp2(ii,3);
    value=temp2(ii,4);
    reformat(irow,31+col)=value; %store available values

    %MV
    ii=temp3(:,2)==t(i);
    if nansum(ii)>0
    value=temp3(ii,3:4);
      reformat(irow,67:68)=value; %store available values
    else
      reformat(irow,67:68)=NaN;
    end

    irow=irow+1;

end

qstime(icustayid,1)=qst; %flag for presumed infection / this is time of sepsis if SOFA >=2 for this patient
%HERE I SAVE FIRST and LAST TIMESTAMPS, in QSTIME, for each ICUSTAYID
qstime(icustayid,2)=t(1);  %first timestamp
qstime(icustayid,3)=t(end);  %last timestamp
qstime(icustayid,4)=d1(2); %dischargetime

end
end
end
end
toc

close(h);
reformat(irow:end,:)=[];  %delete extra unused rows

%% ########################################################################
%                                   OUTLIERS
% ########################################################################

%weight
reformat=deloutabove(reformat,5,300);  %delete outlier above a threshold (300 kg), for variable # 5

%HR
reformat=deloutabove(reformat,8,250);

%BP
reformat=deloutabove(reformat,9,300);
reformat=deloutbelow(reformat,10,0);
reformat=deloutabove(reformat,10,200);
reformat=deloutbelow(reformat,11,0);
reformat=deloutabove(reformat,11,200);

%RR
reformat=deloutabove(reformat,12,80);

%SpO2
reformat=deloutabove(reformat,13,150);
ii=reformat(:,13)>100;reformat(ii,13)=100;

%temp
ii=reformat(:,14)>90 & isnan(reformat(:,15));reformat(ii,15)=reformat(ii,14);
reformat=deloutabove(reformat,14,90);

%interface / is in col 22

% FiO2
reformat=deloutabove(reformat,23,100);
ii=reformat(:,23)<1;reformat(ii,23)=reformat(ii,23)*100;
reformat=deloutbelow(reformat,23,20);
reformat=deloutabove(reformat,24,1.5);

% O2 FLOW
reformat=deloutabove(reformat,25,70);

%PEEP
reformat=deloutbelow(reformat,26,0);
reformat=deloutabove(reformat,26,40);

%TV
reformat=deloutabove(reformat,27,1800);

%MV
reformat=deloutabove(reformat,28,50);

%K+
reformat=deloutbelow(reformat,32,1);
reformat=deloutabove(reformat,32,15);

%Na
reformat=deloutbelow(reformat,33,95);
reformat=deloutabove(reformat,33,178);

%Cl
reformat=deloutbelow(reformat,34,70);
reformat=deloutabove(reformat,34,150);

%Glc
reformat=deloutbelow(reformat,35,1);
reformat=deloutabove(reformat,35,1000);

%Creat
reformat=deloutabove(reformat,37,150);

%Mg
reformat=deloutabove(reformat,38,10);

%Ca
reformat=deloutabove(reformat,39,20);

%ionized Ca
reformat=deloutabove(reformat,40,5);

%CO2
reformat=deloutabove(reformat,41,120);

%SGPT/SGOT
reformat=deloutabove(reformat,42,10000);
reformat=deloutabove(reformat,43,10000);

%Hb/Ht
reformat=deloutabove(reformat,50,20);
reformat=deloutabove(reformat,51,65);

%WBC
reformat=deloutabove(reformat,53,500);

%plt
reformat=deloutabove(reformat,54,2000);

%INR
reformat=deloutabove(reformat,58,20);

%pH
reformat=deloutbelow(reformat,59,6.7);
reformat=deloutabove(reformat,59,8);

%po2
reformat=deloutabove(reformat,60,700);

%pco2
reformat=deloutabove(reformat,61,200);

%BE
reformat=deloutbelow(reformat,62,-50);

%lactate
reformat=deloutabove(reformat,63,30);

% ####################################################################
% some more data manip / imputation from existing values

% estimate GCS from RASS - data from Wesley JAMA 2003
ii=isnan(reformat(:,6))&reformat(:,7)>=0;
reformat(ii,6)=15;
ii=isnan(reformat(:,6))&reformat(:,7)==-1;
reformat(ii,6)=14;
ii=isnan(reformat(:,6))&reformat(:,7)==-2;
reformat(ii,6)=12;
ii=isnan(reformat(:,6))&reformat(:,7)==-3;
reformat(ii,6)=11;
ii=isnan(reformat(:,6))&reformat(:,7)==-4;
reformat(ii,6)=6;
ii=isnan(reformat(:,6))&reformat(:,7)==-5;
reformat(ii,6)=3;

% FiO2
ii=~isnan(reformat(:,23)) & isnan(reformat(:,24));
reformat(ii,24)=reformat(ii,23)./100;
ii=~isnan(reformat(:,24)) & isnan(reformat(:,23));
reformat(ii,23)=reformat(ii,24).*100;

%ESTIMATE FiO2 /// with use of interface / device (cannula, mask, ventilator....)

reformatsah=SAH(reformat,sample_and_hold);  % do SAH first to handle this task

%NO FiO2, YES O2 flow, no interface OR cannula
ii=find(isnan(reformatsah(:,23))&~isnan(reformatsah(:,25))&(reformatsah(:,22)==0|reformatsah(:,22)==2));
reformat(ii(reformatsah(ii,25)<=15),23)=70;
reformat(ii(reformatsah(ii,25)<=12),23)=62;
reformat(ii(reformatsah(ii,25)<=10),23)=55;
reformat(ii(reformatsah(ii,25)<=8),23)=50;
reformat(ii(reformatsah(ii,25)<=6),23)=44;
reformat(ii(reformatsah(ii,25)<=5),23)=40;
reformat(ii(reformatsah(ii,25)<=4),23)=36;
reformat(ii(reformatsah(ii,25)<=3),23)=32;
reformat(ii(reformatsah(ii,25)<=2),23)=28;
reformat(ii(reformatsah(ii,25)<=1),23)=24;

%NO FiO2, NO O2 flow, no interface OR cannula
ii=find(isnan(reformatsah(:,23))&isnan(reformatsah(:,25))&(reformatsah(:,22)==0|reformatsah(:,22)==2));  %no fio2 given and o2flow given, no interface OR cannula
reformat(ii,23)=21;

%NO FiO2, YES O2 flow, face mask OR.... OR ventilator (assume it's face mask)
ii=find(isnan(reformatsah(:,23))&~isnan(reformatsah(:,25))&(reformatsah(:,22)==NaN|reformatsah(:,22)==1|reformatsah(:,22)==3|reformatsah(:,22)==4|reformatsah(:,22)==5|reformatsah(:,22)==6|reformatsah(:,22)==9|reformatsah(:,22)==10));
reformat(ii(reformatsah(ii,25)<=15),23)=75;
reformat(ii(reformatsah(ii,25)<=12),23)=69;
reformat(ii(reformatsah(ii,25)<=10),23)=66;
reformat(ii(reformatsah(ii,25)<=8),23)=58;
reformat(ii(reformatsah(ii,25)<=6),23)=40;
reformat(ii(reformatsah(ii,25)<=4),23)=36;

%NO FiO2, NO O2 flow, face mask OR ....OR ventilator
ii=find(isnan(reformatsah(:,23))&isnan(reformatsah(:,25))&(reformatsah(:,22)==NaN|reformatsah(:,22)==1|reformatsah(:,22)==3|reformatsah(:,22)==4|reformatsah(:,22)==5|reformatsah(:,22)==6|reformatsah(:,22)==9|reformatsah(:,22)==10));  %no fio2 given and o2flow given, no interface OR cannula
reformat(ii,23)=NaN;

%NO FiO2, YES O2 flow, Non rebreather mask
ii=find(isnan(reformatsah(:,23))&~isnan(reformatsah(:,25))&reformatsah(:,22)==7);
reformat(ii(reformatsah(ii,25)>=10),23)=90;
reformat(ii(reformatsah(ii,25)>=15),23)=100;
reformat(ii(reformatsah(ii,25)<10),23)=80;
reformat(ii(reformatsah(ii,25)<=8),23)=70;
reformat(ii(reformatsah(ii,25)<=6),23)=60;

%NO FiO2, NO O2 flow, NRM
ii=find(isnan(reformatsah(:,23))&isnan(reformatsah(:,25))&reformatsah(:,22)==7);  %no fio2 given and o2flow given, no interface OR cannula
reformat(ii,23)=NaN;

% update again FiO2 columns
ii=~isnan(reformat(:,23)) & isnan(reformat(:,24));
reformat(ii,24)=reformat(ii,23)./100;
ii=~isnan(reformat(:,24)) & isnan(reformat(:,23));
reformat(ii,23)=reformat(ii,24).*100;

%BP
ii=~isnan(reformat(:,9))&~isnan(reformat(:,10)) & isnan(reformat(:,11));
reformat(ii,11)=(3*reformat(ii,10)-reformat(ii,9))./2;
ii=~isnan(reformat(:,09))&~isnan(reformat(:,11)) & isnan(reformat(:,10));
reformat(ii,10)=(reformat(ii,9)+2*reformat(ii,11))./3;
ii=~isnan(reformat(:,10))&~isnan(reformat(:,11)) & isnan(reformat(:,9));
reformat(ii,9)=3*reformat(ii,10)-2*reformat(ii,11);

%TEMP
%some values recorded in the wrong column
ii=reformat(:,15)>25&reformat(:,15)<45; %tempF close to 37deg??!
reformat(ii,14)=reformat(ii,15);
reformat(ii,15)=NaN;
ii=reformat(:,14)>70;  %tempC > 70?!!! probably degF
reformat(ii,15)=reformat(ii,14);
reformat(ii,14)=NaN;
ii=~isnan(reformat(:,14)) & isnan(reformat(:,15));
reformat(ii,15)=reformat(ii,14)*1.8+32;
ii=~isnan(reformat(:,15)) & isnan(reformat(:,14));
reformat(ii,14)=(reformat(ii,15)-32)./1.8;

% Hb/Ht
ii=~isnan(reformat(:,50)) & isnan(reformat(:,51));
reformat(ii,51)=(reformat(ii,50)*2.862)+1.216;
ii=~isnan(reformat(:,51)) & isnan(reformat(:,50));
reformat(ii,50)=(reformat(ii,51)-1.216)./2.862;

%BILI
ii=~isnan(reformat(:,44)) & isnan(reformat(:,45));
reformat(ii,45)=(reformat(ii,44)*0.6934)-0.1752;
ii=~isnan(reformat(:,45)) & isnan(reformat(:,44));
reformat(ii,44)=(reformat(ii,45)+0.1752)./0.6934;

%% ########################################################################
%                      SAMPLE AND HOLD on RAW DATA
% ########################################################################

reformat=SAH(reformat(:,1:68),sample_and_hold);

%% ########################################################################
%                             DATA COMBINATION
% ########################################################################

% WARNING: the time window of interest has been defined above (here -48 -> +24)!

timestep=4;  %resolution of timesteps, in hours
irow=1;
icustayidlist=unique(reformat(:,2));
reformat2=nan(size(reformat,1),84);  %output array
h = waitbar(0,'Initializing waitbar...');
npt=numel(icustayidlist);  %number of patients
% Adding 2 empty cols for future shock index=HR/SBP and P/F
reformat(:,69:70)=NaN(size(reformat,1),2);

tic
for i=1:npt

    icustayid=icustayidlist(i);  %1 to 100000, NOT 200 to 300K!

        %CHARTEVENTS AND LAB VALUES
        temp=reformat(reformat(:,2)==icustayid,:);   %subtable of interest
        beg=temp(1,3);   %timestamp of first record

        % IV FLUID STUFF
        iv=find(inputMV(:,1)==icustayid+200000);   %rows of interest in inputMV
        input=inputMV(iv,:);    %subset of interest
        iv=find(inputCV(:,1)==icustayid+200000);   %rows of interest in inputCV
        input2=inputCV(iv,:);    %subset of interest
        startt=input(:,2); %start of all infusions and boluses
        endt=input(:,3); %end of all infusions and boluses
        rate=input(:,8);  %rate of infusion (is NaN for boluses) || corrected for tonicity

        pread=inputpreadm(inputpreadm(:,1)==icustayid+200000,2) ;%preadmission volume
            if ~isempty(pread)             %store the value, if available
                totvol=nansum(pread);
                waitbar(i/npt,h,i/npt*100) %moved here to save some time
            else
                totvol=0;   %if not documented: it's zero
            end

        % compute volume of fluid given before start of record!!!
        t0=0;
        t1=beg;
        %input from MV (4 ways to compute)
        infu=  nansum(rate.*(endt-startt).*(endt<=t1&startt>=t0)/3600   +    rate.*(endt-t0).*(startt<=t0&endt<=t1&endt>=t0)/3600 +     rate.*(t1-startt).*(startt>=t0&endt>=t1&startt<=t1)/3600 +      rate.*(t1-t0).*(endt>=t1&startt<=t0)   /3600);
        %all boluses received during this timestep, from inputMV (need to check rate is NaN) and inputCV (simpler):
        bolus=nansum(input(isnan(input(:,6))& input(:,2)>=t0&input(:,2)<=t1,7)) + nansum(input2(input2(:,2)>=t0&input2(:,2)<=t1,5));
        totvol=nansum([totvol,infu,bolus]);

        %VASOPRESSORS
        iv=find(vasoMV(:,1)==icustayid+200000);   %rows of interest in vasoMV
        vaso1=vasoMV(iv,:);    %subset of interest
        iv=find(vasoCV(:,1)==icustayid+200000);   %rows of interest in vasoCV
        vaso2=vasoCV(iv,:);    %subset of interest
        startv=vaso1(:,3); %start of VP infusion
        endv=vaso1(:,4); %end of VP infusions
        ratev=vaso1(:,5);  %rate of VP infusion

        %DEMOGRAPHICS / gender, age, elixhauser, re-admit, died in hosp?, died within
        %48h of out_time (likely in ICU or soon after), died within 90d after admission?
        demogi=find(demog.icustay_id==icustayid+200000);
        dem=[  demog.gender(demogi) ; demog.age(demogi) ;demog.elixhauser(demogi) ; demog.adm_order(demogi)>1 ;  demog.morta_hosp(demogi); abs(demog.dod(demogi)-demog.outtime(demogi))<(24*3600*2); demog.morta_90(demogi) ; (qstime(icustayid,4)-qstime(icustayid,3))/3600];

        % URINE OUTPUT
        iu=find(UO(:,1)==icustayid+200000);   %rows of interest in inputMV
        output=UO(iu,:);    %subset of interest
        pread=UOpreadm(UOpreadm(:,1)==icustayid,4) ;%preadmission UO
            if ~isempty(pread)     %store the value, if available
                UOtot=nansum(pread);
            else
                UOtot=0;
            end
        % adding the volume of urine produced before start of recording!
        UOnow=nansum(output(output(:,2)>=t0&output(:,2)<=t1,4));  %t0 and t1 defined above
        UOtot=nansum([UOtot UOnow]);

    for j=0:timestep:79 % -52 until +28 = 80 hours in total
        t0=3600*j+ beg;   %left limit of time window
        t1=3600*(j+timestep)+beg;   %right limit of time window
        ii=temp(:,3)>=t0 & temp(:,3)<=t1;  %index of items in this time period
        if sum(ii)>0

        %ICUSTAY_ID, OUTCOMES, DEMOGRAPHICS
        reformat2(irow,1)=(j/timestep)+1;   %'bloc' = timestep (1,2,3...)
        reformat2(irow,2)=icustayid;        %icustay_ID
        reformat2(irow,3)=3600*j+ beg;      %t0 = lower limit of time window
        reformat2(irow,4:11)=dem;           %demographics and outcomes

        %CHARTEVENTS and LAB VALUES (+ includes empty cols for shock index and P/F)
        value=temp(ii,:);%records all values in this timestep

          % #####################   DISCUSS ADDING STUFF HERE / RANGE, MIN, MAX ETC   ################

        if sum(ii)==1   %if only 1 row of values at this timestep
          reformat2(irow,12:78)=value(:,4:end);
        else
          reformat2(irow,12:78)=nanmean(value(:,4:end)); %mean of all available values
        end

        %VASOPRESSORS
            % for CV: dose at timestamps.
            % for MV: 4 possibles cases, each one needing a different way to compute the dose of VP actually administered:
            %----t0---start----end-----t1----
            %----start---t0----end----t1----
            %-----t0---start---t1---end
            %----start---t0----t1---end----

        %MV
        v=(endv>=t0&endv<=t1)|(startv>=t0&endv<=t1)|(startv>=t0&startv<=t1)|(startv<=t0&endv>=t1);
        %CV
        v2=vaso2(vaso2(:,3)>=t0&vaso2(:,3)<=t1,4);
        v1=nanmedian([ratev(v); v2]);
        v2=nanmax([ratev(v); v2]);
        if ~isempty(v1)&~isnan(v1)&~isempty(v2)&~isnan(v2)
        reformat2(irow,79)=v1;    %median of dose of VP
        reformat2(irow,80)=v2;    %max dose of VP
        end

        %INPUT FLUID
        %input from MV (4 ways to compute)
        infu=  nansum(rate.*(endt-startt).*(endt<=t1&startt>=t0)/3600   +    rate.*(endt-t0).*(startt<=t0&endt<=t1&endt>=t0)/3600 +     rate.*(t1-startt).*(startt>=t0&endt>=t1&startt<=t1)/3600 +      rate.*(t1-t0).*(endt>=t1&startt<=t0)   /3600);
        %all boluses received during this timestep, from inputMV (need to check rate is NaN) and inputCV (simpler):
        bolus=nansum(input(isnan(input(:,6))& input(:,2)>=t0&input(:,2)<=t1,7)) + nansum(input2(input2(:,2)>=t0&input2(:,2)<=t1,5));
        %sum fluid given
        totvol=nansum([totvol,infu,bolus]);
        reformat2(irow,81)=totvol;    %total fluid given
        reformat2(irow,82)=nansum([infu,bolus]);   %fluid given at this step

        %UO
        UOnow=nansum(output(output(:,2)>=t0&output(:,2)<=t1,4));
        UOtot=nansum([UOtot UOnow]);
        reformat2(irow,83)=UOtot;    %total UO
        reformat2(irow,84)=nansum(UOnow);   %UO at this step

        %CUMULATED BALANCE
        reformat2(irow,85)=totvol-UOtot;    %cumulated balance

        irow=irow+1;
        end
    end
end
toc

reformat2(irow:end,:)=[];
close(h);

%% ########################################################################
%    CONVERT TO TABLE AND DELETE VARIABLES WITH EXCESSIVE MISSINGNESS
% ########################################################################

dataheaders=[sample_and_hold(1,:) {'Shock_Index' 'PaO2_FiO2'}];
dataheaders=regexprep(dataheaders,'['']','');
dataheaders = ['bloc','icustayid','charttime','gender','age','elixhauser','re_admission', 'died_in_hosp', 'died_within_48h_of_out_time','mortality_90d','delay_end_of_record_and_discharge_or_death',...
    dataheaders,  'median_dose_vaso','max_dose_vaso','input_total','input_4hourly','output_total','output_4hourly','cumulated_balance'];

reformat2t=array2table(reformat2);
reformat2t.Properties.VariableNames=dataheaders;
miss=sum(isnan(reformat2))./size(reformat2,1);

% if values have less than 70% missing values (over 30% of values present): I keep them
reformat3t=reformat2t(:,[true(1,11) miss(12:74)<0.70 true(1,11)]) ;

%% ########################################################################
%             HANDLING OF MISSING VALUES  &  CREATE REFORMAT4T
% ########################################################################

% Do linear interpol where missingness is low (kNN imputation doesnt work if all rows have missing values)
reformat3=table2array(reformat3t);
miss=sum(isnan((reformat3)))./size(reformat3,1);
ii=miss>0&miss<0.05;  %less than 5% missingness
mechventcol=find(ismember(reformat3t.Properties.VariableNames,{'mechvent'}));

for i=11:mechventcol-1 % correct col by col, otherwise it does it wrongly
  if ii(i)==1
    reformat3(:,i)=fixgaps(reformat3(:,i));
  end
end

reformat3t(:,11:mechventcol-1)=array2table(reformat3(:,11:mechventcol-1));

% KNN IMPUTATION -  Done on chunks of 10K records.

mechventcol=find(ismember(reformat3t.Properties.VariableNames,{'mechvent'}));
ref=reformat3(:,11:mechventcol-1);  %columns of interest

tic
for i=1:10000:size(reformat3,1)-9999   %dataset divided in 5K rows chunks (otherwise too large)
    i
    ref(i:i+9999,:)=knnimpute(ref(i:i+9999,:)',1, 'distance','seuclidean')';
end

ref(end-9999:end,:)=knnimpute(ref(end-9999:end,:)',1, 'distance','seuclidean')';  %the last bit is imputed from the last 10K rows

toc

% I paste the data interpolated, but not the demographics and the treatments
reformat3t(:,11:mechventcol-1)=array2table(ref);

reformat4t=reformat3t;
reformat4=table2array(reformat4t);

%% ########################################################################
%        COMPUTE SOME DERIVED VARIABLES: P/F, Shock Index, SOFA, SIRS...
% ########################################################################

% CORRECT GENDER
reformat4t.gender=reformat4t.gender-1;

%CORRECT AGE > 200 yo
ii=reformat4t.age>150*365.25;
reformat4t.age(ii)=91.4*365.25;

% FIX MECHVENT
reformat4t.mechvent(isnan(reformat4t.mechvent))=0;
reformat4t.mechvent(reformat4t.mechvent>0)=1;

% FIX Elixhauser missing values
reformat4t.elixhauser(isnan(reformat4t.elixhauser))=nanmedian(reformat4t.elixhauser);  %use the median value / only a few missing data points

%vasopressors / no NAN
a=find(ismember(reformat4t.Properties.VariableNames,{'median_dose_vaso'}));
ii=isnan(reformat4(:,a));
reformat4t(ii,a)=array2table(zeros(sum(ii),1));
a=find(ismember(reformat4t.Properties.VariableNames,{'max_dose_vaso'}));
ii=isnan(reformat4(:,a));
reformat4t(ii,a)=array2table(zeros(sum(ii),1));

% re-compute P/F with no missing values...
p=find(ismember(reformat4t.Properties.VariableNames,{'paO2'}));
f=find(ismember(reformat4t.Properties.VariableNames,{'FiO2_1'}));
a=find(ismember(reformat4t.Properties.VariableNames,{'PaO2_FiO2'}));
reformat4t(:,a)=array2table(reformat4(:,p)./reformat4(:,f));

%recompute SHOCK INDEX without NAN and INF
p=find(ismember(reformat4t.Properties.VariableNames,{'HR'}));
f=find(ismember(reformat4t.Properties.VariableNames,{'SysBP'}));
a=find(ismember(reformat4t.Properties.VariableNames,{'Shock_Index'}));
reformat4(:,a)=reformat4(:,p)./reformat4(:,f);
reformat4(isinf(reformat4(:,a)),a)=NaN;
d=nanmean(reformat4(:,a));
reformat4(isnan(reformat4(:,a)),a)=d;  %replace NaN with average value ~ 0.8
reformat4t(:,a)=array2table(reformat4(:,a));

% SOFA - at each timepoint
% need (in this order):  P/F  MV  PLT  TOT_BILI  MAP  NORAD(max)  GCS  CR  UO
a=zeros(8,1); % indices of vars used in SOFA
a(1)=find(ismember(reformat4t.Properties.VariableNames,{'PaO2_FiO2'}));
a(2)=find(ismember(reformat4t.Properties.VariableNames,{'Platelets_count'}));
a(3)=find(ismember(reformat4t.Properties.VariableNames,{'Total_bili'}));
a(4)=find(ismember(reformat4t.Properties.VariableNames,{'MeanBP'}));
a(5)=find(ismember(reformat4t.Properties.VariableNames,{'max_dose_vaso'}));
a(6)=find(ismember(reformat4t.Properties.VariableNames,{'GCS'}));
a(7)=find(ismember(reformat4t.Properties.VariableNames,{'Creatinine'}));
a(8)=find(ismember(reformat4t.Properties.VariableNames,{'output_4hourly'}));
s=table2array(reformat4t(:,a));

p=[0 1 2 3 4];

s1=[s(:,1)>400 s(:,1)>=300 &s(:,1)<400 s(:,1)>=200 &s(:,1)<300 s(:,1)>=100 &s(:,1)<200 s(:,1)<100 ];   %count of points for all 6 criteria of sofa
s2=[s(:,2)>150 s(:,2)>=100 &s(:,2)<150 s(:,2)>=50 &s(:,2)<100 s(:,2)>=20 &s(:,2)<50 s(:,2)<20 ];
s3=[s(:,3)<1.2 s(:,3)>=1.2 &s(:,3)<2 s(:,3)>=2 &s(:,3)<6 s(:,3)>=6 &s(:,3)<12 s(:,3)>12 ];
s4=[s(:,4)>=70 s(:,4)<70&s(:,4)>=65 s(:,4)<65 s(:,5)>0 &s(:,5)<=0.1 s(:,5)>0.1 ];
s5=[s(:,6)>14 s(:,6)>12 &s(:,6)<=14 s(:,6)>9 &s(:,6)<=12 s(:,6)>5 &s(:,6)<=9 s(:,6)<=5 ];
s6=[s(:,7)<1.2 s(:,7)>=1.2 &s(:,7)<2 s(:,7)>=2 &s(:,7)<3.5 (s(:,7)>=3.5 &s(:,7)<5)|(s(:,8)<84) (s(:,7)>5)|(s(:,8)<34) ];

nrcol=size(reformat4,2);   %nr of variables in data
reformat4(1,nrcol+1:nrcol+7)=0;
for i=1:size(reformat4,1)
    t=max(p(s1(i,:)))+max(p(s2(i,:)))+max(p(s3(i,:)))+max(p(s4(i,:)))+max(p(s5(i,:)))+max(p(s6(i,:)));  %SUM OF ALL 6 CRITERIA

    if t
    reformat4(i,nrcol+1:nrcol+7)=    [max(p(s1(i,:))) max(p(s2(i,:))) max(p(s3(i,:))) max(p(s4(i,:))) max(p(s5(i,:))) max(p(s6(i,:))) t];
    end
end

% SIRS - at each timepoint |  need: temp HR RR PaCO2 WBC
a=zeros(5,1); % indices of vars used in SOFA
a(1)=find(ismember(reformat4t.Properties.VariableNames,{'Temp_C'}));
a(2)=find(ismember(reformat4t.Properties.VariableNames,{'HR'}));
a(3)=find(ismember(reformat4t.Properties.VariableNames,{'RR'}));
a(4)=find(ismember(reformat4t.Properties.VariableNames,{'paCO2'}));
a(5)=find(ismember(reformat4t.Properties.VariableNames,{'WBC_count'}));
s=table2array(reformat4t(:,a));

s1=[s(:,1)>=38| s(:,1)<=36];   %count of points for all criteria of SIRS
s2=[s(:,2)>90 ];
s3=[s(:,3)>=20|s(:,4)<=32];
s4=[s(:,5)>=12| s(:,5)<4];
reformat4(:,nrcol+8)=s1+s2+s3+s4;

% adds 2 cols for SOFA and SIRS, if necessary
if sum(ismember(reformat4t.Properties.VariableNames,{'SIRS'}))== 0
reformat4t(:,end+1:end+2)=array2table(0);
reformat4t.Properties.VariableNames(end-1:end)= {'SOFA','SIRS'};
end

% records values
reformat4t(:,end-1)=array2table(reformat4(:,end-1));
reformat4t(:,end)=array2table(reformat4(:,end));

%% ########################################################################
%                            EXCLUSION OF SOME PATIENTS
% ########################################################################

numel(unique(reformat4t.icustayid))  %count before

% check for patients with extreme UO = outliers = to be deleted (>40 litres of UO per 4h!!)
a=find(reformat4t.output_4hourly>12000);
i=unique(reformat4t.icustayid(a));
i=find(ismember(reformat4t.icustayid,i));
reformat4t(i,:)=[];

% some have bili = 999999
a=find(reformat4t.Total_bili>10000);
i=unique(reformat4t.icustayid(a));
i=find(ismember(reformat4t.icustayid,i));
reformat4t(i,:)=[];

% check for patients with extreme INTAKE = outliers = to be deleted (>10 litres of intake per 4h!!)
a=find(reformat4t.input_4hourly>10000);
i=unique(reformat4t.icustayid(a));  % 28 ids
i=find(ismember(reformat4t.icustayid,i));
reformat4t(i,:)=[];

% #### exclude early deaths from possible withdrawals ####
% stats per patient
q=reformat4t.bloc==1;
% fence_posts=find(q(:,1)==1);
num_of_trials=numel(unique(reformat4t.icustayid));%size(fence_posts,1);
a=array2table([reformat4t.icustayid reformat4t.mortality_90d reformat4t.max_dose_vaso reformat4t.SOFA]);
a.Properties.VariableNames={'id','mortality_90d','vaso','sofa'};
d=grpstats(a,'id','max');

%finds patients who match our criteria
e=zeros(num_of_trials,1);
for i=1:num_of_trials
    if d.max_mortality_90d(i) ==1
    ii=reformat4t.icustayid==d.id(i) & reformat4t.bloc==d.GroupCount(i);  %last row for this patient
    e(i)=sum((reformat4t.max_dose_vaso(ii)==0 & d.max_vaso(i)>0.3 & reformat4t.SOFA(ii)>=d.max_sofa(i)/2))>0;
    end
end
r=d.id(e==1 & d.GroupCount<20); % ids to be removed
ii=ismember(reformat4t.icustayid,r);
reformat4t(ii,:)=[];

% exclude patients who died in ICU during data collection period
ii=reformat4t.bloc==1&reformat4t.died_within_48h_of_out_time==1& reformat4t.delay_end_of_record_and_discharge_or_death<24;
ii=ismember(icustayidlist,reformat4t.icustayid(ii));
reformat4t(ii,:)=[];

numel(unique(reformat4t.icustayid))   %count after

%% #######################################################################
%                       CREATE SEPSIS COHORT
% ########################################################################

% create array with 1 row per icu admission
% keep only patients with flagged sepsis (max sofa during time period of interest >= 2)
% we assume baseline SOFA of zero (like other publications)

sepsis=zeros(30000,5);
irow=1;

tic
for icustayid=1:100000
    ii=find(ismember(reformat4t.icustayid,icustayid));
    if mod(icustayid,10000)==0;disp([num2str(icustayid/1000), ' %']);end
    if ii

         sofa=reformat4t.SOFA(ii);
         sirs=reformat4t.SIRS(ii);
         sepsis(irow,1)=icustayid+200000;
         sepsis(irow,2)=reformat4t.mortality_90d(ii(1)); % 90-day mortality
         sepsis(irow,3)=max(sofa);
         sepsis(irow,4)=max(sirs);
         sepsis(irow,5)=qstime(icustayid);   %time of onset of sepsis
         irow=irow+1;
    end
end
toc
sepsis(irow:end,:)=[];

sepsis=array2table(sepsis);
sepsis.Properties.VariableNames={'icustayid','morta_90d','max_sofa','max_sirs','sepsis_time'};

% delete all non-sepsis
sepsis(sepsis.max_sofa<2,:)=[];

% final count
size(sepsis,1)

%save cohort
writetable(sepsis,'sepsis_mimiciii.csv','Delimiter',',');


%% THE AGENT BASE CLASS (agent.m) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This class is designed to define a generic agent and import this variables
% into the simulation space for the purpose of multi-vehicle control simulation.
% The agent object is a child of the objectDefintion; the prinicple
% distinctions being:
% sensorRange      - The agent is assumed capable of observing its
%                    surroundings.
% controlFrequency - The frequency at which the control cycle is computed.

% Author: James A. Douthwaite

classdef agent < objectDefinition & agent_tools
    %% AGENT BASE CLASS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % This class contains the basic properties of a generic agent, neither
    % aerial or ground based.
    properties
        % DEFAULT BEHAVIOUR
        v_nominal = 2;                  % Default nominal linear speed (m/s)
        v_max  = 4;                     % Default maximal linear speed (m/s)
        w_max  = 0.5;                   % Default maximal angular speed (rad/s)
        detectionRadius = inf;
        % WAYPOINTS
        targetWaypoint;                 % The current waypoint target
        achievedWaypoints;              % The agents list of locally achieved waypoints
        % DYNAMIC PARAMETERS
        DYNAMICS;
        % SENSOR MEASUREMENT PARAMETERS
        SENSORS = struct('range',inf);  % Only generic parameter is a visual horizon
        % AGENT-SIDE OUTPUT DATA
        DATA;                           % The output container for agent-side data.
    end

    %% ///////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function [this] = agent(varargin)
            % This function is to construct the agent object using the
            % object defintions held in the 'objectDefinition' base class.
            % INPUTS:
            % namestr - Assigned name string
            % OUTPUTS:
            % this     - The constructed object

            % Call the super class
            this@objectDefinition(varargin);

            % //////////////////// SENSOR PARAMETERS //////////////////////
            [this.SENSORS] = this.GetDefaultSensorParameters();               % Default sensing
            % /////////////////////////////////////////////////////////////
            this.SetGLOBAL(this.CreateGLOBAL());    % Assign (agent) GLOBAL structure
            this.DYNAMICS = this.CreateDYNAMICS();  % Assign (agent) DYNAMICS structure
            this.SetBufferSize(1);                  % Assign default buffer size

            % Assign defaults
            this.radius = 0.5;                      % Default radius (m)
            this.localState = zeros(12,1);          % Assign state

            % //////////////// Check for user overrides ///////////////////
            % - It is assumed that overrides to the properties are provided
            %   via the varargin structure.
            this = this.ApplyUserOverrides(varargin);                         % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup (agent default)
        function [this] = setup(this,localXYZvelocity,localXYZrotations)   % [x y z phi theta psi]
            % This function is called in order to build the initial state
            % vector for the generic agent class 'objectDefinition'.
            % INPUTS:

            % ASSUMPTIONS:
            % - The designed state is described in the ENU axes.
            % - The state is of the form:
            % - [x y z phi theta psi dx dy dz dphi dtheta dpsi]

            switch this.Is3D()
                case false
                    % Initialises the state vector [x y psi ; dx dy dpsi]
                    this.setup_2DVelocities(localXYZvelocity,localXYZrotations);
                case true
                    % Initialises the state vector [x y z phi theta psi ; dx dy dz dphi dtheta dpsi]
                    this.setup_3DVelocities(localXYZvelocity,localXYZrotations)
                otherwise
                    error('Object dimensionality unknown');
            end
        end
        % Main
        function [this] = main(this,ENV,varargin)
            % This function is designed to house a generic agent process
            % cycle that results in an acceleration vector in the global axis.
            % INPUTS:
            % ENV     - The TIME simulations structure
            % varargin - Cell array of inputs
            % OUTPUTS:
            % this      - The updated project

            % PLOT AGENT FIGURE
            visualiseProblem = 0;
            visualiseAgent = 1;
            if this.objectID == visualiseAgent && visualiseProblem == 1
                %overHandle = figure('name','testFigure');
                overHandle = gcf;
                ax = gca;
                hold on; grid on;
                axis equal;
                xlabel('x_{m}'); ylabel('y_{m}'); zlabel('z_{m}');
            end

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % UPDATE THE AGENT WITH THE NEW ENVIRONMENTAL INFORMATION
            [this,obstacleSet,agentSet] = this.GetAgentUpdate(ENV,varargin{1});

            % /////////////////// WAYPOINT TRACKING ///////////////////////
            % Design the current desired trajectory from the waypoint.
            [headingVector] = this.GetTargetHeading();
            desiredVelocity = headingVector*this.v_nominal;

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            %                                                             %
            %                         DO NOTHING                          %
            %                                                             %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            % PASS THE DESIRED VELOCITY TO THE DEFAULT CONTROLLER
            [this] = this.Controller(ENV.dt,desiredVelocity);
        end
    end

    %% ////////////////////// BASIC UPDATE FUNCTIONS //////////////////////
    methods
        % AGENT UPDATE (IDEAL) - Update from perfect environmental knowledge
        function [this,obstacleSet,agentSet,waypointSet] = GetAgentUpdate(this,ENV,observedobjects)
            % This function computes the agents process for updating its
            % knowledge of the environment.
            % INPUTS:
            % ENV.dt              - The unit timstep
            % observedobjects - The full observed object structure
            % OUTPUTS:
            % this             - The updated object
            % obstacleSet     - The list of obstacle structures
            % waypointSet     - The list of waypoint structures

            agentSet = []; obstacleSet = []; waypointSet = [];             % No observed objects

            % Check #1 - Nothing is observed..
            if isempty(observedobjects)
                return
            end

            % Input sanity check
            assert(isstruct(ENV),'Expecting environment update structure.');
            assert(isnumeric(ENV.dt),'The time-step must be a numeric timestep.');
            assert(isstruct(observedobjects),'Second parameter is a vector of observation structures.');

            % Update agent memory structure
            for entry = 1:numel(observedobjects)
                % Apply sensor model if there is one
                sensedobject = this.SensorModel(ENV.dt,observedobjects(entry));
                % Update memory structure from measurements
                this = this.UpdateMemoryFromObject(ENV.currentTime,sensedobject);
            end

            % SORT object SET BY PRIORITY
            [this] = this.UpdateMemoryOrderByField('priority');

            % DECERN object TYPES
            % this.MEMORY now contains an updated understanding of all the
            % objects in its visual horizon. From this, the waypoints and
            % obstacles must be parsed.

            % PULL MEMORY ITEMS FOR LATER MANIPULATION
            agentSet    = this.MEMORY([this.MEMORY.type] == OMAS_objectType.agent);
            waypointSet = this.MEMORY([this.MEMORY.type] == OMAS_objectType.waypoint);
            obstacleSet = this.MEMORY([this.MEMORY.type] == OMAS_objectType.obstacle); % Differenciate between the different object types

            % UPDATE THE TARGET WAYPOINT, PRIORTIES
            [this] = this.UpdateTargetWaypoint(waypointSet);                    % Update the target waypoint and heading vector
        end
    end

    %% //////////////////////// SENSING FUNCTIONS /////////////////////////
    methods
        % SENSOR MODEL - TOP LEVEL (Default)
        function [observedobject] = SensorModel(this,dt,observedobject)
            % This function provides an overridable method resembling the
            % sensor model through which all objects are processed.

            % Input sanity check
            assert(isnumeric(dt) && numel(dt) == 1,'The time step value is invalid.');
            assert(numel(observedobject.position) <= 3,'Expecting a position value to be [2x1].');
            assert(numel(observedobject.velocity) <= 3,'Expecting a velocity value to be [2x1].');

            % Get measurements from a camera
            [psi_j,theta_j,alpha_j] = this.GetCameraMeasurements(observedobject);
            % Override ideal parameters with camera model
            observedobject.heading = psi_j;
            observedobject.elevation = theta_j;
            observedobject.width = alpha_j;
            % Get the range estimate
            [observedobject.range] = this.GetRangeFinderMeasurements(observedobject);
        end
        % SENSOR MODEL - CAMERA & RANGE FINDER
        function [psi_j,theta_j,alpha_j] = GetCameraMeasurements(this,observedobject)
            % This function takes the simulation data and calculates the
            % spherical position and radius that would otherwise be sensed
            % by the system.
            % INPUTS:
            % obstacleData - The object observation structure
            % OUTPUTS:
            % range        - The obstacles apparent range
            % azimuth      - The obstacles angular position in the XY plane
            % elevation    - The obstacles angular position in the XZ plane
            % angularWidth - The obstacles angular width in the azimuth

            % Observed pixel coordinates
            psi_j   = observedobject.heading   + this.SENSORS.sigma_camera*randn(1);
            theta_j = observedobject.elevation + this.SENSORS.sigma_camera*randn(1);
            % Observed width
            alpha_j = observedobject.width + this.SENSORS.sigma_camera*randn(1);      % Uncertainty in the angular measurement
        end
        % SENSOR MODEL - RANGE FINDER
        function [d_j] = GetRangeFinderMeasurements(this,observedobject)
            % This function takes a simulation data resembling an object
            % and calculates the apparent range to the agent.

            % EMULATE MEASURED POSITIONAL VARIABLES (measured = range(true) + distortion)
            d_j = observedobject.range + this.SENSORS.sigma_rangeFinder*randn(1);
        end
        % SENSOR MODEL - LOCAL GPS & PITOT TUBE
        function [p_i,v_i,r_i] = GetAgentMeasurements(this)
            % This function makes estimates of the agent's current position
            % and velocity states (defined in this.localState).

            if this.Is3D
                positionIndices = 1:3;
                velocityIndices = 7:9;
            else
                positionIndices  = 1:2;
                velocityIndices = 4:5;
            end
            % GET THE LOCAL ABSOLUTE VARIABLES
            p_i =  this.localState(positionIndices,1) + this.SENSORS.sigma_position*rand(numel(positionIndices),1); % Get the absolute position measurement
            v_i =  this.localState(velocityIndices,1) + this.SENSORS.sigma_velocity*rand(numel(velocityIndices),1); % Get the absolute velocity measurement
            % RADIUS IS ASSUMED KNOWN
            r_i = this.radius();
        end
        % CALCULATE THE NEW STATE ESTIMATE
        function [position,velocity] = linearStateEstimation(this,dt,p0,v0,p1)
            % This function takes the previous known state of the obstacle
            % and estimates its new state.

            % GET THE POSITION
            dX = (p1 - p0);
            velocity = dX/dt;    % Defines the average velocity
            position = p1;
            % NO PREVIOUS VELOCITY RECORDED
            if any(isnan(v0))
                return
            end
        end
    end
    methods (Static)
        % SENSOR MODEL - REPRESENTATIVE SENSING
        function [SENSORS] = GetCustomSensorParameters()
            % This function is designed to populate the SENSOR structure
            % with representative sensing parameters for the assembly of
            % the sensing intervals.
            % BUILD THE SENSOR FACTOR
            SENSORS = struct(...
                'range',inf,...             % Assume the agent has perfect environmental knowledge (m)
                'sigma_position',0.5,...    % Accurate to within 0.5m
                'sigma_velocity',0.1,...    % Accurate to within 0.1m/s
                'sigma_rangeFinder',0.1,... % Accurate to within 0.1m
                'sigma_camera',5.208E-5,... % One pixel in a 1080p image
                'sampleFrequency',inf);     % object has perfect precision
        end
        % SENSOR MODEL - PERFECT SENSING
        function [SENSORS] = GetDefaultSensorParameters()
            % This function is designed to populate the SENSOR field with
            % perfect sensor capabilities.
            % BUILD THE SENSOR FACTOR
            SENSORS = struct(...
                'range',inf,...             % Assume the agent has perfect environmental knowledge (m)
                'sigma_position',0.0,...    % Perfect position measurement
                'sigma_velocity',0.0,...    % Perfect velocity measurement
                'sigma_rangeFinder',0.0,... % Perfect range acquistion
                'sigma_camera',0.0,...      % Infinte resolution
                'sampleFrequency',inf);     % object has perfect precision
        end
    end

    %% //////////////////////// DYNAMICS & CONTROL ////////////////////////
    methods
        % Simple controller
        function [this] = Controller(this,dt,desiredVelocity)
            % This function computes the agents change in state as a result
            % of the desired velocity vector

            % Input sanity check #1 - Is feasible
            assert(isnumeric(dt) && numel(dt) == 1,'The time step must be a numeric scalar.');
            assert(isnumeric(desiredVelocity) && numel(desiredVelocity) == 3,'Requested velocity vector is not a 2D numeric vector');
            assert(~any(isnan(desiredVelocity)),'The desired velocity vector contains NaNs.');

            % ///////////// UPDATE object GLOBAL PROPERTIES ///////////////
            % Input sanity check #2 - Zero vector
            [heading,speed] = this.nullVelocityCheck(desiredVelocity);

            % Get the relative heading
            [dPsi,dTheta]   = this.GetVectorHeadingAngles([1;0;0],heading); % Relative heading angles
            % The desired heading rate
            omega = ([0;dTheta;-dPsi] - zeros(3,1))/dt;

            % Apply kinematic constraints to state changess
            [omega_actual,speed_actual] = this.ApplyKinematicContraints(dt,omega,speed);

            % OMIT TRAJECTORY CHANGES IF IDLE
            if this.IsIdle()
                omega_actual = zeros(3,1);
                speed_actual = 0;
                this.v_nominal = 0;
            end

            dX = [speed_actual;0;0;omega_actual];

            % /////////// SIMPLE DYNAMICS + PURE TRANSLATION //////////////
            [dX] = this.SimpleDynamics(this.localState(1:6),[speed_actual;0;0],omega_actual);
            this.localState(1:6)  = this.localState(1:6) + dt*dX;
            this.localState(7:12) = dX;

            % ////// GLOBAL UPDATE FOR STATE WITH RETAINED VELOCITES //////
            [this] = this.GlobalUpdate_3DVelocities(dt,this.localState);
        end
        % SPEED AND HEADING (4D) PID CONTROLLER
        function [this] = Controller_PID(this,dt,desiredVelocity)
            % This function is desiged to generate feedack from a desired
            % local velocity vector.
            % INPUTS:
            % targetHeading - The unit heading vector
            % targetSpeed   - The target speed in that direction
            % OUTPUTS:
            % heading_fb    - The feedback on the agent heading
            % speed_fb      - The feedback on the agent speed
            % this           - The error-updated agent object

            % Input sanity check
            assert(isnumeric(dt) && numel(dt) == 1,'The time step must be a numeric scalar.');
            assert(isnumeric(desiredVelocity) && numel(desiredVelocity) == 3,'Requested velocity vector must be a 3D local vector.');
            assert(~any(isnan(desiredVelocity)),'The requested velocity contains NaNs.');

            % NUMERICAL SANITY CHECK TWO
            [unitDirection,desiredSpeed] = this.nullVelocityCheck(desiredVelocity);

            % APPLY SPEED CONSTRAINT
            if abs(desiredSpeed) > this.v_max
                desiredSpeed = sign(desiredSpeed)*this.v_max;
            end

            % GET THE EQUIVALENT HEADING ANGLE
            [dPsi,dTheta] = this.GetVectorHeadingAngles([1;0;0],unitDirection); % Relative heading angles
            dHeading = [0;dTheta;-dPsi];

            % RELATIVE SPEED
            e_speed = desiredSpeed - norm(this.localState(7:9));            % The speed error
            controlError = [e_speed;dHeading];                             % -ve in the NED control frame of reference

            % TUNING PARAMETERS
            Kp_linear = 0.8;
            Kd_linear = 0;
            Kp_angular = 1;
            Kd_angular = 0;

            % CALCULATE THE CONTROL FEEDBACK
            control_fb = diag([Kp_linear Kp_angular Kp_angular Kp_angular])*controlError + ...
                diag([Kd_linear Kd_angular Kd_angular Kd_angular])*(controlError - this.priorError);
            % REMEMBER PREVIOUS ERROR
            this.priorError = controlError;

            % PARSE THE CONTROL FEEDBACK FOR EXTERNAL USE
            speedFeedback = control_fb(1);                                 % Absolute speed input
            headingFeedback = control_fb(2:4);
            omega = headingFeedback/dt;

            % /////////// SIMPLE DYNAMICS + PURE TRANSLATION //////////////
            [dX] = this.SimpleDynamics(this.localState(1:6),[speedFeedback;0;0],omega);
            this.localState(1:6)  = this.localState(1:6) + dt*dX;
            this.localState(7:12) = dX;

            % ///////////// UPDATE object GLOBAL PROPERTIES ///////////////
            if this.GLOBAL.idleStatus
                this.localState(7:12) = zeros(6,1);
            end

            % ////// GLOBAL UPDATE FOR STATE WITH RETAINED VELOCITES //////
            [this] = this.GlobalUpdate_3DVelocities(dt,this.localState);
        end
        % Apply the DYNAMICS constraints
        function [omega_actual,speed_actual] = ApplyKinematicContraints(this,dt,omega,speed)
            % This function is designed to take a desired heading and
            % velocity and compare them to the kinematic contraints of the
            % agent.

            % Determine agent dimension
            if this.Is3D
                currentVelocity = this.localState(7:9);
                currentOmega    = this.localState(10:12);
            else
                currentVelocity = this.localState(4:5);
                currentOmega    = this.localState(6);
            end

            % For clarity
            v_lim      = this.DYNAMICS.maxLinearVelocity;
            dv_lim     = this.DYNAMICS.maxLinearAcceleration;
            omega_lim  = this.DYNAMICS.maxAngularVelocity;
            dOmega_lim = this.DYNAMICS.maxAngularAcceleration;

            % /////////////// Constrained heading change //////////////////
            % The implied angular acceleration
            omega_dot = (omega - currentOmega)/dt;
            % Constrain against heading acceleration limits
            bounded_omega_dot = boundValue(omega_dot,-dOmega_lim,dOmega_lim);
            % Calculate actual heading velocity limits
            omega = currentOmega + bounded_omega_dot*dt;
            % Constrain against heading rate limits
            omega_actual = boundValue(omega,-omega_lim,omega_lim);

            % ///////////////// Constrained speed change //////////////////
            % The current speed
            currentSpeed = norm(currentVelocity);
            % The proposed acceleration
            s_dot = (speed - currentSpeed)/dt;
            % Constrain against linear acceleration limits
            bounded_s_dot = boundValue(s_dot,-dv_lim(1),dv_lim(1));
            % Calcullate the actual speed
            speed = currentSpeed + bounded_s_dot*dt;
            % Interesect the velocity with permissible velocities
            speed_actual = boundValue(speed,-v_lim(1),v_lim(1));
        end
        % Create the DYNAMICS structure
        function [DYNAMICS] = CreateDYNAMICS(this)
            % Define basic DYNAMIC container
            [DYNAMICS] = this.CreateDYNAMICS_default();
        end
        % Create the default DYNAMICS constraint structure
        function [DYNAMICS] = CreateDYNAMICS_default(this)
            % Define an infinite "throw" range by default
            DYNAMICS = struct();
            % Define kinematic constraints
            if this.Is3D
                % States defined as [x,y,z,phi,theta,psi,dx,dy,dz,dphi,dtheta,dpsi]^T
                DYNAMICS.maxLinearVelocity      = ones(3,1)*this.v_max;    % Limits on the agents linear velocity
                DYNAMICS.maxLinearAcceleration  = inf(3,1);                % Limits on the agents linear acceleration
                DYNAMICS.maxAngularVelocity     = ones(3,1)*this.w_max;    % Limits on the agents angular velocity
                DYNAMICS.maxAngularAcceleration = inf(3,1);                % Limits on the agents angular acceleration
            else
                % States defined as [x,y,psi,dx,dy,dpsi]^T
                DYNAMICS.maxLinearVelocity      = ones(2,1)*this.v_max;    % Limits on the agents linear velocity
                DYNAMICS.maxLinearAcceleration  = inf(2,1);                % Limits on the agents linear acceleration
                DYNAMICS.maxAngularVelocity     = this.w_max;              % Limits on the agents angular velocity
                DYNAMICS.maxAngularAcceleration = inf(1,1);                % Limits on the agents angular acceleration
            end
        end
    end

    %% ////////////////////// GENERAL STATIC METHODS //////////////////////
    methods (Static)
        % DEFINE THE PITCH AND YAW TO MEET TARGET HEADING (2D & 3D)
        function [lambda,theta] = GetVectorHeadingAngles(V,U)
            % This function calculates the Line Of Sight (LOS) [horezontal]
            % angle which aids in defining the bank angle as a control input to the
            % dynamics.
            % INPUTS:
            % V - The current unity velocity vector
            % U - The unit correction vector
            % OUTPUTS:
            % lambda - The azimuth angle (2D)
            % theta  - The pitch angle   (3D)

            % NORMALISE THE ELEMENTS
            V = V/norm(V);
            U = U/norm(U);
            % GET THE HORIZONTAL ELEMENTS
            Vh = [V(1);V(2);0];
            Uh = [U(1);U(2);0];                                        % Reject the vertical elements
            % GET THE LINE OF SIGHT ANGLE
            rotationAxis = cross(Vh,Uh);

            % HANDLE 3D CASE (ELEVATION & LOS)
            if numel(V) == 3 && numel(U) == 3
                % GET THE LINE OF SIGHT ANGLE
                lambda = sign(rotationAxis(3))*acos(dot(Vh,Uh)/norm(Vh));  % Get the angle, signed by the direction of its cross product
                % GET THE ELEVATION ANGLE
                theta = atan2(U(3),norm(Uh));
            else
                % HANDLE 2D CASE (LOS)
                % GET THE LINE OF SIGHT ANGLE
                lambda = sign(rotationAxis(3))*acos(dot(Vh,Uh)/norm(Vh));
                % GET THE SUDO ELEVATION ANGLE
                theta = 0;
            end
        end
        % CALCULATE THE RADIUS
        function [r]    = GetRadiusFromAngularWidth(d,alpha)
            % Calculate the radius of the object
            r = (sin(alpha/2)/(1-sin(alpha/2)))*d;
        end
        % VALIDATE THE OBSTACLE
        function [tau]  = validateCollision(p,v)
            % CONFIRM WE KNOW THE HEADING OF THE OBSTACLE
            if any(isnan(v))
                tau = -inf;
                return
            end
            % Define the time to closest approach (+ve converging)
            tau = -(dot(p,v)/dot(v,v));
        end
        % BASIC VELOCITY VECTOR CHECK (2D & 3D)
        function [v_unit,v_mag] = nullVelocityCheck(v)
            v_mag = norm(v);
            if v_mag == 0
                v_unit = zeros(numel(v),1);
                v_unit(1) = 1;      % [1 0 0] % Default to current local forward direction
            else
                v_unit = v/v_mag;
            end
        end
    end

    %% ///////////////// VISUALISATION AND GENERAL TOOLS //////////////////
    methods
        % PLOT ALL THE OBSERVABLE OBSTACLES IN THE LOCAL FRAME
        function [figureHandle] = GetobjectScene(this,figureHandle)
            % FIGURE PREPERATION
            if ~exist('figureHandle','var')
                figureHandle = figure();
            else
                cla reset
            end

            ax = get(figureHandle,'CurrentAxes');
            set(ax,'NextPlot','replacechildren')
            hold on; grid on;
            axis equal;
            xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
            view([-120 35]);
            title(ax,'The agents perspective of its neighbourhood');

            % PLOT THE AGENT FOR PERSPECTIVE
            if size(this.GEOMETRY.vertices,1) < 1
                geometry = OMAS_geometry.defineSphere(zeros(3,1),this.radius);
                % REPRESENT GEOMETRY AS A PATCH
                entityHandle = patch(ax,...
                    'Vertices',geometry.vertices,...
                    'Faces',geometry.faces,...
                    'FaceColor',[0.4940, 0.1840, 0.5560]);
            else
                % PLOT THE AGENTS VERTEX DATA
                entityHandle = patch(ax,...
                    'Vertices',this.GEOMETRY.vertices,...
                    'Faces',this.GEOMETRY.faces,...
                    'FaceColor',this.GLOBAL.colour);
            end
            % ADD ANNOTATION
            annotationText = sprintf(' \t%s [ID-%d]',this.name,this.objectID);
            text(0,0,0,char(annotationText));

            % SET THE ENTITY DATA
            set(entityHandle,...
                'EdgeColor','k',...
                'EdgeAlpha',0.3,...
                'FaceLighting','gouraud',...
                'FaceAlpha',0.8,...
                'LineWidth',0.1,...
                'EdgeAlpha',0.7);

            % PLOT AGENT VELOCITY
            q = quiver3(0,0,0,1,0,0,'b');
            q.AutoScaleFactor = 1;
            % MOVE THROUGH THE OBSTACLE SET AND PLOT THE OBSTACLE POSITIONS
            for item = 1:numel(this.MEMORY)
                % Get the second objects data
                [p_j] = this.GetLastMeasurementByobjectID(this.MEMORY(item).objectID,'position');
                [v_j] = this.GetLastMeasurementByobjectID(this.MEMORY(item).objectID,'velocity');
                [r_j] = this.GetLastMeasurementByobjectID(this.MEMORY(item).objectID,'radius');

                if numel(this.MEMORY(item).geometry.vertices) < 1
                    % DEFINE GEOMETRY AS A SPHERE
                    if this.Is3D
                        [geometry] = OMAS_graphics.defineSphere(p_j,r_j);
                    else
                        [geometry] = OMAS_graphics.defineSphere([p_j;0],r_j);
                    end
                else
                    % DEFINE FROM OWN GEOMETRY
                    geometry = this.MEMORY(item).geometry;                 % Should be in the current frame
                end
                % REPRESENT GEOMETRY AS A PATCH
                entityHandle = patch(ax,...
                    'Vertices',geometry.vertices,...
                    'Faces',geometry.faces,...
                    'EdgeColor','k',...
                    'EdgeAlpha',0.2,...
                    'FaceLighting','gouraud',...
                    'FaceAlpha',0.2,...
                    'LineWidth',1);
                % PLOT REPRESENTATION
                switch this.MEMORY(item).type
                    case OMAS_objectType.agent
                        set(entityHandle,'FaceColor','b');
                    case OMAS_objectType.obstacle
                        set(entityHandle,'FaceColor','r');
                    case OMAS_objectType.waypoint
                        set(entityHandle,'FaceColor','g');
                    otherwise
                        set(entityHandle,'FaceColor','m');
                end
                % ADD ANNOTATION
                annotationText = sprintf(' \t%s [ID-%d]',this.MEMORY(item).name,this.MEMORY(item).objectID);
                if this.Is3D
                    text(p_j(1),p_j(2),p_j(3),char(annotationText));       % PLOT AGENT VELOCITY
                    q = quiver3(ax,p_j(1),p_j(2),p_j(3),...
                        v_j(1),v_j(2),v_j(3),'k');              % The velocity vector
                else
                    text(p_j(1),p_j(2),0,char(annotationText));
                    q = quiver3(ax,p_j(1),p_j(2),0,...
                        v_j(1),v_j(2),0,'k');                   % The velocity vector
                end
                q.AutoScaleFactor = 1;
            end
            drawnow;
            hold off;
        end
        % APPEND AN FIGURE FRAME TO ANIMATION
        function [figureHandle] = GetAnimationFrame(this,ENV,figureHandle,fileName)
            % INPUT HANDLING
            if nargin < 4
                fileName = sprintf('bodyAxes - %s [ID-%.0f].gif',this.name,this.objectID);
            end

            % FIGURE PREPERATION
            filePath = strcat(ENV.outputPath,fileName);
            drawnow;                                                       % Ensure all items are plotted before taking the image
            % CAPTURE FIGURE AS FRAME
            frame = getframe(figureHandle);
            im = frame2im(frame);                                          % Capture the figure as an image
            [imind,cm] = rgb2ind(im,256);
            % Write to the GIF File
            if ENV.currentStep == 1
                imwrite(imind,cm,filePath,'gif',...
                    'Loopcount',inf,...
                    'DelayTime',ENV.dt,...
                    'Location',[0 0],...
                    'Comment',sprintf('%s[ID-%.0f]',this.name,this.objectID));
            else
                imwrite(imind,cm,filePath,'gif',...
                    'WriteMode','append',...
                    'DelayTime',ENV.dt,...
                    'Location',[0 0]);
            end
            pause(0.1);                                                    % Pause for stability
        end
        % AGENT DATA STORAGE
        function [this] = writeAgentData(this,TIME,loopIndicator,loopDuration)
            % This function writes a regular DATA structure to the agent
            % .DATA structure to allow it to be easily interpretted.

            % THE AGENT COMPUTATION TIMES
            this.DATA.indicator(TIME.currentStep) = loopIndicator;
            this.DATA.dt(TIME.currentStep) = loopDuration;
            this.DATA.steps(TIME.currentStep) = TIME.currentStep;
            this.DATA.time(TIME.currentStep) = TIME.currentTime;
        end
    end
    %% ///////////////////////// GET/SET METHODS //////////////////////////
    methods
        % Set the maximum linear speed
        function set.v_max(this,v)
            assert(numel(v) == 1,'Expecting a scalar linear rate.');
            this.v_max = v;
            this.DYNAMICS.maxLinearVelocity(:) = v;
        end
        % Set the maximum angular speed
        function set.w_max(this,w)
            assert(numel(w) == 1,'Expecting a scalar angular rate.');
            this.w_max = w;
            this.DYNAMICS.maxAngularVelocity(:) = w;
        end
        % Set the detection radius
        function set.detectionRadius(this,r)
            assert(isnumeric(r) && numel(r) == 1,'Detection radius must be a scalar value.');
            % Override the SENSORS parameter to reflect the change
            if isstruct(this.SENSORS) && isfield(this.SENSORS,'range')
                this.SENSORS.range = r;                                    % Set the sensor range value
            end
            % Assign the value to the GLOBAL structure
            this.SetGLOBAL('detectionRadius',r);                           % Set the detection radius
        end
        % User update (Override)
        function [this] = ApplyUserOverrides(this,pairArray)
            % This function is designed to parse a generic set of user
            % inputs and allow them to be compared to a default input
            % structure. This should be called in the class constructor

            % Input sanity check #1
            if nargin < 2 || numel(pairArray) == 0
                return
            end

            % Call the all parameter overrider
            [this] = GetParameterOverrides_recursive(this,pairArray);

            % Preform dependant value checks
            this.MEMORY = this.CreateMEMORY(this.maxSamples);        % Initialise memory structure (with 2D varient)

            if this.detectionRadius ~= this.SENSORS.range
                % The detection radius is determined by SENSORS.range
                this.SENSORS.range = this.detectionRadius;
            end
        end
    end
end


function [tParam, regParam] = histology_blockface_reg(pathstrh, fileh, pathstrbf, filebf, regParam, polymask, ellipmask, opts)
% HISTOLOGY_BLOCKFACE_REG  Register deformed histology volume to blockface.
%
% Correct a histology volume obtained by intraslice registration with
% histology_blockface_reg(). The steps in this function are:
%
%   1) Each "valid" histology slice is registered to the corresponding
%   blockface slice (typically with a B-spline, but the user can provide
%   any registration parameters). "Valid" slices are pre-selected by the
%   user, and are those where the registration will give a good result,
%   because there are no artifacts in the wax, the tissue has no tears,
%   etc.
%
%   2) B-spline transforms for "invalid" slices are interpolated and
%   extrapolated from the valid ones.
%
%   3) Each B-spline coefficient is smoothed so that it changes slowly
%   between each slice and its neighbours. This preserves the structural
%   integrity of the histology reconstruction.
%
% [TPARAM, REGPARAM] = histology_blockface_reg(PATHSTRH, FILEH, PATHSTRBF, FILEBF, REGPARAM, POLYMASK, ELLIPMASK)
%
%   PATHSTRH and PATHSTRBF are strings with the paths to the histology and
%   blockface images, respectively.
%
%   FILEH, FILEBF are the lists of histology and blockface files, obtained
%   with dir(). Each file contains a slice. FILEH(i) corresponds to
%   FILEBF(i).
%
%   REGPARAM is a struct (or a string with the full path to a file) with
%   the registration parameters for elastix. Typically, you want this
%   registration to use a multi-level B-spline.
%
%   POLYMASK, ELLIPMASK are two binary masks with the polygon highlighting
%   the wax blockface (POLYMASK) and an ellipse around the heart at it's
%   biggest (ELLIPMASK). The masks can be produced with
%   blockface_create_masks().
%
% ... = histology_blockface_reg(..., OPTS)
%
%   OPTS is a struct with parameters for the algorithm.
%
%     'verbose':   (def 0) Verbose output of the registration algorithm.
%
%     't0':        (def []) Initial transform to apply to histology slices
%                  before registration.
%
%     'IdxIgnore': (def []) Indices of slices where instead from a
%                  registration, the transform is obtained by
%                  interpolation/extrapolation from the other slices.
%                  This parameter can be provided as a vector of indices or
%                  as a boolean vector.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

DEBUG = 0;

% check arguments
narginchk(7, 8);
nargoutchk(0, 2);

% defaults
if (nargin < 8 || isempty(opts) || ~isfield(opts, 'verbose'))
    opts.verbose = 0;
end
if (isempty(opts) || ~isfield(opts, 't0'))
    error('OPTS.t0 expected');
end
if (~isfield(opts, 'IdxIgnore'))
    opts.IdxValid = true(1, length(fileh));
elseif (~islogical(opts.IdxValid))
    % if user provides indices as integers, convert to logical vector so
    % that it's easier to get a list of invalid slices
    aux = false(1, length(fileh));
    aux(opts.IdxValid) = true;
    opts.IdxValid = aux;
end
if (isempty(pathstrh))
    pathstrh = '.';
end
if (isempty(pathstrbf))
    pathstrbf = '.';
end

if (~isempty(opts.t0))
    if (ischar(opts.t0))
        opts.t0 = elastix_read_file2param(opts.t0);
    end
    if ((length(opts.t0) == 1))
        opts.t0 = opts.t0(ones(1, length(fileh)));
    end
end

szMask = size(ellipmask);
if (any(szMask ~= size(polymask)))
    error('ELLIPMASK and POLYMASK must be the same size')
end

% if registration parameters are provided as a filename, convert to struct
if (ischar(regParam))
    regParam = elastix_read_file2param(regParam);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initial alignment of histology to blockface
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% we use 10% of the voxels in the slice
regParam.NumberOfSpatialSamples = round(numel(ellipmask) / 10);

% iterate blockface images
for I = find(opts.IdxValid)

    if (opts.verbose)
        disp(['I = ' num2str(I)])
    end

    % load blockface and histology
    imbf = imread([pathstrbf filesep filebf(I).name]);
    imh = imread([pathstrh filesep fileh(I).name]);

    if (any(szMask ~= size(imbf)))
        error(['Blockface image is not the same size as masks: ' ...
            pathstrbf filesep filebf(I).name])
    end

    % plot slices
    if (DEBUG)
        subplot(2, 1, 1)
        imagesc(imbf)
        subplot(2, 1, 2)
        imagesc(imh)
    end

    % preprocessing of blockface and histology to prepare them for
    % registration
    [imh, imbf] = histology_blockface_preprocessing(imh, imbf, ...
        polymask, ellipmask);

    % plot slices
    if (DEBUG)
        subplot(2, 1, 1)
        imagesc(imbf)
        axis([1 size(imbf, 2) 1 size(imbf, 1)])
        subplot(2, 1, 2)
        imagesc(imh)
    end

    % we are going to use the user-provided initial transform for the
    % registration
    optsI.t0 = opts.t0(I);

    % plot initial alignment
    if (DEBUG)
        imh0 = transformix(optsI.t0, imh);
        subplot(2, 1, 2)
        imagesc(imh0)
        axis([1 size(imbf, 2) 1 size(imbf, 1)])
        imshowpair(imbf, imh0)
    end

    % registration of histology on blockface: Similarity
    optsI.verbose = 0;
    optsI.fMask = ellipmask;
    tic
    [tParam(I), imhreg, iterinfo] = elastix(...
        regParam, imbf, imh, optsI);
    toc

    % plot result of initial registration
    if (DEBUG)
        subplot(2, 1, 1)
        imagesc(imhreg)
        axis([1 size(imbf, 2) 1 size(imbf, 1)])
        subplot(1, 1, 1)
        imshowpair(imbf, imhreg)
    end

end

% DEBUG: check result
if (DEBUG)
    for I = 1:length(filebf)

        disp(['I = ' num2str(I)])

        % load blockface and histology
        imbf = imread([pathstrbf filesep filebf(I).name]);
        imh = imread([pathstrh filesep fileh(I).name]);

        imh0 = transformix(tParam(I), imh);
        imshowpair(imbf, imh0)
        title(['I = ' num2str(I)])
        pause
    end
end

% extract transform parameters to matrix
tParamMat = cat(1, tParam(:).TransformParameters);

% smooth out parameters so that we can fit the histology to the
% blockface without breaking the slice correspondence we already have
for I = 1:tParam(1).NumberOfParameters

    % interpolate/extrapolate values of the sections where the user has
    % signaled that the registration fails
    tParamMat(:, I) = interp1(find(opts.IdxValid), tParamMat(opts.IdxValid, I), ...
        1:length(fileh), 'linear', 'extrap');

    % smooth the values of the B-spline coefficients
    tParamMat(:, I) = smooth(tParamMat(:, I), 'rlowess');

end

% overwrite transform parameters with the smoothed values
for I = 1:length(filebf)
    tParam(I).TransformParameters = tParamMat(I, :);
end

end