%% SCENARIO BUILDER CLASS (scenarioBuilder.m) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The scenario builder is a tool designed to provide a set of tools to aid
% in the design of object scenarios in the global scene.

classdef scenarioBuilder
    % BASIC SCENARIO DEFINITION
    % This class handles the methods of generating different scenarios of
    % a defined number of objects/agents.

    % SCENARIO PROPERTIES
    properties
        % META DATA
        objects;
        name = 'Default Scenario.';
        % GLOBAL DATA
        % ALL GLOBAL DATA IS GIVEN IN THE EAST-NORTH-UP (ENU) FRAME OF
        % REFERENCE
        globalTriad = [1,0,0;0,1,0;0,0,1];      % XYZ (Earth) frame of reference
        positions;                              % Global positions
        velocities;                             % Global velocities
        quaternions;                            % The quaternion moving from the global XYZ
    end
    % PUBLIC METHODS
    methods
        % CONSTRUCTION METHOD
        function obj = scenarioBuilder(varargin)
            % The scenario builder is designed to define a scenario for a
            % given number of objects. An instance of the scenario builder
            % class must be created first to ensure the

            % Access the OMAS-common properties
            addpath('environment/common');  % Access to common tools

            % Parse inputs against the builder
            [obj] = obj.configurationParser(obj,varargin);

            % Value checking
            assert(isnumeric(obj.objects),'Expecting a numeric value for field "objects"');
            assert(ischar(obj.name),'Expecting a string for field "name"');

            if obj.objects > 0
                % Initialise the scenario parameters
                obj.positions   = zeros(3,obj.objects);
                obj.velocities  = zeros(3,obj.objects);
                obj.quaternions = repmat([1;0;0;0],[1 obj.objects]);
            end
        end

        %% SCENARIO GENERATORS
        % PLANAR CONCENTRIC DISK %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = planarDisk(obj,varargin)
            % This function generates a configuration representing a given
            % number of objects distributed about a disk defined by a
            % central axis vector.
            % INPUTS:
            % pointA      - The first axis reference point.
            % pointB      - The second axis reference point; the ring center.
            % radius      - The scalar radius of the ring
            % velocity    - The velocity multiplier for the object set.
            % OUTPUTS:
            % definition  - The scenario definition

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'pointA',[0;0;-1],...
                                   'pointB',[0;0;0],...
                                   'zeroAngle',0,...                       % The rotation of the first node
                                   'velocity',0,...
                                   'scale',1);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(isnumeric(config.pointA) && numel(config.pointA) == 3,'The reference point must be a 3D Cartesian point.');
            assert(isnumeric(config.pointB) && numel(config.pointB) == 3,'The centroid point must be a 3D Cartesian point.');
            assert(isnumeric(config.zeroAngle) && numel(config.zeroAngle) == 1,'The zero angle must be a scalar.');
            assert(isnumeric(config.velocity) && numel(config.velocity) == 1,'The object velocity must be a scalar.');
            assert(isnumeric(config.scale) && numel(config.scale) == 1,'The object padding scale must be a scalar.');

            % ////////////// DESIGN THE DISK DISTRIBUTION /////////////////
            % Update scenario label
            obj.name = 'Distributed planar disk.';
            % Define object number
            obj.objects = config.objects;
            % If there is only one object, simply place it at the center
            if obj.objects == 1
                obj.positions(:,1) = config.pointB;
                obj.velocities(:,1) = [1;0;0]*config.velocity;
                obj.quaternions(:,1) = [1;0;0;0];
                return
            end
            remainingObjects = obj.objects;
            layer = 1;
            while layer < obj.objects && remainingObjects > 0
                % NEW LAYER RADIUS
                layerRadius = layer*config.scale;                          % New circle radius
                criticalLayerAngle = 2*asin(1/(2*layer));                  % New maximum nodal angle
                layerMax = floor(2*pi/criticalLayerAngle);                 % Maximum number on the new layer
                % DISTRIBUTE OBJECTS OVER THE LAYERS
                if layerMax > remainingObjects
                    objectsInLayer = remainingObjects;
                else
                    objectsInLayer = layerMax;
                end
                % BUILD THE NEXT LAYER
                [layerConfig] = obj.planarRing(...
                    'objects',objectsInLayer,...
                    'pointA',config.pointA,...
                    'pointB',config.pointB,...
                    'zeroAngle',config.zeroAngle,...
                    'radius',layerRadius);

                % EXTRACT THE LAYER GLOBAL PROPERTIES
                first = 1 + obj.objects - remainingObjects;
                last  = (first - 1) + objectsInLayer;
                indexVector = first:last;
                obj.positions(:,indexVector) = layerConfig.positions(:,1:objectsInLayer);
                obj.velocities(:,indexVector) = layerConfig.velocities(:,1:objectsInLayer);
                obj.quaternions(:,indexVector) = layerConfig.quaternions(:,1:objectsInLayer);
                % INCREMENT LAYER
                remainingObjects = remainingObjects - objectsInLayer;
                layer = layer + 1;
            end

        end
        % PLANAR RING OF STATES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = planarRing(obj,varargin)
            % This function generates a configuration representing a given
            % number of objects destributed around a ring defined by a
            % central axis vector.
            % INPUTS:
            % pointA     - The first axis reference point.
            % pointB     - The second axis reference point; the ring center.
            % mod_radius - The scalar radius of the ring
            % velocity   - The velocity multiplier for the object set.
            % OUTPUTS:
            % definition - The scenario definition

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'pointA',[0;0;-1],...
                                   'pointB',[0;0;0],...
                                   'radius',10,...
                                   'zeroAngle',0,...                       % The rotation of the first node
                                   'offsetAngle',pi/5,...
                                   'velocity',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % Update scenario label
            obj.name = 'Planar ring.';
            % Define object number
            obj.objects = config.objects;

            % HAND NODAL ANGLE TO ringAngle FUNCTION
            [ obj ] = obj.planarAngle(...
                'objects',config.objects,...
                'pointA',config.pointA,...
                'pointB',config.pointB,...
                'radius',config.radius,...
                'zeroAngle',config.zeroAngle,...
                'offsetAngle',(2*pi)/config.objects,... % Define an angle for equal distribution
                'velocity',config.velocity);
        end
        % CONSTANT OFFSET ANGLE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = planarAngle(obj,varargin)
            % GENERATE A SCENARIO DEFINITION
            % INPUTS:
            % pointA        - The first axis reference point.
            % pointB        - The second axis reference point; the ring center.
            % mod_radius    - The scalar radius of the ring
            % velMultiplier - The velocity multiplier for the object set.
            % offsetAngle   - The constant radial angle
            % OUTPUTS:
            % definition    - The scenario deifnition

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'pointA',[0;0;-1],...
                                   'pointB',[0;0;0],...
                                   'radius',10,...
                                   'zeroAngle',0,...                       % The rotation of the first node
                                   'offsetAngle',pi/5,...
                                   'velocity',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(isnumeric(config.pointA) && numel(config.pointA) == 3,'The reference point must be a 3D Cartesian point.');
            assert(isnumeric(config.pointB) && numel(config.pointB) == 3,'The center point must be a 3D Cartesian point.');
            assert(isnumeric(config.radius) && numel(config.radius) == 1,'The arc radius must be a scalar.');
            assert(isnumeric(config.zeroAngle) && numel(config.zeroAngle) == 1,'The initial radial angle must be a scalar.');
            assert(isnumeric(config.offsetAngle) && numel(config.offsetAngle) == 1,'The offset radial angle must be a scalar.');
            assert(isnumeric(config.velocity) && numel(config.velocity) == 1,'The object velocity must be a scalar.');

            % Update scenario label
            obj.name = sprintf('Planar offset angle of %srad.',num2str(config.offsetAngle));
            % Define object number
            obj.objects = config.objects;

            % GENERATE THE NODE(OBJECT) CARTAESIAN POSITIONS
            % GET THE AXIS VECTOR PROPERTIES
            localXAxis = config.pointB - config.pointA;
            unit_axisVector = unit(localXAxis);
            % GET THE RADIAL VECTOR
            perpVector = cross(unit_axisVector,[1;0;0]);
            if sum(perpVector) == 0
                perpVector = cross(unit_axisVector,[0;1;0]);
            end
            perpVector = unit(perpVector); % Re-normalise the perpendicular vector
            % SCALE THE UNIT RADIAL VECTOR
            perpVector = config.radius*perpVector;

            % GET THE NODE POINT SET
            nodalAngle = config.zeroAngle + pi/2; % pi/2 aligns the first object with the x-axis
            for node = 1:config.objects
                % ROTATE THE RADIAL VECTOR TO DEFINE NODAL POSITIONS
                radialVector = OMAS_geometry.rodriguesRotation(perpVector,unit_axisVector,nodalAngle);
                % NODAL POSITIONS
                globalPosition = config.pointB + radialVector;
                % NODAL VELOCITIES
                unitVelocity = - unit(radialVector);        % -ve sign to make concentric velocities +ve
                % NODAL ORIENTATIONS
                % We desire to orientate each object towards the center of
                % the ring. This is done by aligning the local body XY plane
                % With the plane of the ring. The local X axis is then
                % aligned with the cocentric global velocity.

                % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                localTriad = zeros(3);
                localTriad(:,1) = unit(unitVelocity);             % GET THE UNIT DIRECTION VECTOR
                localTriad(:,3) = unit(unit_axisVector);          % UNIT RING AXIS
                localTriad(:,2) = unit(cross(localTriad(:,1),localTriad(:,3)));

                % STORE THE OBJECTS IN SCENARIO DEFINITION
                obj.positions(:,node)   = globalPosition;
                obj.velocities(:,node)  = config.velocity*unitVelocity;
                obj.quaternions(:,node) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);

                % Object data is concatinated vertically
                % Increment the nodal position by the offset angle
                nodalAngle = nodalAngle - config.offsetAngle;
            end
        end
        % SPHERICAL EQUAL DISTRIBUTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = regularSphere(obj,varargin)
            % This function is designed to generate points equally
            % distributed about the circumference of a sphere.

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'radius',10,...
                                   'center',[0;0;0],...
                                   'velocity',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(isnumeric(config.radius) && numel(config.radius) == 1,'The sphere radius must be a scalar.');
            assert(isnumeric(config.center) && numel(config.center) == 3,'The sphere center must be a 3D Cartesian vector.');
            assert(isnumeric(config.velocity) && numel(config.velocity) == 1,'The object velocity must be a scalar.');

            % ATTEMPT TO ADD THE ICOSAHEDRALS LIBRARY
            try
                libraryPath = strcat(cd,'\scenarios\icosahedrals');
                addpath(libraryPath);
            catch
                error('Unable to load the icosahedral generator function, has it been moved?');
            end

            % UPDATE SCENARIO LABEL
            obj.name = 'Equally spaced spherical distribution.';
            obj.objects = config.objects;

            % GET SCALED ICOSAHEDRAL SPHERE
            % This function will generate a vector of points equally
            % distributed about the circumference of a sphere.
            [pointCloud,~] = spheretri(obj.objects);
            XYZ = (pointCloud.*config.radius)';
            XYZ = XYZ + config.center;                                      % Offset and scale the sphere

            % SORT THE POINTS
            XYZ = unique(XYZ','rows');                                     % Remove repeat entries
            XYZ = sortrows(XYZ,1);
            XYZ = XYZ';
            nodalPositions = XYZ();
            nodalPositions = nodalPositions(:,1:obj.objects);
            % ALLOCATE RANDOM SET TO OBJECT GLOBAL POSITIONS
            for index = 1:obj.objects
                % DETERMINE THE NODAL POSITIONS
                nodalPosition = config.center + nodalPositions(:,index);

                errorFlag = 1;
                while errorFlag == 1
                    % DEFINE GLOBAL VELOCITIES
                    unit_radii = unit(config.center - nodalPosition);
                    obj.velocities(:,index) = config.velocity*unit_radii;     % Cocentric velocity
                    % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                    localTriad = zeros(3);
                    localTriad(:,1) = unit_radii;                              % The body axis X direction
                    localTriad(:,3) = unit(cross(localTriad(:,1),obj.globalTriad(:,1)));      % The body axis Y direction
                    localTriad(:,2) = unit(cross(localTriad(:,1),localTriad(:,3)));           % The body axis Z direction
                    % DETERMINE IF A QUATERNION ROTATION EXISTS
                    try
                        % ASSIGN POSITION
                        obj.positions(:,index) = nodalPosition;
                        % ROTATION
                        obj.quaternions(:,index) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);
                        errorFlag = 0;
                    catch
                        % IF ROTATION IS INVALID, RESAMPLE THE POSITION
                        [nodalPosition,~] = datasample(XYZ,1,2,'Replace',false);
                    end
                end
            end
        end
        % COCENTRIC HELICAL STATES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = helix(obj,varargin)

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'pointA',[0;0;0],...
                                   'pointB',[0;0;10],...
                                   'radius',10,...
                                   'offsetAngle',pi/5,...
                                   'velocities',0,...
                                   'plot',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(ischar(config.file),'The provided file path must be a string.');
            assert(isnumeric(config.pointA) && numel(config.pointA) == 3,'The origin point must be a 3D Cartesian point.');
            assert(isnumeric(config.pointB) && numel(config.pointB) == 3,'The terminal point must be a 3D Cartesian point.');
            assert(isnumeric(config.radius) && numel(config.radius) == 1,'The helical radius must be a scalar.');
            assert(isnumeric(config.offsetAngle) && numel(config.offsetAngle) == 1,'The helical angle must be a scalar.');
            assert(isnumeric(config.velocity) && numel(config.velocity) == 1,'The object velocity must be a scalar.');

            % DEFINE THE HELIX AXIS
            helixAxis = config.pointB - config.pointA;
            norm_helixAxis = norm(helixAxis);
            unit_helixAxis = helixAxis/norm_helixAxis;

            % THE UNIT SPACING
            objectNumber = obj.objects;
            axialSeparation = norm_helixAxis/objectNumber;

            % REFERENCE POSITION FOR FIRST NODE
            pointA_i = config.pointA - axialSeparation*unit_helixAxis;
            for i = 1:objectNumber
                % AXIAL PARAMETERS
                pointB_i = pointA_i + axialSeparation*unit_helixAxis;
                radialAngle = config.offsetAngle*(i-1);

                % DEFINE CONFIGURATION FOR OBJECT "i"
                [ ~,planarConfig ] = obj.planarAngle('objects',1,...
                                                     'pointA',pointA_i,...
                                                     'pointB',pointB_i,...
                                                     'radius',config.radius,...
                                                     'zeroAngle',radialAngle,...                       % The rotation of the first node
                                                     'offsetAngle',0);
                % DEFINE THE CONFIGURATION OF THE AGENT
                obj.positions(:,i) = planarConfig.position(:,1);
                obj.velocities(:,i) = planarConfig.velocity(:,1);
                obj.quaternions(:,i) = planarConfig.quaternion(:,1);
                % SHIFT UP THE AXIS VECTOR
                pointA_i = pointB_i;
            end

            obj.name = 'Cocentric helix.';
            % SAVE THE RESULTING SCENARIO
            scenarioConfig = struct('objects',obj.objects,...
                                    'name',obj.name,...
                                    'position',obj.positions,...
                                    'velocity',obj.velocities,...
                                    'quaternion',obj.quaternions);
        end
        % COLINEAR DISTRIBUTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = line(obj,varargin)
            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,...
                                   'pointA',[0;0;0],...
                                   'pointB',[0;0;10],...
                                   'heading',[1;0;0],...
                                   'velocities',1,...
                                   'plot',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(isnumeric(config.pointA) && numel(config.pointA) == 3,'Please provide a 3D point to the start of the line.');
            assert(isnumeric(config.pointB) && numel(config.pointB) == 3,'Please provide a 3D point to the end of the line.');
            assert(isnumeric(config.heading) && numel(config.heading) == 3,'Please provide a 3D vector defining the object heading');
            assert(isnumeric(config.velocities) && numel(config.velocities) == 1,'Please provide a scalar initial speed along the heading vector');

            % Label the scenario
            obj.name = 'Apposing line configuration';
            obj.objects = config.objects;

            % DISTRIBUTE THE OBJECTS EQUALLY ALONG THE AXIS
            if isnumeric(config.objects)
                nObjects = config.objects;
            else
                nObjects = numel(config.objects);
            end

            headingReference = unit(config.heading);    % Ensure the heading is a unit vector
            lineLength = norm(config.pointB - config.pointA);
            lineAxis = (config.pointB - config.pointA)/lineLength;
            % Linearly interpolate
            segments = linspace(0,lineLength,nObjects);

            % Design the parameters for each object
            for node = 1:nObjects
                % OBJECT POSITION
                obj.positions(:,node)  = config.pointA + segments(node)*lineAxis;
                % OBJECT VELOCITY
                obj.velocities(:,node) = config.velocities*config.heading;
                % ORIENTATION
                % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                localTriad = zeros(3);
                localTriad(:,1) = headingReference;                        % GET THE UNIT DIRECTION VECTOR
                localTriad(:,3) = unit_mex(obj.globalTriad(:,3)); % GLOBAL Z-AXIS
                localTriad(:,2) = unit_mex(cross(localTriad(:,1),localTriad(:,3)));
                % RESOLVE ROTATION QUATERNION
                obj.quaternions(:,node) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);
            end
        end

        % RANDOM SPHERE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = randomSphere(obj,varargin)
            % This fuction is designed to generate random positions
            % distributed about the circumference of a sphere.

            % DEFAULT CONFIGURATION
            defaultConfig = struct('objects',0,....
                                   'radius',10,...
                                   'center',[0;0;0],...
                                   'velocity',0);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % INPUT SANITY CHECK
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');
            assert(isnumeric(config.radius) && numel(config.radius) == 1,'Sphere radius must be a scalar.');
            assert(isnumeric(config.center) && numel(config.ceneter) == 3,'Sphere center must be a 3D Cartesian vector.');
            assert(isnumeric(config.velocity) && numel(config.velocity) == 1,'Agent velocity must be a scalar.');

            % ATTEMPT TO ADD THE ICOSAHEDRALS LIBRARY
            try
                libraryPath = strcat(cd,'\scenarios\icosahedrals');
                addpath(libraryPath);
            catch
                error('Unable to load the icosahedral generator function, has it been moved?');
            end

            % GET SCALED ICOSAHEDRAL SPHERE
            % This function will generate a vector of points equally
            % distributed about the circumference of a sphere.
            [pointCloud,~] = spheretri(obj.objects);
            XYZ = (pointCloud.*config.radius)';
            XYZ = XYZ + config.center;                                     % Offset and scale the sphere
            XYZ = unique(XYZ','rows');                                     % Remove repeat entries
            XYZ = XYZ';

            % RANDOMLY SAMPLE FROM THE POINT SET
            [nodalPositions,~] = datasample(XYZ,obj.objects,2,'Replace',false);   % Take one sample column from X coordinates

            % ALLOCATE RANDOM SET TO OBJECT GLOBAL POSITIONS
            for index = 1:config.objects
                % DETERMINE THE NODAL POSITIONS
                nodalPosition = config.center + nodalPositions(:,index);

                errorFlag = 1;
                while errorFlag == 1
                    % DEFINE GLOBAL VELOCITIES
                    unit_radii = unit(sphereCenter - nodalPosition);
                    obj.velocities(:,index) = config.velocity*unit_radii;     % Cocentric velocity
                    % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                    localTriad = zeros(3);
                    localTriad(:,1) = unit_radii;                              % The body axis X direction
                    localTriad(:,3) = unit(cross(localTriad(:,1),obj.globalTriad(:,1)));      % The body axis Y direction
                    localTriad(:,2) = unit(cross(localTriad(:,1),localTriad(:,3)));           % The body axis Z direction
                    % DETERMINE IF A QUATERNION ROTATION EXISTS
                    try
                        % ASSIGN POSITION
                        obj.positions(:,index) = nodalPosition;
                        % ROTATION
                        obj.quaternions(:,index) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);
                        errorFlag = 0;
                    catch
                        % IF ROTATION IS INVALID, RESAMPLE THE POSITION
                        [nodalPosition,~] = datasample(XYZ,1,2,'Replace',false);
                    end
                end
            end
            % SAVE THE RESULTING SCENARIO
            scenarioConfig = struct('objects',obj.objects,...
                                    'name',obj.name,...
                                    'position',obj.positions,...
                                    'velocity',obj.velocities,...
                                    'quaternion',obj.quaternions);
            % UPDATE SCENARIO LABEL
            obj.name = 'Random cocentric sphere';
        end
        % DEFAULT RANDOM STATE GENERATOR - NORMAL %%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = random(obj,varargin)
            % This function is a simple placeholder to facilitate the use
            % of .random for quick access to normally distributed state sets.
            [ obj ] = obj.randomNormal(varargin);
        end
        % RANDOM (NORMAL DISTRIBUTION) SCENARIO %%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = randomNormal(obj,varargin)
            % This function generates a random state vector set using a normal distribution.
            % assuming the order of the states is [x;y;z;u;v;w;phi;theta;psi;p;q;r]
            % INPUT:
            % obj          - The scenario definition object
            % OUTPUT:
            % randomStates - The state vector set [states x objects]
            % obj          - The returned scenario object

            % DEFAULT CONFIGURATION
            defaultConfig = struct(...
                'objects',0,....
                'velocity',0,...
                'positionGain',10,...
                'velocityGain',10,...
                'poseGain',pi);
            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % Input sanity check
            assert(isnumeric(config.objects) && config.objects > 0,'Expecting a scalar number of objects greater than zero.');

            % DEFINE A RANDOM STATE PARAMETERS
            obj.name = 'Normal-randomised';
            % The number of objects
            obj.objects = config.objects;

            stateNum = 9;
            positionTuple = 1:3;
            velocityTuple = 4:6;
            angleTuple = 7:9;
            stateGains = [config.positionGain,config.velocityGain,config.poseGain];
            normalObjectStates = zeros(stateNum,1);

            for index = 1:obj.objects
                % GENERATE RANDOM NUMBER SET
                maxR = max(randn(stateNum,1));
                normalObjectStates(:,index) = randn(stateNum,1)/maxR; % returns an n-by-n matrix of random numbers.

                % ALLOCATE STATE VALUES
                normalObjectStates(positionTuple,index) = stateGains(1)*normalObjectStates(positionTuple,index);
                normalObjectStates(velocityTuple,index) = stateGains(2)*normalObjectStates(velocityTuple,index);

                % NODAL POSITIONS
                obj.positions(:,index)  = normalObjectStates(positionTuple,index);
                % NODAL VELOCITIES
                obj.velocities(:,index) = normalObjectStates(velocityTuple,index);

                % NODAL ORIENTATIONS
                % We desire to orientate each object towards the center of
                % the ring. This is done by aligning the local body XY plane
                % With the plane of the ring. The local X axis is then
                % aligned with the cocentric global velocity.

                % ALLOCATE ANGULAR STATE VALUES
                normalObjectStates(angleTuple,index) = stateGains(3)*normalObjectStates(angleTuple,index);
                normalObjectStates(angleTuple(2:end),index) = 0;           % Remove pitch and yaw from random states (for stability)

                % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                localTriad = zeros(3);
                localTriad(:,1) = unit(normalObjectStates(velocityTuple,index));      % The body axis X direction
                localTriad(:,3) = unit(cross(localTriad(:,1),obj.globalTriad(:,1)));  % The body axis Y direction
                localTriad(:,2) = unit(cross(localTriad(:,1),localTriad(:,3)));       % The body axis Z direction
                obj.quaternions(:,index) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);
            end

        end
        % RANDOM (UNIFORM DISTRIBUTION) SCENARIO %%%%%%%%%%%%%%%%%%%%%%%%%%
        function [ obj ] = randomUniform(obj,varargin)
            % This function generates a random state vector set using a uniform distribution.
            % assuming the order of the states is [x;y;z;u;v;w;phi;theta;psi;p;q;r]
            % INPUT:
            % obj          - The scenario definition object
            % OUTPUT:
            % obj          - The returned scenario object

            % DEFAULT CONFIGURATION
            defaultConfig = struct(...
                'objects',0,....
                'velocity',0,...
                'positionGain',10,...
                'velocityGain',10,...
                'poseGain',pi);

            % PARSE CONFIGURATIONS
            [config] = obj.configurationParser(defaultConfig,varargin);

            % DEFINE A RANDOM STATE PARAMETERS
            obj.name = 'Uniformly-randomised';
            % The number of objects
            obj.objects = config.objects;

            stateNum = 9;
            positionTuple = 1:3;
            velocityTuple = 4:6;

            angleTuple = 7:9;
            stateGains = [config.positionGain,config.velocityGain,config.poseGain];
            uniformObjectStates = zeros(stateNum,1);

            for index = 1:obj.objects
                % GENERATE RANDOM NUMBER SET
                uniformObjectStates(:,index) = (rand([stateNum,1])-0.5).*2;       % returns an n-by-n matrix of random numbers.
                % ALLOCATE LINEAR STATE VALUES
                uniformObjectStates(positionTuple,index) = stateGains(1)*uniformObjectStates(positionTuple,index);
                uniformObjectStates(velocityTuple,index) = stateGains(2)*uniformObjectStates(velocityTuple,index);

                % NODAL POSITIONS
                obj.positions(:,index)  = uniformObjectStates(positionTuple,index);
                % NODAL VELOCITIES
                obj.velocities(:,index) = uniformObjectStates(velocityTuple,index);

                % NODAL ORIENTATIONS
                % We desire to orientate each object towards the center of
                % the ring. This is done by aligning the local body XY plane
                % With the plane of the ring. The local X axis is then
                % aligned with the cocentric global velocity.

                % ALLOCATE ANGULAR STATE VALUES
                uniformObjectStates(angleTuple,index) = stateGains(3)*uniformObjectStates(angleTuple,index);
                uniformObjectStates(angleTuple(2:end),index) = 0; % Remove pitch and yaw from random states (for stability)

                % GET THE LOCAL AXES (ROTATED IN THE GLOBAL FRAME)
                localTriad = zeros(3);
                localTriad(:,1) = unit(uniformObjectStates(velocityTuple,index));     % The body axis X direction
                localTriad(:,3) = unit(cross(localTriad(:,1),obj.globalTriad(:,1)));  % The body axis Y direction
                localTriad(:,2) = unit(cross(localTriad(:,1),localTriad(:,3)));       % The body axis Z direction
                obj.quaternions(:,index) = OMAS_geometry.getAnalyticalTriadRotation(obj.globalTriad,localTriad);
            end
        end
    end
    methods (Static)
        % PLOT SCENARIO FROM INITIALISED OBJECT-INDEX
        function [figureHandle] = plotObjectIndex(objectIndex)
            % This function is designed to plot the configured objectIndex
            % using their global properties and thier simulation
            % object-types

            global plotnum

            % DETERMINE PLOT PROPERTIES
            if ~exist('plotnum','var') || isempty(plotnum)
                plotnum = 1;    % Default to first plot
            end

            % GENERATE THE FIGURE
            figureHandle = figure(plotnum);
            axis vis3d;
            view([45,45]);
            xlabel('x (m)'); ylabel('y (m)'); zlabel('z (m)');
            hold on; grid on;
            ax = gca;
            set(ax,'FontSize',12,'fontWeight','bold');

            % OBJECT COUNTERS
            agents = 0; obstacles = 0; waypoints = 0;
            for index = 1:numel(objectIndex)
                % GET THE OBSTACLES GLOBAL PARAMETERS
                GLB = objectIndex{index}.GetGLOBAL();
                objectName  = objectIndex{index}.name;
                objectID    = objectIndex{index}.objectID;

                % GET THE ROTATION MATRIX GOING FROM BODY-GLOBAL
                [R_q] = OMAS_geometry.quaternionToRotationMatrix(GLB.quaternion);
                % PLOT THE OBJECT ORIENTATION TRIAD
                OMAS_graphics.drawTriad(figureHandle,GLB.position,R_q');
                % PLOT THE POSITIONS
                scatter3(GLB.position(1),GLB.position(2),GLB.position(3),'r');
                % PLOT THE VELOCITY VECTORS
                quiv = quiver3(GLB.position(1),GLB.position(2),GLB.position(3),...
                               GLB.velocity(1),GLB.velocity(2),GLB.velocity(3),'c');
                quiv.AutoScaleFactor = 1;
                % DETERMINE REPRESENTATION IF THE OBJECT HAS GEOMETRY
                if size(objectIndex{index}.GEOMETRY.vertices,1) < 1
                    % REPRESENT AS SPHERE WITH DEFINED RADIUS
                    [geometry] = OMAS_graphics.defineSphere(GLB.position,GLB.radius);
                    vertexData = geometry.vertices;
                    faceData = geometry.faces;
                    entityFaceAlpha = 0.3;
                    entityLineWidth = 0.05;
                    entityEdgeAlpha = 0.1;                                 % Show representative shapes with higher alpha
                else
                    vertexData = objectIndex{index}.GEOMETRY.vertices*R_q + GLB.position';
                    faceData   = objectIndex{index}.GEOMETRY.faces;
                    entityFaceAlpha = 0.8;
                    entityLineWidth = 1;
                    entityEdgeAlpha = 0.8; %0.2;
                end
                % REPRESENT GEOMETRY AS A PATCH
                entityHandle = patch(ax,...
                                    'Vertices',vertexData,...
                                    'Faces',faceData,...
                                    'EdgeColor','k',...
                                    'EdgeAlpha',entityEdgeAlpha,...
                                    'FaceAlpha',entityFaceAlpha,...
                                    'FaceLighting','gouraud',...
                                    'LineWidth',entityLineWidth);
                % PLOT REPRESENTATION
                switch GLB.type
                    case OMAS_objectType.agent
                        set(entityHandle,'FaceColor','b');
                        agents = agents + 1;
                    case OMAS_objectType.obstacle
                        set(entityHandle,'FaceColor','r');
                        obstacles = obstacles + 1;
                    case OMAS_objectType.waypoint
                        set(entityHandle,'FaceColor','g');
                        waypoints = waypoints + 1;
                    otherwise
                        set(entityHandle,'FaceColor','m');
                end
                % ADD ANNOTATION
                annotationText = sprintf('    %s [ID:%s]',objectName,num2str(objectID));
                text(GLB.position(1),GLB.position(2),GLB.position(3),char(annotationText));
            end
            % ADD TITLE
            titleStr = sprintf('Test scenario: %.0f agents, %.0f obstacles and %.0f waypoints.',agents,obstacles,waypoints);
            title(titleStr);
            hold off;
        end
        % PARSE A GENERIC INPUT SET AGAINST A DEFAULT CONFIG STRUCTURE
        function [config] = configurationParser(defaultConfig,scenarioParameters)
            % 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 from a get_scenario file.

            % Call the generic parameter overrider
            [config] = GetParameterOverrides_recursive(defaultConfig,scenarioParameters);
        end
    end
end


function polygon_properties_test ( )

%*****************************************************************************80
%
%% POLYGON_PROPERTIES_TEST tests the POLYGON_PROPERTIES library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the POLYGON_PROPERTIES library.\n' );

  polygon_properties_test01 ( );
  polygon_properties_test02 ( );
  polygon_properties_test03 ( );
  polygon_properties_test04 ( );
  polygon_properties_test05 ( );
  polygon_properties_test06 ( );
  polygon_properties_test07 ( );
  polygon_properties_test08 ( );
  polygon_properties_test09 ( );

  polygon_properties_test10 ( );
  polygon_properties_test11 ( );
  polygon_properties_test12 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_PRB\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [node,elem] = delmesh(node,elem,expr)
%% DELMESH delete part of the mesh

dim = size(node,2); elemdim = size(elem,2);
%% delete element
switch elemdim
    case 3
        center = (node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:))/3;
    case 4
        center = (node(elem(:,1),:) + node(elem(:,2),:) ...
                + node(elem(:,3),:) + node(elem(:,4),:))/4;
end
x = center(:,1);  y = center(:,2); %#ok<*NASGU>
if dim == 3
	z = center(:,3); %#ok<*NASGU>
end
idx = eval(expr);
elem(idx,:) = [];

%% delete vertices
isValidNode = false(size(node,1),1);
isValidNode(elem(:)) = true;
node = node(isValidNode,:);

%% shift index of element
Nnew = sum(isValidNode);
indexMap(isValidNode) = (1:Nnew)';
elem = indexMap(elem);


%%*************************************************************
%% Asum: compute the matrix
%%
%%  Ay = Asum(blk,A,y)
%%
%%  input:  A = a CELL ARRAY with m columns.
%%          y = mx1 vector.
%%          permAy = a permutation of [1:m] coding the order to
%%                   sum the matrices y(k)*Ak, k = 1,...m.
%%          iscmp = 1,  if Ay is complex
%%                  0,  otherwise.
%%
%%  output: Ay = sum_{k=1}^m y(k)*Ak, a column CELL ARRAY
%%               with the same structure as A{:,1}.
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 2 Feb 01
%%*************************************************************

   function Ay = Asum(blk,A,y)

   Ay = cell(size(blk,1),1);
   m = length(y);
   for p = 1:size(blk,1)
      n = sum(blk{p,2});
      blktmp = blk{p,2};
      if strcmp(blk{p,1},'s');
         tmp = sparse(n,n);
         for k = 1:m; tmp = tmp + A{p,k}*y(k); end;
         if (length(blktmp) == 1)
            if (nnz(tmp) > 0.15*n*n);
               if issparse(tmp); tmp = full(tmp); end;
            else;
               if ~issparse(tmp); tmp = sparse(tmp); end;
            end;
         elseif (length(blktmp) > 1);
            if ~issparse(tmp); tmp = sparse(tmp); end;
         end;
      elseif strcmp(blk{p,1},'l');
         tmp = zeros(n,1);
         for k = 1:m; tmp = tmp + A{p,k}*y(k); end;
         if (nnz(tmp) > 0.15*n);
            if issparse(tmp); tmp = full(tmp); end;
         else;
            if ~issparse(tmp); tmp = sparse(tmp); end;
         end;
      end;
      Ay{p} = tmp;
   end;
%%-------------------------------------------------------------


function indices = matRad_convRtssContours2Indices(structure,ct)
% matRad function to convert a polygon segmentation from an rt structure
% set into a binary segmentation as required within matRad's cst struct
%
% call
%   indices = matRad_convRtssContours2Indices(contPoints,ct)
%
% input
%   structure:      information about a single structure
%   ct:             matRad ct struct where the binary segmentations will
%                   be aligned to
%
% output
%   indicies:       indices of voxels of the ct cube that are inside the
%                   contour
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

voiCube = zeros(ct.cubeDim);

% loop over all closed contour items
for i = 1:size(structure.item,2)

    if ~isempty(structure.item(i).points)

        dicomCtSlicePos = unique(structure.item(i).points(:,3));

        if numel(dicomCtSlicePos) > 1
            error('Contour defined over multiple planes\n');
        end

        round2 = @(a,b) round(a*10^b)/10^b;
        dicomCtSliceThickness = ct.dicomInfo.SliceThickness(round2(ct.dicomInfo.SlicePositions,1)==round2(dicomCtSlicePos,1));

        coords1 = interp1(ct.x,1:ct.cubeDim(2),structure.item(i).points(:,1),'linear','extrap');
        coords2 = interp1(ct.y,1:ct.cubeDim(1),structure.item(i).points(:,2),'linear','extrap');

        binIn = poly2mask(coords1,coords2,ct.cubeDim(1),ct.cubeDim(2));

        slicesInMatradCt = find(dicomCtSlicePos+dicomCtSliceThickness/2 > ct.z & dicomCtSlicePos-dicomCtSliceThickness/2 <= ct.z);

        % loop over all slices in matRad ct
        for j = 1:numel(slicesInMatradCt)
            voiCube(:,:,slicesInMatradCt(j)) = voiCube(:,:,slicesInMatradCt(j)) | binIn;
        end

    end

end

indices = find(voiCube(:));


function [g1, g2] = disimXdisimKernGradient(disimKern1, disimKern2, t1, t2, covGrad)

% DISIMXDISIMKERNGRADIENT Compute a cross gradient between two DISIM kernels.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two disim kernels for the multiple output kernel.
% ARG disimKern1 : the kernel structure associated with the first DISIM
% kernel.
% ARG disimKern2 : the kernel structure associated with the second DISIM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see disimKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see disimKernExtractParam.
%
% FORMAT
% DESC computes cross kernel terms between two DISIM kernels for
% the multiple output kernel.
% ARG disimKern1 : the kernel structure associated with the first DISIM
% kernel.
% ARG disimKern2 : the kernel structure associated with the second DISIM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see disimKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see disimKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, disimKernParamInit, disimKernExtractParam
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007-2009

% KERN

arg{1}=t1;
if nargin < 5
  covGrad = t2;
  t2 = t1;
else
  arg{2}=t2;
end
if size(t1, 2) > 1 || size(t2, 2) > 1
  error('Input can only have one column');
end
if disimKern1.inverseWidth ~= disimKern2.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end
if disimKern1.di_decay ~= disimKern2.di_decay
  error('Kernels cannot be cross combined if they have different driving input decays.');
end
if disimKern1.di_variance ~= disimKern2.di_variance
  error('Kernels cannot be cross combined if they have different driving input variances.');
end
if disimKern1.rbf_variance ~= disimKern2.rbf_variance
  error('Kernels cannot be cross combined if they have different RBF variances.');
end

delta = disimKern1.di_decay;
D1 = disimKern1.decay;
D2 = disimKern2.decay;

l = sqrt(2/disimKern1.inverseWidth);
[h1, dh1_ddelta, dh1_dD1, dh1_dD2, dh1_dl] = disimComputeH(t1, t2, delta, D1, D2, l);
[hp1, dhp1_ddelta, dhp1_dD1, dhp1_dD2, dhp1_dl] = disimComputeHPrime(t1, t2, delta, D1, D2, l);

% Avoid making the expensive call twice with the same arguments
if ((length(t1) == length(t2)) && all(t1 == t2) && ...
    (D1 == D2)),
  h2 = h1;
  dh2_ddelta = dh1_ddelta;
  dh2_dD2 = dh1_dD1;
  dh2_dD1 = dh1_dD2;
  dh2_dl = dh1_dl;

  hp2 = hp1;
  dhp2_ddelta = dhp1_ddelta;
  dhp2_dD2 = dhp1_dD1;
  dhp2_dD1 = dhp1_dD2;
  dhp2_dl = dhp1_dl;
else,
  [h2, dh2_ddelta, dh2_dD2, dh2_dD1, dh2_dl] = disimComputeH(t2, t1, delta, D2, D1, l);
  [hp2, dhp2_ddelta, dhp2_dD2, dhp2_dD1, dhp2_dl] = disimComputeHPrime(t2, t1, delta, D2, D1, l);
end

dK_ddelta = dh1_ddelta + dh2_ddelta' + dhp1_ddelta + dhp2_ddelta';
dK_dD1 = dh1_dD1 + dh2_dD1' + dhp1_dD1 + dhp2_dD1';
dK_dD2 = dh1_dD2 + dh2_dD2' + dhp1_dD2 + dhp2_dD2';
dK_dl = dh1_dl + dh2_dl' + dhp1_dl + dhp2_dl';

C0 = disimKern1.di_variance;
C1 = sqrt(disimKern1.variance);
C2 = sqrt(disimKern2.variance);
C3 = disimKern1.rbf_variance;
K = h1 + h2' + hp1 + hp2';
K = 0.5*K*sqrt(pi);
var2 = C0*C1*C2*C3;
dk_ddelta = (sum(sum(covGrad.*dK_ddelta)))*0.5*sqrt(pi)*l*var2;
dk_dD1 = (sum(sum(covGrad.*dK_dD1)))*0.5*sqrt(pi)*l*var2;
dk_dD2 = (sum(sum(covGrad.*dK_dD2)))*0.5*sqrt(pi)*l*var2;
dk_dl = sum(sum(covGrad.*(dK_dl*0.5*sqrt(pi)*l + K)))*var2;
K = l*K;
dk_dC0 = C1*C2*C3*sum(sum(covGrad.*K));
dk_dC1 = C0*C2*C3*sum(sum(covGrad.*K));
dk_dC2 = C0*C1*C3*sum(sum(covGrad.*K));
dk_dC3 = C0*C1*C2*sum(sum(covGrad.*K));

dk_dDIVariance = dk_dC0;
dk_dDisim1Variance = dk_dC1*0.5/C1;
dk_dDisim2Variance = dk_dC2*0.5/C2;
dk_dRBFVariance = dk_dC3;

dk_dinvWidth = -0.5*sqrt(2)/(disimKern1.inverseWidth* ...
                             sqrt(disimKern1.inverseWidth))*dk_dl;

K = var2*K;

if isfield(disimKern1, 'gaussianInitial') && disimKern1.gaussianInitial && ...
  isfield(disimKern2, 'gaussianInitial') && disimKern2.gaussianInitial,
  if disimKern1.initialVariance ~= disimKern2.initialVariance
    error('Kernels cannot be cross combined if they have different initial variances.');
  end

  dim1 = size(t1, 1);
  dim2 = size(t2, 1);
  t1Mat = t1(:, ones(1, dim2));
  t2Mat = t2(:, ones(1, dim1))';

  the_rest = (exp(- delta * t1Mat) - exp(- D1 * t1Mat)) ./ (D1 - delta) .* ...
      (exp(- delta * t2Mat) - exp(- D2 * t2Mat)) ./ (D2 - delta);

  dk_dinitVariance = sum(sum((sqrt(disimKern1.variance) * ...
			      sqrt(disimKern2.variance) * ...
			      the_rest) .* covGrad));
  dk_dDisim1Variance = dk_dDisim1Variance + ...
      sum(sum((.5 ./ sqrt(disimKern1.variance) * ...
	       disimKern1.initialVariance * sqrt(disimKern2.variance) * the_rest) .* covGrad));
  dk_dDisim2Variance = dk_dDisim2Variance + ...
      sum(sum((.5 ./ sqrt(disimKern2.variance) * ...
	       disimKern1.initialVariance * sqrt(disimKern1.variance) * the_rest) .* covGrad));

  dk_dD1 = dk_dD1 + ...
	   sum(sum((disimKern1.initialVariance * ...
		    sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
		    (t1Mat * (D1 - delta).*exp(-D1*t1Mat) - exp(-delta*t1Mat) + exp(-D1*t1Mat)) ./ (D1-delta).^2 .* ...
		    (exp(- delta * t2Mat) - exp(- D2 * t2Mat)) ./ (D2 - delta)).*covGrad));

  dk_dD2 = dk_dD2 + ...
	   sum(sum((disimKern1.initialVariance * ...
		    sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
		    (t2Mat * (D2 - delta).*exp(-D2*t2Mat) - exp(-delta*t2Mat) + exp(-D2*t2Mat)) ./ (D2-delta).^2 .* ...
		    (exp(- delta * t1Mat) - exp(-D1 * t1Mat)) ./ (D1 - delta)).*covGrad));

  dk_ddelta = dk_ddelta + ...
      sum(sum((disimKern1.initialVariance * ...
	       sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
	       ((-t2Mat * (D2 - delta).*exp(-delta*t2Mat) + exp(-delta*t2Mat) - exp(-D2*t2Mat)) ./ (D2-delta).^2 .* ...
	       (exp(- delta * t1Mat) - exp(-D1 * t1Mat)) ./ (D1 - delta) + ...
		(-t1Mat * (D1 - delta).*exp(-delta*t1Mat) + exp(-delta*t1Mat) - exp(-D1*t1Mat)) ./ (D1-delta).^2 .* ...
		(exp(- delta * t2Mat) - exp(-D2 * t2Mat)) ./ (D2 - delta))).*covGrad));

  g1 = [dk_ddelta dk_dinvWidth dk_dDIVariance dk_dD1 dk_dDisim1Variance dk_dRBFVariance dk_dinitVariance];
  g2 = [0 0 0 dk_dD2 dk_dDisim2Variance 0 0];
else
  % only pass the gradient with respect to the inverse width to one
  % of the gradient vectors ... otherwise it is counted twice.
  g1 = [dk_ddelta dk_dinvWidth dk_dDIVariance dk_dD1 dk_dDisim1Variance dk_dRBFVariance];
  g2 = [0 0 0 dk_dD2 dk_dDisim2Variance 0];
end


%Author : Athi Narayanan S
%M.E, Embedded Systems,
%K.S.R College of Engineering
%Erode, Tamil Nadu, India.
%http://sites.google.com/site/athisnarayanan/

function q5=mat_dec(data1,blkx)
[m,n]=size(data1);
r3=m/blkx;c3=n/blkx;q4=0;q1=0;
for i=1:r3
   for j=1:c3
      for s=1:blkx
         for k=1:blkx
            p3=s+q4;
            q2=k+q1;
            q5(s,k,i,j)=data1(p3,q2);
         end
      end
      q1=q1+blkx;
   end
   q4=q4+blkx;q1=0;
end


% DATA = HADSST2_PARSEDATA()

% Last modified 2011-01-11
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function data = hadsst3_load_data()

filename = '/share/climate/data/HadSST3/HadSST3_mostlikely.nc';
%filename = '/share/climate/data/UK_Met_Office/HadSST2/HadSST2_nobs.nc';

longitude = nc_varget(filename, 'longitude');
latitude = nc_varget(filename, 'latitude');
[LON,LAT] = meshgrid(longitude, latitude);
coordinates = [LON(:)';LAT(:)'];

time = nc_varget(filename, 'time');
time = time(:)' + datenum([1850 0 0]);

observations = nc_varget(filename, 'sst');
%observations(observations==raw.data.FillValue) = NaN;
observations = reshape(observations, [length(time), size(coordinates, 2)])';

data = struct('observations', double(observations), ...
              'coordinates', double(coordinates), ...
              'longitude', double(longitude), ...
              'latitude', double(latitude), ...
              'time', double(time));


function B = TTeMPS_op_laplace_to_TT_matrix( A )
    %TTeMPS_to_TT Convert to TT Toolbox matrix format.
    %   TT = TT_to_TTeMPS( A ) takes the TTeMPS Laplace operator A and converts it into
    %   a tt_matrix object using the TT Toolbox 2.x from Oseledets et al.
    %   This toolbox needs to be installed, of course.
    %
    %   See also TTeMPS_to_TT, TTeMPS_op_to_TT, TTeMPS_op_laplace_to_TTeMPS_op.

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    B = TTeMPS_op_to_TT_matrix( TTeMPS_op_laplace_to_TTeMPS_op( A ));

end


function pass = test_multipleOutputs(~)
% TEST_MULTIPLEOUTPUTS   Check that syntax [u,v,w] = N\0 works.

%% BVP scalar case
dom  = [0, 2];
N = chebop(@(x,u) diff(u,2) + 0.01*sin(u), dom);
N.lbc = 0;
N.rbc = 2;
u = N\0;
pass(1) = isa(u, 'chebfun');

%% BVP scalar case with info
dom  = [0, 2];
N = chebop(@(x,u) diff(u,2) + 0.01*sin(u), dom);
N.lbc = 0;
N.rbc = 2;
[u, info] = N\0;
pass(2) = isa(u, 'chebfun') && isstruct(info);

%% IVP scalar case
dom  = [0, 2];
N = chebop(@(x,u) diff(u,2) + sin(u), dom);
N.lbc = @(u) [u; diff(u)-1];
u = N\0;
pass(3) = isa(u, 'chebfun');

%% IVP scalar case with info
dom  = [0, 2];
N = chebop(@(x,u) diff(u,2) + sin(u), dom);
N.lbc = @(u) [u; diff(u)-1];
[u, info] = N\0;
pass(4) = isa(u, 'chebfun') && isstruct(info);

%% BVP two var case, one output
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) + v; diff(v,2) + 0.1*cos(v) + u], dom);
N.lbc = @(u,v) [u - 1 ; diff(v)];
N.rbc = @(u,v) [diff(v); v + 1];
u = N\0;
pass(5) = isa(u, 'chebmatrix');

%% BVP two var case, two outputs
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) + v; diff(v,2) + 0.1*cos(v) + u], dom);
N.lbc = @(u,v) [u - 1 ; diff(v)];
N.rbc = @(u,v) [diff(v); v + 1];
[u, v] = N\0;
pass(6) = isa(u, 'chebfun') && isa(v, 'chebfun');

%% BVP two var case with info
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) + v; diff(v,2) + 0.1*cos(v) + u], dom);
N.lbc = @(u,v) [u - 1 ; diff(v)];
N.rbc = @(u,v) [diff(v); v + 1];
[u, v, info] = N\0;
pass(7) = isa(u, 'chebfun') && isa(v, 'chebfun') && isstruct(info);

%% IVP two var case
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) - v; diff(v,2) + 0.1*cos(v) - u], dom);
N.lbc = @(u,v) [u - 1 ; v + 1; diff(u); diff(v)];
u = N\0;
pass(8) = isa(u, 'chebmatrix');

%% IVP two var case, two outputs
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) - v; diff(v,2) + 0.1*cos(v) - u], dom);
N.lbc = @(u,v) [u - 1 ; v + 1; diff(u); diff(v)];
[u, v] = N\0;
pass(9) = isa(u, 'chebfun') && isa(v, 'chebfun');

%% IVP two var case with info
dom  = [0, 2];
N = chebop(@(x,u,v) [diff(u,2) + 0.1*sin(u) - v; diff(v,2) + 0.1*cos(v) - u], dom);
N.lbc = @(u,v) [u - 1 ; v + 1; diff(u); diff(v)];
[u, v, info] = N\0;
pass(10) = isa(u, 'chebfun') && isa(v, 'chebfun') && isstruct(info);

%% BVP four var case
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1];
N.rbc = @(u,v,w,y) [w + .5; y - .5];
u = N\0;
pass(11) = isa(u, 'chebmatrix');

%% BVP four var case, multiple outputs
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1];
N.rbc = @(u,v,w,y) [w + .5; y - .5];
[u,v,w,y] = N\0;
pass(12) = isa(u, 'chebfun') && isa(v, 'chebfun') && ...
    isa(w, 'chebfun') && isa(y, 'chebfun');

%% BVP four var case with info
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1];
N.rbc = @(u,v,w,y) [w + .5; y - .5];
[u,v,w,y,info] = N\0;
pass(13) = isa(u, 'chebfun') && isa(v, 'chebfun') && ...
    isa(w, 'chebfun') && isa(y, 'chebfun') && isstruct(info);

%% IVP four var case
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1; w-.5; y+.5];
u = N\0;
pass(14) = isa(u, 'chebmatrix');

%% IVP four var case, multiple outputs
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1; w-.5; y+.5];
[u,v,w,y] = N\0;
pass(15) = isa(u, 'chebfun') && isa(v, 'chebfun') && ...
    isa(w, 'chebfun') && isa(y, 'chebfun');

%% IVP four var case with info
dom  = [0, 2];
N = chebop(@(x,u,v,w,y) [ ...
    diff(u) + 0.1*sin(u) + w; ...
    diff(v) + 0.1*sin(w) + y;
    diff(w) + 0.1*sin(y) - u;
    diff(y) + 0.1*sin(u) - v], dom);
N.lbc = @(u,v,w,y) [u - 1 ; v + 1; w-.5; y+.5];
[u,v,w,y] = N\0;
pass(16) = isa(u, 'chebfun') && isa(v, 'chebfun') && ...
    isa(w, 'chebfun') && isa(y, 'chebfun') && isstruct(info);

end


function [Fval,A,f,sig,sd] = ftestc(data,params,p,plt)
% computes the F-statistic for sine wave in locally-white noise (continuous data).
%
% [Fval,A,f,sig,sd] = ftestc(data,params,p,plt)
%
%  Inputs:
%       data        (data in [N,C] i.e. time x channels/trials or a single
%       vector) - required.
%       params      structure containing parameters - params has the
%       following fields: tapers, Fs, fpass, pad
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                    (1) A numeric vector [TW K] where TW is the
%                        time-bandwidth product and K is the number of
%                        tapers to be used (less than or equal to
%                        2TW-1).
%                    (2) A numeric vector [W T p] where W is the
%                        bandwidth, T is the duration of the data and p
%                        is an integer such that 2TW-p tapers are used. In
%                        this form there is no default i.e. to specify
%                        the bandwidth, you have to specify T and p as
%                        well. Note that the units of W and T have to be
%                        consistent: if W is in Hz, T must be in seconds
%                        and vice versa. Note that these units must also
%                        be consistent with the units of params.Fs: W can
%                        be in Hz if and only if params.Fs is in Hz.
%                        The default is to use form 1 with TW=3 and K=5
%
%	        Fs 	        (sampling frequency) -- optional. Defaults to 1.
%           fpass       (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%	    p		    (P-value to calculate error bars for) - optional.
%                           Defaults to 0.05/N where N is the number of samples which
%	                 corresponds to a false detect probability of approximately 0.05.
%       plt         (y/n for plot and no plot respectively)
%
%  Outputs:
%       Fval        (F-statistic in frequency x channels/trials form)
%  	    A		    (Line amplitude for X in frequency x channels/trials form)
%	    f		    (frequencies of evaluation)
%       sig         (F distribution (1-p)% confidence level)
%       sd          (standard deviation of the amplitude C)
if nargin < 1; error('Need data'); end;
if nargin < 2 || isempty(params); params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear err trialave
data=change_row_to_column(data);
[N,C]=size(data);
if nargin<3 || isempty(p);p=0.05/N;end;
if nargin<4 || isempty(plt); plt='n';end;
tapers=dpsschk(tapers,N,Fs); % calculate the tapers
[N,K]=size(tapers);
nfft=max(2^(nextpow2(N)+pad),N);% number of points in fft
[f,findx]=getfgrid(Fs,nfft,fpass);% frequency grid to be returned
% errorchk = 0; % set error checking to default (no errors calculated)
% if nargout <= 3 % if called with 4 output arguments, activate error checking
%     errorchk = 0;
% else
%     errorchk = 1;
% end
Kodd=1:2:K;
Keven=2:2:K;
J=mtfftc(data,tapers,nfft,Fs);% tapered fft of data - f x K x C
Jp=J(findx,Kodd,:); % drop the even ffts and restrict fft to specified frequency grid - f x K x C
tapers=tapers(:,:,ones(1,C)); % add channel indices to the tapers - t x K x C
H0 = squeeze(sum(tapers(:,Kodd,:),1)); % calculate sum of tapers for even prolates - K x C
if C==1;H0=H0';end;
Nf=length(findx);% number of frequencies
H0 = H0(:,:,ones(1,Nf)); % add frequency indices to H0 - K x C x f
H0=permute(H0,[3 1 2]); % permute H0 to get dimensions to match those of Jp - f x K x C
H0sq=sum(H0.*H0,2);% sum of squares of H0^2 across taper indices - f x C
JpH0=sum(Jp.*squeeze(H0),2);% sum of the product of Jp and H0 across taper indices - f x C
A=squeeze(JpH0./H0sq); % amplitudes for all frequencies and channels
Kp=size(Jp,2); % number of even prolates
Ap=A(:,:,ones(1,Kp)); % add the taper index to C
Ap=permute(Ap,[1 3 2]); % permute indices to match those of H0
Jhat=Ap.*H0; % fitted value for the fft

num=(K-1).*(abs(A).^2).*squeeze(H0sq);%numerator for F-statistic
den=squeeze(sum(abs(Jp-Jhat).^2,2)+sum(abs(J(findx,Keven,:)).^2,2));% denominator for F-statistic
Fval=num./den; % F-statisitic
if nargout > 3
   sig=finv(1-p,2,2*K-2); % F-distribution based 1-p% point
   var=den./(K*squeeze(H0sq)); % variance of amplitude
   sd=sqrt(var);% standard deviation of amplitude
end;
if nargout==0 || strcmp(plt,'y');
   [S,f]=mtspectrumc(detrend(data),params);subplot(211); plot(f,10*log10(S));xlabel('frequency Hz'); ylabel('Spectrum dB');
   subplot(212);plot(f,Fval); line(get(gca,'xlim'),[sig sig],'Color','r');xlabel('frequency Hz');
   ylabel('F ratio');
end
A=A*Fs;