function [ output ] = ReadEstimationData( filename)
%READESTIMATIONDATA Function for read input/output system data from file
%   INPUT
%   filename - path to the file
%   OUTPUT
%   output - loaded data in MotionIdentTwinCAT class


% PARSEXML Convert XML file to a MATLAB structure.
try
   tree = xmlread(filename);
catch
   error('Failed to read file %s.',filename);
end

% Recurse over child nodes. This could run into problems 
% with very deeply nested trees.
try
   xData = parseChildNodes(tree);
catch
   error('Unable to parse file %s.',filename);
end

output = MotionIdentTwinCAT();

% Version
index = GetIndexAttribute(xData.Children(2).Attributes, 'Version');
if(index == 0)
    error('Attribute Version was not found');
end
Version = int32(str2double(xData.Children(2).Attributes(index).Value));

% Ams Net ID
index = GetIndexAttribute(xData.Children(2).Attributes, 'AmsNetId');
if(index == 0)
    error('Attribute AmsNetId was not found');
end
output.AmsNetId = xData.Children(2).Attributes(index).Value;

% AdsPort
index = GetIndexAttribute(xData.Children(2).Attributes, 'AdsPort');
if(index == 0)
    error('Attribute AdsPort was not found');
end
output.AdsPort = int32(str2double(xData.Children(2).Attributes(index).Value));

% PathExtGen
index = GetIndexAttribute(xData.Children(2).Attributes, 'PathExtGen');
if(index == 0)
    error('Attribute PathExtGen was not found');
end
output.PathExtGen = xData.Children(2).Attributes(index).Value;

% NameAxis
index = GetIndexAttribute(xData.Children(2).Attributes, 'NameAxis');
if(index == 0)
    error('Attribute NameAxis was not found');
end
output.NameAxis = xData.Children(2).Attributes(index).Value;

% NameInputSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'NameInputSignal');
if(index == 0)
    error('Attribute NameInputSignal was not found');
end
output.NameInputSignal = xData.Children(2).Attributes(index).Value;

% IdInputSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'IdInputSignal');
if(index == 0)
    error('Attribute IdInputSignal was not found');
end
output.IdInputSignal = int32(str2double(xData.Children(2).Attributes(index).Value));

% PeriodInputSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'PeriodInputSignal');
if(index == 0)
    error('Attribute PeriodInputSignal was not found');
end
output.PeriodInputSignal = str2double(xData.Children(2).Attributes(index).Value);

% CountRepeat
index = GetIndexAttribute(xData.Children(2).Attributes, 'CountRepeat');
if(index == 0)
    error('Attribute CountRepeat was not found');
end
output.CountRepeat = int32(str2double(xData.Children(2).Attributes(index).Value));

% ControlMode
index = GetIndexAttribute(xData.Children(2).Attributes, 'ControlMode');
if(index == 0)
    error('Attribute ControlMode was not found');
end
output.ControlMode = int32(str2double(xData.Children(2).Attributes(index).Value));

% Units
index = GetIndexAttribute(xData.Children(2).Attributes, 'Units');
if(index == 0)
    error('Attribute Units was not found');
end
output.Units = int32(str2double(xData.Children(2).Attributes(index).Value));

% DateTime
index = GetIndexAttribute(xData.Children(2).Attributes, 'DateTime');
if(index == 0)
    error('Attribute DateTime was not found');
end
output.DateTimeMd = xData.Children(2).Attributes(index).Value;

% IsLinear
index = GetIndexAttribute(xData.Children(2).Attributes, 'IsLinear');
if(index == 0)
    error('Attribute IsLinear was not found');
end
tempBool = xData.Children(2).Attributes(index).Value;
if(strcmp('True', tempBool) == true)
    output.IsLinear = true;
else
    output.IsLinear = false;
end

% xScale
index = GetIndexAttribute(xData.Children(2).Attributes, 'xScale');
if(index == 0)
    error('Attribute xScale was not found');
end
output.xScale = str2double(xData.Children(2).Attributes(index).Value);

% vScale
index = GetIndexAttribute(xData.Children(2).Attributes, 'vScale');
if(index == 0)
    error('Attribute vScale was not found');
end
output.vScale = str2double(xData.Children(2).Attributes(index).Value);

% FeedConstant
index = GetIndexAttribute(xData.Children(2).Attributes, 'FeedConstant');
if(index == 0)
    error('Attribute FeedConstant was not found');
end
output.FeedConstant = str2double(xData.Children(2).Attributes(index).Value);

% TaskTime
index = GetIndexAttribute(xData.Children(2).Attributes, 'TaskTime');
if(index == 0)
    error('Attribute TaskTime was not found');
end
output.TaskTime = str2double(xData.Children(2).Attributes(index).Value);

% AmplitudeInputSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'AmplitudeInputSignal');
if(index == 0)
    error('Attribute AmplitudeInputSignal was not found');
end
output.AmplitudeInputSignal = str2double(xData.Children(2).Attributes(index).Value);

% AmplitudeAuxiliaryMoveSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'AmplitudeAuxiliaryMoveSignal');
if(index == 0)
    error('Attribute AmplitudeAuxiliaryMoveSignal was not found');
end
output.AmplAuxMove = str2double(xData.Children(2).Attributes(index).Value);

% FrequencyAuxiliaryMoveSignal
index = GetIndexAttribute(xData.Children(2).Attributes, 'FrequencyAuxiliaryMoveSignal');
if(index == 0)
    error('Attribute FrequencyAuxiliaryMoveSignal was not found');
end
output.FreqAuxMove = str2double(xData.Children(2).Attributes(index).Value);

% StepPreTrigger
index = GetIndexAttribute(xData.Children(2).Attributes, 'StepPreTrigger');
if(index == 0)
    error('Attribute StepPreTrigger was not found');
end
output.StepPreTrigger = str2double(xData.Children(2).Attributes(index).Value);

% StepTime
index = GetIndexAttribute(xData.Children(2).Attributes, 'StepTime');
if(index == 0)
    error('Attribute StepTime was not found');
end
output.StepTime = str2double(xData.Children(2).Attributes(index).Value);

% StepInitLevel
index = GetIndexAttribute(xData.Children(2).Attributes, 'StepInitLevel');
if(index == 0)
    error('Attribute StepInitLevel was not found');
end
output.StepInitLevel = str2double(xData.Children(2).Attributes(index).Value);

% StepFinalLevel
index = GetIndexAttribute(xData.Children(2).Attributes, 'StepFinalLevel');
if(index == 0)
    error('Attribute StepFinalLevel was not found');
end
output.StepFinalLevel = str2double(xData.Children(2).Attributes(index).Value);
 
% LengthWindow
index = GetIndexAttribute(xData.Children(2).Attributes, 'LengthWindow');
if(index == 0)
    error('Attribute LengthWindow was not found');
end
output.LengthWindow = int32(str2double(xData.Children(2).Attributes(index).Value));

% ModeSpectAnalys
index = GetIndexAttribute(xData.Children(2).Attributes, 'ModeSpectAnalys');
if(index == 0)
    error('Attribute ModeSpectAnalys was not found');
end
output.ModeSpectAnalys = int32(str2double(xData.Children(2).Attributes(index).Value));
 
% CheckPhaseOverflow
index = GetIndexAttribute(xData.Children(2).Attributes, 'CheckPhaseOverflow');
if(index == 0)
    error('Attribute CheckPhaseOverflow was not found');
end
tempBool = xData.Children(2).Attributes(index).Value;
if(strcmp('True', tempBool) == true)
    output.CheckPhaseOverflow = true;
else
    output.CheckPhaseOverflow = false;
end

% OffsetIOdata
index = GetIndexAttribute(xData.Children(2).Attributes, 'OffsetIOdata');
if(index == 0)
    error('Attribute OffsetIOdata was not found');
end
output.OffsetIOdata = int32(str2double(xData.Children(2).Attributes(index).Value));

% AllowedPositionRange
index = GetIndexAttribute(xData.Children(2).Attributes, 'AllowedPositionRange');
if(index == 0)
    error('Attribute AllowedPositionRange was not found');
end
output.AllowedPositionRange = str2double(xData.Children(2).Attributes(index).Value); 

% EnablePositionRangeCheck
index = GetIndexAttribute(xData.Children(2).Attributes, 'EnablePositionRangeCheck');
if(index == 0)
    error('Attribute EnablePositionRangeCheck was not found');
end
tempBool = xData.Children(2).Attributes(index).Value;
if(strcmp('True', tempBool) == true)
    output.EnablePositionRangeCheck = true;
else
    output.EnablePositionRangeCheck = false;
end

% AllowedTorqueRangeIdent
index = GetIndexAttribute(xData.Children(2).Attributes, 'AllowedTorqueRangeIdent');
if(index == 0)
    error('Attribute AllowedTorqueRangeIdent was not found');
end
output.AllowedTorqueRangeIdent = str2double(xData.Children(2).Attributes(index).Value); 

% EnableTorqueRangeCheckIdent
index = GetIndexAttribute(xData.Children(2).Attributes, 'EnableTorqueRangeCheckIdent');
if(index == 0)
    error('Attribute EnableTorqueRangeCheckIdent was not found');
end
tempBool = xData.Children(2).Attributes(index).Value;
if(strcmp('True', tempBool) == true)
    output.EnableTorqueRangeCheckIdent = true;
else
    output.EnableTorqueRangeCheckIdent = false;
end

% AllowedTorqueRangeStRes
index = GetIndexAttribute(xData.Children(2).Attributes, 'AllowedTorqueRangeStRes');
if(index == 0)
    error('Attribute AllowedTorqueRangeStRes was not found');
end
output.AllowedTorqueRangeStRes = str2double(xData.Children(2).Attributes(index).Value); 

% EnableTorqueRangeCheckStRes
index = GetIndexAttribute(xData.Children(2).Attributes, 'EnableTorqueRangeCheckStRes');
if(index == 0)
    error('Attribute EnableTorqueRangeCheckStRes was not found');
end
tempBool = xData.Children(2).Attributes(index).Value;
if(strcmp('True', tempBool) == true)
    output.EnableTorqueRangeCheckStRes = true;
else
    output.EnableTorqueRangeCheckStRes = false;
end

%% Identification
if output.CountRepeat ~= 0
    % CommandValues
    CountSamples = str2double(xData.Children(4).Children(2).Attributes(1).Value);
    % LengthInputSignal
    output.LengthInputSignal = CountSamples/(output.PeriodInputSignal/output.TaskTime)/output.CountRepeat;

    xData64base =  xData.Children(4).Children(2).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    AllCommandValue = typecast(Data64base,'int32');

    % FeedbackValues
    xData64base =  xData.Children(4).Children(4).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    AllFeedbackValue = typecast(Data64base,'int32');

    output = output.SetValues( AllCommandValue, AllFeedbackValue);
end

%% Step response
% CommandValues
CountSamples = str2double(xData.Children(4).Children(6).Attributes(1).Value);

if CountSamples ~= 0
    xData64base =  xData.Children(4).Children(6).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    output.allCommandStepResValue = typecast(Data64base,'int32');

    % FeedbackValues
    xData64base =  xData.Children(4).Children(8).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    output.allFeedbackStepResValue = typecast(Data64base,'int32');
end

% CurrentValues
CountSamples = str2double(xData.Children(4).Children(10).Attributes(1).Value);
if CountSamples ~= 0
    xData64base =  xData.Children(4).Children(10).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    output.allCurrentValue = typecast(Data64base,'int16');
end

%% Notes
if strcmp(xData.Children(4).Children(12).Attributes(1).Value, '0') == true
    output.Notes = '';
else
    xData64base =  xData.Children(4).Children(12).Children(2).Children.Data;
    Data64base =  base64decode(xData64base);
    output.Notes = native2unicode(Data64base);    
end

end

% ----- Local function PARSECHILDNODES -----
function children = parseChildNodes(theNode)
% Recurse over node children.
children = [];
if theNode.hasChildNodes
   childNodes = theNode.getChildNodes;
   numChildNodes = childNodes.getLength;
   allocCell = cell(1, numChildNodes);

   children = struct(             ...
      'Name', allocCell, 'Attributes', allocCell,    ...
      'Data', allocCell, 'Children', allocCell);

    for count = 1:numChildNodes
        theChild = childNodes.item(count-1);
        children(count) = makeStructFromNode(theChild);
    end
end
end

% ----- Local function MAKESTRUCTFROMNODE -----
function nodeStruct = makeStructFromNode(theNode)
% Create structure of node info.

nodeStruct = struct(                        ...
   'Name', char(theNode.getNodeName),       ...
   'Attributes', parseAttributes(theNode),  ...
   'Data', '',                              ...
   'Children', parseChildNodes(theNode));

if any(strcmp(methods(theNode), 'getData'))
   nodeStruct.Data = char(theNode.getData); 
else
   nodeStruct.Data = '';
end
end

% ----- Local function PARSEATTRIBUTES -----
function attributes = parseAttributes(theNode)
% Create attributes structure.

attributes = [];
if theNode.hasAttributes
   theAttributes = theNode.getAttributes;
   numAttributes = theAttributes.getLength;
   allocCell = cell(1, numAttributes);
   attributes = struct('Name', allocCell, 'Value', ...
                       allocCell);

   for count = 1:numAttributes
      attrib = theAttributes.item(count-1);
      attributes(count).Name = char(attrib.getName);
      attributes(count).Value = char(attrib.getValue);
   end
end
end

% ----- Local function GETINDEXATTRIBUTE -----
function index = GetIndexAttribute(List, Attribute)
    index = 0;
    for i = 1:length(List)
        if(strcmp(List(i).Name, Attribute))
            index = i;
            break;
        end
    end
end



