%% OMAS GRAPHICS (OMAS_graphics.m) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This utility provides tools for interacting with 3D models within OMAS
% based on the 'stlTools' library. This function does in no way take credit
% for the design of the functions inclosed, only for their arrangment as
% part of the OMAS package.

% Author: James A. Douthwaite 08/03/18

classdef OMAS_graphics
    %% GENERAL GEOMETRY MANIPULATION
    methods (Static, Access = public)
        % SORT POINT CLOUD IN A CLOCK WISE ABOUT A VECTOR
        function [verticesCW] = sortVerticesCW(planarNormal,centroid,vertexData)
            % This function takes a list of vertex data and sorts them into
            % angularly in relation to the planar normal vector.

            % INPUT HANDLING
            if numel(centroid) == 2
                centroid = [centroid;0];
            end
            % THE REFERENCE
            unit_reference = [1;0;0];

            % DEFINE THE VECTORS RELATIVE TO THE CENTROID
            vertexVectors = vertexData - centroid';

            tempMatrix = zeros(size(vertexVectors,1),2);
            for i = 1:size(vertexVectors,1)
                unit_v = vertexVectors(i,:)/norm(vertexVectors(i,:));
                vertexNormal = cross(unit_reference,unit_v);
                % THE ROTATION DIRECTION
                tempMatrix(i,1) = -sign(dot(vertexNormal,planarNormal));
                % THE ROTATION MAGNITUDE
                tempMatrix(i,2) = acos(dot(unit_reference,unit_v));
                % MAP TO CCW ROTATION
                if tempMatrix(i,1) < 0
                    tempMatrix(i,2) = 2*pi - tempMatrix(i,2) ;
                end
            end
            % SORT THE VERTICE BY ANGLE RELATIVE
            [~,sortedIndices] = sortrows(tempMatrix(:,2),1);
            % GET THE SORTED VERTEX ARRAY
            verticesCW = vertexData(sortedIndices,:);
        end
        % SORT POINT CLOUD IN A COUNTER-CLOCK WISE ABOUT A VECTOR
        function [verticesCCW] = sortVerticesCCW(planarNormal,centroid,vertexData)
            % This function takes a list of vertex data and sorts them into
            % angularly in relation to the planar normal vector.

            % INPUT HANDLING
            if numel(centroid) < 3
                centroid = [centroid;0];
            end
            % THE REFERENCE
            unit_reference = [1;0;0];

            % DEFINE THE VECTORS RELATIVE TO THE CENTROID
            vertexVectors = vertexData - centroid';

            tempMatrix = zeros(size(vertexVectors,1),2);
            for i = 1:size(vertexVectors,1)
                unit_v = vertexVectors(i,:)/norm(vertexVectors(i,:));
                vertexNormal = cross(unit_reference,unit_v);
                % THE ROTATION DIRECTION
                tempMatrix(i,1) = sign(dot(vertexNormal,planarNormal));
                % THE ROTATION MAGNITUDE
                tempMatrix(i,2) = acos(dot(unit_reference,unit_v));
                % MAP TO CCW ROTATION
                if tempMatrix(i,1) < 0
                    tempMatrix(i,2) = 2*pi - tempMatrix(i,2) ;
                end
            end
            % SORT THE VERTICE BY ANGLE RELATIVE
            [~,sortedIndices] = sortrows(tempMatrix(:,2),1);
            % GET THE SORTED VERTEX ARRAY
            verticesCCW = vertexData(sortedIndices,:);

%             % DEBUG PLOTS
%             figure(3)
%             hold on; axis equal; grid on;
%             scale = 10;
%             q = quiver3(centroid(1),centroid(2),centroid(3),...
%                         scale*unit_reference(1),scale*unit_reference(2),scale*unit_reference(3),'r');
%             q = quiver3(centroid(1),centroid(2),centroid(3),...
%                         scale*planarNormal(1),scale*planarNormal(2),scale*planarNormal(3),'g');
%             for i = 1:size(verticesCCW,1)
%                 plot3(verticesCCW(i,1),verticesCCW(i,2),verticesCCW(i,3),'ro');
%             end
        end
        % PLANAR PROHECTION OF A 3D GEOMETRY
        function [planarVertices] = geometryPlanarProjection(planarNormal,centroid,vertexData)
            % This function returns a vertex projections on a plane defined
            % by the normal vector provided

            % INPUT HANDLING
            assert(size(planarNormal,1) == size(centroid,1),'The planar normal and centroid must be [3x1]');
            assert(size(planarNormal,2) == 1,'The planar normal and centroid must be [3x1]');
            assert(size(vertexData,2) == 3,'The geometry data must be [:x3].');

            % DEFINE THE VECTORS BETWEEN THE CENTROID AND VERTICES
            vertexData = vertexData - centroid';
            planarVertices = zeros(size(vertexData,1),3);
            for i = 1:size(vertexData,1)
                % The vertex planar projections
                [projection] = OMAS_geometry.vectorPlanarProjection(...
                                    planarNormal,...
                                    vertexData(i,:)');
                % REFORMAT THE PROJECTIONS
                planarVertices(i,:) = projection' + centroid';
            end
            % REMOVE DUPLICATES
            [planarVertices,~] =  unique(planarVertices, 'rows');
            % We cannot gaurantee the geometry will be the same one the
            % unique command is ran. The face data may reference vertices
            % that have been moved or deleted.
        end
        % DEFINE SCALE OF GEOMETRY STRUCTURE
        function [geometry] = scale(geometry,scale)
            % This function scales a geometry in accordance to a provided
            % scalar or vector of dimensional scalars. A geometry is
            % defined as a vertices, faces structure.

            assert(numel(scale) == 1 || numel(scale) == 3,'Please provide a valid scale value.');
            % Apply the scaling either by dimension or unilaterally.
            if numel(scale) == 3
                geometry.vertices = geometry.vertices*diag(scale);
            else
                geometry.vertices = geometry.vertices*(eye(3)*scale);
            end
        end
        % NORMALISE THE STL FILE
        function [geometry] = normalise(geometry)
            % This function normalises the STL geometry to allow it to be
            % scaled appropriately.
            % GEOMETRY CHECK
            assert(size(geometry.vertices,2) == 3 && size(geometry.faces,2) == 3,...
                'The provided patch object has invalid vertex and faces assignments.');

            % GET THE VERTEX MAGNITUDES RELATIVE TO THE ORIGIN
            verticesNorms = abs(geometry.vertices);
            % MAXIMUMS IN THE FIRST AND SECOND DIMENSIONS
            dimMaximal = max(max(abs(verticesNorms),[],1),[],2);
            % SCALE GEOMETRY SPECIFIED BY VIRTUAL.radius
            geometry.vertices = (geometry.vertices/dimMaximal);
        end
        % REMOVE FACE/VERTEX DUPLICATES
        function [geometry] = removeDuplicateVertices(geometry)
            % PATCHSLIM removes duplicate vertices in surface meshes.
            %
            % This function finds and removes duplicate vertices.
            %
            % USAGE: [v, f]=patchslim(v, f)
            %
            % Where v is the vertex list and f is the face list specifying vertex
            % connectivity.
            %
            % v contains the vertices for all triangles [3*n x 3].
            % f contains the vertex lists defining each triangle face [n x 3].
            %
            % This will reduce the size of typical v matrix by about a factor of 6.
            %
            % For more information see:
            %  http://www.esmonde-white.com/home/diversions/matlab-program-for-loading-stl-files
            %
            % Francis Esmonde-White, May 2010

            if ~isfield(geometry,'vertices')
                error('The geometry does not have a specified "vertex" list.');
            end
            if ~isfield(geometry,'faces')
                error('The geometry does not have a specified "face" triangulation list.');
            end

            % REMOVE DUPLICATES
            [geometry.vertices,~,indexn] =  unique(geometry.vertices, 'rows');
            geometry.faces = indexn(geometry.faces);
        end
        % CALCULATE GEOMETRY SURFACE NORMALS
        function [normals]  = normals(geometry)
            % This function computes the surface normals of a defined
            % geometry structure
            normals = zeros(size(geometry.faces));
            for face = 1:size(geometry.faces,1)
                % MEMBERS IF THE PLANE
                memberID_A = geometry.faces(face,1);
                memberID_B = geometry.faces(face,2);
                memberID_C = geometry.faces(face,3);
                % SURFACE DEFINING VECTORS
                BA = geometry.vertices(memberID_B,:) - geometry.vertices(memberID_A,:);
                CA = geometry.vertices(memberID_C,:) - geometry.vertices(memberID_A,:);
                % THE NORMAL
                normals(face,:) = cross(BA,CA);
                normals(face,:) = normals(face,:)/norm(normals(face,:));
            end
        end
    end

    %% UNIVERSAL DRAWING MECHANISMS
    methods (Static)
        % GET HIT-BOX GEOMETRY
        function [hitBoxGeometry] = getHitBoxGeometry(VIRTUAL,geometry)
            % Input check
            assert(isa(VIRTUAL.hitBoxType,'uint8'),'Object hit-box type must be of type "uint8".');
            % Derive hit-box geometry
            switch VIRTUAL.hitBoxType
                case OMAS_hitBoxType.none
                    % Assemble the spherical constraint volume
                    hitBoxGeometry = [];
                case OMAS_hitBoxType.spherical
                    % Assemble the spherical constraint volume
                    hitBoxGeometry = OMAS_graphics.defineSphere(zeros(3,1),VIRTUAL.radius,10);
                case OMAS_hitBoxType.AABB
                    % Assemble the AABB constraint volume
                    R = OMAS_geometry.quaternionToRotationMatrix(VIRTUAL.quaternion);
                    % Rotate the body before defining constraint volume
                    geometry.vertices = R*geometry.vertices;
                    % DEFINE AN ALIGNED AABB CUBOID
                    hitBoxGeometry = OMAS_graphics.defineCuboid(min(geometry.vertices),...
                                                                  max(geometry.vertices));
                case OMAS_hitBoxType.OBB
                    % DEFINE AN ALIGNED OBB CUBOID
                    hitBoxGeometry = OMAS_graphics.defineCuboid(min(geometry.vertices),...
                                                                  max(geometry.vertices));
                    % Assemble the OBB constraint volume
                    R = OMAS_geometry.quaternionToRotationMatrix(VIRTUAL.quaternion);
                    % Rotate the hit-box to be aligned with the geometry
                    hitBoxGeometry.vertices = R*hitBoxGeometry.vertices;
                otherwise
                    error('Hit-box type not recognised');
            end
        end
        % DEFINE CUBOID (MINOR) FROM RADIUS
        function [geometry,minExtents,maxExtents] = defineCuboidFromRadius(center,radius)
            % This function creates a set of vertices for a cube
            % encapsuated in a sphere of a given radius.
            assert(numel(center) == 3,'The cuboid center must be a cartesian vector.');
            assert(numel(radius) == 1,'The radius of the sphere must be a scalar and non-zero.');
            % RATE OF DEMENSIONAL EXPANSION
            h = radius/1.7321;
            % DEFINE THE CUBOID EXTENTS
            minExtents = center - h;
            maxExtents = center + h;
            % DEFINE THE CUBOID VERTICES
            [geometry] = OMAS_graphics.defineCuboid(minExtents,maxExtents);
        end
        % DEFINE CUBOID
        function [geometry,minExtents,maxExtents] = defineCuboid(minExtents,maxExtents)
            % Return a matrix of point defining a cuboid scaled to that of
            % a dimensions provided.

            % Define vertex data from limits
            vertices = zeros(8,3);
            vertices(1,:) = [maxExtents(1),maxExtents(2),maxExtents(3)];
            vertices(2,:) = [maxExtents(1),maxExtents(2),minExtents(3)];
            vertices(3,:) = [maxExtents(1),minExtents(2),minExtents(3)];
            vertices(4,:) = [maxExtents(1),minExtents(2),maxExtents(3)];
            vertices(5,:) = [minExtents(1),minExtents(2),minExtents(3)];
            vertices(6,:) = [minExtents(1),minExtents(2),maxExtents(3)];
            vertices(7,:) = [minExtents(1),maxExtents(2),maxExtents(3)];
            vertices(8,:) = [minExtents(1),maxExtents(2),minExtents(3)];
            geometry.vertices = vertices;
            % Define face connectivity matrix
            geometry.faces =  [  1     2     7
                                 1     4     2
                                 1     7     4
                                 2     3     8
                                 2     4     3
                                 2     8     7
                                 3     4     6
                                 3     5     8
                                 3     6     5
                                 4     7     6
                                 5     6     8
                                 6     7     8];
        end
        % DRAW SPHERE
        function [geometry] = defineSphere(position,radius,faces)
            % This function returns a sphere coordinate cloud at a given
            % position in space, of a given radius. 'figureHandle' is used
            % to provide the function context.

            % INPUT CHECKING
            if nargin < 3
                faces = 10;
            end
            % DEFINE SPHERE TRIANGULATION
            [X,Y,Z] = sphere(faces);
            X = X.*radius + position(1);
            Y = Y.*radius + position(2);
            Z = Z.*radius + position(3);
            % CONVERT TO PATCH OBJECT
            [geometry.faces,geometry.vertices,~] = surf2patch(X,Y,Z,'triangles');
            % DEFINE CENTROID
            geometry.centroid = position'; %reducepatch(P, R)
        end
        % DRAW UNIT TRIAD
        function [triadHandle] = drawTriad(figureHandle,position,R,scale)
            % Draw a unit triad at a cartesian position, rotated by R.
            % FigureHandl Here is used to bring the current figure to the
            % function context.

            % INPUT CHECL
            if nargin < 4
                scale = 1;
            end

            colourVector = 'rgb';
            triadVectors = scale*eye(3);
            for axis = 1:size(triadVectors,2)
                triadVectors(:,axis) = R*triadVectors(:,axis);
                % Draw vectors
                triadHandle(axis) = quiver3(gca,position(1),position(2),position(3),triadVectors(1,axis),triadVectors(2,axis),triadVectors(3,axis),colourVector(axis));
            end
        end
    end

    %% STL IMPORTATION
    methods (Static, Access = public)
        % PREPARE THE PATCH FILES FOR VISUALS
        function [geometry,successFlag] = importStlFromFile(filename)
            % This function prepares object STL files for presentation. The assumption
            % is that object stl file has the same name as the object being simulated.

            try
                % GET THE STL FILE
                geometry = OMAS_graphics.stlRead(filename);
                successFlag = 1;
            catch
                % NO STL WAS FOUND BY THAT FILE NAME (fail quietly)
                geometry = [];
                successFlag = 0;
            end
            % ENSURE UNIQUE VERTICES
            if successFlag
                geometry = OMAS_graphics.removeDuplicateVertices(geometry);
            end
        end
        % READ AND STL INTO VERTICIES, FACES
        function [geometry, name] = stlRead(fileName)
            %STLREAD reads any STL file not depending on its format
            %V are the vertices
            %F are the faces
            %N are the normals
            %NAME is the name of the STL object (NOT the name of the STL file)

            [format,isSuccessful] = OMAS_graphics.stlGetFormat(fileName);

            if strcmp(format,'ascii')
                [geometry,name] = OMAS_graphics.stlReadAscii(fileName);
            elseif strcmp(format,'binary')
                [geometry,name] = OMAS_graphics.stlReadBinary(fileName);
            end
        end
        % IDENTIFY STL TYPE
        function [format,isSuccessful] = stlGetFormat(fileName)
            %STLGETFORMAT identifies the format of the STL file and returns 'binary' or
            %'ascii'

            fid = fopen(fileName);
            if fid == -1        % Unsuccessful file load
                isSuccessful = 0;
            else
                isSuccessful = 1;
            end

            % Check the file size first, since binary files MUST have a size of 84+(50*n)
            fseek(fid,0,1);         % Go to the end of the file
            fidSIZE = ftell(fid);   % Check the size of the file
            if rem(fidSIZE-84,50) > 0
                format = 'ascii';
            else
                % Files with a size of 84+(50*n), might be either ascii or binary...
                % Read first 80 characters of the file.
                % For an ASCII file, the data should begin immediately (give or take a few
                % blank lines or spaces) and the first word must be 'solid'.
                % For a binary file, the first 80 characters contains the header.
                % It is bad practice to begin the header of a binary file with the word
                % 'solid', so it can be used to identify whether the file is ASCII or
                % binary.
                fseek(fid,0,-1);                                           % go to the beginning of the file
                header = strtrim(char(fread(fid,80,'uchar')'));            % trim leading and trailing spaces
                isSolid = strcmp(header(1:min(5,length(header))),'solid'); % take first 5 char
                fseek(fid,-80,1);                                          % go to the end of the file minus 80 characters
                tail = char(fread(fid,80,'uchar')');
                isEndSolid = findstr(tail,'endsolid');

                % Double check by reading the last 80 characters of the file.
                % For an ASCII file, the data should end (give or take a few
                % blank lines or spaces) with 'endsolid <object_name>'.
                % If the last 80 characters contains the word 'endsolid' then this
                % confirms that the file is indeed ASCII.
                if isSolid && isEndSolid
                    format = 'ascii';
                else
                    format = 'binary';
                end
            end
            fclose(fid);
        end
        % INTERPRET A BINARY STL FILE
        function [geometry, name] = stlReadBinary(fileName)
            %STLREADBINARY reads a STL file written in BINARY format
            %V are the vertices
            %F are the faces
            %N are the normals
            %NAME is the name of the STL object (NOT the name of the STL file)

            %=======================
            % STL binary file format
            %=======================
            % Binary STL files have an 84 byte header followed by 50-byte records, each
            % describing a single facet of the mesh.  Technically each facet could be
            % any 2D shape, but that would screw up the 50-byte-per-facet structure, so
            % in practice only triangular facets are used.  The present code ONLY works
            % for meshes composed of triangular facets.
            %
            % HEADER:
            % 80 bytes:  Header text
            % 4 bytes:   (int) The number of facets in the STL mesh
            %
            % DATA:
            % 4 bytes:  (float) normal x
            % 4 bytes:  (float) normal y
            % 4 bytes:  (float) normal z
            % 4 bytes:  (float) vertex1 x
            % 4 bytes:  (float) vertex1 y
            % 4 bytes:  (float) vertex1 z
            % 4 bytes:  (float) vertex2 x
            % 4 bytes:  (float) vertex2 y
            % 4 bytes:  (float) vertex2 z
            % 4 bytes:  (float) vertex3 x
            % 4 bytes:  (float) vertex3 y
            % 4 bytes:  (float) vertex3 z
            % 2 bytes:  Padding to make the data for each facet 50-bytes in length
            %   ...and repeat for next facet...

            fid = fopen(fileName);
            header = fread(fid,80,'int8'); % reading header's 80 bytes
            name = deblank(native2unicode(header,'ascii')');
            if isempty(name)
                name = 'Unnamed Object'; % no object name in binary files!
            end
            nfaces = fread(fid,1,'int32');  % reading the number of facets in the stl file (next 4 byters)
            nvert = 3*nfaces; % number of vertices
            % reserve memory for vectors (increase the processing speed)
            n = zeros(nfaces,3);
            v = zeros(nvert,3);
            f = zeros(nfaces,3);
            for i = 1 : nfaces % read the data for each facet
                tmp = fread(fid,3*4,'float'); % read coordinates
                n(i,:) = tmp(1:3); % x,y,z components of the facet's normal vector
                v(3*i-2,:) = tmp(4:6); % x,y,z coordinates of vertex 1
                v(3*i-1,:) = tmp(7:9); % x,y,z coordinates of vertex 2
                v(3*i,:) = tmp(10:12); % x,y,z coordinates of vertex 3
                f(i,:) = [3*i-2 3*i-1 3*i]; % face
                fread(fid,1,'int16'); % Move to the start of the next facet (2 bytes of padding)
            end
            fclose(fid);
            % Geometric structure
            geometry = struct('vertices',v,'faces',f,'normals',n);
            [geometry] = OMAS_graphics.removeDuplicateVertices(geometry);
        end
        % INTERPRET AN ASCII STL FILE
        function [geometry, n, name] = stlReadAscii(fileName)
            %STLREADASCII reads a STL file written in ASCII format
            %V are the vertices
            %F are the faces
            %N are the normals
            %NAME is the name of the STL object (NOT the name of the STL file)

            %======================
            % STL ascii file format
            %======================
            % ASCII STL files have the following structure.  Technically each facet
            % could be any 2D shape, but in practice only triangular facets tend to be
            % used.  The present code ONLY works for meshes composed of triangular
            % facets.
            %
            % solid object_name
            % facet normal x y z
            %   outer loop
            %     vertex x y z
            %     vertex x y z
            %     vertex x y z
            %   endloop
            % endfacet
            %
            % <Repeat for all facets...>
            %
            % endsolid object_name

            fid = fopen(fileName);
            cellcontent = textscan(fid,'%s','delimiter','\n');             % read all the file and put content in cells
            content = cellcontent{:}(logical(~strcmp(cellcontent{:},''))); % remove all blank lines
            fclose(fid);

            % read the STL name
            line1 = char(content(1));
            if (size(line1,2) >= 7)
                name = line1(7:end);
            else
                name = 'Unnamed Object';
            end

            % read the vector normals
            normals = char(content(logical(strncmp(content,'facet normal',12))));
            n = str2double(normals(:,13:end));

            % read the vertex coordinates (vertices)
            vertices = char(content(logical(strncmp(content,'vertex',6))));
            v = str2double(vertices(:,7:end));
            nvert = size(vertices,1);                       % number of vertices
            nfaces = sum(strcmp(content,'endfacet'));       % number of faces
            if (nvert == 3*nfaces)
                f = reshape(1:nvert,[3 nfaces])';           % create faces
            end
            % Geometric structure
            geometry = struct('vertices',v,'faces',f,'normals',n);
            % slim the file (delete duplicated vertices)
            [geometry] = OMAS_graphics.removeDuplicateVertices(geometry);
        end
        % PLOT THE STL
        function stlPlot(geometry, name)
            %STLPLOT is an easy way to plot an STL object
            %V is the Nx3 array of vertices
            %F is the Mx3 array of faces
            %NAME is the name of the object, that will be displayed as a title

            figure;
            patch(geometry,...
                'FaceColor',[0.8 0.8 1.0], ...
                'EdgeColor','none',...
                'FaceLighting','gouraud',...
                'AmbientStrength', 0.15);

            % Add a camera light, and tone down the specular highlighting
            camlight('headlight');
            material('dull');

            % Fix the axes scaling, and set a nice view angle
            axis('image');
            view([-135 35]);
            grid on;
            title(name);
        end
    end
end


function h = ldivide(f,g)
%.\   Pointwise CHEBFUN left divide.
%   F.\G returns a CHEBFUN that represents the function G(x)/F(x).
%
% See also RDIVIDE, MLDIVIDE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Call RDIVIDE():
h = rdivide(g, f);

end


function out = ind2char(i,s)
% calculates coordinate from one index
% i - index
% s - size of matrix

out = '(';
for d = 1:length(s)
	out = [out,int2str(mod(ceil(i/prod(s(1:d-1)))-1,s(d))+1),',']; %#ok<AGROW>
end
out(end) = ')';


function plotCI(data, m1, s1, ci1, m2, s2, ci2)

% plotCI(data, m1, s1, mci1, m2, s2, mci2)
%   Plot profiles, average profiles, and confidence intervals
%
% Input:
%   data - historical energy usage data
%   m1 - mean daily profile
%   s1 - standard deviation of daily profile
%   ci1 - 95% confidence interval of daily profile
%   m2 - mean month profile
%   s2 - standard deviation of month profile
%   ci2 - 95% confidence interval of month profile

% Copyright 2006-2009 The MathWorks, Inc.

%% Get number of days and number of hours
  numDays  = size(data, 1);
  numHours = size(data, 2);

%% Create Figure
  figure('Units', 'Pixels', 'Position', [100, 100, 800, 400]);

  % Create subplot for visualizing daily profile
  subplot(1,2,1);
  plot(data', 'Color', [.8 .8 .8]);
  patch([1:numHours,numHours:-1:1],[ci1(1,:), ci1(2,end:-1:1)], [.8 .8 .8])
  line(1:numHours, m1, 'Linewidth', 2, 'Color', 'r');
  grid on; xlim([1 numHours]);
  xlabel('hours');
  ylabel('system load (MW)');
  title('Daily Profile (mean & CI)');

  % Create subplot for visualizing month profile
  subplot(1,2,2);
  plot(data, 'Color', [.8 .8 .8]);
  patch([1:numDays,numDays:-1:1],[ci2(1,:), ci2(2,end:-1:1)], [.8 .8 .8])
  line(1:numDays, m2, 'Linewidth', 2, 'Color', 'r');
  grid on;xlim([1 numDays]);
  xlabel('days');
  ylabel('system load (MW)');
  title('Month Profile (mean & CI)');


function h = huniform ( p, varargin )

%*****************************************************************************80
%
%% HUNIFORM returns a uniform mesh size function.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,ND), the point coordinates.
%
%    Input, VARARGIN, room for extra arguments.
%
%    Output, real H(NP,1), the mesh size function.
%
  np = size ( p, 1 );
  h = ones ( np, 1 );

  return
end


% DEMDIMREDVARGPLVM A simple demonstration of dimensionality reduction for
% the Bayesian GP-LVM
% COPYRIGHT: Andreas C. Damianou, 2012
% VARGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

% Constants
dataSetName = 'toy';
experimentNo = 1;
latentDim = 2; % Anything > 3 and < 10
dynamicsUsed = false;
embedLinearly = false;

%%%%%%%%%%%%%%
t = linspace(0,4*pi,100);
% The original signal will be a cosine, a sine and a squared cosine.
Z1 = cos(t)';
Z2 = sin(t)';
%Z3= (cos(t)').^2;

% Store the original signals. Normally, the corresponding latent functions
% found for these signals will not be ordered, i.e. Z1 does not necessarily
% correspond to x_1(t). But with fixed random seeds it seems that the following
% correspondence is done Z1 ->x_3(t), Z2->x_2(t), X3 ->x_1(t)
%if dynamicsUsed
%    Z{1} = Z3; Z{2} = Z2; Z{3} = Z1;
%else
%    Z{1} = Z1; Z{2} = Z2; Z{3} = Z3;
%end
Z{1} = Z1; Z{2} = Z2;

% Scale and center data
bias_Z1 = mean(Z1);
Z1 = Z1 - repmat(bias_Z1,size(Z1,1),1);
scale_Z1 = max(max(abs(Z1)));
Z1 = Z1 ./scale_Z1;

bias_Z2 = mean(Z2);
Z2 = Z2 - repmat(bias_Z2,size(Z2,1),1);
scale_Z2 = max(max(abs(Z2)));
Z2 = Z2 ./ scale_Z2;

noiseLevel = 0; % Default: 0.1 (or 0.5)

%------- LINEAR EMBEDING-----
% Map 1-D to 3-D and add some noise
if embedLinearly
    Z2p = Z2*rand(1,3);
    Z2p = Z2p + noiseLevel.*randn(size(Z2p));
    Z1p = Z1*rand(1,3);
    Z1p = Z1p + noiseLevel.*randn(size(Z1p));
else
    Z2p = exp((Z2*rand(1,3))).^2;
    Z2p = Z2p + noiseLevel.*randn(size(Z2p));
    Z1p = exp((Z1*rand(1,3))).^2;
    Z1p = Z1p + noiseLevel.*randn(size(Z1p));
%    x = linspace(-1, 1, 100)'; % input indices in the x-axis
%     trueKern = kernCreate(x, 'matern32'); % Generate samples from this kernel
%     K = kernCompute(trueKern, x) + eye(size(x, 1))*noiseVar;
%     % Sample some true function values.
%     yTrue = gsamp(zeros(size(x))', K, 3)';
end

% Form dataset by concatenating the signals
Y = [Z1p Z2p];
t = t';
%%%%%%%%%%%%%%

% Set up model
options = vargplvmOptions('dtcvar');
if embedLinearly
    options.kern = {'linard2', 'bias', 'white'};
else
    options.kern = {'rbfard2', 'bias', 'white'};
end
options.numActive = 60; % Number of inducing points
options.optimiser = 'scg';
timeStampsTraining = t;

d = size(Y, 2);
fprintf(1,'# Creating the model...\n');
model = vargplvmCreate(latentDim, d, Y, options);
model = vargplvmParamInit(model, model.m, model.X);
model.beta=1/(0.01*var(model.m(:)));
model.vardist.covars = 0.5*ones(size(model.vardist.covars)) + 0.001*randn(size(model.vardist.covars));

if dynamicsUsed
    %-------- Add dynamics to the model -----
    optionsDyn.type = 'vargpTime';
    optionsDyn.t=t;

    % Dynamic kernel:
    % Putting a "whitefixed" instead of "white", will make the model give
    % samples x(t) that are more realisticly related to the observed data.
    kern = kernCreate(t, {'rbfperiodic', 'white'});
    % The following is related to the expected number of
    % zero-crossings.(larger inv.width numerator, rougher function)
    if ~strcmp(kern.comp{1}.type,'ou')
        kern.comp{1}.inverseWidth = 5./(((max(t)-min(t))).^2);
        kern.comp{1}.variance = 1;
    end
    optionsDyn.kern = kern;

    % Fill in with default values whatever is not already set
    optionsDyn = vargplvmOptionsDyn(optionsDyn);
    model = vargplvmAddDynamics(model, 'vargpTime', optionsDyn, optionsDyn.t, 0, 0,optionsDyn.seq);
    fprintf(1,'# Further calibration of the initial parameters...\n');
    model = vargplvmInitDynamics(model,optionsDyn);
end
modelInit = model;

%---------- OPTIMISATION ------
% do not learn beta for few iterations for intitilization
model.learnBeta = 0;
display = 1;
fprintf(1,'# Intitiliazing the model (fixed beta) %d iterations...\n',300);
model = vargplvmOptimise(model, display, 300);
disp('# Saving model after optimising beta...')
%modelWriteResult(model, dataSetName, experimentNo);

% Optimise the model.
model.learnBeta = 1;
iters = 200; % Default: 1000
fprintf(1,'# Optimising the model for %d iterations...\n',iters);
model = vargplvmOptimise(model, display, iters);
% Save the results.
fprintf(1,'# Saving model after doing %d iterations\n',iters)
%modelWriteResult(model, dataSetName, experimentNo);

%--------- SIMPLE EVALUATION -----
% See the final lengthscales (see how some dimensions are switched-off).
bar(model.kern.comp{1}.inputScales); title('final lengthscales'); xlabel('q')

[a,ind]=sort(model.kern.comp{1}.inputScales,'descend'); % sort lengthscales
fprintf('Plotting the 2 latent functions corresponding to the largest 3 dimensions (scales):\n');
% Note: this is a plot of the latent functions x_q, they don't necessarily have
% to match the observed data Y, but their shape can provide insights, e.g. we
% know to expect something in the form of sines and cosines.
figure
for i=1:2
    subplot(1,2,i)
    plot(model.X(:,ind(i)));hold on, plot(Z{i},'r'), hold off
end
xlabel('N'); legend('latent function','original signal');

if dynamicsUsed
    figure
    dt = t(end)-t(end-1);
    timeStampsTest = (t(end)+dt:dt:t(end)+100*dt)'; % 100 time points in the future
    % Predict only given a test time vector (no partial information)
    [Testmeans2 Testcovars2] = vargplvmPredictPoint(model.dynamics, timeStampsTest);
    Varmu2 = vargplvmPosteriorMeanVar(model, Testmeans2, Testcovars2); % Predicted values
    model.dynamics.t_star = timeStampsTest;
    % Sample
    [ySamp, xSamp] = vargpTimeDynamicsSample(model, 1);
    d=3;
    plot(t,Y(:,d)), hold on, plot(timeStampsTest,Varmu2(:,d),'r'), plot(timeStampsTest,ySamp(:,d),'g')
    title(['Plot for dimension d=' num2str(d)]); legend('Y(:,d)','Prediction given t','Sampling'); xlabel('t');
end

%%%%%%%%%%%%%%%%%%%%%
%pcaX = pcaEmbed(Y,2);
[U,V] = pca(Y,2);
pcaX = Y*V;

subplot(4,2,1); plot3(model.X(:,1), model.X(:,2),t); xlabel('x_1'); ylabel('x_2'); zlabel('t');
subplot(4,2,2); plot3(Z1,Z2,t);     xlabel('Z1'); ylabel('Z2'); zlabel('t');
subplot(4,2,3); plot(model.X(:,1)); ylabel('x_1'); xlabel('t');
subplot(4,2,4); plot(model.X(:,2)); ylabel('x_2'); xlabel('t');
subplot(4,2,5); plot(pcaX(:,1)); ylabel('pcaX_1'); xlabel('t');
subplot(4,2,6); plot(pcaX(:,2)); ylabel('pcaX_2'); xlabel('t');
subplot(4,2,7); surf(Z1p);          title('Z1p');
subplot(4,2,8); surf(Z2p);          title('Z2p');


%% Demo 2: With missing data

% Add path
addpath(genpath('../nricp'));
addpath(genpath('../../toolbox_graph/toolbox_graph/'));
addpath(genpath('../../icp'));

% load data
load ('../data/faceSource.mat');
load ('../data/faceTargetMissing.mat');

% Specify that surface normals are available and can be used.
Options.useNormals = 1;

% Specify that the source deformations should be plotted.
Options.plot = 1;

% Perform non-rigid ICP
[pointsTransformed, X] = nricp_landmarks(Source, TargetMissing, Options);


% This program is an implementation of the on-line
% Co-Active Neuro-Fuzzy Inference System (CANFIS) algorithm.
% The structure of the network is determoned by the user.
% The premise (nonlinear) parameters are estimated by Gradient Descent (GD)
% through error backpropagation.
% The consequent (linear) parameters are estimated by Recursive LSE.

clc;
clear;
close all;
colordef black;

NumOfEpochs   = 10;
NumOfSamples = 400;
NumInVars         = 6;
NumOutVars      = 3;
NumInTerms      = 2;
lamda                 = 1;
rmse                   = zeros(NumOutVars,NumOfEpochs);
NumRules          = NumInTerms^NumInVars;
Ita1                      = .0015;
Ita2                      = .0025;
alpha1                = 0.00;
alpha2                = 0.00;
P                         = 1e8*eye((NumInVars+1)*NumRules);

% INPUT-OUTPUT training pair construction.
% We will try to teach the ANFIS network the chaotic

load lorenz_data.mat;
Process  = lorenz_dat; % a (3 X 8056) Lorenz Attractor Solution [X Y Z]' Points Data Matrix.

% Target Output is formulated as a matrix whose lines represent different
% output variables and its columns different time instants.

e         = zeros(NumOutVars,NumOfSamples);
TrgtOut   = zeros(NumOutVars,NumOfSamples);
TrgtOutCh = zeros(NumOutVars,NumOfSamples);

ThetaL4  = zeros((NumInVars+1)*NumRules,NumOutVars);  % Consequent Parameters in Layer 4.
dThetaL4 = zeros((NumInVars+1)*NumRules,NumOutVars);

In1           = zeros(NumInVars,NumOfSamples);
In11         = zeros(NumInVars,NumOfSamples);
Out5        = zeros(NumOutVars,NumOfSamples);
Theta21  = zeros(NumInVars,NumInTerms);
Theta32  = zeros(NumRules,NumRules);
mean1    = zeros(NumInVars,NumInTerms);
sigma1   = zeros(NumInVars,NumInTerms);
dmean1  = zeros(NumInVars,NumInTerms);
dsigma1 = zeros(NumInVars,NumInTerms);
db1          = zeros(NumInVars,NumInTerms);

% Each row corresponds to a different Input Linguistic Variable Xi.
% Each i-j entry corresponds to a different mean (center)
% of the bell-shaped function of the j-th Term of the i-th Input Linguistic
% Variable Xi.

Process0 = Process(:,1:NumOfSamples);
Process1 = Process(:,2:NumOfSamples+1);
Process2 = Process(:,3:NumOfSamples+2);

In1 = [Process1; Process0;];
%In1 = Process1;
TrgtOut = Process2;

Process0ch = Process(:,5*NumOfSamples+1:6*NumOfSamples);
Process1ch = Process(:,5*NumOfSamples+2:6*NumOfSamples+1);
Process2ch = Process(:,5*NumOfSamples+3:6*NumOfSamples+2);

%In11 = [Process1ch; Process0ch;];
In11 = Process1ch;
TrgtOutCh = Process2ch;

for i=1:NumInVars
	 mean1(i,:) = linspace(min(In1(i,:)), max(In1(i,:)),NumInTerms);
	 sigma1(i,:) = ((mean1(i,2)-mean1(i,1))/2)*ones(1,NumInTerms);
end
b1     = 2*ones(NumInVars,NumInTerms);

% plotgenbell(mean1,sigma1,b1,[-60,80]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                			Beginning of the Main "for" loop of the ANFIS  program.        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ne=1:NumOfEpochs

for n=1:NumOfSamples
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        								 							     				                                              %
%   			                  	NETWORK FUNCTIONALITY SECTION						      %
%      																					                                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% LAYER 1 - INPUT TERM NODES
In2 = In1(:,n)*ones(1,NumInTerms);
Out1 = 1./(1 + (abs((In2-mean1)./sigma1)).^(2*b1));

% LAYER 2 - PRODUCT NODES
precond = comb(Out1);
Out2 = prod(precond,2);
S_2 = sum(Out2);

% LAYER 3 - NORMALIZATION NODES
 Out3 = Out2/S_2;

% LAYERS 4 - 5: CONSEQUENT NODES - SUMMING NODE
Aux1 = [In1(:,n); 1]*Out3';

a = reshape(Aux1,(NumInVars+1)*NumRules,1);  % New Input Training Data shaped as a column vector.

Out5(:,n) = ThetaL4'*a;

e(:,n) = TrgtOut(:,n)-Out5(:,n);

%%%%%%%%%%% END OF NETWORK FUNCTIONALITY SECTION %%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%			 					PARAMETER LEARNING SECTION	                    			 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% BACKWARD PASS. Error Backpropagation

% We may adjust the Consequent Parameters by LMS.
 for m=1:NumOutVars
     deltaThetaL4 = a*e(m,n);
     dThetaL4(:,m) = Ita1*deltaThetaL4 + alpha1*dThetaL4(:,m);           % Classic LMS.
 end

% LAYER 4
for m=1:NumOutVars
        ThetaL4_mat = reshape(ThetaL4(:,m),NumInVars+1,NumRules);
        f(m,:) = [In1(:,n)' 1]*ThetaL4_mat*e(m,n);
end

% LAYER 3
e3 = sum(f,1);
denom = S_2*S_2;

% LAYER 2
ThetaE32 = zeros(NumRules,NumRules);
if denom~=0
    for k1=1:NumRules
         for k2=1:NumRules
              if k1==k2
                 ThetaE32(k1,k2) = ((S_2-Out2(k2))/denom)*e3(k2);
              else
                 ThetaE32(k1,k2) = -(Out2(k2)/denom)*e3(k2);
              end
         end
    end

else
    continue;
end

e2 = sum(ThetaE32,2);

% LAYER 1
Q = zeros(NumInVars,NumInTerms,NumRules);
for i=1:NumInVars
     for j=1:NumInTerms
          for k=1:NumRules
      	       if Out1(i,j)== precond(k,i) && Out1(i,j)~=0
      			  Q(i,j,k) = (Out2(k)/Out1(i,j))*e2(k);
               end
          end
      end
 end

ThetaE21 = sum(Q,3);

% LAYER 1 PARAMETER ADJUSTMENT BY GRADIENT DESCENT.
deltamean1   = -ThetaE21.*(2*b1./(In2-mean1)).*Out1.*(1-Out1);
deltab1          = -ThetaE21.*(-2).*log(abs((In2-mean1)./sigma1)).*Out1.*(1-Out1);
deltasigma1  = -ThetaE21.*(2*b1./sigma1).*Out1.*(1-Out1);

% dmean1 = -Ita2*deltamean1 + alpha2*dmean1;
% mean1 = mean1 + dmean1;
%
% dsigma1 = -Ita2*deltasigma1 + alpha2*dsigma1;
% sigma1 = sigma1 + dsigma1;
%
% db1 = -Ita2*deltab1 + alpha2*db1;
% b1 = b1 + db1;

% Now update the Layer 4 linear parameters.
ThetaL4_old = ThetaL4;  % Keep here the old values of ThetaL4.
% Fixing of Consequent Parameters by LMS.
ThetaL4  = ThetaL4 + dThetaL4;

% Fixing of Consequent Parameters by Recursive LSE.
% P = (1./lamda).*(P - P*a*a'*P./(lamda+a'*P*a));
% ThetaL4 = ThetaL4 + P*a*e(:,n)';

%%%%%%%%%%%% END OF PARAMETER LEARNING PROCESS %%%%%%%%%%%%

end %Of for i=1:NumOfSamples loop.

for m=1:NumOutVars
    rmse(m,ne) = norm(e(m,:))/sqrt(NumOfSamples);
end

rmse(:,ne)

end   %Of for i=1:NumOfEpochs loop.

figure
subplot(3,1,1),plot(rmse(1,:),'*r');
title('RMSE for X-coordinate');
grid on;

subplot(3,1,2),plot(rmse(2,:),'*g');
title('RMSE for Y-coordinate');
grid on;

subplot(3,1,3),plot(rmse(3,:),'*b');
title('RMSE for Z-coordinate');
grid on;

X = 1:NumOfSamples;
figure;
subplot(3,2,1)
plot(X,TrgtOut(1,:),'r',X,Out5(1,:),'m');
title('Training Target X-coordinate (Red) VS ANFIS X Output (Magenta)')
grid on;

subplot(3,2,2)
plot(X,e(1,:),'r')
title('Training Error(n) for X-coordinate')
grid on;

subplot(3,2,3)
plot(X,TrgtOut(2,:),'b',X,Out5(2,:),'c');
title('Training Target Y-coordinate (Blue) VS ANFIS Y Output (Cyan)')
grid on;

subplot(3,2,4)
plot(X,e(2,:),'b')
title('Training Error(n) for Y-coordinate')
grid on;

subplot(3,2,5)
plot(X,TrgtOut(3,:),'g',X,Out5(3,:),'y');
title('Training Target Z-coordinate (Green) VS ANFIS Z Output (Yellow)')
grid on;

subplot(3,2,6)
plot(X,e(3,:),'g')
title('Training Error(n) for Z-coordinate')
grid on

% plotgenbell(mean1,sigma1,b1,[-60,80]);

%% %%% %%%%%%% Gradient Checking Section %%%%%%%%%%%%%
ThetaJ_mean = zeros(NumInVars,NumInTerms);
ThetaJ_sigma = zeros(NumInVars,NumInTerms);
ThetaJ_b         = zeros(NumInVars,NumInTerms);
epsilon             = 1e-4;

for i=1:NumInVars
    for j=1:NumInTerms

        mean1_up = mean1;
        mean1_do = mean1;

        mean1_up(i,j) = mean1(i,j) + epsilon;
        mean1_do(i,j) = mean1(i,j) - epsilon;

        J_up_mean = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1_up,sigma1,b1,ThetaL4_old)).^2);
        J_do_mean = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1_do,sigma1,b1,ThetaL4_old)).^2);

        ThetaJ_mean(i,j) = (J_up_mean - J_do_mean)/2/epsilon;

        sigma1_up = sigma1;
        sigma1_do = sigma1;

        sigma1_up(i,j) = sigma1(i,j) + epsilon;
        sigma1_do(i,j) = sigma1(i,j) - epsilon;

        J_up_sigma = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1_up,b1,ThetaL4_old)).^2);
        J_do_sigma = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1_do,b1,ThetaL4_old)).^2);

        ThetaJ_sigma(i,j) = (J_up_sigma - J_do_sigma)/2/epsilon;

        b1_up = b1;
        b1_do = b1;

        b1_up(i,j) = b1(i,j) + epsilon;
        b1_do(i,j) = b1(i,j) - epsilon;

        J_up_b = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1,b1_up,ThetaL4_old)).^2);
        J_do_b = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1,b1_do,ThetaL4_old)).^2);

        ThetaJ_b(i,j) = (J_up_b - J_do_b)/2/epsilon;

    end
end

rel_diff_mean  = deltamean1(:) - ThetaJ_mean(:);
rel_diff_sigma = deltasigma1(:) - ThetaJ_sigma(:);
rel_diff_b         = deltab1(:) - ThetaJ_b(:);

[rel_diff_mean rel_diff_sigma rel_diff_b]

[deltamean1(:)   ThetaJ_mean(:) ]

[deltasigma1(:)  ThetaJ_sigma(:)]

[deltab1(:)           ThetaJ_b(:)        ]